Waypoint generating library
Dependents: WRS2021_mecanum_driver
waypoint_generator.hpp
- Committer:
- sgrsn
- Date:
- 2021-08-23
- Revision:
- 0:429555e1ef06
- Child:
- 1:d4b2668eea6c
File content as of revision 0:429555e1ef06:
#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