[SMAC planner] inflation cost not respected on narrow corridor

[BriceRenaudeau]

Bug report

The path computed by smac planner can go in "inscribed" areas and is not pushed toward middle of free space if the corridor is a bit narrow. When the corridor is a bit wider (2nd image), the path is correct:

The purple cost is lethal cost and is created using forbidden areas.

• Full path with expansion
  

Another strange behavior is that the smoother is not applied when this bug happens. The blue line is the unsmoothed path, and the green is the smoothed one, In the first image, both paths are the same.

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Iron
• Version or commit hash:
  • iron
• DDS implementation:
  • cyclone

Steps to reproduce issue

Create a map with a narrow corridor and plan a path from one side to the other.
It can be reproduced with a smaller test environment and footprint:

Expected behavior

The path should respect the costmap and be more in the middle of the free space.

Actual behavior

The path doesn't respect the costmap and cut in the 'inscribed' cost.

Additional information

The planner config file

GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      debug_visualizations: true
      downsample_costmap: false # whether or not to downsample the map
      downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
      tolerance: 0.25 # 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: -1 # 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: 6.0 # max time in s for planner to plan, smooth
      motion_model_for_search: "REEDS_SHEPP" # Hybrid-A* Dubin, Redds-Shepp
      angle_quantization_bins: 360 # 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: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
      minimum_turning_radius: 0.4 # minimum turning radius in m of path / vehicle
      reverse_penalty: 1.01 # Penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0
      non_straight_penalty: 1.0 # Penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 1.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.1  # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier 
      lookup_table_size: 1.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 subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
      smooth_path: True # 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 

Costmap config:

  ros__parameters:
    footprint: "[ [0.335, 0.23], [0.335, -0.23], [-0.335, -0.23], [-0.335, 0.23] ]"
    # footprint_padding set in costmap specific yaml

    static_layer:
      plugin: "nav2_costmap_2d::StaticLayer"
      map_subscribe_transient_local: True

    inflation_layer:
      plugin: "nav2_costmap_2d::InflationLayer"
      cost_scaling_factor: 2.0
      inflation_radius: 2.0

[BriceRenaudeau]

I just found the culprit. It's the same that makes me create the "cost_critic" instead of the "obstacle_critic".

In narrow spaces, the getCost() returns the footprintCost but it returns INSCRIBED for every cell where the footprint passes over the INSCRIBED cost. So the planner has no info on where the middle is and all the cells have the same cost.

Same quick fix, in inCollision replace the use of the footprint_cost_ attribute by a local varaible:

navigation2/nav2_smac_planner/src/collision_checker.cpp

Lines 154 to 161 in 8c2d301

	footprint_cost_ = static_cast<float>(footprintCost(current_footprint));
	if (footprint_cost_ == UNKNOWN_COST && traverse_unknown) {
	return false;
	}
	// if occupied or unknown and not to traverse unknown space
	return footprint_cost_ >= OCCUPIED_COST;

By:

    float footprint_cost = static_cast<float>(footprintCost(current_footprint));

    if (footprint_cost == UNKNOWN && traverse_unknown) {
      return false;
    }

    // if occupied or unknown and not to traverse unknown space
    return footprint_cost >= OCCUPIED;

This keeps the result on inCollision but when using getCost(), it returns the middle cell cost.

[BriceRenaudeau]

It's not the only culprit.
It's way better and faster but the path can still go into inscribed or very close to it.

[BriceRenaudeau]

Found the impacted code: adjustedFootprintCost() must not be used in the heuristic computation.

[SteveMacenski]

Howdy - some general thoughts first:

Try turning on primitive interpolation (https://github.com/ros-navigation/navigation2/blob/main/nav2_smac_planner/src/smac_planner_hybrid.cpp#L109) -- though it is not available in Iron. I'm not sure if that'll help with the content of the ticket, but will definitely help with the wobbly path. It very well actually fix the problem by pushing the search more easily into more open areas, disincentivizing the expansions that yield plans near the inflated zone in the firstplace.

retrospective_penalty: 0.1

This is very high, I do not recommend this. This is majorly contributing to the wobbly paths you showed in the pictures. 0.005 - 0.02 is a 'reasonable' range in my opinion.

cost_penalty: 1.0

Seems low, but perfectly valid. It can be tuned for behavior w.r.t. the inflation layer, so if you're happy with the cost-repulsion of the paths, then no problem.

angle_quantization_bins: 360

See previous comment on the interpolation of primitives. I'm not sure this is doing what you think it is doing. You'd be better off with a lower value probably.

---

It's the same that makes me create the "cost_critic" instead of the "obstacle_critic". In narrow spaces, the getCost() returns the footprintCost but it returns INSCRIBED for every cell where the footprint passes over the INSCRIBED cost. So the planner has no info on where the middle is and all the cells have the same cost.

We do actually use an approximated footprint cost in the heuristic computation. Its estimated since we cannot get the actual footprint in non-feasible grid expansion, but we adjust the costs and compare it to its respective constraints on inflated for circular or literal collision for non-circular: https://github.com/ros-navigation/navigation2/blob/main/nav2_smac_planner/src/node_hybrid.cpp#L674-L682.

However, it does not look like we do this in Iron, so that would be a problem for your case in Iron if the collision checker is returning the footprint cost but the heuristic is using center points. I don't see adjustedFootprintCost() even in the Iron branch (given you said you were on Iron) for that to have been a problem for you (?). It sounds like you're maintaining a separate fork with some main features -- maybe you missed some backported feature? Can you reproduce this using the main branch version of Smac Planner?

Once on main, the collision checker should be returning footprint costs and the heuristic should be using adjusted footprint cost approximates, so they should be aligned.

Some clarification might be good since it sounds like you ripped out one, which would make sense if you only had one and not both. But if you have both footprint considerations in the collision checker and heuristic, it should be good to go.

Note though that there is some related discussion in #4314 (comment) (and my subsequent reply which is actually highly related to our conversation) that I don't recall on hand, but also relates to this same bit of code. Its on my schedule for late January to refamiliarize myself with and resolve the underlying problem, but it sounds like you're looking into this now and could be completed sooner than later. I recall and scanned through that there is some issue with the adjusted cost usage, so maybe that's what you're responding to if your Iron branch did indeed have properly implemented adjusted heuristic cost and collision checker costs.

Another strange behavior is that the smoother is not applied when this bug happens.

This point is not actually strange at all. If the smoother detects that its smoothing causes a collision, it uses the last iteration's path and returns it. This works too for the case if the plan itself is in collision. This is to prevent the smoother from making a path in collision with the environment.

[BriceRenaudeau]

Try turning on primitive interpolation

No difference.

It sounds like you're maintaining a separate fork with some main features -- maybe you missed some backported feature?

Yes, we are on a late sync "Commit 654e7e0"

retrospective_penalty: 0.005 - 0.02 is a 'reasonable' range in my opinion.

No diffeence.

cost_penalty: 1.0, Seems low, but perfectly valid. It can be tuned for behavior w.r.t. the inflation layer, so if you're happy with the cost-repulsion of the paths, then no problem.

Our footprint can be very long, and I found the computation time very related to this parameter, it seems that the expansion explodes and the computation time can take more than 10s on a huge map with narrow corridors. But with the fix mentioned here, the computation is less impacted by this parameter.

angle_quantization_bins: 360. I'm not sure this is doing what you think it is doing. You'd be better off with a lower value probably.

In some really narrow and maze-like maps, the default value of 64 prevents the planner from finding a path. Pushing it to 360 makes the planner succeed and globally faster.

We do actually use an approximated footprint cost in the heuristic computation.

Yes, like in the "obstacle_critic". But it doesn't help when the footprint_cost = "inscribed" all over the corridor.
You will keep getting the same adjusted cost, so there is no additional info to steer the expansion.

Can you reproduce this using the main branch version of Smac Planner?

I will try

Some clarification might be good since it sounds like you ripped out one, which would make sense if you only had one and not both. But if you have both footprint considerations in the collision checker and heuristic, it should be good to go.

I first found the use of footprint_cost in getCost(). And then found the adjustFootprintCost(). I create this issue with these two pieces of code active. When removing one, the path is not good (3rd comment). But when removing both. The path is straight, not in the "inscribed" area, and more in the middle. So the smoother can kick in and make a perfect plan.

Note though that there is some related discussion in #4314 (comment)

Yes, it is the same expansion problem. If the score is 0 everywhere or 253 everywhere, the expansion goes in every direction/orientation so the computation time explodes and the path can make strange loops or multiple turns.

[BriceRenaudeau]

[SteveMacenski]

Try turning on primitive interpolation

No difference.

Can you check if you have that feature in your fork? I almost can't believe that 😆 It was made to handle situations like this wobble, unless there's other things not backported. I would be interested in testing on the current state of main. The retrospective_penalty will also create wobbles, since the higher the value, the more it won't attempt to go back in the expansion tree to try new branches and just 'commit' to existing branches that might be bad.

In some really narrow and maze-like maps, the default value of 64 prevents the planner from finding a path. Pushing it to 360 makes the planner succeed and globally faster.

The primitive interpolation should really help with that. Also the image you sent I think it would help with that too! If you don't have primitive interpolation, you're not actually using 360 bins. You might have 360 bins, but if your primitive moves 8 bins each left/right turn, you're really only using 360/8 of them since the others are unreachable (I'd have to calculate what actual number of bins you use with your settings, 8 is just an example). Making the number higher and higher doesn't necessarily increase resolution unless you create additional primitives that use larger turning radius' to hit more of the orientation quantizations for exploration.

Anyway, probably different topics for a slack thread!

---

But it doesn't help when the footprint_cost = "inscribed" all over the corridor. You will keep getting the same adjusted cost, so there is no additional info to steer the expansion.

The pictures you show aren't inscribed all over the corridor though - it look to me like there's plenty of space in the middle. What I'd be interested to understand is what is the difference in distance between those 2 corridor widths, and somewhere in the middle between those 2 values, does any meaningful quantity about your robot's footprint lay? If you, for instance, made your length shorter, does it still occur?

I'm wondering if there's a problem in the collision checker in the footprint_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f calculation (or calculation of possible_collision_cost_), such that good costs are not being reported.

For example, when we're in open space, the footprint cost is the center cost, but when we're in confined space and we check everything, we're in the actual footprint cost.

I know the heuristic just uses the approximated footprint cost (which may or may not be computed correctly from that other ticket), so when we're in open space, we're underestimating moreso than when we're in confined spaces. We could adjust here too, but that's not feeling like the right answer. The heuristic can be estimated, the traversal cost function should be more concrete.

Maybe the right answer is what you said that we should just remove the optimization I made for approximate adjusted costs in the heuristic and always return center costs for the traversal cost's value. We use footprint for critical collision checking, but both the heuristic and traversal function based on center costs. That would remove the discrepancy in the cost measurements that the adjusted cost was seeking to address, just in the other way.

The problem I see with that is that

1. We have the actual footprint cost, so that would be the best actual estimate of the traversal cost when impacting search. We'd just be throwing that away and using center costs. This is mostly definitely why I went the way that I went, its better to use more precise information if possible. For long / highly non-circular platforms, that performance will yield some sub-optimal total paths I would think -- even if still known to not be in collision -- since we're not scoring based on the orientation whereas a corner could be in a pretty not great spot
2. This would change the search behavior of non-circular platforms & impact compute times. I would like you to test that these changes don't have too bad of downstream effects on the quality of the plans & the time it takes to compute them. If you find that you're satiated, I can test myself as well with some Nav2 user maps / footprint configs I have to also validate in more open / larger spaces.

Overall, you're swaying me over to your side of things. I think its just about validating that (a) the compute times don't go up, (b) the path quality stays good and (c) the change in the search behavior doesn't result in "weird" paths that get closer to obstacles in its length-wise dimension for long-non-circular robots. If those are true, I think this is a good choice.