Allow path end pose deviation revive (#4065)

* Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]>
ros-navigation · Apr 4, 2024 · f7f0fdf · f7f0fdf
1 parent 1dea56b
commit f7f0fdf
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
@@ -408,7 +408,10 @@ PlannerServer::computePlanThroughPoses()
       if (i == 0) {
         curr_start = start;
       } else {
-        curr_start = goal->goals[i - 1];
+        // pick the end of the last planning task as the start for the next one
+        // to allow for path tolerance deviations
+        curr_start = concat_path.poses.back();
+        curr_start.header = concat_path.header;
       }
       curr_goal = goal->goals[i];