Nav2 GPS Waypoint Following on a Real Outdoor Rover: Costmaps and Field Failures
Tom Cosgrove,

TL;DR: Nav2 GPS waypoint following outdoor works reliably when you fuse RTK GNSS with wheel odometry and IMU through robot_localization, tune the local costmap for terrain (not indoor floors), and handle RTK fix loss as a first-class failure mode. This guide walks through a working configuration on a Leo Rover, shows the relevant Nav2 GPS waypoint-following configuration, and discusses field logs where RTK carrier-solution quality dropped from Fixed to Float and the rover deviated from its intended path. A rosbag2 recording is linked at the end.
Why Nav2 GPS waypoint following is different outdoors
Nav2 is the ROS 2 navigation stack. Most tutorials cover indoor SLAM with a 2D lidar on a flat floor. Outdoor UGV work breaks several of those assumptions: the ground is not planar, GNSS accuracy varies second to second, and obstacles include tall grass that a lidar treats as a wall but the rover can sometimes drive through. Nav2 GPS waypoint following outdoor is the combination of three things: a global frame anchored to Earth (usually map aligned via navsat_transform_node), a behavior tree or waypoint-following client that consumes geographic_msgs/GeoPose waypoints, and costmaps tuned to reflect real terrain traversability.
The reference implementation is available in the Nav2 tutorial packages, including nav2_gps_waypoint_follower_demo. Fictionlab’s field testing on the Leo Rover uses that upstream configuration as the starting point, then modifies the costmap and localization parameters for a small 4×4 skid-steer platform.
What hardware and software stack the test used
The setup is a Leo Rover 1.7 with the standard 4WD skid-steer drive, an Ardusimple simpleRTK2B (u-blox ZED-F9P) receiver pulling NTRIP corrections over a 4G modem, a BNO055 IMU on the chassis, and wheel encoders exposed through the stock leo_fw firmware. Software is ROS 2 Humble on Ubuntu 22.04 running on the Raspberry Pi 4 inside the rover. Nav2 is installed from the ROS 2 Humble packages.
The transform tree the setup requires:
map -> odom: published by the global EKF inrobot_localization(fuses GPS vianavsat_transform_node)odom -> base_link: published by the local EKF (fuses wheel odom + IMU only)base_link -> gps_link,base_link -> imu_link: static transforms from URDF
In the usual dual-EKF setup, do not fuse GPS into the local EKF. If a GNSS jump propagates into odom -> base_link, the local frame and locally planned motion will jump as well. Keep GPS in the global EKF only.
How to configure the Nav2 GPS waypoint follower step by step
The steps below assume you already have a working ros2 launch nav2_bringup navigation_launch.py on the robot and can drive it with the DWB or Regulated Pure Pursuit controller.
- Install the GPS demo dependencies:
sudo apt install ros-humble-robot-localization ros-humble-nav2-bringup ros-humble-mapviz ros-humble-mapviz-plugins ros-humble-tile-map. - Clone the reference demo:
git clone -b humble https://github.com/ros-navigation/navigation2_tutorials.gitand usenav2_gps_waypoint_follower_demoas your starting point. - Edit
dual_ekf_navsat_params.yaml. Setmagnetic_declination_radiansto your location’s value from a current geomagnetic model. Setyaw_offsetto1.5708only when needed to convert an IMU convention with zero yaw facing north to the east-zero ENU convention expected bynavsat_transform_node. - Launch localization:
ros2 launch nav2_gps_waypoint_follower_demo dual_ekf_navsat.launch.py. Verify withros2 run tf2_ros tf2_echo map base_linkthat the transform updates and is consistent with the GNSS position after the receiver obtains an RTK fixed carrier solution. - Point Nav2 at the GPS behavior tree. In
nav2_params.yaml, setdefault_nav_through_poses_bt_xmltonavigate_through_gps_poses_w_replanning_and_recovery.xmlwhen using the GPS waypoint demo’s corresponding behavior tree and plugins. - Launch Nav2:
ros2 launch nav2_bringup navigation_launch.py params_file:=/path/to/nav2_params.yaml use_sim_time:=false. - Send waypoints. The GPS waypoint follower ultimately sends
geographic_msgs/GeoPosegoals through thefollow_gps_waypointsaction. The exact input-file format, if any, is defined by the client application rather than by the action itself.
A minimal waypoint file for a custom client that accepts latitude, longitude, and yaw can look like this:
waypoints:
- latitude: 52.237049
longitude: 21.017532
yaw: 0.0
- latitude: 52.237105
longitude: 21.017610
yaw: 1.57
How to tune the costmap for terrain instead of a floor
The default Nav2 costmap parameters assume a clean indoor environment with a 2D lidar 20 cm off the floor. Outdoors, three things go wrong: grass blades trigger obstacle marks, small pitch changes make the lidar scan sweep the ground, and the inflation radius is too tight for a rover that cannot pivot on a coin.
These are the parameter changes that made the biggest difference in Fictionlab’s field runs:
- Local costmap resolution: raise from 0.05 to 0.10 m. You do not need 5 cm cells outdoors, and the CPU savings matter on a Raspberry Pi 4.
- Local costmap size: 6 x 6 m rolling window. Smaller windows can provide too little look-ahead when the rover pitches or travels quickly.
- Obstacle layer
obstacle_max_range: 4.0 m. Beyond that, lidar returns from tall grass can dominate. - Voxel layer
min_obstacle_height: 0.15 m. This can filter low returns, but it must be validated against the lidar mounting height, terrain slope, and obstacles that must remain detectable. - Inflation layer
inflation_radius: 0.55 m for a Leo Rover (footprint radius approximately 0.22 m). Skid-steer platforms need extra room because heading corrections consume lateral space. - Inflation layer
cost_scaling_factor: 2.5. A lower value produces a wider, smoother cost gradient and can help the Regulated Pure Pursuit controller stay on smoother paths.
Do not enable the static layer for outdoor GPS navigation when no prior occupancy map exists. In that case, configure the global costmap as a rolling costmap and use only the layers supported by your available sensors, typically obstacle and inflation layers.
What breaks when the RTK fix drops
This is the failure mode almost no tutorial covers. During a test run at a field near Warsaw, the NTRIP connection lost packets for approximately 4 seconds. The receiver’s carrier-solution quality changed from RTK Fixed to RTK Float and then briefly lost its 3D-quality solution. On common NMEA GGA output, quality values 4 and 5 denote RTK Fixed and RTK Float respectively; these values should not be confused with the u-blox UBX fixType field.
The relevant log excerpt from navsat_transform_node and the global EKF:
[navsat_transform-4] [WARN] GPS covariance is large: 4.410000. Consider tuning the sensor.
[ekf_filter_node_map-3] [WARN] Detected jump back in time of 0.000s. Rewinding state.
[controller_server-6] [WARN] Control loop missed its desired rate of 20.0000Hz. Current loop rate is 11.3204Hz
[bt_navigator-7] [INFO] Goal was rejected by the action server
What happened physically: the map -> odom transform jumped by approximately 1.8 m in one global-EKF update. A correctly configured local costmap normally remains in the continuous odom frame, so it does not itself snap with map -> odom. However, the transformed global plan and robot pose can become inconsistent with the local map, causing the controller to stop or trigger recovery behavior. The rover rotated in place for 6 seconds, then resumed once the fix recovered.
Three mitigations that helped in later runs:
- Set
navsat_transform_nodeparameteruse_odometry_yaw: falsewhen a calibrated IMU orientation is available. This prevents the node from using the odometry yaw source; it does not by itself guarantee that heading is independent of all GNSS-related effects. - Add a covariance gate in a custom node: reject or down-weight GPS messages when their reported covariance exceeds a threshold appropriate to the application, for example
position_covariance[0] > 0.25. The threshold is in square metres and should be tested against the receiver’s actual covariance behavior. - Use a custom GNSS-quality condition or safety node that pauses or cancels waypoint execution when the receiver reports a non-fixed carrier solution.
GoalUpdaterupdates goals; it is not a general-purpose GPS-quality pause mechanism. RTK quality normally has to be obtained from a receiver-specific status topic or parsed UBX/NMEA data, becausesensor_msgs/NavSatStatusdoes not expose RTK Fixed versus RTK Float.
The full rosbag2 of this failure, including /fix, /odometry/gps, /tf, and the local costmap, is available on request via the research applications page.
What the Leo Rover contributes to this test setup
The Leo Rover is convenient for Nav2 GPS waypoint following outdoor work because the ROS 2 packages (leo_description, leo_bringup, leo_fw) can provide the robot model and wheel-odometry interfaces needed for the integration, and the payload plate accepts a GNSS antenna with suitable mounting hardware. The 4WD skid-steer geometry does mean you should model the footprint as a circle slightly larger than the physical extent, because in-place rotation on soft ground consumes lateral space through wheel slip.
Frequently asked questions
Do you need RTK for Nav2 GPS waypoint following?
No, but standard GNSS typically gives metre-level horizontal accuracy in open sky. That is fine for reaching a waypoint with a several-metre tolerance but too coarse for row-crop or inspection work. A stable RTK fixed solution can provide centimetre-level relative positioning, subject to correction quality, antenna placement, multipath, and the reference-station setup.
Can you run Nav2 GPS waypoints without a lidar?
Yes. Disable obstacle-detection layers if no obstacle sensor is available. The resulting costmap will be effectively empty, so the rover will drive blind between waypoints. This is appropriate only for open, pre-scouted areas with an independent safety strategy.
Why does the rover spin in place when it reaches a waypoint?
The configured behavior tree may include a Spin recovery. More commonly, the yaw tolerance in the active goal-checker plugin is tighter than the heading estimate can support. For example, set general_goal_checker.yaw_goal_tolerance to 0.25 rad when using Nav2’s standard SimpleGoalChecker configuration, then validate the value for the platform and task.
How do you handle magnetic declination correctly?
Set magnetic_declination_radians in navsat_transform_node to the value from a current World Magnetic Model calculator for your exact location and date, if the IMU yaw is magnetic-north referenced. Declination errors normally cause a heading error of degrees, not 180 degrees; large heading errors usually indicate an incorrect frame convention, sensor mounting transform, yaw offset, or quaternion interpretation.
What is the minimum GPS update rate for Nav2?
There is no universal minimum GPS update rate for Nav2. A 5 Hz GNSS update can be sufficient for slow waypoint following, but the required rate depends on speed, GNSS latency, wheel odometry quality, and EKF tuning. The ZED-F9P can be configured for update rates up to 20 Hz in supported configurations. An EKF output frequency of 30 Hz does not mean that higher-rate measurements provide no benefit; it defines the filter’s publication/update schedule.
Can you use this setup with dual-antenna GNSS for heading?
Yes, and it can remove the need to use a magnetometer for heading. The heading solution must be converted into a correctly framed sensor_msgs/Imu orientation message, with an appropriate covariance, before it can be used as the IMU yaw input by navsat_transform_node. Dual-antenna or moving-baseline RTK systems based on u-blox F9P receivers are a common choice.
Where can you download the rosbag2 from the failure log?
The bag is approximately 340 MB and covers 90 seconds around the RTK fix drop. Contact Fictionlab through the research page linked above.
If you want to reproduce this setup on your own outdoor platform or extend it with a payload-specific sensor, the Leo Rover ROS 2 packages and hardware are available in the Fictionlab shop.