Add nav2_gps_waypoint_follower #2111

jediofgever · 2020-12-01T04:58:42Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(#1631)
Primary OS tested on	(Ubuntu 20.04)
Robotic platform tested on	(Custom built ackerman robot under Gazebo)

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_follower and robot_localization. Since this is based on GPS, the localization chain map->odom->base_link is supposed to be acquired by robot_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 by robot_localization;

Description of documentation updates required from your changes

Future work that may be required in bullet points

Add a example client node to navigation2_tutorials that makes use of action(FollowGPSWaypoints) provided by this package, to follow GPS waypoints.
in progress at add nav2_gps_waypoint_follower_demo navigation2_tutorials#16

SteveMacenski · 2020-12-03T02:57:58Z

2 major questions

Can this node just be in the nav2_waypoint_follower package? Since it uses the nav2_waypoint_follower anyhow, it would make sense.
Given that the only real difference here is converting GPS to map and then following them, could we just expose another action server within the waypoint follower node? Then that action would simply call the conversion service and then either trigger the other action (and relay info) or if we refactor the nav2 waypoint follower node, we can just call it directly (e.g. refactor the general waypoint follower action server into functions that can be used in both once converted).

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.

nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp

jediofgever · 2020-12-03T03:41:01Z

Can this node just be in the nav2_waypoint_follower package? Since it uses the nav2_waypoint_follower anyhow, it would make sense.

Certainly possible. This is an initial draft and open to conceptual changes.

Given that the only real difference here is converting GPS

When writing, I kept that in mind therefore convertGPSWaypointstoPosesinMap is static and can actually directly be used in nav2_waypoint_follower. Only reason this is a separate package is, GPS waypoint following isn't really common use-case for navigation2 ATM , so it is isolated from nav2_waypoint_follower.

If you are thinking that this should be coupled with existing FollowWaypoints, then there might not be need for another separate action server, we can just add additional fields to FollowWaypoints such as;

#goal definition
uint8 POSE_TYPE_IN_MAP=0
uint8 POSE_TYPE_IN_GPS=1
uint8 pose_type
geometry_msgs/PoseStamped[] map_poses
sensor_msgs/NavsatFix[] gps_poses
---
#result definition
int32[] missed_waypoints
---
#feedback
uint32 current_waypoint

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 pose_type and if it is GPS then convertGPSWaypointstoPosesinMap and rest the same. This of course helps to address redundancy but also introduces complexity.
If you have another approach/structure recommendation please do let me know about it, so I can step forward to shape this into better structure. Answers to both of your questions can be done but at this point I am a bit confused on which direction to go.

SteveMacenski · 2020-12-03T21:50:33Z

I agree that it should be a separate action server within the waypoint follower class (with a different Follow action definition). For the same reasons you mentioned (they're not used interchangably, some people will use one or the other) they can sit next to each other without much fear of issue. If someone really does want to use both at the same time, they can always launch 2 instances of it and use the remapped instance appropriate for each request.

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 followWaypoints action callback function to get the action requests and call another function to do it. That way a followGpsWaypoints action callback can get the action requests, convert to map frame, and then call the same functions.

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.

jediofgever · 2020-12-04T07:36:31Z

Hopefully the structuring is better in shape now. They are now within same class each with its own action server definition.

We'll have to refactor the followWaypoints action callback function to get the action requests

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 FollowGPSWaypoints is not complete yet I pushed the code to make sure we are on the same page. I think it might be better to put FollowGPSWaypoints action into a form similar to this .

SteveMacenski

Really good first draft. I think there's some more places we can improve 😄

nav2_waypoint_follower/README.md

SteveMacenski · 2020-12-04T21:59:45Z

nav2_waypoint_follower/README.md

@@ -24,3 +24,29 @@ In the first, the ``nav2_waypoint_follower`` is fully sufficient to create a pro
 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.


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.

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

nav2_waypoint_follower/src/waypoint_follower.cpp

SteveMacenski · 2020-12-04T22:15:49Z

nav2_waypoint_follower/src/waypoint_follower.cpp

+    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;


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

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 ?

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.

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.

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.

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

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/src/waypoint_follower.cpp

nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp

jediofgever · 2020-12-07T09:26:45Z

I think it is time to think about how to deal with the dependency robot_localization, AFAIK there is no branch that tracks ros2 rolling.

nav2_waypoint_follower/README.md

nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp

nav2_waypoint_follower/src/waypoint_follower.cpp

nav2_waypoint_follower/CMakeLists.txt

SteveMacenski · 2020-12-08T20:17:20Z

Good way of cleaning up the preemption. I had tried doing something similar recently and failed in the smac planner, so I'm happy to see if you've gotten a solution.

jediofgever · 2020-12-08T21:31:59Z

I had tried doing something similar recently

sfinae concept might also be useful in your case but, if available(thankfully) Constexpr_if is recommended to deal with compile time branching.

SteveMacenski

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.

nav2_waypoint_follower/src/waypoint_follower.cpp

tools/underlay.repos

SteveMacenski · 2020-12-08T21:59:57Z

have you tested both sets of following and still work?

jediofgever · 2020-12-08T22:08:45Z

have you tested both sets of following and still work?

Yes they work , in case of GPS though it needs that navsat_transfrom_node and robot_localization is set-up correctly as it relies on that to do conversion

jediofgever · 2020-12-08T22:17:37Z

orientation element we're discussing in another review comment

How about we add new msg as OrientedNavSatFix.msg;

sensor_msgs/NavSatFix point
geometry_msgs/Quaternion orientation

and use this new msg in FollowGPSWaypoints.action as ;

#goal definition
nav2_msgs/OrientedNavSatFix[] waypoints
---
#result definition
int32[] missed_waypoints
---
#feedback
uint32 current_waypoint

this can make inclusion of orientation convenient

adding the new action server to the WP follower docs / migration guide.

This is in TODOs list after this gets merged or ready to be merged

SteveMacenski · 2021-01-04T19:55:19Z

@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

nav2_waypoint_follower/src/waypoint_follower.cpp

SteveMacenski · 2021-01-08T21:36:06Z

@jediofgever there are 2 places that warning exists

SteveMacenski · 2021-01-22T18:58:15Z

@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!

jediofgever · 2021-01-23T06:22:51Z

@jediofgever there are 2 places that warning exists

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.

jediofgever · 2021-01-23T06:29:38Z

There is one thing I realized recently,
https://github.com/cra-ros-pkg/robot_localization/blob/79162b2ac53a112c51d23859c499e8438cf9938e/src/navsat_transform.cpp#L394

When robot_locaization's fromLL converts a LL to map , it seems that it does not consider possible rotation between utm and map frames. if navsat_transfrom_node was given an yaw_offset, then there will be rotation difference between utm and map frames.
So I added that consideration too while converting gps points to map points.

SteveMacenski · 2021-01-25T20:11:36Z

@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

SteveMacenski · 2021-01-25T20:13:11Z

nav2_waypoint_follower/src/waypoint_follower.cpp

-      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/


Should a ticket be filed about this / fixed there?

SteveMacenski · 2021-01-25T20:13:48Z

nav2_waypoint_follower/src/waypoint_follower.cpp

-        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);


Please use parameterized frames

SteveMacenski · 2021-01-25T20:15:27Z

nav2_waypoint_follower/src/waypoint_follower.cpp

-        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);


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.

SteveMacenski · 2021-01-25T20:16:03Z

nav2_waypoint_follower/src/waypoint_follower.cpp

+      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);


don't do multi-line

SteveMacenski · 2021-01-25T20:16:36Z

nav2_waypoint_follower/src/waypoint_follower.cpp

+      // 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);


Please use proper vector operations - this is fragile and won't work for non-SE2 navigation types

sisaha9 · 2021-03-30T05:53:39Z

What's the status of this request right now?

SteveMacenski · 2021-03-30T17:30:07Z

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

sisaha9 · 2021-04-19T07:30:15Z

What is left? Will try helping in any area I can understand

SteveMacenski · 2021-04-20T18:00:28Z

Please review the review comments / discussion in the thread for the remaining topics

mergify · 2021-10-15T20:22:39Z

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

mergify · 2021-10-15T21:00:14Z

@jediofgever, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

klewisBE · 2021-11-12T17:03:23Z

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?

SteveMacenski · 2021-11-15T19:24:47Z

I think the biggest thing is updating this PR to be in the current main branch and looking over my PR review comments that are not resolved. I think this was 85% ready to go, just needed some touch ups and documentation to match so that users know how to use it on navigation.ros.org. I'd absolutely love your help @klewisBE to push this over the finish line! I simply don't have a GPS-enabled robot to finish it myself confidently and write docs for setup. But happy to support you any way I can to get this completed.

SteveMacenski · 2022-02-11T21:39:50Z

#2814 supersedes

Add nav2_gps_waypoint_follower

5f33ac5

jediofgever mentioned this pull request Dec 1, 2020

add nav2_gps_waypoint_follower_demo ros-navigation/navigation2_tutorials#16

Closed

use correct client node while calling it to spin

277c1e1

SteveMacenski reviewed Dec 3, 2020

View reviewed changes

nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp Outdated Show resolved Hide resolved

nav2_gps_waypoint_follower/src/gps_waypoint_follower.cpp Outdated Show resolved Hide resolved

changed after 1'st review

c6ee437

SteveMacenski requested changes Dec 4, 2020

View reviewed changes

SteveMacenski reviewed Dec 4, 2020

View reviewed changes

nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp Outdated Show resolved Hide resolved

jediofgever added 2 commits December 7, 2020 14:14

apply requested changes

7b6f8cf

nav2_util::ServiceClient instead of CallbackGroup

ac94d2d

SteveMacenski requested changes Dec 7, 2020

View reviewed changes

another iteration to adress issues

cd9a287

SteveMacenski reviewed Dec 8, 2020

View reviewed changes

nav2_waypoint_follower/src/waypoint_follower.cpp Show resolved Hide resolved

SteveMacenski reviewed Dec 8, 2020

View reviewed changes

nav2_waypoint_follower/CMakeLists.txt Show resolved Hide resolved

jediofgever added 4 commits December 9, 2020 04:53

update poses with function in the follower logic

9c0e408

add deps of robot_localization: diagnostics

f94e3b3

fix typo in underlay.repo

e4fe184

add deps of robot_localization: geographic_info

c2f4d95

jediofgever mentioned this pull request Dec 8, 2020

Adapt changes from ROS2 rolling to ros2-devel branch ros/diagnostics#171

Closed

SteveMacenski requested changes Dec 8, 2020

View reviewed changes

minor clean-ups

98804f1

jediofgever mentioned this pull request Dec 23, 2020

Add documentation for GPS waypoint following introduces in pull/2111 at navigation2 ros-navigation/docs.nav2.org#127

Closed

4 tasks

SteveMacenski reviewed Jan 4, 2021

View reviewed changes

nav2_waypoint_follower/src/waypoint_follower.cpp Outdated Show resolved Hide resolved

jediofgever added 2 commits January 5, 2021 09:49

pull changes from ros-planing::navigation2::main

c28b93e

update warning for empty requests

c6613a8

jediofgever added 2 commits January 23, 2021 11:51

get upstream changes

0d0d59c

consider utm -> map yaw offset

d7e2ad4

jediofgever added 2 commits January 23, 2021 14:35

fix missed RCLCPP info

d6527d2

reorrect action;s name

f99ebff

SteveMacenski requested changes Jan 25, 2021

View reviewed changes

jediofgever added 2 commits March 18, 2021 16:01

get mainstream changes

e0a3aa6

waypoint stamps need to be updated

5fcfb83

ros-navigation deleted a comment from mergify bot Oct 15, 2021

This was referenced Feb 11, 2022

Add nav2_gps_waypoint_follower #2814

Merged

Update nav2_gps_waypoint_follower_demo ros-navigation/navigation2_tutorials#47

Closed

SteveMacenski closed this Feb 11, 2022

		@@ -24,3 +24,29 @@ In the first, the ``nav2_waypoint_follower`` is fully sufficient to create a pro
		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.

Add nav2_gps_waypoint_follower #2111

Add nav2_gps_waypoint_follower #2111

Conversation

jediofgever commented Dec 1, 2020 • edited Loading

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

SteveMacenski commented Dec 3, 2020 • edited Loading

jediofgever commented Dec 3, 2020 • edited Loading

SteveMacenski commented Dec 3, 2020 • edited Loading

jediofgever commented Dec 4, 2020 • edited Loading

SteveMacenski left a comment

Choose a reason for hiding this comment

SteveMacenski Dec 4, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jediofgever Dec 14, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jediofgever Dec 15, 2020 • edited Loading

Choose a reason for hiding this comment

SteveMacenski Dec 16, 2020 • edited Loading

Choose a reason for hiding this comment

jediofgever commented Dec 7, 2020

SteveMacenski commented Dec 8, 2020

jediofgever commented Dec 8, 2020

SteveMacenski left a comment • edited Loading

Choose a reason for hiding this comment

SteveMacenski commented Dec 8, 2020

jediofgever commented Dec 8, 2020

jediofgever commented Dec 8, 2020 • edited Loading

SteveMacenski commented Jan 4, 2021 • edited Loading

SteveMacenski commented Jan 8, 2021

SteveMacenski commented Jan 22, 2021 • edited Loading

jediofgever commented Jan 23, 2021

jediofgever commented Jan 23, 2021 • edited Loading

SteveMacenski commented Jan 25, 2021

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sisaha9 commented Mar 30, 2021

SteveMacenski commented Mar 30, 2021

sisaha9 commented Apr 19, 2021

SteveMacenski commented Apr 20, 2021

mergify bot commented Oct 15, 2021

mergify bot commented Oct 15, 2021

klewisBE commented Nov 12, 2021

SteveMacenski commented Nov 15, 2021 • edited Loading

SteveMacenski commented Feb 11, 2022

jediofgever commented Dec 1, 2020 •

edited

Loading

SteveMacenski commented Dec 3, 2020 •

edited

Loading

jediofgever commented Dec 3, 2020 •

edited

Loading

SteveMacenski commented Dec 3, 2020 •

edited

Loading

jediofgever commented Dec 4, 2020 •

edited

Loading

SteveMacenski Dec 4, 2020 •

edited

Loading

jediofgever Dec 14, 2020 •

edited

Loading

jediofgever Dec 15, 2020 •

edited

Loading

SteveMacenski Dec 16, 2020 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

jediofgever commented Dec 8, 2020 •

edited

Loading

SteveMacenski commented Jan 4, 2021 •

edited

Loading

SteveMacenski commented Jan 22, 2021 •

edited

Loading

jediofgever commented Jan 23, 2021 •

edited

Loading

SteveMacenski commented Nov 15, 2021 •

edited

Loading