fix: check predicted footprints for LETHAL_OBSTACLE instead of INSCRIBED_INFLATED_OBSTACLE

[rokusottervanger]

Fixes #16

[Timple]

Hmm, bummer. The test fails.

This is because the collision that is expected never happens. The slow-down behavior is such that maximum-slowdown is already achieved in the light-blue area (INSCRIBED_INFLATED_OBSTACLE). So the robot keeps crawling about until a timeout occurs.

The nice part of the inflation layer was that is had a nice gradient to it. Now there is an inflation present with a fixed value (INSCRIBED_INFLATED_OBSTACLE).

Not sure how this gradient can be re-introduced here... :/

[rokusottervanger]

The same test still fails, but now because of the tracking error. We now check the costmap value at the location of base_link and the projected predictions of base_link. Because of this, the velocity remains higher for a longer time. This results in a tracking error in the U turn. This happens in every U-turn test, but this test just happens to stop before the controller converges.

IMO, this is a drawback of the tests, and not a problem this PR should solve. I think we should split up this test script into more atomic test cases. This test's responsibility is not to check the tracking error, but if stopping occurs when collision is imminent.

[rokusottervanger]

Fixed the tests anyway ;)

[Timple]

We now check the costmap value at the location of base_link and the projected predictions of base_link

So on a long rectangular vehicle, do we slow down more if the side of the vehicle is passing an obstacle, than when a corner is passing the obstacle?

[rokusottervanger]

Yes. I'm afraid that's how costmap2d was designed. In such a case, the inflation radius can be tuned to be high enough for the emerging behavior to be acceptable.

[Timple]

We do have vehicles which a significantly longer than wide. So having them slow down just a little when corners are close to obstacles seems like quite a regression :(

Can we fix this so that the obstacle <-> vehicle distance is still used, instead of the obstacle <-> base_link?

[rokusottervanger]

I discussed with @Rayman, and we found that this is a shortcoming of the assumptions made in the design of costmap2d.

In the ideal situation, the costmap would contain only distances to obstacles, providing the user (a controller or planner) with the freedom to determine its own cost related to proximity. Implementing this would render our controller incompatible with normal use of the costmap. The controller would no longer be interchangeable with others.

I agree that slowing down just a little when corners are close to obstacles does not seem appropriate, but slowing down enough can still be achieved by increasing the inflation radius and/or the cost scaling factor (the exponent of the cost decay function). The question then becomes: would this slowdown be acceptable (performance-wise) also when it is not strictly necessary because the robot happens to have a more favorable orientation?

[cesar-lopez-mar]

I do recognize this shortcoming of costmap2d. Especially in applications where there are objects all around, like warehouses, and we use a very asymmetric robot (IDA, JAX are also examples). I do think the change in this PR makes more sense than the current implementation for the reasons given by @rokusottervanger. But I think we need to change the logic how we decelerate when the robot is moving towards obstacles. An idea is that instead of using the costmap values to compute the new velocities, we should use the current velocity, a range of desired decelerations and distance to obstacles along the direction of movement. In any case we can open a new issue and design together such a strategy.

[rokusottervanger]

I agree. And for now, to get the desired speed reduction, we can just increase the inflation_radius and decrease the cost_scaling_factor with this PR merged in.

[rokusottervanger]

@Timple I wouldn't want to merge without your approval. If there's anything else you'd need, let me know, so I can make changes accordingly.

[Timple]

Thanks for waiting!

And for now, to get the desired speed reduction, we can just increase the inflation_radius and decrease the cost_scaling_factor with this PR merged in.

That won't give the exact same behavior as before right?

[rokusottervanger]

That won't give the exact same behavior as before right?

That is correct. With some tuning of the inflation layer you can get it to be at least as conservative, though.

The robot will slow down based on the distance between base_link and obstacle, instead of between its footprint and the obstacle. This means you can get the robot to slow down enough so that it is slow enough when approaching an obstacle along the long side, but that means you sacrifice some performance when approaching along the short side (you'll be slower than necessary to be safe).

@cesar-lopez-mar I noticed you already opened an issue (#71) for this, thanks!

On the flip side, the controller used to control the speed back to 0.5% of original velocity while any point of the footprint was within the inscribed radius of an obstacle. That is much more conservative (or should I say restrictive) than it is using this PR. This PR allows robots using the PTPID to drive past walls at a safe distance, with safe velocity, vs. not allowing it at all.

[Timple]

Thanks for the explanation. Lets await tomorrows discussion 👍