[SMAC Planner Hybrid] Inscribed costs cause navigation with footprint to fail

[karl-schulz]

Problem Description

I encountered a problem when navigating a differential drive robot around obstacles, using not robot_radius but the footprint. The planner is able to create a feasible path around the obstacle and starts following it. When turning around, the footprint slightly touches the inflated obstacle, causing the navigation to fail. However, the robot is in safe distance to the obstacle. There is currently no way to not use the inflation layer with SMAC Hybrid (see below).

The only way to have the robot crash less is to increase the cost_penalty in the SMAC params and avoiding the inflated areas. This works fine, but leads the robot to avoid the obstacles even more - it is already staying away from them because of the inscribed costs.

This means the robot cannot pass obstacles in a distance less than its smallest radius, and even in this distance the navigation is likely to fail if we don't add more buffer by inflating the cost of close trajectories.

Explanation

The usage of footprint instead of robot_radius has multiple effects:

• The inflation layer is set to INSCRIBED (253) around obstacles with a radius of the "minimal" distance of the footprint. That makes sense in the way that if the robot center is inside an INSCRIBED cell, it is guaranteed to have a collision. But it can also have a collision when it is in a not-INSCRIBED cell.
• The SMAC CollisionChecker also uses the inflation layer to skip the cost calculation in areas where the robot center is in a cell with low inflation
  https://github.com/ros-planning/navigation2/blob/8a6607b7891c5a9ae216e9126e996de194c603eb/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp#L118-L120

I think the core issue is that the INSCRIBED cell around a LETHAL OBSTACLE is used with two different meanings:

• In the costmap inflation layer: As a "collision with the LETHAL_OBSTACLE" when the center of the robot is inside.
• As a plain collision or LETHAL_OBSTACLE itself "collision/lethal obstacle itself" for the SMAC planner and controller - even if only one edge of the footprint touches the INSCRIBED cell and the robot center is outside.

Possible Solution

• We could entirely remove the INSCRIBED costs from the inflation layer when using the footprint. The user could no more tell the robot to keep a safe distance using the costmap, but in the case of the footprint, the footprint_padding or just a artificially increased footprint would have the same effect. To me that makes sense: The INSCRIBED does in fact not really provide meaningful information: The robot may touch the obstacle, but it also might not. But I don't know what side effects that would have to other modules in the stack.
• We could tell the SMAC collision checker and the controller collision checker to not treat INSCRIBED as a collision when generating trajectories. For safety distances, footprint_padding could be used. In the planner that would mean changing code, in the controller we can just not use any inflation.

Do you have other solutions for this issue?

[karl-schulz]

I tried the second solution and it seems to work as expected.

[SteveMacenski]

You're saying you're navigating too closely, increasing the cost penalty to make it want to stay away more seems really reasonable to me.

but leads the robot to avoid the obstacles even more - it is already staying away from them because of the inscribed costs.

What's the issue with that? It'll still navigate through tight areas if required, it just navigates further away when it has the option to. Below you bring up an issue we can solve as well, we should address that.

---

As a plain collision or LETHAL_OBSTACLE itself "collision/lethal obstacle itself" for the SMAC planner and controller - even if only one edge of the footprint touches the INSCRIBED cell and the robot center is outside.

That is a good point. I think L128 should be if (footprint_cost_ == OCCUPIED) { not if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) {. Agreed? I think that's what you proposed as well. A PR to resolve would be welcome.

[karl-schulz]

Hi Steve,

yes that is basically what I think too. With the first point I was saying that because of that behavior, a 40cm wide robot can not navigate though a 50cm wide passage if the inscribe radius is 5cm on each side.

These changes worked out for us (a couple of early returns had to modified, too):
angsa-robotics@e1db29a

Apart from the SMAC collision checker, they should affect no other module except for some minor performance reduction.

[SteveMacenski]

That looks good to me, did you notice any changes in run time performance in doing so? I don't think it would be significant, but I would just like to know what the impact was, if any. Please open a PR

[SteveMacenski]

merging