sbpl_lattice_planner ignores protruding parts of the footprint when planning?

[corot]

I use sbpl_lattice_planner on a rectangular robot with the diff drive center displaced from the center. When forced to do so (for example rotate within a narrow passage), sbpl_lattice_planner creates paths that will put the salient part of the footprint in collision, for example here: Note that the "star" points bring the robot frontal part well within the lethal obstacle area. Is this a limitation of the planner? Or, if the selected primitives are checked against footprint-costmap overlapping, can be a bug in the implementation? Any hint on which part of the code to check?

[mintar]

Same problem here:

The robot looks weird, but it actually has an 8m long rectangular footprint with the rotation center in the middle and a unicycle kinematic. Not the easiest thing to plan for.

The robot is moving down here, and the planned path clearly takes the robot footprint into collision. Even if I increase the corridor size to make it easier to find a feasible plan, sbpl still takes the robot into collision a lot. It seems that sbpl tries to follow the minimum cost in the costmap (which is in the middle of the corridor) and completely ignores the footprint.

I have checked that the footprint parameter is correctly read in the sbpl code.

I am actually using the indigo-devel branch from ricardo-samaniego/sbpl_lattice_planner, but I believe the problem is in the sbpl planner.

[corot]

Hi @mintar, did you tried to change the lethal_obstacle parameter? I set it lower than the default value (20 -> 10) because if not planner fails to provide plans through narrow passages. But I noticed that, with such a small value, the SBPLLatticePlanner::costMapCostToSBPLCost method can trunk costs to 0 on inflated cells farther than 10 cells away from an INSCRIBED_INFLATED_OBSTACLE. I use now a ceil function instead of truncating to integer, and looks like that improved a bit the problem, but didn't solve it entirely. Looks like this change makes illegal poses more expensive, but still cheap-enough to be accepted in a "valid" plan in difficould situations. Please tell me if you try this fix; I didn't PR it because I'm not confident enough that it's a real improvement.

[mintar]

Thanks @corot , that fixed half of the problem, and I found the other half: The cost of a cell at the circumscribed robot radius was overestimated, and therefore when the cost at the robot's center was below the (wrongly estimated) cost at the circumscribed radius, SBPL didn't bother to check the robot footprint, since in that case collisions are impossible.

Regarding your fix, I didn't choose ceil(), because it can round up where it shouldn't. Instead, I simply made sure that non-zero input values are always converted to at least 1, so the footprint is checked for collision.

Fixed in ricardo-samaniego/sbpl_lattice_planner#1 .

[corot]

Welcome! And thanks to you for pointing the problem with the ceil! Interestingly enough, the fork of sbpl_lattice_planner I work with already contained the fix dividing by the the costmap resolution. Looks like there are a bunch of versions of this package out there!

[mintar]

Yes, it's a mess.

Which fork are you using?

[mintar]

Fixed by #20 .