-
Notifications
You must be signed in to change notification settings - Fork 1.7k
Add nav2_gps_waypoint_follower #2111
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
Conversation
|
2 major questions
The benefit of question 2 is to reduce code redundancy as much as possible. This capability could be added to with as few as 30 lines I think. Question 1 is mostly for bookkeeping and I think GPS waypoint following still falls into the waypoint following category (Q3 is then: what do we call the non-GPS waypoint following? Just waypoint following? Rename it to something else?). I don't want to overwhelm this repo with packages with single servers - and this is distinctly related to the existing package. |
Certainly possible. This is an initial draft and open to conceptual changes.
When writing, I kept that in mind therefore If you are thinking that this should be coupled with existing When calling the action also specify the pose type and fill in the relevant poses accordingly. In the action callback then we can then determine the |
|
I agree that it should be a separate action server within the waypoint follower class (with a different So I think this GPS work could go inside of the waypoint follower class. That class will expose 2 action servers for GPS or cartesian. We'll have to refactor the I suppose we could make it a parameter to which is exposed, but I think for now both is totally fine. I don't see a strong reason to add that parameter. if you don't want to use the other action, there's no requirement you do so and it has negligible run-time impacts. |
|
Hopefully the structuring is better in shape now. They are now within same class each with its own action server definition.
I am not sure if I understand here completely, it is hard for me to visualize it without seeing some code. The current form of |
SteveMacenski
left a comment
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Really good first draft. I think there's some more places we can improve 😄
|
|
||
| In the first, the ``nav2_waypoint_follower`` is fully sufficient to create a production-grade on-robot solution. Since the autonomy system / dispatcher is taking into account things like the robot's pose, battery level, current task, and more when assigning tasks, the application on the robot just needs to worry about the task at hand and not the other complexities of the system complete the requested task. In this situation, you should think of a request to the waypoint follower as 1 unit of work (e.g. 1 pick in a warehouse, 1 security patrole loop, 1 aisle, etc) to do a task and then return to the dispatcher for the next task or request to recharge. In this school of thought, the waypoint following application is just one step above navigation and below the system autonomy application. | ||
|
|
||
| In the second, the ``nav2_waypoint_follower`` is a nice sample application / proof of concept, but you really need your waypoint following / autonomy system on the robot to carry more weight in making a robust solution. In this case, you should use the ``nav2_behavior_tree`` package to create a custom application-level behavior tree using navigation to complete the task. This can include subtrees like checking for the charge status mid-task for returning to dock or handling more than 1 unit of work in a more complex task. Soon, there will be a ``nav2_bt_waypoint_follower`` (name subject to adjustment) that will allow you to create this application more easily. In this school of thought, the waypoint following application is more closely tied to the system autonomy, or in many cases, is the system autonomy. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
We also need any new parameters or actions exposed by the waypoint follower added to the navigation.ros.org website configuration docs. Also adding a node in the migration guide that WP follower now does GPS in the Foxy migration guide. The WP follower configuration page should also add a paragraph on top with the basic package description mentioning GPS waypoint following now. That's part of another documentation PR, just FYI. You don't need to do this right now, we can wait until this is ready.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yes, lets make this PR good enough to merge then navigation.ros.org will be next
nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp
Outdated
Show resolved
Hide resolved
nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp
Outdated
Show resolved
Hide resolved
| quat_tf.setRPY(0, 0, 0); | ||
| geometry_msgs::msg::Quaternion quat_msg; | ||
| tf2::convert(quat_msg, quat_tf); | ||
| curr_waypoint_in_map_frame.pose.orientation = quat_msg; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'm confused, why create a quat_tf and then convert it into a quat_msg to pass to the orientation without setting any orientation? You never used the results from the service to set the orientation.
Also an aside, but actually geometry_msgs::msg::Quaternion defaults to a identity transform (0 0 0 1) now. No need for the rest of that setting to 0/0/0
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Actually, there is one thing we might clarify here. Since GPS data has no information of orientation, if we set orientation to zero at each waypoint the robot will behave quite not smart. IMO good option here will be set orientation to current robot orientation so recovery server wont intervene if robot is unable to reorient itself. We can use tf of map->base_link to get robot orientation but is there another simpler way ?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I understand the issue you bring up (that we don't have orientation in GPS coords, so what orientation should we use?) but I don't understand the suggested solution.
My suggestion would be to modify the message and have each GPS waypoint also define an orientation for the waypoint. Then that would trickle down to the WP follower to have an orientation to use. The question then is what frame is the orientation in (GPS?) and if GPS, how to convert it to map. This must be a common thing people do so I imagine there's conventions for it in GPS-land.
I think that's the best solution, however for some users that don't fill in the waypoints completely, I agree having some reasonable default behavior if all the waypoint orientations are 0 is good, but not sure what the right option is. If you take the line segment created by wp i and wp i+1, find the map-frame orientation of the vector, and use that? That way on approach to the waypoint, the orientation will be the same as the approach vector.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I have been thinking about this so it took some time for me to fully realize. To my understanding the IMU is represented in utm coordinates.
Given that IMU reports in ENU, if we transform the orientation reported by IMU from utm to map the transformed orientation should be final orientation that we can set for goals. I have achieved this conclusion based on my understanding of robot_localization's wiki, integration of GPS data. I would appreciate if we can get insights of some people that follow here, just to confirm this. However I will lose no time and code current approach, we can always modify.

There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think what you're saying is that the orientations can be transformed using the RL service, right? It should be pretty easy to test in gazebo if you set some angles you know what they should be and see if the robot ends up there in map frame.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
using the RL service, right?
Yes but indirectly, because the broadcaster of utm ->map transform is RL so yeah basically we rely on RL here, that transform gives the goal's orientation in map frame, which robot understands correctly. I did a test in Gazebo, the approach seems to work.
As an illustration;
wp0: [49.899954067243776, 8.899896379691963, 0.6342220147844688,1.57]
wp1: [49.899954067243776, 8.899896379691963, 0.6342220147844688, 3.14]
Gazebo IMU report angle increases counterclockwise, the above waypoints positions are the same, robot reaches to first waypoint, heading 1.57 rad. left to origin of world, and then rotates 1.57 rad more , finally reaching correct orientation. This is was of course in fully perfect simulation environment, not sure how this will perform in real but we will see.
For this to work as expected though, following navsat_transform_node params needs to be set true, which will be covered in navigation.ros.org PR;
broadcast_utm_transform: true
broadcast_utm_transform_as_parent_frame: true
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
That would be good for you to mention in the navigation.ros.org PR in migration guide / WP follower configuration page. That's a subtle detail easy to miss.
nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp
Outdated
Show resolved
Hide resolved
|
I think it is time to think about how to deal with the dependency |
nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp
Outdated
Show resolved
Hide resolved
|
Good way of cleaning up the preemption. I had tried doing something similar recently and failed in the |
sfinae concept might also be useful in your case but, if available(thankfully) Constexpr_if is recommended to deal with compile time branching. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Last finishing details from a fine-tooth comb review. Just some clean up stuff to make it more in style mostly with the rest of the package.
Major item left to figure out is the orientation element we're discussing in another review comment & adding the new action server to the WP follower docs / migration guide.
Couldn't have implemented this better myself. Great job. I love the basically non-existent code increase.
|
have you tested both sets of following and still work? |
Yes they work , in case of GPS though it needs that |
How about we add new msg as and use this new msg in this can make inclusion of orientation convenient
This is in TODOs list after this gets merged or ready to be merged |
|
@jediofgever the docs need to be done in parallel to this so that we can merge them at the same time. Else the docs will be out of sync and I don't want to get in the habit of allowing for that to happen or we'd slip into a bad habit in general Other than the text of the warning, this looks good to me now, I think we can merge this once the docs are prepared |
|
@jediofgever there are 2 places that warning exists |
|
@jediofgever there's just one more small issue to update the warning statements and I think this is good to merge! (after the simple conflict resolution). Then its just on the docs PR, on the final stretch! |
Yes but they are both required I think. If the user preempted waypoint following with an empty vector then it is better we give out that information. |
|
There is one thing I realized recently, When robot_locaization's |
|
@jediofgever what I mean is that you updated the language of one of them in c6613a8 but you didn't update the language in the others to match |
| curr_pose_utm_frame.header.frame_id = "utm"; | ||
| // fromll_client converted the point from LL to Map frames | ||
| // but it actually did not consider the possible yaw offset between utm and map frames ; | ||
| // "https://github.com/cra-ros-pkg/robot_localization/blob/ |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Should a ticket be filed about this / fixed there?
| tf_buffer_->transform( | ||
| curr_pose_utm_frame, curr_pose_map_frame, "map", | ||
| tf2::durationFromSec(transform_tolerance_)); | ||
| utm_to_map_transform = tf_buffer_->lookupTransform("utm", "map", tf2::TimePointZero); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Please use parameterized frames
| tf_buffer_->transform( | ||
| curr_pose_utm_frame, curr_pose_map_frame, "map", | ||
| tf2::durationFromSec(transform_tolerance_)); | ||
| utm_to_map_transform = tf_buffer_->lookupTransform("utm", "map", tf2::TimePointZero); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I don't see how these differ. In the previous version you give it the info and let it transform, now you're handling the same operations manually - both between the same 2 frames.
| tf2::Quaternion utm_to_map_quat; | ||
| tf2::fromMsg(utm_to_map_transform.transform.rotation, utm_to_map_quat); | ||
| double roll, pitch, yaw; | ||
| tf2::Matrix3x3 m(utm_to_map_quat); m.getRPY(roll, pitch, yaw); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
don't do multi-line
| // rotate x , y with amount of yaw | ||
| double x = response->map_point.x; | ||
| double y = response->map_point.y; | ||
| double x_dot = x * std::cos(yaw) - y * std::sin(yaw); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Please use proper vector operations - this is fragile and won't work for non-SE2 navigation types
|
What's the status of this request right now? |
|
Its in cold storage at the moment, @jediofgever no longer has time to finish up the review comments but eventually we'd like to get this completed and merged in. If you're interested in helping out, it would be appreciated |
|
What is left? Will try helping in any area I can understand |
|
Please review the review comments / discussion in the thread for the remaining topics |
|
This pull request is in conflict. Could you fix it @jediofgever? |
|
@jediofgever, your PR has failed to build. Please check CI outputs and resolve issues. |
|
I need this for a project, I plan on picking it up and having something next week. @jediofgever is there anything you think I should change about how you implemented it? |
|
I think the biggest thing is updating this PR to be in the current |
|
#2814 supersedes |
Basic Info
Description of contribution in a few bullet points
Add a package for conveniently doing GPS waypoint following. The package makes uses of existent
nav2_waypoint_followerandrobot_localization. Since this is based on GPS, the localization chainmap->odom->base_linkis supposed to be acquired byrobot_localization. For example see the launch file here to get an glance of localization with multiple sources of sensors usinf EKF and navsat_transform_node all provided byrobot_localization;Description of documentation updates required from your changes
Future work that may be required in bullet points
navigation2_tutorialsthat makes use of action(FollowGPSWaypoints) provided by this package, to follow GPS waypoints.