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?
User | Revision | Line number | New 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 |