sbpl_lattice_planner ignores footprint / creates unfeasible paths

[kajep09]

Hello

I have been trying to use the sbpl_lattice_planner as the planner for a long (~3m) rectangular robot with mecanum wheel drive.

I am facing a lot of issues when it comes to the configuration and interlink of the sbpl planner params, costmap inflation_layer params and footprint params. Some of the same issues are described in #11 (@corot @mintar), and according to that issue it seems like there was a fix - but I am still experiencing the issues now.

For example, the picture below shows the closest waypoint I can create with the robot parallel to the wall (green footprint is where the robot footprint would be for that waypoint/goal). This makes sense because any closer and the robot footprint would be in collision with the wall (or the robot center would be in collision with a lethal cost cell).

However, this picture shows the closest goal that SBPL accepts where the robot is perpendicular to the wall. The footprint of the robot is clearly in collision with the wall, but sbpl still says it can find a solution. I think this is because the robot center is not in a lethal cost cell.

This leads to behavior such as this where the robot thinks it can turn around in a narrow corridor or even park itself perpendicular to the corridor.

So I don't understand why sbpl doesn't reject solutions where the footprint is clearly in collision, and I don't understand how to correctly tune the the costmap inflation so that this doesn't happen. It seems like there is something wrong with the costmap inflation_layer's inscribed or circumscribed cost (or both?).

Any suggestions? I have tried with the newest binary release as well as building from source.

My sbpl parameters are as follows:
I have tried to play with lethal_obstacle but I don't see any change in this specific behavior between ~10 and ~253.

SBPLLatticePlanner:
  environment_type: XYThetaLattice
  planner_type: ARAPlanner
  allocated_time: 10.0
  initial_epsilon: 1.0 
  forward_search: false
  nominalvel_mpersecs: 0.2 
  timetoturn45degsinplace_secs: 8.5
  lethal_obstacle: 20

I appreciate the effort in getting the sbpl planner maintained again and I hope you can help so I can continue using it.

Thank you!.

[mintar]

I've never had to change lethal_obstacle; I'd recommend leaving it at the default of 20. The parameter specifies what value SBPL uses internally for a lethal cost, and the costmap_2d values are scaled so that costmap_2d::INSCRIBED_INFLATED_OBSTACLE = 253 is converted to lethal_obstacle = 20. I don't quite understand why, but as I said I've never had to touch that parameter.

I can explain however what's going on in your corridor pictures: The costmap_2d value at the robot center is 0, so SBPL skips the footprint check.

In detail, this is how SBPL checks if the robot is in collision (pseudocode):

if costmap value at robot center == 0:
    return "not in collision"
else if costmap value at robot center >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE:
    return "in collision"
else:
    perform detailed footprint check

So your problem is that you're landing in case (1): When the costmap value is 0, the robot is guaranteed not to be in collision, so there's no need for the footprint check. In order to land in case (3) and trigger the footprint check, you have to make sure that the costmap value at the footprint center is at least 1.

In order to do that, you'll have to fine-tune the following costmap_2d parameters:

• cost_scaling_factor: 4.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
• inflation_radius: 2.0 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.

Update: The only problematic parameter is inflation_radius (see comment below).

The best way to fine-tune these parameters is by using rqt_reconfigure and watching the result in RViz. First you should set inflation_radius to something bigger than the circumscribed radius. It's a cutoff, so if the costmap value has not yet reached 0 at this distance, it's forced to 0. This parameter isn't that important as long as it's bigger than your circumscribed radius. The parameter cost_scaling_factor is an exponential dropoff, so you have to reduce it below the default of 10. The values 4.0 and 2.0 above are rough guesstimates based on your robot's dimensions. Also see: http://wiki.ros.org/costmap_2d#Inflation .

I don't quite understand why the thing in your second picture happens: It looks like the goal pose is within the non-zero, non-lethal region of the costmap, in which case the footprint check should have been triggered. Let me know if that problem persists.

[kajep09]

Thank you for the quick answer and the explanation for when sbpl checks the footprint. I was on the right track, but I think what irritated me about the inflation_layer is that it is unintuitive to tune it for a non circular (or vaguely square) robot. I had played around with the inflation layer values a lot, but didn't realize that the planner would return "not in collision" if the robot center was at a 0 cost - which was often the case with my rectangular robot and sharp cutoff inflation layer.

However, I am not completely satisfied since I don't understand what is happening below:
First picture is cost_scaling_factor: 2.0 and inflation_radius: 1.0 and sbpl allows me to choose a goal on a non-zero cost cell, but one that is clearly in collision with the footprint.

The second picture shows the same situation but with cost_scaling_factor: 2.0 and inflation_radius: 2.0 and here sbpl correctly says there is no solution found.

In both cases the goal was set on a cell with a non-zero cost, but in the first scenario the robot (starting pose) was on a zero cost cell. That got me thinking about whether the cost of the starting point matters?
However, I went back and tested and picture 3 shows the cost_scaling_factor: 2.0 and inflation_radius: 2.0 situation but with the robot starting at a zero cost position, but the planner still has the correct behavior of rejecting the goal.
I tried to do the same experiment with forward_search: true instead of false, but it didn't seem to change the behavior.

So all in all, I don't know how to explain the situation in picture 1 in this post and picture 2 in my original post.

Thank you for the help!

[mintar]

I've just checked the code again, and I can't find anything that explains this behavior. However, the costmap parameters are only taken into account by sbpl_lattice_planner during initialization. Therefore, if you change cost_scaling_factor using rqt_reconfigure (as I recommended), and use the already-initialized SBPL afterwards, you will get undefined behavior (because SBPL gets the new costmap values, but interprets them according to the old cost_scaling_factor). You should use rqt_reconfigure only to find good values, then write those values into your yaml or launch files, and restart move_base before doing any experiments.

Funnily, during my experiments I got the opposite problem: I couldn't get SBPL to crash into a wall (which is good), but I couldn't get it to drive very close to a wall either. Update: That last problem was just due to an uncommitted change that I had made locally. It's fixed now.

[mintar]

I've found out some more things about how cost_scaling_factor and inflation_radius influence SBPL:

• SBPL computes the costmap value at the robot's circumscribed radius (which is computed from the footprint), ignoring inflation_radius (because there is no API access to inflation_radius).
• If the costmap value at the robot's center is below that threshold, we know that the nearest obstacle is farther away than the circumscribed radius, therefore no footprint check is necessary.
• If the cost_scaling_factor is too large for the robot footprint, cost_circumscribed_tresh will be 0; therefore, a footprint check is always performed. This will cause a performance penalty, but no infeasible paths.
• However, inflation_radius is taken into account by costmap_2d when computing the costmap_value_at_robot_center. If inflation_radius is < circumscribed_radius, this can lead to a situation where costmap_value_at_robot_center == 0, but cost_circumscribed_tresh > 0, even though the robot is in collision. SBPL will skip the footprint check and produce an infeasible path.

So the updated pseudocode is this:

cost_circumscribed_tresh = ((costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1) * exp(-1.0 * cost_scaling_factor * (circumscribed_radius - inscribed_radius)))

if costmap_value_at_robot_center < cost_circumscribed_tresh:
    return "not in collision"
else if costmap value at robot center >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE:
    return "in collision"
else:
    perform detailed footprint check

Takeaways:

• SBPL will only return infeasible paths if the inflation_radius is smaller than the circumscribed footprint. Adjust inflation_radius accordingly.
• To avoid a performance penalty, set cost_scaling_factor so that the cost at the circumscribed radius is greater than 0.

If you follow these guidelines, you should be good. Thanks for bringing up the issue!

[mintar]

I've pushed commit f18f010, which reinitializes SBPL when the footprint or cost_scaling_factor changes. Now it's not necessary any more to restart move_base after changing the parameters in dynamic_reconfigure.

[mintar]

One more thing regarding your pictures: SBPL only cares about the global costmap, not the local costmap. So you should make sure that you're only visualizing the global costmap (I think in some of your pictures, it might have been the local costmap instead).

[kajep09]

Thank you for all the information and the convenience changes for use with dynamic reconfigure.
I will be trying all your suggestions the next chance I get.

All the pictures in my posts are with the global costmap visualized, but some of them have been where I only changed the parameters with dyanmic_reconfigure and didn't reinitialize SBPL with the new params.
I guess other people who are going to use SBPL might run into similar issues (or what they think will be an issue :)), so maybe it would make sense to list some of these guidelines in a readme file in the package and on http://wiki.ros.org/sbpl_lattice_planner.

I will close the issue in a few days when I have had the time to test things out if that is okay.

[mintar]

Yes, I definitely plan to put the important info into a README or on the wiki page. This is one of the reasons why I wrote this down as exactly as possible, even though I know you already knew most of this. It would be great to get your feedback if everything's working as expected now. If you want to test the latest changes, you'll have to build from source.

[corot]

Very well explained @mintar, thank you a lot. As you propose, we avoid this issue by having an inflation_radius bigger than the farthest point in the footprint.

[mintar]

READMEs added.