SmacPlanner-Hybrid generate really not optimal path

[FrGrQuim]

Bug report

Required Info:

• Operating System:
  • Linux ubuntu 22.04
• ROS2 Version:
  • Iron binary
• Version or commit hash:
  • 1.2.9
• DDS implementation:
  • CyclonDDS

Steps to reproduce issue

1 - provide to the smac planner two goals that are placed like this:

The smac planner have the following parameters:

SmacPlannerHybrid:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      max_iterations: -1
      max_planning_time: 30.0              # max time in s for planner to plan, smooth
      minimum_turning_radius: 1.8         # minimum turning radius in m of path / vehicle
      analytic_expansion_max_length: 2.0
      cost_penalty: 100.0
      use_quadratic_cost_penalty: True
      cache_obstacle_heuristic: True

Expected behavior

The planner output this kind of path:

Actual behavior

The planner outputs these kind of paths.

or

Additional information

• When we periodically re-compute the path, sometimes, the SmacPlanner hybrid generates a good path (similar to the one described in the Expected behavior section).
• Our robot follows a rectangular pattern, and this issue seems to always occur in the same corners. In other corners, the SmacPlanner consistently generates a near-optimal path.

Do you have any idea why this behavior occurs and how to fix it?

[SteveMacenski]

Sorry for the delay, ROSCon and ROS-I (+ a short vacation) kept me from my terminal.

This does seem concerning to me and worth looking into. Can you create a reproducable example of this happening so that I can personally look into it? We have a nice tool that Dexory put together (https://github.com/botsandus/planner_playground) which you can put in your planner, global costmap parameters and map. Then, you can set the start/goal point so that we can see this in action in a reproducable way to debug.

The first path you sent seems on the surface reasonable -- like the minimum turning radius is just slightly larger than what the robot can accomplish from its pose, so you need to do that maneuver by the constraints you set on the turning radius. I'd have to see precise start / goal poses and do some math to know if we're on the right side of that calculation or not to know if there's an issue.

The second path is very weird and clearly not correct. This is actually so simple that the analytic expansion should be able to immediately find a route to that goal without even going through the planning phase. Though I can't tell scale, if its less than analytic_expansion_max_length, it should be able to just "do it".

What are the green arrows though? If those are NavigateThroughPose (or eq.) pose constraints that the path has to go through, that would explain all of the 2 images you showed. It very well may not have been possible from the minimum turning radius you set to be able to go from the robot pose into the first arrow around a round-about maneuver. Eyeballing it, it looks a little short to me, but again I'd need reproducable exact numbers to calculate radiuses based off of to understand if its possible to do the direct path with your settings. If you modestly reduce your turning radius in these examples, does this go away? If so, that would point to the radius of turn you requested given the constraints being smaller than the minimum turning radius the planner is allowed to use.

I do note that your cost_penalty seems very, very high. Not that I think that it is causing issues w.r.t. this current issue, I would like to know if you set that to something more ... modest like 2.0 if this issue persists. It could be that you penalize small amounts of cost so high that it does wacky things which are much, much longer to above some cost. Though, I assume what you're showing is your global costmap and it doesn't show any cost in the planning region (on the other hand, I don't know what that polygon and the points in it are representing or feeding into the costmap or not).

[FrGrQuim]

Hi Steve,

I've managed to reproduce the problem using the planner_playground. I've attached the configuration I used for the tests: planner_playground_config.zip. A README file within the folder provides detailed steps on how to replicate the issue.

I've included two paths generated with the same planner from the same starting point. The notable detail is that the only difference between these two paths is the goal orientation, rotated by only 0.5°:

Short path Quaternion: (x=0.0, y=0.0, z=1.0, w=0.0)
Long path Quaternion: (x=0.0, y=0.0, z=0.9999918142371225, w=0.004046165931884697)
However, it appears that the planner interprets this slight rotation as a significantly larger difference.

Could you shed some light on why such a small change in goal orientation leads to a major deviation in path generation?

Best regards.

[SteveMacenski]

That appears to be more than 0.5 degrees from what the plans show. What happens if you set allow_primitive_interpolation: true?

I'm noticing that you have quite a large turning radius / resolution but still a very finite number of angular quantizations (72, by default, by not setting otherwise). Back of the envelope calculations show me that this set of configurations would result in each motion primitive being sufficiently wide to cause leaving 1 cell-bin to be equal to 9-10 angle quantizations -- effectively reducing the resolution of search from 72 to 7-8. That's why a seemingly small change in angle look so large, its not actually searching at the 72 resolution you asked it to due to your kinematic/costmap settings. Each primitive is then really like 45 degrees then. Yicks. And that looks like what you showed too.

By setting that to true, we make more than just pure left/right turns to the extreme to fill in the gaps. That should resolve your issue.

[FrGrQuim]

Unfortunately, this parameter is not available in the version we are using (1.2.9).

[SteveMacenski]

You'd wamt to upgrade then :-) Iron is EOL this month, so you would want to upgrade around now anyhow. The other option is looking into a backport of this feature into Iron without breaking ABI, but I'm not sure that's worth your energy if you have to move off of it in the immediate future anyhow

[SteveMacenski]

@FrGrQuim any update?

[alejandro-alonso-isi]

@SteveMacenski I had the same problem with SmacPlannerHybrid
only when using the navigation_though_poses action with this configuration

plugin: "nav2_smac_planner/SmacPlannerHybrid" 
     downsample_costmap: false           
     downsampling_factor: 1             
     tolerance: 0.5                     
     allow_unknown: False                 
     max_iterations: 1000000             
     max_on_approach_iterations: 1000    
     max_planning_time: 10.0              
     motion_model_for_search: "DUBIN"    
     angle_quantization_bins: 72        
     analytic_expansion_ratio: 3.5       
     analytic_expansion_max_length: 3.0 
     analytic_expansion_max_cost: 200.0  
     analytic_expansion_max_cost_override: false   
     minimum_turning_radius: 0.4        
     reverse_penalty: 2.0                
     change_penalty: 0.0                 
     non_straight_penalty: 1.0          
     cost_penalty: 3.5                   
     retrospective_penalty: 0.015
     lookup_table_size: 20.0            
     cache_obstacle_heuristic: false     
     debug_visualizations: false         
     use_quadratic_cost_penalty: True
     downsample_obstacle_heuristic: True
     allow_primitive_interpolation: True
     smooth_path: True                   
     use_final_approach_orientation: true
     smoother:
       max_iterations: 1000
       w_smooth: 0.3
       w_data: 0.2
       tolerance: 1.0e-10
       do_refinement: true
       refinement_num: 2

[SteveMacenski]

That probably makes sense then - if you have navigate through poses on, and you set a pose that requires some orientation change it'll do a turn. Then when you move onto the next pose that is in the opposite direction, it'll "complete" the loop. This is the maneuver that you've requested by your pose constraints, which consider the orientation of the intermediary goals since Smac is a feasible path planner.

Is that what you're seeing?