Add nav2_gps_waypoint_follower

[pepisg]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#1631, pick up #2111
Primary OS tested on	Ubuntu 20.04
Robotic platform tested on	Custom skid steer drive robot under gazebo (to be tested on real robot)

---

Description of contribution in a few bullet points

Pick up the work started in #2111: Add an action server into nav2_waypoint_follower to follow GPS waypoints. It makes use of robot_localization to provide the location chain map->odom->base_link and transform GPS coordinates into poses given in the global frame using its /fromLL service

Description of documentation updates required from your changes

• Documentation needs to be updated on navigation.ros.org on how to use this action server.
• A tutorial on navigation2_tutorials was underway. A new PR will be opened to address its comments and update it to account from some changes made on this PR.

---

Future work that may be required in bullet points

• Complete the tutorial on navigation2_tutorials
• Test this on a real robot

For Maintainers:

• [x] Check that any new parameters added are updated in navigation.ros.org
• [x] 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
• [x] Check that any new functions have Doxygen added
• [ ] Check that any new features have test coverage
• [x] Check that any new plugins is added to the plugins page
• [x] If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mergify[bot]]

@pepisg, please properly fill in PR template in the future. @stevemacenski, use this instead.

• [ ] 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

[mergify[bot]]

@pepisg, 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).

[indraneelpatil]

@shrijitsingh99

[mergify[bot]]

@pepisg, 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).

[mergify[bot]]

@pepisg, 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).

[mergify[bot]]

@pepisg, 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).

[mergify[bot]]

@pepisg, 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).

[VincentHFZ]

您好，您的邮件我已经接收到，我会尽快处理--黄发展

[SteveMacenski]

I think this is good and mostly just waiting on Tom to give us feedback on the ticket

In the meantime, we can do the documentation/testing:

• Needs a system test in nav2_system_tests for GPS navigation to exercise the code you added
• Add new parameters to the configuration guide for waypoint follower
• Update the docs that waypoint follower can do GPS now
• Add to the migration guide the support for the GPS server

[pepisg]

Sure!

[SteveMacenski]

What's the status here?

[pepisg]

Hi. I will try to make time next week to finish what is missing. I have tested this successfully on a real robot. No answer still on the robot_localization issue

[SteveMacenski]

I reached out on the ticket to Tom, we'll see what he says. Thanks!

You may also want to pull in main so that CI will turn over and use the latest code since this has gotten a little stale

[SteveMacenski]

Did that thread settle everything for you?

[mergify[bot]]

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

[pepisg]

Yes, I have already removed this rotation in the map poses. I hope to complete the missing tasks this week so this can be merged. Thanks!

[SteveMacenski]

OK!

[SteveMacenski]

Hey! Any update? We also had a meeting recently with the folks at Micro-Strain, they were interested in helping with GPS documentation as well

[mergify[bot]]

@pepisg, 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).

[pepisg]

Hi @SteveMacenski . I'm really sorry for the late reply. Following what you requested on this PR:

Needs a system test in nav2_system_tests for GPS navigation to exercise the code you added:

In progress. Are there any guidelines/docs for making tests for nav2 that I should follow?

Add new parameters to the configuration guide for waypoint follower:

Done, see this PR

Update the docs that waypoint follower can do GPS now:

The documentation on ros-planning/navigation2 is done. Let me know if something else should be added. Also, a new tutorial PR was opened

Add to the migration guide the support for the GPS server:

Done see this PR

[SteveMacenski]

Are there any guidelines/docs for making tests for nav2 that I should follow?

I'd just look at the generic go to pose tests. Typically, we have a launch file which uses a Launch Testing method that will launch a tester node + the simulation bringup stuff. When the tester node exists, the launch system will kill the launch file (and simulation) with it. Certainly you could do the tester in C++, but most of the ones we have are in python and probably easier for scripting tests.

I'd put the robot with some GPS sensor in an environment and just have it follow 2-3 waypoints around some central obstacle and just make sure it can do that in GPS coordinates successfully within some time window.

Usually I'd say the system tests can be as sloppy as required, but this is also going to be the working demonstration of GPS in Nav2, so having a relatively clean configuration file and testing script would be great.

[SteveMacenski]

Beyond that last comment, this is good to go with testing.

Also, I wasn't on the top of my game yesterday apparently. We have a waypoint follower tutorial that you can use as a base. You can pretty much copy paste that with the robot model containing a GPS sensor and just convert sending to the GPS interface

[ghanim-mukhtar]

@pepisg I apologize in advance if this is not the right place to post this question :) So I am interested in this feature (integrating GPS with navigation2 stack) so I checked out this PR and tried to compile it under a docker image created with foxy as the ROS version but I had errors related to basic ROS2 packages (tf2 geometry msgs, rclcpp ...etc) here are the docker file I am using and the log I had. Can you please provide some help about this?

Dockerfile.txt log.txt

Thanks in advance

[pepisg]

Hi @ghanim-mukhtar. You need to build in ubuntu 22.04 and ros rolling

[samuk]

@pepisg have you tried it on Humble?

[pepisg]

Hi! @samuk . Not yet, I'm sorry about that :(. You should be able to compile it though

[codecov[bot]]

Codecov Report

Base: 88.97% // Head: 85.60% // Decreases project coverage by -3.36% :warning:

Coverage data is based on head (a8c7a0f) compared to base (4bd3f73). Patch coverage: 73.52% of modified lines in pull request are covered.

:exclamation: Current head a8c7a0f differs from pull request most recent head f6623ae. Consider uploading reports for the commit f6623ae to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2814      +/-   ##
==========================================
- Coverage   88.97%   85.60%   -3.37%     
==========================================
  Files         351      341      -10     
  Lines       15658    15586      -72     
==========================================
- Hits        13932    13343     -589     
- Misses       1726     2243     +517     

Impacted Files	Coverage Δ
nav2_waypoint_follower/src/waypoint_follower.cpp	80.50% <73.13%> (-2.95%)	:arrow_down:
nav2_util/include/nav2_util/service_client.hpp	70.00% <100.00%> (ø)
nav2_amcl/src/map/map_range.c	0.00% <0.00%> (-100.00%)	:arrow_down:
nav2_amcl/src/sensors/laser/beam_model.cpp	0.00% <0.00%> (-100.00%)	:arrow_down:
...clude/nav2_amcl/motion_model/omni_motion_model.hpp	0.00% <0.00%> (-100.00%)	:arrow_down:
.../src/sensors/laser/likelihood_field_model_prob.cpp	0.00% <0.00%> (-97.47%)	:arrow_down:
nav2_amcl/src/motion_model/omni_motion_model.cpp	2.70% <0.00%> (-97.30%)	:arrow_down:
...er/dwb_core/include/dwb_core/dwb_local_planner.hpp	25.00% <0.00%> (-75.00%)	:arrow_down:
...avigator/src/navigators/navigate_through_poses.cpp	24.59% <0.00%> (-68.86%)	:arrow_down:
...re/include/dwb_core/illegal_trajectory_tracker.hpp	40.00% <0.00%> (-60.00%)	:arrow_down:
... and 54 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

:umbrella: View full report at Codecov.
:loudspeaker: Do you have feedback about the report comment? Let us know in this issue.

[pepisg]

Hi @SteveMacenski! I'm now adding the tests for the GPS waypoint follower and I think I will need to add the following:

1. A yaml parameters file for robot localization like this one.
2. A launch file for robot localization that uses the params above.
3. A launch file for using nav2 without a map/amcl, since RL will be providing the whole map->odom->base_link tf chain (I could also add a parameter to not launch amcl in the default bringup_launch.py)

I was thinking of placing all that on nav2_bringup but I'm not sure that's the best place, what do you think?

[SteveMacenski]

Add to a new directory in nav2_system_tests any additional files / configurations needed for a GPS test -- you can add a new gps_navigation test folder to hold the test launch files / test scripts / configs / etc.

[pepisg]

Hi @SteveMacenski! I think I have finished the tests for the gps waypoint follower, however something happened with the cache of the CI and now for some reason the tests I added are not being run (or I cannot see their output). I looks like other tests are failing and are preventing mine from running. This failed run was the last in which I see the output produced by my tests (they can be found easily searching for the word gps). After that it seems like everything was rebuilt and retested without cache. which seemed to cause this behavior.

How should I proceed?

PD: I apologize if this is a dumb question, to be honest I'm not very familiar on how nav2 CI is set up