Apply suggestions

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

Author: CihatAltiparmak

nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml

@@ -12,15 +12,18 @@
         <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
         <RateController hz="1.0">
           <RecoveryNode number_of_retries="1" name="ComputePathToPose">
-            <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
+            <Sequence>
+              <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
+              <SmoothPath unsmoothed_path="{path}" smoothed_path="{smoothed_path}" smoother_id="simple_smoother" error_code_id="{smoother_error_code}" error_msg="{smoother_error_msg}"/>
+            </Sequence>
             <Sequence>
               <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
               <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
             </Sequence>
           </RecoveryNode>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">
-          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+          <FollowPath path="{smoothed_path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
           <Sequence>
             <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
             <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>

nav2_smoother/include/nav2_smoother/smoother_utils.hpp

@@ -99,10 +99,10 @@ inline void updateApproximatePathOrientations(
   reversing_segment = false;
 
   // Find if this path segment is in reverse
-  dx = path.poses[1].pose.position.x - path.poses[0].pose.position.x;
-  dy = path.poses[1].pose.position.y - path.poses[0].pose.position.y;
+  dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x;
+  dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y;
   theta = atan2(dy, dx);
-  pt_yaw = tf2::getYaw(path.poses[0].pose.orientation);
+  pt_yaw = tf2::getYaw(path.poses[1].pose.orientation);
   if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) {
     reversing_segment = true;
   }

nav2_smoother/src/simple_smoother.cpp

@@ -81,27 +81,29 @@ bool SimpleSmoother::smooth(
   std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
 
   for (unsigned int i = 0; i != path_segments.size(); i++) {
-    // Populate path segment
-    curr_path_segment.poses.clear();
-    std::copy(
-      path.poses.begin() + path_segments[i].start,
-      path.poses.begin() + path_segments[i].end + 1,
-      std::back_inserter(curr_path_segment.poses));
-
-    // Make sure we're still able to smooth with time remaining
-    steady_clock::time_point now = steady_clock::now();
-    time_remaining = max_time.seconds() - duration_cast<duration<double>>(now - start).count();
-    refinement_ctr_ = 0;
-
-    // Attempt to smooth the segment
-    // May throw SmootherTimedOut
-    smoothImpl(curr_path_segment, reversing_segment, costmap.get(), time_remaining);
-
-    // Assemble the path changes to the main path
-    std::copy(
-      curr_path_segment.poses.begin(),
-      curr_path_segment.poses.end(),
-      path.poses.begin() + path_segments[i].start);
+    if (path_segments[i].end - path_segments[i].start > 3) {
+      // Populate path segment
+      curr_path_segment.poses.clear();
+      std::copy(
+        path.poses.begin() + path_segments[i].start,
+        path.poses.begin() + path_segments[i].end + 1,
+        std::back_inserter(curr_path_segment.poses));
+
+      // Make sure we're still able to smooth with time remaining
+      steady_clock::time_point now = steady_clock::now();
+      time_remaining = max_time.seconds() - duration_cast<duration<double>>(now - start).count();
+      refinement_ctr_ = 0;
+
+      // Attempt to smooth the segment
+      // May throw SmootherTimedOut
+      smoothImpl(curr_path_segment, reversing_segment, costmap.get(), time_remaining);
+
+      // Assemble the path changes to the main path
+      std::copy(
+        curr_path_segment.poses.begin(),
+        curr_path_segment.poses.end(),
+        path.poses.begin() + path_segments[i].start);
+    }
   }
 
   return true;