hidaka sato / WaypointManager

Waypoint generating library

Dependents:   WRS2021_mecanum_driver

waypoint_generator.hpp@0:429555e1ef06, 2021-08-23 (annotated)

Committer:
sgrsn
Date:
Mon Aug 23 17:02:16 2021 +0000
Revision:
0:429555e1ef06
Child:
1:d4b2668eea6c
one

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 0:429555e1ef06 10 WaypointGenerator(PoseClass start, PoseClass goal, PoseClass gap, double duration_time)
sgrsn 0:429555e1ef06 11 {
sgrsn 0:429555e1ef06 12 int n = (goal - start) / gap;
sgrsn 0:429555e1ef06 13 //printf("n is %d\r\n", n);
sgrsn 0:429555e1ef06 14 double time_step = duration_time / n;
sgrsn 0:429555e1ef06 15 gap = (goal - start) / n;
sgrsn 0:429555e1ef06 16 for(int i = 0; i <= n; i++)
sgrsn 0:429555e1ef06 17 {
sgrsn 0:429555e1ef06 18 auto way = start + (gap * i);
sgrsn 0:429555e1ef06 19 way = constraint(start, goal, way);
sgrsn 0:429555e1ef06 20 way.time_stamp = time_step * (double)i;
sgrsn 0:429555e1ef06 21 waypoint_list_.push_back( std::make_unique<Pose2D>(way) );
sgrsn 0:429555e1ef06 22 }
sgrsn 0:429555e1ef06 23 way_n_ = waypoint_list_.size();
sgrsn 0:429555e1ef06 24 //dump();
sgrsn 0:429555e1ef06 25 }
sgrsn 0:429555e1ef06 26
sgrsn 0:429555e1ef06 27 PoseClass constraint(PoseClass start, PoseClass goal, PoseClass way)
sgrsn 0:429555e1ef06 28 {
sgrsn 0:429555e1ef06 29 way.ConstraintUpper(goal, goal - start);
sgrsn 0:429555e1ef06 30 way.ConstraintLower(start, goal - start);
sgrsn 0:429555e1ef06 31 return way;
sgrsn 0:429555e1ef06 32 }
sgrsn 0:429555e1ef06 33
sgrsn 0:429555e1ef06 34 PoseClass GetPose(int i)
sgrsn 0:429555e1ef06 35 {
sgrsn 0:429555e1ef06 36 if(i >= way_n_) return *(waypoint_list_[way_n_-1]);
sgrsn 0:429555e1ef06 37 else return *(waypoint_list_[i]);
sgrsn 0:429555e1ef06 38 }
sgrsn 0:429555e1ef06 39
sgrsn 0:429555e1ef06 40 void dump()
sgrsn 0:429555e1ef06 41 {
sgrsn 0:429555e1ef06 42 for(int i = 0; i<waypoint_list_.size(); i++)
sgrsn 0:429555e1ef06 43 {
sgrsn 0:429555e1ef06 44 printf("x:%d, y:%d, th:%d, t:%d\r\n"
sgrsn 0:429555e1ef06 45 , (int)((*(waypoint_list_[i])).x*1000)
sgrsn 0:429555e1ef06 46 , (int)((*(waypoint_list_[i])).y*1000)
sgrsn 0:429555e1ef06 47 , (int)((*(waypoint_list_[i])).theta*1000)
sgrsn 0:429555e1ef06 48 , (int)((*(waypoint_list_[i])).time_stamp*1000)
sgrsn 0:429555e1ef06 49 );
sgrsn 0:429555e1ef06 50 }
sgrsn 0:429555e1ef06 51 }
sgrsn 0:429555e1ef06 52
sgrsn 0:429555e1ef06 53 private:
sgrsn 0:429555e1ef06 54 std::vector<std::unique_ptr<PoseClass>> waypoint_list_;
sgrsn 0:429555e1ef06 55 int way_n_;
sgrsn 0:429555e1ef06 56 };
sgrsn 0:429555e1ef06 57
sgrsn 0:429555e1ef06 58 #endif
sgrsn 0:429555e1ef06 59