Skip to content

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

Closed
#5423
Closed
Simple Path Smoother does not set orientaion when path segment has less than 10 poses#5422
#5423
Labels
in progress
@CihatAltiparmak

Description

@CihatAltiparmak
CihatAltiparmak
opened on Aug 6, 2025

Bug report

Required Info:

  • Operating System:
    • nav2_docker (apt updated & upgraded)
  • Computer:
    • PC
  • ROS2 Version:
    • rolling latest
  • Version or commit hash:
  • 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.

Metadata

Metadata

Assignees

No one assigned

    Labels

    in progress

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions