Allow path end pose deviation revive (ros-navigation#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]> Signed-off-by: Brice <[email protected]>
BriceRenaudeau · Jan 29, 2024 · f3e8c2c · f3e8c2c
1 parent c290215
commit f3e8c2c
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
@@ -400,7 +400,10 @@ void 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];