nav2_smac_planner, path is not continuous

[wang-fire]

Required Info:

Operating System: Ubuntu22.04

Computer: X86_64

ROS2 Version:ros-humble（**humble_main branch**）

Version or commit hash: humble_main:latest

Steps to reproduce issue

Simulation Environment: Turtlebot3 + gazebo + nav2_smac_planner (Hybrid a star)

Question

I am using humble_main branch, nav2_smac_planner, I believe this route is divided into two segments: one segment is the forward extension, and the second segment is generated by the analytic_expansion. My questions are:
1) The endpoint of the forward extension and the starting point of the analytic_expansion should be coincide, rather than overlap?
2) If question1) is right, I want to avoid any backward motion caused by 1）. Can this be achieved through a smoothing plugin?

#### Tuning / Configuration Goal
<!-- Description in a few sentences what the aim is that you're trying to tune / configure -->

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 4.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      # footprint: "[[-0.27, -0.32], [-0.27, 0.32], [0.27, 0.32], [0.27, -0.32]]"
      footprint: "[[-0.125, -0.125], [-0.125, 0.125], [0.125, 0.125], [0.125, -0.125]]"
      #robot_radius: 0.22
      plugins: ["voxel_layer", "inflation_layer"]
      # filters: ["keepout_filter"]
      # keepout_filter:
      #   plugin: "nav2_costmap_2d::KeepoutFilter"
      #   enabled: True
      #   filter_info_topic: "costmap_filter_info"
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.40
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          # A relative topic will be appended to the parent of the local_costmap namespace.
          # For example:
          #   * User chosen namespace is `tb4`.
          #   * User chosen topic is `scan`.
          #   * Topic will be remapped to `/tb4/scan` without `local_costmap`.
          #   * Use global topic `/scan` if you do not wish the node namespace to apply
          topic: scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True
      service_introspection_mode: "disabled"

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      #robot_radius: 0.22
      # footprint: "[[-0.27, -0.32], [-0.27, 0.32], [0.27, 0.32], [0.27, -0.32]]"
      footprint: "[[-0.125, -0.125], [-0.125, 0.125], [0.125, 0.125], [0.125, -0.125]]"
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      # filters: ["keepout_filter"]
      # keepout_filter:
      #   plugin: "nav2_costmap_2d::KeepoutFilter"
      #   enabled: True
      #   filter_info_topic: "costmap_filter_info"
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          # A relative topic will be appended to the parent of the global_costmap namespace.
          # For example:
          #   * User chosen namespace is `tb4`.
          #   * User chosen topic is `scan`.
          #   * Topic will be remapped to `/tb4/scan` without `global_costmap`.
          #   * Use global topic `/scan` if you do not wish the node namespace to apply
          topic: scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.40
      always_send_full_costmap: True
      service_introspection_mode: "disabled"

map_server:
  ros__parameters:
    # yaml_filename: "map.yaml"
    service_introspection_mode: "disabled"

map_saver:
  ros__parameters:
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True
    service_introspection_mode: "disabled"

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    planner_plugins: ["GridBased"]
    costmap_update_timeout: 1.0
    service_introspection_mode: "disabled" 
    GridBased:
      plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::"
      downsample_costmap: False           # whether or not to downsample the map
      downsampling_factor: 2             
      tolerance: 0.10                     
      allow_unknown: true                 # allow traveling in unknown space
      max_iterations: 1000000            
      max_on_approach_iterations: 1000    # Maximum number of iterations after within tolerances to continue to try to find exact solution
      max_planning_time: 5.0              # max time in s for planner to plan, smooth
      motion_model_for_search: "REEDS_SHEPP"    # Hybrid-A* Dubin, Redds-Shepp
      angle_quantization_bins: 72         # Number of angle bins for search
      analytic_expansion_ratio: 1.0       # The ratio to attempt analytic expansions during search for final approach.
      analytic_expansion_max_length: 1.5  
      analytic_expansion_max_cost: 200.0  # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal
      analytic_expansion_max_cost_override: False  #  Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
      minimum_turning_radius: 0.40        # minimum turning radius in m of path / vehicle
      reverse_penalty: 100.0                # Penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.3                 # Penalty to apply if motion is changing directions (L to R), must be >= 0
      non_straight_penalty: 1.5           # Penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 5.0                  
      retrospective_penalty: 0.015
      lookup_table_size: 5.0             # Size of the dubin/reeds-sheep distance window to cache, in meters.
      cache_obstacle_heuristic: False    
      debug_visualizations: True         
      use_quadratic_cost_penalty: False
      downsample_obstacle_heuristic: False
      allow_primitive_interpolation: True
      coarse_search_resolution: 2         # Number of bins to skip when doing a coarse search for the path. Only used for all_direction goal heading mode.
      goal_heading_mode: "DEFAULT"       
      smooth_path: False                  
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1.0e-10
        do_refinement: true
        refinement_num: 2

[SteveMacenski]

Does this happen on Humble or Jazzy branch? I'm wondering if some recent work introduced a regression #4127. That work is not backported to Humble / Jazzy so if you don't see it there, that would point to an issue with this PR which touches the part of the code that could cause something like this.

This should absolutely not be happening with the only exception: If your robot's turning radius is too large to make a turn, it might actually choose to go backwards at a turn, to go forward again from another angle in order to make the turn. Think of it as a 3-point turn maneuver. Please verify that this is not the case first before we go further (but from these 2 images it looks like its at the junction with the analytic expansions, so I don't think that's the case - it looks like a regression. You'd see this behavior elsewhere than just the switch to the analytic expansions if it were a turning radius related issue).

FYI @stevedanomodolor I think this points out a potential issue with your PR

[stevedanomodolor]

Does this happen on Humble or Jazzy branch? I'm wondering if some recent work introduced a regression #4127. That work is not backported to Humble / Jazzy so if you don't see it there, that would point to an issue with this PR which touches the part of the code that could cause something like this.

This should absolutely not be happening with the only exception: If your robot's turning radius is too large to make a turn, it might actually choose to go backwards at a turn, to go forward again from another angle in order to make the turn. Think of it as a 3-point turn maneuver. Please verify that this is not the case first before we go further (but from these 2 images it looks like its at the junction with the analytic expansions, so I don't think that's the case - it looks like a regression. You'd see this behavior elsewhere than just the switch to the analytic expansions if it were a turning radius related issue).

FYI @stevedanomodolor I think this points out a potential issue with your PR

I will verify if i get this behavior locally because the analytical expansion should have the same behavior as is. What was done was only do more copmutation when you have multiple goals.

[wang-fire]

Does this happen on Humble or Jazzy branch? I'm wondering if some recent work introduced a regression #4127. That work is not backported to Humble / Jazzy so if you don't see it there, that would point to an issue with this PR which touches the part of the code that could cause something like this.

This should absolutely not be happening with the only exception: If your robot's turning radius is too large to make a turn, it might actually choose to go backwards at a turn, to go forward again from another angle in order to make the turn. Think of it as a 3-point turn maneuver. Please verify that this is not the case first before we go further (but from these 2 images it looks like its at the junction with the analytic expansions, so I don't think that's the case - it looks like a regression. You'd see this behavior elsewhere than just the switch to the analytic expansions if it were a turning radius related issue).

FYI @stevedanomodolor I think this points out a potential issue with your PR

I have not encountered this issue on the Humble branch.

I also found two other issues in analytic_expansion.cpp on this branch:

1） On line 100 and line 119 (bool score = refineAnalyticPath(current_node, current_goal_node, getter, analytic_nodes);), the refineAnalyticPath function returns a float value, but it is assigned to a variable of type bool.

2）If refineAnalyticPath executes successfully, the subsequent association of the current_node is incorrect because the parent of the path generated by the refine analytical expansion is no longer the current_node.

@SteveMacenski @stevedanomodolor

[stevedanomodolor]

Does this happen on Humble or Jazzy branch? I'm wondering if some recent work introduced a regression #4127. That work is not backported to Humble / Jazzy so if you don't see it there, that would point to an issue with this PR which touches the part of the code that could cause something like this.

This should absolutely not be happening with the only exception: If your robot's turning radius is too large to make a turn, it might actually choose to go backwards at a turn, to go forward again from another angle in order to make the turn. Think of it as a 3-point turn maneuver. Please verify that this is not the case first before we go further (but from these 2 images it looks like its at the junction with the analytic expansions, so I don't think that's the case - it looks like a regression. You'd see this behavior elsewhere than just the switch to the analytic expansions if it were a turning radius related issue).

FYI @stevedanomodolor I think this points out a potential issue with your PR

I have not encountered this issue on the Humble branch.

I also found two other issues in analytic_expansion.cpp on this branch:

1） On line 100 and line 119 (bool score = refineAnalyticPath(current_node, current_goal_node, getter, analytic_nodes);), the refineAnalyticPath function returns a float value, but it is assigned to a variable of type bool.

2）If refineAnalyticPath executes successfully, the subsequent association of the current_node is incorrect because the parent of the path generated by the refine analytical expansion is no longer the current_node.

@SteveMacenski @stevedanomodolor

Nice catch 😌, my mistake 😅. Can you test it and see if you still get the issue you are having? I also noticed the two issues you mentioned, but can not test until the evening.

[wang-fire]

Does this happen on Humble or Jazzy branch? I'm wondering if some recent work introduced a regression #4127. That work is not backported to Humble / Jazzy so if you don't see it there, that would point to an issue with this PR which touches the part of the code that could cause something like this.
This should absolutely not be happening with the only exception: If your robot's turning radius is too large to make a turn, it might actually choose to go backwards at a turn, to go forward again from another angle in order to make the turn. Think of it as a 3-point turn maneuver. Please verify that this is not the case first before we go further (but from these 2 images it looks like its at the junction with the analytic expansions, so I don't think that's the case - it looks like a regression. You'd see this behavior elsewhere than just the switch to the analytic expansions if it were a turning radius related issue).
FYI @stevedanomodolor I think this points out a potential issue with your PR

I have encountered the same issue on the Jazzy branch.
I also found two other issues in analytic_expansion.cpp on this branch:
1） On line 100 and line 119 (bool score = refineAnalyticPath(current_node, current_goal_node, getter, analytic_nodes);), the refineAnalyticPath function returns a float value, but it is assigned to a variable of type bool.

2）If refineAnalyticPath executes successfully, the subsequent association of the current_node is incorrect because the parent of the path generated by the refine analytical expansion is no longer the current_node.

@SteveMacenski @stevedanomodolor

Nice catch 😌, my mistake 😅. Can you test it and see if you still get the issue you are having? I also noticed the two issues you mentioned, but can not test until the evening.

Test again， I have encountered the same issue on the Jazzy branch

The other two issues I mentioned earlier have been fixed. Can you give me some ideas on how to fix the issue of the analytic path and forward path being discontinuous? This problem occurs about once in every ten attempts in my environment and has been bothering me for 1-2 days.

[stevedanomodolor]

@SteveMacenski The issue still persist, i tried this git commit b6a5387. One before my merge. See video below. Myabe it is configuration issue?. I am using the exact same configuration as @wang-fire

Screencast.from.2025-05-27.21-31-39.webm

[SteveMacenski]

@stevedanomodolor do you see it after your fix? From both of your experiences, it is always just '1' that is off?

Can this be reproduced with the defaults in general? Does adjusting cost_penalty reverse_penalty and non_straight_penalty to be more aligned with our defaults change anything (leaving the rest the same)? I'm curious if there's an actual "need" for this behavior in this tuning in order to maximize scoring. When you have the cost penalty so high, its possible that it incentivizes taking small maneuvers in the reeds-shepp path to set itself up to reduce the amount of cells it spends in non-0-costed space later. I have shown in the past if I blow up the penalties in extreme directions I can get somewhat odd behavior for correct reasons. The reeds-shepp analytic expansion will do a refinement stage and its possible in that from the cost score that it chooses to go backwards for a bit in order to get setup at a better angle for reducing integrated path cost.

[slazarev8]

Hello!
I'm afraid I've discovered another issue related to this PR. Specifically, a planner (I use the lattice one, humble_main branch) can sometimes generate an incorrect x (and possibly y) value in the goal pose, resulting in an entirely incorrect path to the another, not target, position. I tested reverting commit f5543c3, and that resolved the problem. But I have no idea how is it possible.

[stevedanomodolor]

Hello!
I'm afraid I've discovered another issue related to this PR. Specifically, a planner (I use the lattice one, humble_main branch) can sometimes generate an incorrect x (and possibly y) value in the goal pose, resulting in an entirely incorrect path to the another, not target, position. I tested reverting commit f5543c3, and that resolved the problem. But I have no idea how is it possible.

How can i reproduce this, what goals are you sending? I am not getting this behavior locally. Can you with this pr #5198 to see if you get the x and y issue.
Also lets stick to one branch, the one with the fix, so we don't mix problems.

[slazarev8]

How can i reproduce this, what goals are you sending?

I use diff platform with reverse_expansion, the footprint is rectangle. The goal is about 10m far away from robot. Not sure other details are important, because I don't use full nav2 stack.

Can you with this pr #5198 to see if you get the x and y issue.

No, this fix does not treat my issue =(

[stevedanomodolor]

How can i reproduce this, what goals are you sending?

I use diff platform with reverse_expansion, the footprint is rectangle. The goal is about 10m far away from robot. Not sure other details are important, because I don't use full nav2 stack.

Can you with this pr #5198 to see if you get the x and y issue.

No, this fix does not treat my issue =(

To prevent from mixing issues and given that your stack is not full nav2, while i work on understanding the initial issue with the path including backward and forward movement, i recommend you look and log into the input that gets sent to the planners. You can find this here

navigation2/nav2_smac_planner/src/smac_planner_hybrid.cpp

Line 449 in 1278df7

_goal_heading_mode, _coarse_search_resolution);

.

The x y, seem to be an issue that would be quite noticeable when it happens and i have not seen it happen.

Maybe @SteveMacenski has more insight on the x y issue.