From f7f0fdfaaf0ae7c0315ed48aa43fb352e57c861f Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 Jan 2024 16:29:41 -0500 Subject: [PATCH] Allow path end pose deviation revive (#4065) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg Co-authored-by: pepisg Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski --- nav2_planner/src/planner_server.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 01b2384845..0e393f8304 100644 --- 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];