Support starting on the last pose of planned path instead of the exact waypoint in compute path through poses action

[pepisg]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	None
Primary OS tested on	Ubuntu
Robotic platform tested on	Gazebo simulation

---

Description of contribution in a few bullet points

Currently the compute_path_through_poses action will fail if any of the poses lies in occupied space even if its possible to get within the planning tolerance to said pose (blue path in the image below). If the planner can plan to this pose within its tolerace, it would make sense to allow the action to plan to the next goal using the last point in the path to the occupied one instead of failing (green path in the image). This PR adds the allow_path_end_pose_deviation to the planner_server for that purpose

Description of documentation updates required from your changes

• Added new parameter, so need to add that to default configs and documentation page

---

Future work that may be required in bullet points

@SteveMacenski where should I put tests for this functionality?

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[SteveMacenski]

Tests could be in nav2_planner/tests or in nav2_system_tests/planning. I'd basically manually load up a costmap with some obstacle spots with the NavFn planner and goals on them. Then do compute path through poses & check that it returns a result and doesn't go through the obstacle points directly

[SteveMacenski]

Curious: test with Smac Hybrid, Lattice, NavFn. Does the curr_start == goal->goals[i - 1] or within floating point error of it? I believe it might. If so, we don't even necessarily need to make this a parameter nor a if/else condition. We could just do this as the default behavior and add to our migration guide that it supports deviation up to the planner's tolerance.

[pepisg]

I tested with this planners and it seems to work (hybrid and lattice seem to be harder to use in close proximity of obstacles in general with the default configuration though):
NavFn:

Hybrid:

Lattice:

We could just do this as the default behavior and add to our migration guide that it supports deviation up to the planner's tolerance.

I agree this should be the default behavior: if there's a user defined tolerance navigation should succeed as long as the planner can get within that tolerance to goals, I can't think of a good example where an user may want compute_path_though_poses to fail where that is possible. They can always set a very low tolerance if having goals in non-traversable space is a critical condition

[SteveMacenski]

Lattice shows a strange discontinuity - its hard for me to tell from the resolution of the picture of that's a problem or not. Is that within a single costmap cell (and just goal approximation within a cell issues)? Does that happen when the approximate option is disabled?

[SteveMacenski]

@pepisg any update?

[pepisg]

Hey! I havent had time to keep reviewing this, specially for writing the new tests for the navigate through poses action (planner tests seem to test the functions on planner server directly, so it looked like a complete new testing module to test the actions would have to be written).

I will try to make time for this in the coming weeks, sorry for the delay

[SteveMacenski]

I see you pushed some updates - but still a question on the discontinuity (that didn't exist before this PR?). Did you test my suggestion with just always using it? That would simplify it!

I'd really like to get this in before EOY, it seems like its just minor details away

[pepisg]

I haven't reviewed this yet. I can run some quick tests planning through multiple poses using lattice with the code as it was before the PR to check whether the discontinuity is introduced by this changes and let you know. However I don't think I will be able to make the tests this year because it seemed to me (please correct me if I'm wrong) that there's no testing module in place for calling the actions of planner server, the module calls individual functions instead, which I think means that a new testing module needs to be built, this I think will need some more time from my side

[SteveMacenski]

I can run some quick tests planning through multiple poses using lattice with the code as it was before the PR to check whether the discontinuity is introduced by this changes and let you know.

That works! Something like this I think is perfectly suitable to test using the actual system and provide screen shots as proof points.

I think it should be the case that you can always just use the last point's pose instead of the goal, so the parameter option may be moot, but its worth testing with all the planners to ensure (which would be quick / easy, only HybridA*/Lattice promise continuity in a way that could be problematic).

As long as the previous planning was successful, curr_start = concat_path.poses.back(); seems to me to work, but the continuity for the Smac planners is important to understand / fix if there's an issue.

There definitely are ways for unit tests to do this, we have the python3 API that makes it simple, but I'm not pushing for that.

[pepisg]

I spent some time today trying to reproduce the discontinuity in Lattice but wasn't able to do so despite trying to reproduce as close as possible the position and waypoints of the previous picture (i did not log the waypoints 😢 ). Here's a video of some runs

lattice.mp4

@SteveMacenski do you think any recent change may have influenced the behavior of lattice planning through multiple poses?

I think this PR does not introduce any changes to how lattice works, at the end it is my understanding that the plan through poses actions just calls individual path planning actions successively given a start and end pose. My guess is that it shouldn't make any difference in lattice itself how the initial pose of one segment was selected, and the same behavior should be obtained if you set the goal pose in a traversable tile (call it A) and that tile is selected as start for the next segment, as well as if you set the goal on an untraversable tile (call it B) and the path ends up in tile A that is within tolerance, which is selected again as start pose for the next segment.

If you can think of a better way of trying to reproduce the result of lattice on the discontinuity picture please let me know

[SteveMacenski]

Its hard for me to tell from the video's resolution -- at those transition points those seem to look pretty discontinuous? Are you using the Diff Drive model where in place rotations are possible & see that those are happening there causing that? Something clearly doesn't look right there, but that could be explained by diff drive model assuming that that's what's happening (which I can't tell from the video)

I assume all these videos are with allow_path_through_poses_goal_deviation_ = true? Do you see this same behavior with it as false (e.g. no relative change)?

What happens if you use the Ackermann model where no in place rotations are allowed? Assuming you weren't already doing that, that should be a far more clear demonstration.

[mergify]

This pull request is in conflict. Could you fix it @pepisg?

[SteveMacenski]

#4065 supersedes as a follow on. Thanks @pepisg for this great work / idea!