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 <stevenmacenski@gmail.com>

* Rename parameter to allow_path_through_poses_goal_deviation

* Fix description

* restore nav2_params

* missing whitespace

* lint fix

* removed parameter

Signed-off-by: gg <josho.wallace@gmail.com>

* Update planner_server.hpp

* Update planner_server.cpp

---------

Signed-off-by: gg <josho.wallace@gmail.com>
Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: enricosutera <enricosutera@outlook.com>

Author: 3 people

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];