Navigation following a set of waypoints#1327
Navigation following a set of waypoints#1327fmrico wants to merge 2 commits intoros-navigation:masterfrom
Conversation
Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>
|
What happens if you were to set a goal pose during the waypoint following? Would it be added to the queue, pre-empt the current waypoint but keep following the next, or pre-empt the entire queue and navigate only to the new goal? |
There was a problem hiding this comment.
(I'll leave the discussion about approaches to waypoint navigation to #803 and only
provide feedback about this particular solution.)
I have some minor comments and questions:
- Compute a path through all waypoints at each update?
- Should we differentiate between
NavigateToPosesandNavigateWaypoints? - Should we support providing a single waypoint at a time instead of all upfront?
| { | ||
|
|
||
| class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPose> | ||
| class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPoses> |
There was a problem hiding this comment.
Change NavigateToPoseAction to NavigateToPosesAction.
| - /Global Planner1/Global Costmap1 | ||
| Splitter Ratio: 0.5833333134651184 | ||
| Tree Height: 464 | ||
| Tree Height: 452 |
There was a problem hiding this comment.
Are the changes to this file needed?
There was a problem hiding this comment.
Yes, I should not have committed this file.
| <Fallback> | ||
| <GoalReached/> | ||
| <ComputePathToPose goal="{goal}"/> | ||
| <ComputePathToPoses goal="{goal}"/> |
There was a problem hiding this comment.
Do you think this would be a better name?
<ComputeWaypointsPath goal="{goal}"/>| @@ -1,5 +1,5 @@ | |||
| #goal definition | |||
There was a problem hiding this comment.
NavigateToPoses or NavigateWaypoints? Former could mean that robot's pose should align with intermediate poses. The latter could have a final PoseStamped msg and Point array for waypoint
There was a problem hiding this comment.
On a separate PR we could add feedback as waypoints are reached.
|
|
||
| result->path.poses.clear(); | ||
| result->path.poses.push_back(start); | ||
| for (auto current_waypoint : goal->poses.poses) |
There was a problem hiding this comment.
Here we're computing a path from the current location to final path going through all waypoints at each cycle. It might be more efficient to only compute a path to the next one or two waypoints.
Also the dwb controller will be receiving the entire path and given some configurations trying to align with respect to the final pose instead of the next waypoint.
| { | ||
| auto partial_path = planner_->createPlan(result->path.poses.back(), current_waypoint); | ||
| result->path.header = partial_path.header; | ||
| result->path.poses.insert(result->path.poses.end(), partial_path.poses.begin(), partial_path.poses.end()); |
There was a problem hiding this comment.
Run ament_cpplint, ament_uncrustify.
| result->path.poses.push_back(start); | ||
| for (auto current_waypoint : goal->poses.poses) | ||
| { | ||
| auto partial_path = planner_->createPlan(result->path.poses.back(), current_waypoint); |
There was a problem hiding this comment.
I'm not sure if this would fail immediately if a path to the current_waypoint is not possible or continue the loop for the next waypoints.
@bpwilcox I think, @fmrico correct me if I'm wrong, but when a new nav goal (defined as an array of waypoints, not as a single pose or waypoint) comes in, it will preempt the old goal (replacing the entire array of poses). I have a question about this here. |
|
I have implementation issues here but in spirit this seems like something we could do. That ticket was referencing something completely different to give an example application using the navigation stack, but I don’t think that blocks this from also being a possibility, but it does bring up some optionality this PR will need to support. Lets talk next week. I might be able to review tomorrow afternoon. |
There was a problem hiding this comment.
Ok, on a first go, I have a conceptual issue with this.
I think there's alot of complexity being added that should be placed a layer up from here. The question this asks is: should navigation care about more than its current task, and my opinion is a resounding no.
Navigation is the process of going from one place to another, it's trivial to add a layer above navigation to chose to go from one navigation goal to the next using the exposed action server interface. This lets a user specify exactly what they want to have happen in a failure mode or in a stuck condition. This adds a great deal of ambiguity as to what happens when a single point task in the task list fails or what happens in a timeout condition. These are things we can create parameters to fix, but I wonder if that's architecturally the right thing to try to propagate to the navigator level. This would specify too much for a user in terms of faulty behavior that's very case dependent.
My current opinion is that this doesn't merge, but I'm very open to changing that opinion with additional context and thoughts about why its done this way in particular.
Now that would be really terrible of me to just say "no" and not offer an alternative. I think the alternative that I like (and would be willing to implement) is a package that uses the nav2 action server to accomplish waypoint following and gives the user parameters of what to do on failure (die, or continue to next one). This is what I proposed in the ticket to both give the community that capability but also give a concrete example of a use of the navigation stack that ROS1 never provided.
| <BehaviorTree ID="MainTree"> | ||
| <Sequence name="root"> | ||
| <ComputePathToPose goal="${goal}"/> | ||
| <ComputePathToPoses goal="${goal}"/> |
| <Fallback> | ||
| <GoalReached/> | ||
| <ComputePathToPose goal="${goal}"/> | ||
| <ComputePathToPoses goal="${goal}"/> |
|
|
||
| goal_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>( | ||
| "goal_pose", | ||
| goal_sub_ = create_subscription<nav_msgs::msg::Path>( |
There was a problem hiding this comment.
You cannot change this, revert.
|
|
||
| RCLCPP_INFO(get_logger(), "Begin navigating from current location to (%.2f, %.2f)", | ||
| goal->pose.pose.position.x, goal->pose.pose.position.y); | ||
| RCLCPP_INFO(get_logger(), "Begin navigating from current location to (%.2f, %.2f) in %zu steps", |
There was a problem hiding this comment.
This isn't accurate, you're not navigating to the last in this vector
|
|
||
| // Update the goal pose on the blackboard | ||
| blackboard_->set("goal", goal->pose); | ||
| blackboard_->set("goal", goal->poses); |
| @@ -1,5 +1,5 @@ | |||
| #goal definition | |||
| geometry_msgs/PoseStamped pose | |||
| nav_msgs/Path poses | |||
There was a problem hiding this comment.
I.... don't know how I feel right now about using the nav_msgs/Path type for this. In one way of thinking about this, you could consider a set of waypoint goals to be a .... path. However, we generally talk about a path as a set of waypoints a planner creates to a goal, so this is really messing with definitions.
There was a problem hiding this comment.
Yes, I doubted if using nav_msgs/Path or geometry_smsgs/PoseStamped[]. You are right; it is not very accurate to call path to a sequence of waypoints.
|
Hi all, After reading your comments and questions in this PR, and the related issue, I think that I should change the approach. The approach presented in this PR does not support preempt, add or cancel waypoints. Monitorization is not allowed either, as information about waypoints is also lost once the path is created. I agree with @SteveMacenski ; another layer should provide this functionality. As @orduno commented on the issue, a Mission Executor or a Bt Waypoint Navigator could be a better approach. I developed a related feature using AI planning, with ROSPlan, here [1], without modifications of the original ROS Navigation packages. Instead of fixing this PR with the reviews that you made in the code, I think it would be better to close this PR and to work on any of the approaches you suggested to me. Thank you for your comments and the revisions!!! [1] "Planning Topological Navigation for complex Indoor Environments." Francisco Martín, Jonathan Ginés, David Vargas, Francisco J. Rodríguez-Lera, Vicente Matellán. 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2018), October 2018. |
|
If you’re also willing to wait on me, I can submit what I think is the way to go and hear folks thoughts. Its a few days of work but frankly you’ve started a good dialog and I think it would be good to have a bunch of concrete ideas taking shape to pick from. How do you feel about that? |
|
I think we need to do some prep work before jumping to the implementation. Here's what I suggest:
|
|
Hi @orduno We are discussing this topic both in this PR and in #803. Maybe we could unify the conversation. I agree with the requirements in #803 . I should have read this list carefully before starting my approach. I don't meet most of them. I agree with @SteveMacenski with the idea of “stupid” navigation. But, if we want that robot does not stop and ignores rotation in intermediate waypoints, some "intelligence" must be added anywhere. A solution could be complete the FollowPath action with a feedback value (percentage of completion, or distance to goal). We could implement a new behavior tree action that delivers the path to the next waypoint as soon the robot is near to reach the current waypoint. This BT action should manage the preemption or cancelation of waypoints. What do you think about this approach? Regarding implementing the approach of my paper, I could do at the current state of nav2. After porting ROSPlan to ROS2, of course, but I think that this is out of the scope of Nav2. |
|
Hi, See #1338 for what I was referring to. @fmrico let me know your thoughts.
True, but this wouldn't do that either. One of the value propositions of an action server is that you can preempt with a new goal and not stop. So its easy to modify this code to change the goal before reaching the "end" condition (within some radius of a goal) if you choose so you don't stop. This would be easier to implement in my framework I believe, however you could also implement it in yours with a custom BT node like you mention but probably using the feedback topic (that doesn't currently exist unfortunately) What paper are you referring to? Sorry, I guess I wasn't involved in that context. |
|
Based on the discussion in the Nav2 WG, we decided to use the other method to implement this (#1338). Thank you anyway for the contribution and we hope that you will continue to contribute going forward! |
|
Oh, one thing I forgot to discuss - the RViz part of this PR. I actually really like that and think it would be useful. @ fmrico, any chance you might want to provide that in a separate PR but change it to use the new Waypoint Follower in #1338? |
|
Great @mkhansen-intel !! I will make a separate PR with the RViz part. I am happy to be able to take advantage of some of the work of this PR :) |
|
@fmrico what's the rviz functionality you added, can you be specific? Is this like a "drop a bunch of markers for waypoints then execute"-type thing? |
|
Hi @SteveMacenski, It would work as shown in the video of this PR, but with the approach of #1338. I added a button for changing from single pose navigation to waypoint navigation. You can start collecting waypoints by pushing this button and using the navigation2 Goal button of the toolbar. You can cancel this process at any time. When you had selected all the waypoints, push the button again (now the label of this button is start navigation), you call the action (it will be your action in #1338 ) with all the Poses collected. The only weakness of this change in RViz plugin is that you don't see the accumulated waypoints. Maybe I could publish these waypoints using visualization markers to be displayed in RViz. What do you think? |
|
Got it, more or less what I expected. I see you hover over Feel free to PR that against my branch and I'll merge it in. That would be very helpful for me to test with when finishing up my PR. It would be trivial to make the rviz panel take the points and publish a marker at the position to keep it marked for visualization. One thing this shows that my approach wont do is plan all the waypoint paths at once. I don't know if that's good or bad, just different. I would say its bad if the plans made an attempt for continuity between waypoints (ei. smooth, continuous) but it doesn't do that since the planner has no constraints on the orientations. Also when replanning, does that replan the full trajectory between all |
No, I am not selecting anything. It's just a confusing mouse movement that I noticed when I uploaded the video :S
Ok
I think it's good as we discussed yesterday in the nav2 WG. This video shows the third design option that presented Carlos in the diagram at #803, which we decided to discard. Now, implementing the first design option, you'll be showing only the path to the next waypoint, I guess. |
|
Dear @orduno @SteveMacenski and @mkhansen-intel I know that this PR is closed, but since Nov 7th @orduno told me:
I have been working on Plansys2, which was a requirement for doing topological navigation. Today, Plansys2 is a reality, and I have an example of mixing planning and navigation I don't know if you find it interesting for any future development in Navigation2. Maybe a topological navigation? Best regards |
Basic Info
Description of contribution in a few bullet points
Future work that may be required in bullet points