Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Closed
karl-schulz opened this issue Jul 17, 2021 · 5 comments

Comments

@karl-schulz
Copy link
Contributor

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).

image

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:

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
Copy link
Contributor Author

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

@SteveMacenski
Copy link
Member

SteveMacenski commented Jul 19, 2021

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
Copy link
Contributor Author

karl-schulz commented Jul 19, 2021

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
Copy link
Member

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
Copy link
Member

merging

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants