hidaka sato / WaypointManager

Waypoint generating library

Dependents:   WRS2021_mecanum_driver

waypoint_generator.hpp@1:d4b2668eea6c, 2021-08-25 (annotated)

Committer:
sgrsn
Date:
Wed Aug 25 08:53:29 2021 +0000
Revision:
1:d4b2668eea6c
Parent:
0:429555e1ef06 
Change the waypoint generation algorithm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:429555e1ef06 1 #ifndef WAYPOINT_GENERATOR_HPP
sgrsn 0:429555e1ef06 2 #define WAYPOINT_GENERATOR_HPP
sgrsn 0:429555e1ef06 3
sgrsn 0:429555e1ef06 4 #include <list>
sgrsn 0:429555e1ef06 5
sgrsn 0:429555e1ef06 6 template <class PoseClass>
sgrsn 0:429555e1ef06 7 class WaypointGenerator
sgrsn 0:429555e1ef06 8 {
sgrsn 0:429555e1ef06 9 public:
sgrsn 1:d4b2668eea6c 10 WaypointGenerator(PoseClass start, PoseClass goal, double duration_time)
sgrsn 1:d4b2668eea6c 11 {
sgrsn 1:d4b2668eea6c 12 start_ = start;
sgrsn 1:d4b2668eea6c 13 goal_ = goal;
sgrsn 1:d4b2668eea6c 14 duration_time_ = duration_time;
sgrsn 1:d4b2668eea6c 15 }
sgrsn 0:429555e1ef06 16 WaypointGenerator(PoseClass start, PoseClass goal, PoseClass gap, double duration_time)
sgrsn 0:429555e1ef06 17 {
sgrsn 1:d4b2668eea6c 18 /*int n = (goal - start) / gap;
sgrsn 0:429555e1ef06 19 //printf("n is %d\r\n", n);
sgrsn 0:429555e1ef06 20 double time_step = duration_time / n;
sgrsn 0:429555e1ef06 21 gap = (goal - start) / n;
sgrsn 0:429555e1ef06 22 for(int i = 0; i <= n; i++)
sgrsn 0:429555e1ef06 23 {
sgrsn 0:429555e1ef06 24 auto way = start + (gap * i);
sgrsn 0:429555e1ef06 25 way = constraint(start, goal, way);
sgrsn 0:429555e1ef06 26 way.time_stamp = time_step * (double)i;
sgrsn 0:429555e1ef06 27 waypoint_list_.push_back( std::make_unique<Pose2D>(way) );
sgrsn 0:429555e1ef06 28 }
sgrsn 1:d4b2668eea6c 29 way_n_ = waypoint_list_.size();*/
sgrsn 0:429555e1ef06 30 //dump();
sgrsn 0:429555e1ef06 31 }
sgrsn 0:429555e1ef06 32
sgrsn 0:429555e1ef06 33 PoseClass constraint(PoseClass start, PoseClass goal, PoseClass way)
sgrsn 0:429555e1ef06 34 {
sgrsn 0:429555e1ef06 35 way.ConstraintUpper(goal, goal - start);
sgrsn 0:429555e1ef06 36 way.ConstraintLower(start, goal - start);
sgrsn 0:429555e1ef06 37 return way;
sgrsn 0:429555e1ef06 38 }
sgrsn 0:429555e1ef06 39
sgrsn 0:429555e1ef06 40 PoseClass GetPose(int i)
sgrsn 0:429555e1ef06 41 {
sgrsn 0:429555e1ef06 42 if(i >= way_n_) return *(waypoint_list_[way_n_-1]);
sgrsn 0:429555e1ef06 43 else return *(waypoint_list_[i]);
sgrsn 0:429555e1ef06 44 }
sgrsn 0:429555e1ef06 45
sgrsn 1:d4b2668eea6c 46 PoseClass GetPose(float t)
sgrsn 1:d4b2668eea6c 47 {
sgrsn 1:d4b2668eea6c 48 PoseClass way = start_ + (goal_ - start_) * (t/duration_time_);
sgrsn 1:d4b2668eea6c 49 way = constraint(start_, goal_, way);
sgrsn 1:d4b2668eea6c 50 return way;
sgrsn 1:d4b2668eea6c 51 }
sgrsn 1:d4b2668eea6c 52
sgrsn 0:429555e1ef06 53 void dump()
sgrsn 0:429555e1ef06 54 {
sgrsn 1:d4b2668eea6c 55 /*for(int i = 0; i<waypoint_list_.size(); i++)
sgrsn 0:429555e1ef06 56 {
sgrsn 0:429555e1ef06 57 printf("x:%d, y:%d, th:%d, t:%d\r\n"
sgrsn 0:429555e1ef06 58 , (int)((*(waypoint_list_[i])).x*1000)
sgrsn 0:429555e1ef06 59 , (int)((*(waypoint_list_[i])).y*1000)
sgrsn 0:429555e1ef06 60 , (int)((*(waypoint_list_[i])).theta*1000)
sgrsn 0:429555e1ef06 61 , (int)((*(waypoint_list_[i])).time_stamp*1000)
sgrsn 0:429555e1ef06 62 );
sgrsn 1:d4b2668eea6c 63 }*/
sgrsn 0:429555e1ef06 64 }
sgrsn 0:429555e1ef06 65
sgrsn 0:429555e1ef06 66 private:
sgrsn 0:429555e1ef06 67 std::vector<std::unique_ptr<PoseClass>> waypoint_list_;
sgrsn 0:429555e1ef06 68 int way_n_;
sgrsn 1:d4b2668eea6c 69 PoseClass start_;
sgrsn 1:d4b2668eea6c 70 PoseClass goal_;
sgrsn 1:d4b2668eea6c 71 double duration_time_;
sgrsn 0:429555e1ef06 72 };
sgrsn 0:429555e1ef06 73
sgrsn 0:429555e1ef06 74 #endif
sgrsn 0:429555e1ef06 75