Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Closed

Conversation

pepisg
Copy link
Contributor

@pepisg pepisg commented Nov 2, 2023


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

image

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
Copy link
Member

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

nav2_planner/src/planner_server.cpp Show resolved Hide resolved
nav2_planner/src/planner_server.cpp Outdated Show resolved Hide resolved
curr_start = goal->goals[i - 1];
if (allow_path_end_pose_deviation_) {
// pick the end of the last planning task as the start for the next one
curr_start = concat_path.poses.back();
Copy link
Member

@SteveMacenski SteveMacenski Nov 2, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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:
Screenshot from 2023-11-06 07-56-09
Hybrid:
Screenshot from 2023-11-06 08-09-23
Lattice:
Screenshot from 2023-11-06 08-30-42

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

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pepisg any update?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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
Copy link
Member

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
Copy link
Contributor Author

pepisg commented Dec 17, 2023

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
Copy link
Member

SteveMacenski commented Dec 18, 2023

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
Copy link
Contributor Author

pepisg commented Dec 19, 2023

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
Copy link
Member

SteveMacenski commented Dec 22, 2023

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.

Copy link
Contributor

mergify bot commented Jan 4, 2024

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

@SteveMacenski
Copy link
Member

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants