## How to Use GPS With the Robot Localization Package – ROS 2

In this tutorial, we will integrate GPS data into a mobile robot in order to localize in an environment.

The official instructions for doing this are on this page, but we will walk through the entire process below.

You can get the entire code for this project here.

Let’s get started!

# Prerequisites

You have completed the Ultimate Guide to the ROS 2 Navigation Stack (also known as Nav2). We will not be using the Navigation Stack, but that Ultimate Guide helps you set up all the packages that you will use in this tutorial.

If you know ROS 2 and understand how to read Launch files, you should be able to follow along with this tutorial quite well without having to go through the Ultimate Guide.

# Calculate the Magnetic Declination of Your Location

First, we need to calculate the magnetic declination in radians.

Enter your latitude and longitude, and click “Calculate”. If you don’t know your latitude and longitude, you can look it up by zip code.

My magnetic declination is 5° 20′ W.

Convert that value to decimal format. Since the units above are 5 degrees and 20 minutes, this value is the following in decimal format:

5.333333 degrees

Now we need to convert that value to radians.

# Set the Latitude, Longitude, and Elevation in the World File

We need to set the latitude, longitude, and elevation of the origin in Gazebo.

Go to your world file. Open a terminal window, and type:

`colcon_cd basic_mobile_robot`
`cd worlds/basic_mobile_bot_world`
`gedit smalltown.world`

Inside the <spherical_coordinates> tag at the bottom of the file, modify the latitude, longitude, and elevation to wherever you are. Here is my smalltown.world file.

I am in Atlanta, Georgia. My latitude is 33.83, and my longitude is -84.42. The elevation at this location is 836.6 feet, which is 254.99568 meters.

Save the file and close it.

• x = East
• y = North
• z = Up

So, for example, if the robot is in Atlanta and heading in the positive x-direction in the world coordinate frame, the robot is moving eastward, and we should expect the longitude to get less negative (i.e. move towards 0).

If the robot is heading in the positive y-direction in the world coordinate frame, the robot is moving northward, and we should expect the latitude to increase (i.e. move towards 90 degrees).

# Set the Configuration Parameters

We now need to specify the configuration parameters of the ekf_node by creating a YAML file.

Open a new terminal window, and type the following command to move to the basic_mobile_robot package:

`colcon_cd basic_mobile_robot`
`cd config`
`gedit ekf_with_gps.yaml`

Copy and paste this code inside the YAML file.

Save and close the file.

You can get a complete description of all the parameters on this page

# Create a Launch File

Open a new terminal window, and move to your launch folder.

`colcon_cd basic_mobile_robot`
`cd launch`
`gedit basic_mobile_bot_v6.launch.py`

Copy and paste this code into the file.

Save the file, and close it.

# Build the Package

Now build the package by opening a terminal window, and typing the following command:

`cd ~/dev_ws`
`colcon build --packages-select basic_mobile_robot`

# Launch the Robot

Open a new terminal, and launch the robot.

`cd ~/dev_ws/`
`ros2 launch basic_mobile_robot basic_mobile_bot_v6.launch.py`

Call a ROS service to set the GPS’s origin for the navsat_transform node.

`ros2 service call /datum robot_localization/srv/SetDatum '{geo_pose: {position: {latitude: 33.83, longitude: -84.42, altitude: 254.99568}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}'`

Now let’s check out the coordinate frames. Open a new terminal window, and type:

`ros2 run tf2_tools view_frames.py`

In newer versions of ROS 2, you might have to type:

`ros2 run tf2_tools view_frames`

In the current working directory, you will have a file called frames.pdf. Open that file.

`evince frames.pdf`

Here is what my coordinate transform (i.e. tf) tree looks like:

Check out the topics.

`ros2 topic list`

We can see the raw GPS data.

`ros2 topic echo /gps/fix`

We can see the GPS data after we have processed the raw GPS data through an Extended Kalman Filter.

`ros2 topic echo /gps/filtered`

The pose of the robot in the map frame before data processing through an Extended Kalman Filter.

`ros2 topic echo /odometry/gps`

The pose of the robot in the map frame after data processing through an Extended Kalman Filter. This value below is a combination of wheel encoder information, IMU data, and GPS data.

`ros2 topic echo /odometry/global`

The pose of the robot with respect to the starting point of the robot (i.e. with respect to the odom frame). This value below is a combination of wheel encoder information and IMU data.

`ros2 topic echo /odometry/local`

To steer the robot, open a new terminal window, and type the following command:

`rqt_robot_steering`

If you go straight down the positive x-axis, you will notice that the latitude value is getting less negative. This trend makes sense given the robot’s eastward trajectory.

That’s it! Keep building!

## The Ultimate Guide to the ROS 2 Navigation Stack

In the tutorials below, we will cover the ROS 2 Navigation Stack (also known as Nav2) in detail, step-by-step. The ROS 2 Navigation Stack is a collection of software packages that you can use to help your mobile robot move from a starting location to a goal location safely. Here will be our final output:

Credit to Ramkumar Gandhinathan and Lentin Joseph’s awesome book ROS Robotics Projects Second Edition (Disclosure: As an Amazon Associate I earn from qualifying purchases) for the world file, which comes from their book’s public GitHub page.

# Real-World Applications

The ROS 2 Navigation Stack can be used in a number of real-world robotic applications:

• Ground Delivery
• Hospitals and Medical Centers
• Hotels (Room Service)
• Offices
• Restaurants
• Warehouses
• And more…

In this project, we will work with a simulated robot in a simulated world. Roboticists like to simulate robots before building them in order to test out different algorithms. You can imagine the cost of making mistakes with a physical robot can be high (e.g. crashing a mobile robot into a wall at high speed means lost money).

Let’s get started!

# Prerequisites

• ROS 2 Foxy Fitzroy installed on Ubuntu Linux 20.04
• If you are using another ROS 2 distribution, you will need to replace ‘foxy’ with the name of your distribution everywhere I mention ‘foxy’ in this tutorial.
• I highly recommend you get the newest version of ROS 2. If you are using a newer version of ROS 2, you can still follow most of the steps in this tutorial. I will point out the areas where you will need to do things differently.
• You have already created a ROS 2 workspace. The name of our workspace is “dev_ws”, which stands for “development workspace.”

For future reference, here is a complete package (named ‘two_wheeled_robot‘) I developed that uses both URDF and SDF robot model files with the ROS 2 Navigation Stack. You can use this as a template after you have gone through the tutorials below.

# Directions

Complete the following five tutorials in order, step by step. When you are done, you will have a deep understanding of the ROS 2 Navigation Stack and will be ready to confidently use this package in your own robotics projects.

Enjoy!

In this ROS 2 Navigation Stack tutorial, we will use information obtained from LIDAR scans to build a map of the environment and to localize on the map. The purpose of doing this is to enable our robot to navigate autonomously through both known and unknown environments (i.e. SLAM). Here is what you will build:

Navigating a Known Environment (Video)

Navigating an Unknown Environment Using SLAM (Video)

As noted in the official documentation, the two most commonly used packages for localization are the nav2_amcl package and the slam_toolbox. Both of these packages publish the map -> odom coordinate transformation which is necessary for a robot to localize on a map.

This tutorial is the fifth tutorial in my Ultimate Guide to the ROS 2 Navigation Stack (also known as Nav2).

You can get the entire code for this project here if you are using ROS Foxy.

If you are using ROS Galactic or newer, you can get the code here.

Let’s get started!

# Prerequisites

You have completed the first four tutorials of this series:

# Create a Launch File

Open a new terminal window, and move to your launch folder.

`colcon_cd basic_mobile_robot`
`cd launch`
`gedit basic_mobile_bot_v5.launch.py`

Copy and paste this code into the file. I have also copied and pasted the code below:

```# Author: Addison Sears-Collins
# Date: September 2, 2021
# Description: Launch a basic mobile robot using the ROS 2 Navigation Stack

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():

# Set the path to different files and folders.
pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
pkg_share = FindPackageShare(package='basic_mobile_robot').find('basic_mobile_robot')
default_launch_dir = os.path.join(pkg_share, 'launch')
default_model_path = os.path.join(pkg_share, 'models/basic_mobile_bot_v2.urdf')
robot_localization_file_path = os.path.join(pkg_share, 'config/ekf.yaml')
robot_name_in_urdf = 'basic_mobile_bot'
default_rviz_config_path = os.path.join(pkg_share, 'rviz/nav2_config.rviz')
world_file_name = 'basic_mobile_bot_world/smalltown.world'
world_path = os.path.join(pkg_share, 'worlds', world_file_name)
nav2_dir = FindPackageShare(package='nav2_bringup').find('nav2_bringup')
nav2_launch_dir = os.path.join(nav2_dir, 'launch')
static_map_path = os.path.join(pkg_share, 'maps', 'smalltown_world.yaml')
nav2_params_path = os.path.join(pkg_share, 'params', 'nav2_params.yaml')
nav2_bt_path = FindPackageShare(package='nav2_bt_navigator').find('nav2_bt_navigator')
behavior_tree_xml_path = os.path.join(nav2_bt_path, 'behavior_trees', 'navigate_w_replanning_and_recovery.xml')

# Launch configuration variables specific to simulation
autostart = LaunchConfiguration('autostart')
default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
map_yaml_file = LaunchConfiguration('map')
model = LaunchConfiguration('model')
namespace = LaunchConfiguration('namespace')
params_file = LaunchConfiguration('params_file')
rviz_config_file = LaunchConfiguration('rviz_config_file')
slam = LaunchConfiguration('slam')
use_namespace = LaunchConfiguration('use_namespace')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
use_rviz = LaunchConfiguration('use_rviz')
use_sim_time = LaunchConfiguration('use_sim_time')
use_simulator = LaunchConfiguration('use_simulator')
world = LaunchConfiguration('world')

# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
# https://github.com/ros/geometry2/issues/32
# https://github.com/ros/robot_state_publisher/pull/30
# TODO(orduno) Substitute with `PushNodeRemapping`
#              https://github.com/ros2/launch_ros/issues/56
remappings = [('/tf', 'tf'),
('/tf_static', 'tf_static')]

# Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
name='namespace',
default_value='',
description='Top-level namespace')

declare_use_namespace_cmd = DeclareLaunchArgument(
name='use_namespace',
default_value='False',
description='Whether to apply a namespace to the navigation stack')

declare_autostart_cmd = DeclareLaunchArgument(
name='autostart',
default_value='true',
description='Automatically startup the nav2 stack')

declare_bt_xml_cmd = DeclareLaunchArgument(
name='default_bt_xml_filename',
default_value=behavior_tree_xml_path,
description='Full path to the behavior tree xml file to use')

declare_map_yaml_cmd = DeclareLaunchArgument(
name='map',
default_value=static_map_path,
description='Full path to map file to load')

declare_model_path_cmd = DeclareLaunchArgument(
name='model',
default_value=default_model_path,
description='Absolute path to robot urdf file')

declare_params_file_cmd = DeclareLaunchArgument(
name='params_file',
default_value=nav2_params_path,
description='Full path to the ROS2 parameters file to use for all launched nodes')

declare_rviz_config_file_cmd = DeclareLaunchArgument(
name='rviz_config_file',
default_value=default_rviz_config_path,
description='Full path to the RVIZ config file to use')

declare_simulator_cmd = DeclareLaunchArgument(
default_value='False',
description='Whether to execute gzclient')

declare_slam_cmd = DeclareLaunchArgument(
name='slam',
default_value='False',
description='Whether to run SLAM')

declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
name='use_robot_state_pub',
default_value='True',
description='Whether to start the robot state publisher')

declare_use_rviz_cmd = DeclareLaunchArgument(
name='use_rviz',
default_value='True',
description='Whether to start RVIZ')

declare_use_sim_time_cmd = DeclareLaunchArgument(
name='use_sim_time',
default_value='True',
description='Use simulation (Gazebo) clock if true')

declare_use_simulator_cmd = DeclareLaunchArgument(
name='use_simulator',
default_value='True',
description='Whether to start the simulator')

declare_world_cmd = DeclareLaunchArgument(
name='world',
default_value=world_path,
description='Full path to the world model file to load')

# Specify the actions

# Start Gazebo server
start_gazebo_server_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
condition=IfCondition(use_simulator),
launch_arguments={'world': world}.items())

# Start Gazebo client
start_gazebo_client_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')),
condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])))

# Start robot localization using an Extended Kalman filter
start_robot_localization_cmd = Node(
package='robot_localization',
executable='ekf_node',
name='ekf_filter_node',
output='screen',
parameters=[robot_localization_file_path,
{'use_sim_time': use_sim_time}])

# Subscribe to the joint states of the robot, and publish the 3D pose of each link.
start_robot_state_publisher_cmd = Node(
condition=IfCondition(use_robot_state_pub),
package='robot_state_publisher',
executable='robot_state_publisher',
namespace=namespace,
parameters=[{'use_sim_time': use_sim_time,
'robot_description': Command(['xacro ', model])}],
remappings=remappings,
arguments=[default_model_path])

# Launch RViz
start_rviz_cmd = Node(
condition=IfCondition(use_rviz),
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', rviz_config_file])

# Launch the ROS 2 Navigation Stack
PythonLaunchDescriptionSource(os.path.join(nav2_launch_dir, 'bringup_launch.py')),
launch_arguments = {'namespace': namespace,
'use_namespace': use_namespace,
'slam': slam,
'map': map_yaml_file,
'use_sim_time': use_sim_time,
'params_file': params_file,
'default_bt_xml_filename': default_bt_xml_filename,
'autostart': autostart}.items())

# Create the launch description and populate
ld = LaunchDescription()

# Declare the launch options

return ld
```

Save the file, and close it.

We now need to add a static map of our world so our robot can plan an obstacle-free path between two points.

I already created a map of the world in a previous tutorial, so we’ll use the yaml and pgm file from that tutorial.

Place this pgm file and this yaml file inside the folder.

Let’s add parameters for the ROS 2 Navigation Stack. The official Configuration Guide has a full breakdown of all the tunable parameters. The parameters enable you to do all sorts of things with the ROS 2 Navigation Stack.

The most important parameters are for the Costmap 2D package. You can learn about this package here and here.

A costmap is a map made up of numerous grid cells. Each grid cell has a “cost”. The cost represents the difficulty a robot would have trying to move through that cell.

For example, a cell containing an obstacle would have a high cost. A cell that has no obstacle in it would have a low cost.

The ROS Navigation Stack uses two costmaps to store information about obstacles in the world.

1. Global costmap: This costmap is used to generate long term plans over the entire environment….for example, to calculate the shortest path from point A to point B on a map.
2. Local costmap: This costmap is used to generate short term plans over the environment….for example, to avoid obstacles.

We will use the AMCL (Adaptive Monte Carlo Localization) algorithm for localizing the robot in the world and for publishing the coordinate transform from the map to odom frame.

AMCL localizes the robot in the world using LIDAR scans. It does this by matching real-time scan information to a known map. You can read more about AMCL here and here.

Place this nav2_params.yaml file inside the folder.

If you are using ROS 2 Galactic or newer, your code is here.

# Create an RViz Configuration File

`colcon_cd basic_mobile_robot`
`cd rviz `

Create a new RViz file.

`gedit nav2_config.rviz`

Copy and paste this code inside the file.

Save the file, and close it.

# Update the Plugin Parameters

I updated the LIDAR plugin parameters inside model.sdf inside the basic_mobile_robot_description folder.

I also updated the differential drive plugin to use odometry data from the WORLD as the source rather than ENCODER.

`cd ~/dev_ws/src/basic_mobile_robot/models/basic_mobile_bot_description`
`gedit model.sdf`

Make sure you copy and paste this code into the model.sdf file, and then save and close it.

# Update the Robot Localization Parameters

Inside my ekf.yaml file, I updated the map_frame since we will be using a map. The robot_localization package will not be using the map, but I still want to update this parameter so that it is there if I need it.

`cd ~/dev_ws/src/basic_mobile_robot/config`
`gedit ekf.yaml`

Make sure you copy and paste this code into the ekf.yaml file, and then save and close it.

# Build the Package

Now build the package by opening a terminal window, and typing the following command:

`cd ~/dev_ws`
`colcon build`

# Launch the Robot Without SLAM

Open a new terminal window, and type the following command.

`colcon_cd basic_mobile_robot`

Launch the robot.

`ros2 launch basic_mobile_robot basic_mobile_bot_v5.launch.py`

## Move the Robot From Point A to Point B

Now go to the RViz screen.

Set the initial pose of the robot by clicking the “2D Pose Estimate” on top of the rviz2 screen (Note: we could have also set the set_initial_pose and initial_pose parameters in the nav2_params.yaml file to True in order to automatically set an initial pose.)

Then click on the map in the estimated position where the robot is in Gazebo.

Set a goal for the robot to move to. Click “Navigation2 Goal” button in RViz, and click on a desired destination.

You can also request goals through the terminal by using the following command:

`ros2 topic pub /goal_pose geometry_msgs/PoseStamped "{header: {stamp: {sec: 0}, frame_id: 'map'}, pose: {position: {x: 5.0, y: -2.0, z: 0.0}, orientation: {w: 1.0}}}"`

You will notice that we published the goal to the /goal_pose topic.

The wheeled robot will move to the goal destination.

In the bottom left of the screen, you can Pause and Reset.

If the robot does not move at all, press CTRL+C in all windows to close everything down. Then try launching the robot again.

The key to getting good performance with the ROS 2 Navigation Stack is to spend a lot of time (it can take me several days) tweaking the parameters in the nav2_params.yaml file we built earlier. Yes, it is super frustrating, but this is the only way to get navigation to work properly.

Common things you can try changing are the robot_radius and the inflaition_radius parameters. You can also try changing the expected_planner_frequency, update_frequency, publish_frequency, and width/height of the rolling window in the local_costmap.

Also, you can try modifying the update_rate in the LIDAR sensor inside your robot model.sdf file.

Don’t change too many things all at once. Just change something, build the package, and then launch the robot again to see what happens. Then change another thing, and watch what happens, etc.

Now let’s check out the coordinate frames. Open a new terminal window, and type:

`ros2 run tf2_tools view_frames.py`

If you are using ROS 2 Galactic or newer, type:

`ros2 run tf2_tools view_frames`

In the current working directory, you will have a file called frames.pdf. Open that file.

`evince frames.pdf`

Here is what my coordinate transform (i.e. tf) tree looks like:

To see an image of the architecture of our ROS system, open a new terminal window, and type the following command:

`rqt_graph`

Press CTRL + C on all terminal windows to close down the programs.

## Move the Robot Through Waypoints

Open a new terminal window, and type the following command.

`colcon_cd basic_mobile_robot`

Launch the robot.

`ros2 launch basic_mobile_robot basic_mobile_bot_v5.launch.py`

Now go to the RViz screen.

Set the initial pose of the robot by clicking the “2D Pose Estimate” on top of the rviz2 screen.

Then click on the map in the estimated position where the robot is in Gazebo.

Now click the Waypoint mode button in the bottom left corner of RViz. Clicking this button puts the system in waypoint follower mode.

Click “Navigation2 Goal” button, and click on areas of the map where you would like your robot to go (i.e. select your waypoints). Select as many waypoints as you want.

I chose five waypoints. Each waypoint is labeled wp_#, where # is the number of the waypoint.

You should see your robot autonomously navigate to all the waypoints. At each waypoint, your robot will stop for 10-20 seconds, and then it will move to the next waypoint.

If your robot does not navigate to the waypoints, relaunch the robot and try again. Try selecting different waypoints.

The ROS 2 Navigation Stack waypoint follower functionality isn’t perfect. Many times, the robot will skip over waypoints or abandon them completely. The most common error I get when this happens is the following:

[bt_navigator]: Action server failed while executing action callback: “send_goal failed”

[bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.

This issue is a known problem in ROS 2 Foxy, and it appears to be fixed in the latest version of ROS 2 (i.e. Galactic). We won’t upgrade ROS right now, but this is something to keep in mind if you are using a version of ROS 2 that is newer than ROS 2 Foxy.

In addition, I like to play around with the parameters in the nav2_params.yaml file located inside the params folder of your package. A complete guide to all the parameters is here.

Finally, let’s check out the active ROS 2 topics.

`ros2 topic list`

# Launch the Robot With SLAM

Make sure the SLAM toolbox is installed. Open a terminal window, and type:

`sudo apt install ros-foxy-slam-toolbox`

The syntax is:

`sudo apt install ros-<ros2-distro>-slam-toolbox`

Open the model.sdf file inside the basic_mobile_robot/models/basic_mobile_bot_description folder, and change the number of LIDAR samples (inside the <samples></samples> tag) to some high number like 120.

To launch the robot with SLAM (simultaneous localization and mapping), open a terminal window, and run the following command:

`ros2 launch basic_mobile_robot basic_mobile_bot_v5.launch.py slam:=True`

Use the rqt_robot_steering tool to slowly drive the robot around the room. Open a terminal window, and type:

`rqt_robot_steering`

If you are using ROS 2 Galactic or newer, type:

`sudo apt-get install ros-galactic-rqt-robot-steering`

Where the syntax is:

`sudo apt-get install ros-<ros-distribution>-rqt-robot-steering`

Then type:

`ros2 run rqt_robot_steering rqt_robot_steering --force-discover`

The robot will build a map and localize at the same time. You can also use autonomous navigation using the RViz buttons like we did in the last section.

## Save the Map (ROS Foxy and Older)

If you are using a ROS Distribution that is ROS Foxy and older, you will have to follow these instructions to save the map you have built. These instructions will have to be done before you launch the robot with SLAM. Let’s walk through the process below.

Open a new terminal window, and type:

`colcon_cd basic_mobile_robot`
`cd launch`

Now go back to the terminal window, and type the following command:

`colcon_cd basic_mobile_robot`
`cd params`

Go back to the terminal window.

Build the package by typing the following commands:

`cd ~/dev_ws`
`colcon build`

Launch the robot again with SLAM from your maps directory.

`colcon_cd basic_mobile_robot`
`cd maps`
`ros2 launch basic_mobile_robot basic_mobile_bot_v5.launch.py slam:=True`

Drive the robot around to create the map. In a new terminal window, you will type the following command to pull up the steering controller:

`rqt_robot_steering`

Execute the launch file once you’re done mapping the environment. Open a new terminal window, and type:

`ros2 launch basic_mobile_robot map_saver.launch.py`

Ignore any error messages that appear in the terminal window when you type the command above.

In a separate terminal, call the service to generate your map. We will call the map “my_map”:

`ros2 service call /map_saver/save_map nav2_msgs/srv/SaveMap "{map_topic: map, map_url: my_map, image_format: pgm, map_mode: trinary, free_thresh: 0.25, occupied_thresh: 0.65}"`

Your my_map.pgm and my_map.yaml file will save to the maps directory of your basic_mobile_robot package.

## Save the Map (ROS Galactic and Newer)

If you have ROS Galactic or newer, open a new terminal window, and type:

`colcon_cd basic_mobile_robot`
`cd maps`

When you are happy with the map you have built, open a new terminal window, and type the following command to save the map:

`ros2 run nav2_map_server map_saver_cli -f my_map`

The syntax is:

`ros2 run nav2_map_server map_saver_cli -f <map_name>`

Your my_map.pgm and my_map.yaml map files will automatically save to the maps directory of your basic_mobile_robot package.

That’s it!

In the next tutorial, we will take a look at how to incorporate GPS data to create better localization. Stay tuned!