Extend the dwb_local_planner with a split_path option

[mintar]

When split_path is set to true, upon receiving a new path from the global planner, dwb will now split the path into segments of the same movement direction (roughly forwards/backwards/in-place-rotation). The segments will be treated as if they were given by the global planner one by one. This avoids taking shortcuts during complex maneuvers.

This is especially useful when used with the SBPL global planner. For a non-holonomic robot, SBPL will plan multiple segments (driving forward, then switching driving direction to backward, sometimes turning in place). Ideally, you want each "intermediate goal" where the driving direction changes to be treated like the global goal would be: the PathAlign critic should stop trying to keep the nose close to the path, the RotateToGoal critic and the goal checker should activate and so on. Instead of modifying each critic, it's much easier to simply split the path into segments and send them to the critics one by one, which is exactly what this PR does.

The split_path option is disabled by default, so the default behavior is not changed by this PR.

Here's an example of an SBPL path where the driving direction changes (from the mir_robot Gazebo simulation):

[mintar]

FYI @niniemann

[mintar]

Note that this PR also includes the commit from #27. I've changed my mind on this; the critics really should be reset whenever the global planner replans, not when the user sends a new goal. This is important in situations where e.g. the robot is already in the goal position, but cannot turn to the final orientation because of obstacles. The new global plan might require the robot to leave the goal position again, and so the critics (like for example the RotateToGoal critic) need to be reset.

Here's a picture of such a situation (beware my mad MS Paint skillz!):

[mintar]

@ros-pull-request-builder retest this please

[SteveMacenski]

Its easier to retrigger by closing and reopening again (or adding a dummy commit)

[mintar]

Closing and reopening to retrigger tests.

[mintar]

The tests were failing because of roslint style checking. I've fixed the style and now they should pass.

@DLu : Any comments on the PR?

[SteveMacenski]

If you want to move to ROS2, I’d merge that into our Nav2 DWB version :wink:

[DLu]

I disagree a little bit with you on where this should be handled. If it were me, what I would do is to add this logic to the onNewGlobalPlan method within locomotor and then pass each piece to the local planner in turn.

However, I acknowledge that you may not be using locomotor and might be more constrained by the inflexible fist of move_base.

Could you start by extracting the main grouping logic and placing it in a method in nav_2d_utils/path_ops.h with parameter(s) replacing the constants?

[mintar]

I disagree a little bit with you on where this should be handled. If it were me, what I would do is to add this logic to the onNewGlobalPlan method within locomotor and then pass each piece to the local planner in turn.

I fully agree with that; in the long run, it would probably be better to handle it in the navigation framework, this way not just dwb_local_planner but any other local planner would automatically benefit from this.

However, I acknowledge that you may not be using locomotor and might be more constrained by the inflexible fist of move_base.

Alas, that's the problem. And because I'm firmly in the grasp of that fist for now I've put it into the dwb_local_planner.

Could you start by extracting the main grouping logic and placing it in a method in nav_2d_utils/path_ops.h with parameter(s) replacing the constants?

Done! I've also added a test for this new function.