SmacPlannerHybrid plans close to obstacles even with high cost penalty

[samiamlabs]

Bug report

• Operating System:
  • Ubuntu 20.04
• ROS2 Version:
  • Galactic binaries
• Version or commit hash:
  • output of: dpkg-query --show "ros-$ROS_DISTRO-navigation2"
  • ros-galactic-navigation2 1.0.8-1focal.20220203.051427
• DDS implementation:
  • Fast-RTPS

Steps to reproduce issue

Hi! This is a bit of an edge-case use case for the SmacPlannerHybrid planner. I'm using it with 0.5 meters costmap resolution and the smoother disabled (by setting max_iterations to 0). The cost_penalty is 3.5 in the images below. For the inflation layer, cost_scaling_factor = 0.3 and inflation_radius was about 5 mteres

Expected behavior

I would like the planner to respond to the inflated global costmap and not plan so close to obstacles when cost_penalty is high (more like the pained green line in the picture below). I have tried to increase cost_penalty way past the recommended 3.5, but this did not help.

Actual behavior

Well, it often prioritizes driving straight and the shortest path and drives close to obstacles even with high cost_penalty.

[SteveMacenski]

Have you tried to tune your inflation layer as well? I see the high costs degrade pretty quickly. The dials between inflation layer exponential functions and cost penalty are intrinsically related. NavFn doesn't expose its penalty (though similarly structured as this) and gives you only the exponential to work with. Smac gives you both so there is a bit of ebb-and-flow that you need to perform to get the desirable outcome.

The cost function for smac and navfn, while different, they "rhyme" with each other so I think you should largely be able to get the behavior you want out of this if you had it with NavFn -- within the kinematic limits of the platform, obviously.

It would also be good to understand what it is you're trying to get out of this and making sure that expectation is reasonable. I'm looking at the green path and to me, given the size of your robot, that looks pretty good, especially considering your very coarse costmap resolution relative to the platform size. I think with better inflation parameters you can push it out a bit more into the center of the space, but there are limits of how much you can realistically penalize cost vs length without making the compute time spike to something unreasonable. But I'm sure there are improvements to be made prior to that point.

Also mind that a local trajectory planner would be / could be easily tuned to penalize cost as well to deviate. Though personally I would like my planner to get as good as it can be so that the local trajectory planner needs to do as little work as possible. There are other components available to you as well.

[samiamlabs]

Thank you for your detailed answer @SteveMacenski.

Just to make sure we are on the same page about the green path. I painted the green path manually to show the behavior I would like. The blue path is what the planner gives me.

I tried to tune the inflation layer to degrade as slowly as possible with the gradient still visible (layer_cost_scaling = 0.1) but I still get the same behavior.

I get pretty much the same straight blue path even when I set extreme values like non_straight_penalty=1.01 and cost_penalty=100

About what I'm trying to get out of this:

I'm working on a robot that picks up golf balls at ranges. The robot has a turning radius constraint of about 2.5 m. SmacPlannerHybrid and RegulatedPurePursuitController have been working well for both point-to-point navigation and for following coverage plans. There are some flags and signs etc. on the range that the robot needs to avoid, and SmacPlannerHybrid avoids them without issue. What I'm trying to do now is to vary the inflation around obstacles while the robot is driving around to get it to not drive exactly the same path around obstacles every time. This is to prevent ugly tire marks in the ground.

[SteveMacenski]

Ooof, ok yeah clearly something is wrong there and that is not the behavior that I see myself in use, so lets investigate that. That blue path is unaccaptable and not representative of what you should be seeing.

I would ask you to clone main and grab the nav2_smac_planner directory, replace that with your galactic one and try again (you may need to change the WeakPtr for the node in the on_configure() constructor to SharedPtr and remove the lock() call). Let me know if you still see that behavior.

What I suspect is happening is that there is a straight-line solution to the path available at the start pose. We check if there are analytic expansions possible periodically during planning and use them if they are collision free. I suspect that is what you are seeing there. In main, we made a change that limits that by the parameter analytic_expansion_max_length (default 3.0m) that would only use the analytic solution if the path is less than that value. This allows actual search algorithm to drive the narrative until within that boundary and then let the analytic solutions "bring it home". I believe this is not (yet) backported to galactic due to ABI breaking changes but is well tested and in use in ros2 / main.

This is a unique issue because there is a straightline path that is technically valid from start - goal, even if suboptimal. Folks pointed that out and came up with the limited length solution which works unreasonably well for its simplicity.

We said that we'd revisit it though if it was insufficient and we could also add a maximum cost (e.g. analytic_expansion_max_cost) as well so that it would reject maneuvers like what you see where it skirts the obstacle unnecessarily. We can absolutely chat about that if you find what the current state in main isn't sufficient for your needs. But I suspect it should be fine. What you requested with the green line is very reasonable.

Note: if this is your issue, setting your inflation / cost penalty to infinity will do nothing; the analytic expansions don't pay attention to cost, only validity. But you should not be seeing this behavior if there is no straightline path immediately available (but then could happen once one is).

[ErikOrjehag]

It appears to have problems with the costmap even if we don't have straight line of sight with the goal. I work on the same project as samuel btw.

cost_penalty: 3.0
reverse_penalty: 2.0
change_penalty: 0.0
non_straight_penalty: 1.0

[samiamlabs]

I tried grabbing the nav2_smac_planner from main like you suggested @SteveMacenski and that did the trick!

The version from main also deals a lot better with the labyrinth Erik posted above

On a side note: The galactic planner could only have goal coordinates in the middle of cells and it looks like the same is true for main. I get that this is not really an issue with the standard 0.05 resolution but with 0.5 it becomes one. We fixed the issue on the galactic branch but it was a bit hard to get the fix to compile for all the planners and not break any tests.

[samiamlabs]

For clarification, the grid in the picture is set to 0.5 meters and is the same as the costmap grid. After the fix, the goal pose can also be like this:

[SteveMacenski]

The way we convert from continuous coordinates to the grid coordinates is using the costmap's conversion functions https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/src/smac_planner_hybrid.cpp#L276 which gives us cell centers. I think that could be updated if you wanted to be replaced and compute partial cell coordinates as well. I see no immediate reason why that would cause any issues, though yes, some tests may need to be slightly updated.

Glad to see main fixed all your problems (and a much nicer looking path in the maze)!

[SteveMacenski]

I'd be happy to have a PR to look over that would implement that if its useful to you. Closing this ticket though since the initial titled issue has been resolved. Feel free to open another ticket or PR on the cell-center bit (or anything else).