fix edge case

Signed-off-by: ladianchad <qhrejddlvltm@gmail.com>
floatic-unicorn · Jul 5, 2023 · 287d199 · 287d199
1 parent 630a22e
commit 287d199
Showing 1 changed file with 5 additions and 3 deletions.
diff --git a/teb_local_planner/src/teb_local_planner_ros.cpp b/teb_local_planner/src/teb_local_planner_ros.cpp
@@ -661,6 +661,10 @@ bool TebLocalPlannerROS::pruneGlobalPlan(const geometry_msgs::msg::PoseStamped&
     //robot.setData( global_to_plan_transform * global_pose );
 
     double dist_thresh_sq = dist_behind_robot*dist_behind_robot;
+
+    if(global_plan.size() == 1){
+      return true;
+    }
 
     // iterate plan until a pose close the robot is found
     std::vector<geometry_msgs::msg::PoseStamped>::iterator it = global_plan.begin();
@@ -679,9 +683,7 @@ bool TebLocalPlannerROS::pruneGlobalPlan(const geometry_msgs::msg::PoseStamped&
     }
     if (erase_end == global_plan.end())
       return false;
-
-    if (erase_end != global_plan.begin())
-      global_plan.erase(global_plan.begin(), erase_end);
+    global_plan.erase(global_plan.begin(), erase_end);
   }
   catch (const tf2::TransformException& ex)
   {