Waypoint generating library
Dependents: WRS2021_mecanum_driver
waypoint_generator.hpp
- Committer:
- sgrsn
- Date:
- 2021-08-25
- Revision:
- 1:d4b2668eea6c
- Parent:
- 0:429555e1ef06
File content as of revision 1:d4b2668eea6c:
#ifndef WAYPOINT_GENERATOR_HPP #define WAYPOINT_GENERATOR_HPP #include <list> template <class PoseClass> class WaypointGenerator { public: WaypointGenerator(PoseClass start, PoseClass goal, double duration_time) { start_ = start; goal_ = goal; duration_time_ = duration_time; } 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]); } PoseClass GetPose(float t) { PoseClass way = start_ + (goal_ - start_) * (t/duration_time_); way = constraint(start_, goal_, way); return way; } 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_; PoseClass start_; PoseClass goal_; double duration_time_; }; #endif