Simple Path Smoother does not set orientaion when path segment has less than 10 poses

[CihatAltiparmak]

Bug report

Required Info:

• Operating System:
  • nav2_docker (apt updated & upgraded)
• Computer:
  • PC
• ROS2 Version:
  • rolling latest
• Version or commit hash:
  • 9cd0f9f
• DDS implementation:
  • fastrtps

Steps to reproduce issue

When robot plans so much near to the goal pose, path smoother does not smooth corresponding path segment which has less than 10 pose. So the orientations of the path poses is not calculated and assigned as zero. This situation can affect the controller considering yaw angle in a bad way or somebody may use these orientations elsewhere. at least for nonholonomic diff drive robot.

Expected behavior

The orientations of the path poses should be feasible.

Actual behavior

The orientations are not calculated correctly.

Reproduction instructions

Just apply this path and then play with it (send goal pose near to the robot).

diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
index fdc96c7f..fd9d09a6 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
+++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
@@ -12,7 +12,10 @@
         <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"/>
@@ -20,7 +23,7 @@
           </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"/>

Here is the video which shows the actual behavior.

Screencast.from.08-07-2025.01.09.53.AM.webm

Here is my fix (I need a thorough inspection as to whether my changes affect navigation pipeline in a bad way. : defensive programming method etc.)

Screencast.from.08-07-2025.01.12.50.AM.webm

Additional information

Firstly, thanks to Mister @BCKSELFDRIVEWORLD (without him, I could'nt find the problem)

I would love to hear your feedbacks and am strongly willing to contribute to fix this.

[SteveMacenski]

Ah ok! Taking it up in the ticket 👍