Open corot opened 3 months ago
How often does this happen for the choice of cost scaling factor? I.e. is what you show every single time or just on a rare occasion? Are you (reasonably) sure this doesn't also happen for values of 3/8?
This is odd, I agree. When this happens, printing some data would be nice to see what these are returning in terms of cost.
If you uncomment this block (and adjust the resolution/center), what's the heuristic look like? https://github.com/open-navigation/navigation2/blob/main/nav2_smac_planner/src/node_hybrid.cpp#L717-L732
Happens consistently, 100% reproducible. Yes, I tried a bunch of values:
as weird as it can get!
I enabled the code, and I only get zeros in either case
OK, I won't get to it this week, but this has been noted by me as something I'm going to personally investigate. If you have time / interest to look into it yourself more to make progress, that can make this move faster to a resolution.
OK, I won't get to it this week, but this has been noted by me as something I'm going to personally investigate. If you have time / interest to look into it yourself more to make progress, that can make this move faster to a resolution.
sorry.... I already tried and failed to find the problem...
I can confirm that I have the exact same issue. I was (in Galactic) used to have the cost_scaling_factor
with value 5.
But since upgrading to Humble (patch 8 from February 2024) recently, I wondered why the Humble's SmacPlannerHybrid was barely (/ not at all) able to find any routes when using bigger robot footprints. I tested also with the Rolling version (downloaded/build at 4.4.2024, almost synonym to Jazzy at this point) to see with the debug_visualizations
option, how the routes are generated, and got the following results:
With a footprint size of around 4m x 4m, things still work:
But when increasing it to over 5m x 5m, things seem to break:
But when using the 5m x 5m and reducing the cost_scaling_factor
from 5 to 3, everything works again nicely:
The (relevant) parameters used (for 4m x 4m, commented for 5m x 5m):
planner_server:
ros__parameters:
planner_plugins:
- GridBased
use_sim_time: false
GridBased:
plugin: nav2_smac_planner::SmacPlannerHybrid
debug_visualizations: true
tolerance: 0.01
downsample_costmap: false
downsampling_factor: 1
allow_unknown: false
max_iterations: 100000
max_on_approach_iterations: 1000
max_planning_time: 10.0
motion_model_for_search: DUBIN
cost_travel_multiplier: 2.0
angle_quantization_bins: 64
analytic_expansion_ratio: 3.5
analytic_expansion_max_length: 2.0
minimum_turning_radius: 0.4
reverse_penalty: 2.1
change_penalty: 0.0
non_straight_penalty: 1.2
cost_penalty: 2.0
retrospective_penalty: 0.025
rotation_penalty: 5.0
lookup_table_size: 20.0
cache_obstacle_heuristic: true
allow_reverse_expansion: false
smooth_path: true
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 1e-10
do_refinement: true
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: false
rolling_window: false
resolution: 0.05
plugins:
- static_layer
- inflation_layer
inflation_layer:
plugin: nav2_costmap_2d::InflationLayer
enabled: true
inflation_radius: 4.16 # 5.46 for 5m x 5m (autoscripted)
cost_scaling_factor: 5.0
inflate_unknown: true
inflate_around_unknown: true
static_layer:
plugin: nav2_costmap_2d::StaticLayer
map_subscribe_transient_local: true
enabled: true
subscribe_to_updates: true
transform_tolerance: 0.1
always_send_full_costmap: false
footprint: '[[2.08, 2.08], [-2.08, 2.08], [-2.08, -2.08], [2.08, -2.08]]' # 4m x 4m, works always
#footprint: '[[2.73, 2.73], [-2.73, 2.73], [-2.73, -2.73], [2.73, -2.73]]' # 5m x 5m, does not work with cost scaling factor 5
This "cost scaling factor 5" behaviour happens both in the Humble version as well as in the one month old Rolling version. In Galactic, there is not such an issue. Seems that there was quite a lot of (heuristical calculation) changes to the Smac Planner in the migration from Galactic to Humble, so likely something there created this "side effect". E.g.
"Replacing the wavefront heuristic with a new, and novel, heuristic dubbed the obstacle heuristic."
sound like a potential source for this, but I haven't done any further/deeper investigation of the actual cause. Luckily the issue is avoidable by just a simple parameter value change (thanks to @corot for finding this affecting parameter), so that will be enough for me now 🙂
As an extra info, the cost_scaling_factor
with value 3 is not again enough when the footprint goes even bigger. Here, a frame size of 13m x 13m will have again the same issues:
inflation_layer:
plugin: nav2_costmap_2d::InflationLayer
enabled: true
inflation_radius: 13.26 # autoscripted to match footprint size
cost_scaling_factor: 3.0
inflate_unknown: true
inflate_around_unknown: true
static_layer:
plugin: nav2_costmap_2d::StaticLayer
map_subscribe_transient_local: true
enabled: true
subscribe_to_updates: true
transform_tolerance: 0.1
always_send_full_costmap: false
footprint: '[[6.63, 6.63], [-6.63, 6.63], [-6.63, -6.63], [6.63, -6.63]]'
In the end, putting the cost_scaling_factor
to (its default value) 1 seems to be the best option here, as it does not seem to cause weird behaviours with a wide range of robot footprint sizes.
That is so frecking strange, thanks @ahopsu for more follow up and detail. This definitely needs to be looked into.
I was debugging another issue but I think I ran into this as well with SmacPlannerHybrid
I was using the planner playground from @doisyg, here is my branch with the params to reproduce: https://github.com/angsa-robotics/planner_playground/tree/custom-testing-angsa
I have been playing around with this for the last day or so. I have some observations but no reason as to why my fix works.
I believe the root cause is here:
https://github.com/ros-navigation/navigation2/blob/d1ad6401fa8939a8eee6c6d15b31cd656995ecb5/nav2_smac_planner/src/node_hybrid.cpp#L677
If this is changed to INSCRIBED
the planner works.
This led me down a rabbit-hole trying to understand why there might have been a different condition for footprint vs radius calculations. So I started looking at the adjustedFootprintCost
function.
https://github.com/ros-navigation/navigation2/blob/d1ad6401fa8939a8eee6c6d15b31cd656995ecb5/nav2_smac_planner/src/node_hybrid.cpp#L551 calculates the distance from robot centre to obstacle by reversing the inflation layer cost function.
The inscribed radius is removed a few lines later:
https://github.com/ros-navigation/navigation2/blob/d1ad6401fa8939a8eee6c6d15b31cd656995ecb5/nav2_smac_planner/src/node_hybrid.cpp#L554
(Note, this could be achieved on the first line by leaving out the scale_factor * min_radius
term)
The distance-from-inscribed-circle-to-obstacle is sent to the inflation layer to have a cost calculated for this distance: https://github.com/ros-navigation/navigation2/blob/d1ad6401fa8939a8eee6c6d15b31cd656995ecb5/nav2_smac_planner/src/node_hybrid.cpp#L561 But when this calculation is performed, the inscribed radius is subtracted again: https://github.com/ros-navigation/navigation2/blob/d1ad6401fa8939a8eee6c6d15b31cd656995ecb5/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp#L159
If the double inflation radius subtraction is removed, it breaks again. I think this is because if you do that the maths ends up converting from cost->distance->cost, but because costs are quantised to integer values it ends up rounding some of them down which is a problem if the original cost was 253 and it goes to 252 - invalid becomes valid.
Of course, if the double subtraction is removed then there is no point to the adjustedFootprintCost
function. I don't understand the intention/implementation of this function well enough to suggest a fix.
tl;dr Changing the rejection condition from OCCUPIED
to INSCRIBED
makes things work. The adjustedFootprintCost
function probably needs a revisit.
If this is changed to INSCRIBED the planner works.
Graphically, can you show the planning expansion footprints before and after (we have a debug topic for that now)? I want to make sure its not overly pruning all possible footprints that any part of them are in collision with the environment. Obviously if you have confined space and a big robot, parts of the robot's footprint need to be able to enter inscribed space.
why there might have been a different condition for footprint vs radius calculations
Allow me to explain my thinking.
Robot radius checks happens based on the robot's center cost. So the heuristic's use of getCost(x,y)
is the same cost as would be seen when computing the traversal costs in getTraversalCost
. All good there.
However, robot footprint checks however compute the cost of the footprint edges, not the center cost, and return the single highest value (isNodeValid
's collision checker). Thus, to make sure that the heuristic's cost is measuring the same thing as traversal cost, we need to consider what the approximate cost at the edge of a footprint would be at that heuristic expansion point. If currently expanding at cell getCost(x,y)
as the center, we need to add back in the radius and check what that cost would be, which is what adjustedFootprintCost
aims to do.
Its not exact (since no orientation and not using the footprint), but its approximate and we use the inscribed radius, not the circumscribed radius, so its conservative. The cost used by the heuristic is thus always at least the cost that the robot would actually experience with the traversal cost function. If the robot is non-circular, it could be higher actually, but getting closer to the value (and measuring the same thing between T
and H
) fixed some problems with long-range planning in confined environments from some users. The better the heuristic is at estimating, the more efficient the search.
I'll note that this feature was added while I was working on a train in the UK, so I'm just as suspicious as you now that maybe in fixing that problem I introduced another in missing a detail - but it did hugely improve the problem I was tackling at the time, so its not all bad!
But when this calculation is performed, the inscribed radius is subtracted again: navigation2/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
I'm thinking now that this was an oversight on my part. With the context above on the heuristic needing footprint-edge costs instead of center-costs, I'd like to know what you suggest we do here instead? This treatment of the adjusted costs did really improve performance for non-circular robots non-trivially, so I think that's still the right thing to do, even if the implementation of how we accomplish that changes.
I'm hoping you can review my other uses of computeCost
, as around the same time, I changed some of the collision checking in MPPI, Smac, and a utils method to use that method and now I feel the need to have an outside opinion validate me or tell me where I messed that up.
Thanks for this dive!
(Note, this could be achieved on the first line by leaving out the scale_factor * min_radius term)
Oh, if we keep that in there, please do open a PR for that. I was using the same conversion code from another part of the codebase and then modified it to remove the radius without adjusting the original term.
Bug report
Not sure if this is a bug, but the behavior is surely puzzling, so I think it's worth reporting. I found that, for certain values of
cost_scaling_factor
parameter (I saw with 4.0 and 5.0), the Hybrid A* search becomes way less effective. Indeed looks like the obstacles heuristic somehow stops working, e,gcost_scaling_factor = 3
cost_scaling_factor = 8
cost_scaling_factor = 5 !!!!
This only happens with a non-circular footprint, so I guess it's related with how we calculate the cost here. But I cannot imagine how changing this values to an intermediate value can have such a dramatic effect. In the turtlebot 3 little world, it still finds a valid path, but I discover this on a much larger map where it was unable to find a path in several seconds.
Required Info:
Steps to reproduce issue
Replace
nav2_bringup/params/nav2_params.yaml
with the parameter file below. It's similar to the default params except:Replace
nav2_bringup/maps/turtlebot3_world.pgm
with the attached map (with a wall bisecting the map with a single passage far from the robot)Run TB3 demo. Don not move the robot
Add display for
/expansions
topic on RViz (planner_server/GridBased/debug_visualizations must be true)Send goals to the other half of the world, as in the pictures above