Diff: waypoint_generator.hpp

Revision:

0:429555e1ef06

Child:

1:d4b2668eea6c

diff -r 000000000000 -r 429555e1ef06 waypoint_generator.hpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/waypoint_generator.hpp	Mon Aug 23 17:02:16 2021 +0000
@@ -0,0 +1,59 @@
+#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
+