backward motion at begining of the path(maybe a bug) generated by nav2_smac_planner (hybrid a star)

[wang-fire]

Bug report

Required Info:

• Operating System: Ubuntu22.04
• Computer:Jetson Orin
• ROS2 Version:ROS2 humble_main branch
• Version or commit hash: humble_main branch , latest
• DDS implementation:

Steps to reproduce issue

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 4.0
      publish_frequency: 1.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.03
      footprint: "[[-0.275, -0.33], [-0.275, 0.33], [0.275, 0.33], [0.275, -0.33]]"
      #robot_radius: 0.22
      plugins: ["obstacle_layer", "inflation_layer"]
      filters: ["keepout_filter"]
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: False
        filter_info_topic: "keepout_costmap_filter_info"
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.80
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: <obstacle_scan_topic>
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          inf_is_valid: 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: <robot_prefix>base_link
      #robot_radius: 0.22
      footprint: "[[-0.275, -0.33], [-0.275, 0.33], [0.275, 0.33], [0.275, -0.33]]"
      resolution: 0.03
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer", "speed_filter"]
      filters: ["keepout_filter"]
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: False
        filter_info_topic: "keepout_costmap_filter_info"
      speed_filter:
        plugin: "nav2_costmap_2d::SpeedFilter"
        enabled: <speed_enabled>
        filter_info_topic: "speed_costmap_filter_info"
        speed_limit_topic: <speed_limit_topic>
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: <obstacle_scan_topic>
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          inf_is_valid: 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.80
      always_send_full_costmap: True
      service_introspection_mode: "disabled"

planner_server:
  ros__parameters:
    expected_planner_frequency: 25.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              # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
      tolerance: 0.10                     # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
      allow_unknown: False                 # allow traveling in unknown space
      max_iterations: 1000000             # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
      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: 60         # Number of angle bins for search
      analytic_expansion_ratio: 3.5       # The ratio to attempt analytic expansions during search for final approach.
      analytic_expansion_max_length: 1.5  # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
      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.5        # minimum turning radius in m of path / vehicle
      reverse_penalty: 10.0                # Penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.25                 # Penalty to apply if motion is changing directions (L to R), must be >= 0
      non_straight_penalty: 1.3           # Penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 3.0                   # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
      retrospective_penalty: 0.015
      lookup_table_size: 3.0             # Size of the dubin/reeds-sheep distance window to cache, in meters.
      cache_obstacle_heuristic: False     # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
      debug_visualizations: True         # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance.
      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"        # DEFAULT, BIDIRECTIONAL, ALL_DIRECTION
      smooth_path: False                   # If true, does a simple and quick smoothing post-processing to the path
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1.0e-10
        do_refinement: true
        refinement_num: 2

smoother_server:
  ros__parameters:
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      refinement_num: 2
      # True for Hybrid A* or State Lattice to not smooth over directional changes
      enforce_path_inversion: True
      do_refinement: True

Expected behavior

There should be no backward motion at the beginning of the route.

Actual behavior

1. have a backward motion at the beginning of the route

Reproduction instructions

1. backward motion occurs even when it is unnecessary, and it still happens even if I increase the retreat reverse penalty parameter to 100.
2. The backward motion is always one step at start of the route.
3. This problem almost always occurs when the target point is behind the starting point; when the target point is in front of the starting point, the probability of this issue is very low.

Additional information

[SteveMacenski]

There should be no backward motion at the beginning of the route.

Why not?

It looks like that is the shortest path, by doing that maneuver, it sets itself up at a better orientation in order to make the turn and then fluidly go into the analytic expansion. It probably does help with being the shortest path.

But, lets look into this. It looks like the reverse penalty might be applied for the first expansion https://github.com/ros-navigation/navigation2/blob/main/nav2_smac_planner/src/node_hybrid.cpp#L401-L403. That seems like something that should be straight forward to fix - would you be interested in giving it a shot? 😄

[wang-fire]

@SteveMacenski Thank you very much for your reply. I still don't fully understand your thoughts; here are some of my thoughts.

Why do I think it is a bug?

1. Because whenever the target point is behind the starting point, the path starts by retreating one step.
2. In the vast majority of scenarios, it only retreats one step.
   This is a very strange behavior, so I believe it is a bug.

Question 1

1. You mean that using this line of code to determine if it is the first point is incorrect; the _motion_primitive_index of the first point is undefined. It may not be std::numeric_limits<unsigned int>::max(), but more likely it is 0. If I want to fix this issue, I can set it to std::numeric_limits<unsigned int>::max() in the constructor.
2. There is also a question about the getTraversalCost function. This function calculates the cost to reach the child, but why is it using the parent's _motion_primitive_index? Is my understanding incorrect?

Question 2
I am not yet familiar enough with this package. Can you give me some ideas on how to explore why the first step retreats? If I find the cause of the issue, I am very willing to contribute.

[SteveMacenski]

But, lets look into this. It looks like the reverse penalty might be applied for the first expansion https://github.com/ros-navigation/navigation2/blob/main/nav2_smac_planner/src/node_hybrid.cpp#L401-L403. That seems like something that should be straight forward to fix - would you be interested in giving it a shot? 😄

What I'm explaining here is why that behavior is happening - due to this line skipping the scoring when its the first expansion. We can still apply some of the penalties, like the reverse penalty, but it just isn't being done yet.

In that condition statement, I'm suggesting you find a way to identify if the transition from the node to the child is a reversing move to apply the reverse penalty (maybe others as well?). If I solved all the problems for you, then it wouldn't be fun to work on, now would it? 😉

[wang-fire]

But, lets look into this. It looks like the reverse penalty might be applied for the first expansion https://github.com/ros-navigation/navigation2/blob/main/nav2_smac_planner/src/node_hybrid.cpp#L401-L403. That seems like something that should be straight forward to fix - would you be interested in giving it a shot? 😄

What I'm explaining here is why that behavior is happening - due to this line skipping the scoring when its the first expansion. We can still apply some of the penalties, like the reverse penalty, but it just isn't being done yet.

In that condition statement, I'm suggesting you find a way to identify if the transition from the node to the child is a reversing move to apply the reverse penalty (maybe others as well?). If I solved all the problems for you, then it wouldn't be fun to work on, now would it? 😉

I believe I’ve figured it out. The getTravelCost function calculates the cost from the parent to the child. However, if it detects that the parent node is the first node and directly returns a constant cost, that behavior is unreasonable. In other words, the expansion cost for the first node is incorrect. Therefore, to fix this issue, I need to remove the handling logic for the “first node.” After removing it, I tested the change and achieved the expected results.

Is my understanding correct?

[SteveMacenski]

Test it out and let me know 😉 Make sure to look at the operations happening after this one. Is anything there using the parent's information that may or may not exist given that it is the first node?

[wang-fire]

Test it out and let me know 😉 Make sure to look at the operations happening after this one. Is anything there using the parent's information that may or may not exist given that it is the first node?

After removing the code related to the “first node” check, I didn’t find any parts of the remaining getTravelCost logic that were affected. I also tested various scenarios, including:

1. When the goal point is in front of the robot — everything works normally.

2. When the goal point is behind the robot — this case now also behaves correctly, no longer showing the issue where the robot first moves backward and then moves forward.

3. When the goal and start positions are the same — the generated path contains only one point, and no abnormalities occur.

Here are some of my observations and understanding:

1. Given the current context of this function, removing the “first node” condition doesn’t seem to cause any issues, because getTravelCost is always used by the parent node to evaluate the cost to its child nodes. In this context, all child nodes appear to be non-nullptr. I also tested the case where the start and goal are the same — meaning the final path contains only one point — and no problems occurred.

2. However, considering that getTravelCost might be used in other scenarios in the future, for safety, we can add a check to see whether the child node is nullptr. If it is, we can return a 0 cost. This would prevent potential crashes in situations where a nullptr child node might appear during expansion.

So the final solution would be either:

1. Simply remove the condition related to the “first node,” or

2. Remove the “first node” condition and add a check to determine whether the child node is nullptr. If it is nullptr, return a cost of 0.

In the current situation, I would prefer to choose option 1.

[SteveMacenski]

Please open a PR! Glad you found that it works just fine -- we didn't used to have that function formulated in that way so it looks like now that we focus on the child's metdata rather than the parent, this is all OK!