Controller is too conservative in its collision checking

[rokusottervanger]

The controller checks if there is any cell within the collision polygon that is of value costmap2d::INSCRIBED_INFLATED_OBASTACLE. If there is, it concludes that a collision is imminent. This is not true. the robot footprint is perfectly allowed to enter space of costmap2d::INSCRIBED_INFLATED_OBASTACLE value. Here's an example where there is overlap, but no collision:

The cheap way to check for imminent collisions, assuming a (approximately) circular base is to check if the cell in which (a predicted) base_link is located is >= costmap2d::INSCRIBED_INFLATED_OBASTACLE. That would mean a guaranteed collision.

However, with a non-circular base, this is not enough. Because a different orientation may also lead to collision before base_link reaches this space. Instead, you can check if any of the cells in the collision polygon exceeds this value. That means it reaches a value of >= costmap2d::LETHAL_OBSTACLE.

[Timple]

Why are the obstacles inflated in the first place?

[rokusottervanger]

Because you can either disable the inflation entirely, then you don't get the exponential decay cost either (and you do want that, for velocity scaledown and all that), or you can enable it, and that also gives you the footprint inflation. The concept is explained quite well here: http://wiki.ros.org/costmap_2d#Inflation

[Timple]

From the image:

cost_inscribed = definitely in collision
I assume that this is where the logic originated from. But they probably meant that the center-point is definitely in collision.
Am I summarizing this correctly?

[Timple]

In that case the simple fix would be to modify the logic.

if nav_link is costmap2d::INSCRIBED_INFLATED_OBASTACLE, early-out. Otherwise proceed to more expensive footprint-check.

[rokusottervanger]

I assume that this is where the logic originated from. But they probably meant that the center-point is definitely in collision.
Am I summarizing this correctly?

Not sure who you mean by "they". The costmap2d authors, or the path_tracking_pid author(s).

If the center point (base_link) is within the costmap2d::INSCRIBED_INFLATED_OBASTACLE zone, the footprint definitely collides with an obstacle somewhere. If that's what you mean, you're correct.

if nav_link is costmap2d::INSCRIBED_INFLATED_OBASTACLE, early-out. Otherwise proceed to more expensive footprint-check.

Sounds good, assuming those (predicted) poses are available. That avoids the need for the footprint check in case this is already in collision. This early return doesn't save you much computing power, though. In the nominal case there is no collision, so there won't be an early return.

In any case, the footprint check needs to use the condition >= costmap2d::LETHAL_OBSTACLE instead of costmap2d::INSCRIBED_INFLATED_OBASTACLE

[Timple]

In any case, the footprint check needs to use the condition >= costmap2d::LETHAL_OBSTACLE instead of costmap2d::INSCRIBED_INFLATED_OBASTACLE

Agreed! PR Welcome

[rokusottervanger]

In any case, the footprint check needs to use the condition >= costmap2d::LETHAL_OBSTACLE instead of costmap2d::INSCRIBED_INFLATED_OBASTACLE

After inspecting the code a little further, I suggest making only this change. An early return based on checking base_link with the inflated obstacle layer almost never occurs, so it makes the code more complex without adding any significant benefit. Additionally, you lose the visualization of where the obstacle is detected if you only check base_link with the inflated obstacle layer.

[Timple]

Agreed!