Revision 1:d4b2668eea6c, committed 2021-08-25

Comitter:

sgrsn

Date:

Wed Aug 25 08:53:29 2021 +0000

Parent:

0:429555e1ef06

Commit message:

Change the waypoint generation algorithm

Changed in this revision

diff -r 429555e1ef06 -r d4b2668eea6c waypoint_generator.hpp
--- a/waypoint_generator.hpp	Mon Aug 23 17:02:16 2021 +0000
+++ b/waypoint_generator.hpp	Wed Aug 25 08:53:29 2021 +0000
@@ -7,9 +7,15 @@
 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;
+        /*int n = (goal - start) / gap;
         //printf("n is %d\r\n", n);
         double time_step = duration_time / n;
         gap = (goal - start) / n;
@@ -20,7 +26,7 @@
             way.time_stamp = time_step * (double)i;
             waypoint_list_.push_back( std::make_unique<Pose2D>(way) );
         }
-        way_n_ = waypoint_list_.size();
+        way_n_ = waypoint_list_.size();*/
         //dump();
     }
     
@@ -37,9 +43,16 @@
         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++)
+        /*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)
@@ -47,12 +60,15 @@
             , (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