fix(obstacle_stop_planner): extendTrajectory function causes NaN poses (

autowarefoundation#3739) Signed-off-by: Berkay Karaman <[email protected]>
Interplai · May 19, 2023 · faf4bd0 · faf4bd0
1 parent 3feb007
commit faf4bd0
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp
@@ -455,9 +455,9 @@ TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double e
   }
 
   const auto goal_point = input.back();
-  double interpolation_distance = 0.1;
+  constexpr double interpolation_distance = 0.1;
 
-  double extend_sum = 0.0;
+  double extend_sum = interpolation_distance;
   while (extend_sum <= (extend_distance - interpolation_distance)) {
     const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point);
     output.push_back(extend_trajectory_point);