smac hybrid : Footprint collision detection does not work ?

[xianglunkai]

In the robot experiment, I found that the planned r path by HybridAsta clearly collides with obstacles, due to the size of the robot itself.
I see that the code has already considered footprint collisions, why did it not work?
It would be better to publish the footprint path of the plan

[SteveMacenski]

You will need to provide much more detailed information and some visuals. Perhaps you did not specify the footprint properly or have not set up your costmap properly.

[xianglunkai]

[xianglunkai]

Please take a look, is there a problem here?

[SteveMacenski]

You have laser scanner points in the longer range being shown which are not in the costmap (e.g. you don't see any colored cells). Consider increasing your obstacle range for inclusion.

I see the path going over some in the static layer, but its unclear to me if that's the local or global costmap - we only care about the global costmap. If you don't have the static layer in the global costmap, that could explain everything we see. The robot doesn't collide with any sensor data that is marked in the costmap, just those that are either out of range or from the static map.

I also see a suspicious lack of inflation in the inflation layer, which tells me this is poorly configured.

I think fixing your inflation layer, processing sensor data further away for obstacle layer, and checking that your static layer are enabled for the static map resolves all of these issues. The behavior of the planner seems rational to me under those conditions.

[SteveMacenski]

I spent a little bit of time thinking and I think some of the problem is definitely also the inflation layer -- but the obstacle range is definitely also part of it. We find the cost at which the robot might ever be in collision with the environment when the footprint is noncircular. For your case, that value is 0 before you don't have any meaningful inflation. That is messing, I believe, with the collision checker's optimizations to only check for full footprint collision when close to the environment.

If you make the inflation radius / gain at least have some inflation at least the robot's half-diagonal in size, then you should be good to go there. That is not usually a problem for "normal" sized robots because we have larger inflations to keep away from walls, but for micro-robots like you showed with poor inflation layer configurations, you can cause this behavior which I did not anticipate.

But my recommended advise w.r.t. inflation is here https://github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields which would have prevented the issue from ever showing up if followed. But, it is Definitely worth my time to add a warning to https://github.com/ros-planning/navigation2/blob/1b5920cc253e33a14cd94c81cccf670d13ee13cf/nav2_smac_planner/src/smac_planner_hybrid.cpp#LL193C22-L193C22 if that cost == 0 representing that its not working properly (or falling back to full-footprint checks)

But if better setting up your costmap layers fixes the issue, then I can take it as an action item from there to make sure this is more clear to future users.

[xianglunkai]

@SteveMacenski
Txs! I will try it!

[xianglunkai]

I tried it and found that you were right. I made the following modifications：

1. make the inflation radius = robot's half-diagonal in size (why it works well ?)
2. increase obstacle range from 2.5m upto 3.0m

[xianglunkai]

@SteveMacenski
sorry! I set inflation radius equal to the robot diagonal in size!

[SteveMacenski]

Great! I'll take it as an action item to better error handle for that case or at least log something. If you're interested in contributing that, you're more than welcome to, but that's my mistake and I'll take responsibility for making sure its handled sooner than later

You can set the radius even larger than that too, which is probably good. That half-diagonal size is just a minimum 😄 If you read the docs I wrote, its probably best to have more (though for simulation of that tiny environment, its probably not that important. More important in the real world)

[xianglunkai]

Txs!
I would try it as soon as possilble.

[BriceRenaudeau]

I got a similar issue. The planner was computing the path to a goal where the footprint collided with the static layer but cost < INSCRIBED_INFLATED_OBSTACLE.

This is due to the threshold used by the collision checker to reduce computation time in areas without obstacles. The possible_inscribed_cost is computed using the inflation_layer parameters. The collision checker uses the footprint only If the cost at the robot pose is greater than this threshold.

@xianglunkai, In your case (screenshot above) the inflation radius was too small. The robot pose cost is always 0 until the robot is in collision.

@SteveMacenski, I found the same behavior in MPPI obstacles_critic. It is also an issue when the footprint change. It requires the collision_checker to be updated. Now, it is only done .angle_quantization_bins is changed (smac_hybrid). In the obstacles_critic it is done when initialized. It should be triggered on footprint change.

Should I open another issue for more details?

[SteveMacenski]

The issue here is that when you don't set a "reasonable" inflation radius to cover at least half of the robot's largest cross section (and it should pretty realistically always be much greater than that), then the possibly inscribed cost is 0 so we don't trigger the full SE2 checks.

I would hope @BriceRenaudeau that you have this more properly set! But, the Smac logic is similar to MPPI so I appreciate the note, I'll make sure to make that correction in MPPI alongside Smac at the same time! This is something on my radar that I need to do, it might be a good first-dev-project-back-home thing I'll tackle next week.

It is also an issue when the footprint change. It requires the collision_checker to be updated.

is this only an MPPI issue or a Smac issue as well? Yes, this is a separate issue, that would be good to discuss in a separate thread - though I'm guessing you need this sooner than later so if there's an obvious path forward that you see, we can also just discuss over the PR to make that change :-)

[SteveMacenski]

Working on this - will have a PR open to handle shortly