Waypoint generating library
Dependents: WRS2021_mecanum_driver
Diff: waypoint_generator.hpp
- Revision:
- 0:429555e1ef06
- Child:
- 1:d4b2668eea6c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/waypoint_generator.hpp Mon Aug 23 17:02:16 2021 +0000 @@ -0,0 +1,59 @@ +#ifndef WAYPOINT_GENERATOR_HPP +#define WAYPOINT_GENERATOR_HPP + +#include <list> + +template <class PoseClass> +class WaypointGenerator +{ + public: + WaypointGenerator(PoseClass start, PoseClass goal, PoseClass gap, double duration_time) + { + int n = (goal - start) / gap; + //printf("n is %d\r\n", n); + double time_step = duration_time / n; + gap = (goal - start) / n; + for(int i = 0; i <= n; i++) + { + auto way = start + (gap * i); + way = constraint(start, goal, way); + way.time_stamp = time_step * (double)i; + waypoint_list_.push_back( std::make_unique<Pose2D>(way) ); + } + way_n_ = waypoint_list_.size(); + //dump(); + } + + PoseClass constraint(PoseClass start, PoseClass goal, PoseClass way) + { + way.ConstraintUpper(goal, goal - start); + way.ConstraintLower(start, goal - start); + return way; + } + + PoseClass GetPose(int i) + { + if(i >= way_n_) return *(waypoint_list_[way_n_-1]); + else return *(waypoint_list_[i]); + } + + void dump() + { + for(int i = 0; i<waypoint_list_.size(); i++) + { + printf("x:%d, y:%d, th:%d, t:%d\r\n" + , (int)((*(waypoint_list_[i])).x*1000) + , (int)((*(waypoint_list_[i])).y*1000) + , (int)((*(waypoint_list_[i])).theta*1000) + , (int)((*(waypoint_list_[i])).time_stamp*1000) + ); + } + } + + private: + std::vector<std::unique_ptr<PoseClass>> waypoint_list_; + int way_n_; +}; + +#endif +