Week-7-8-ROS2-Navigation

Week 7-8: Navigation

This is how far we will get by the end of this lesson:

Table of Contents

1. Introduction
   1.1. Download ROS package
   1.2. Test the starter package
2. Mapping
   2.1. SLAM toolbox
3. Localization
   3.1. Localization with AMCL
   3.2. Localization with SLAM toolbox
4. Navigation
   4.1. Waypoint navigation
   4.2. Navigation with SLAM
5. Exploration

Introduction

In this lesson we’ll lear how to map the robot’s environment, how to do localization on an existing map and we’ll learn to use ROS2’s navigation stack.

Download ROS package

To download the starter package, clone the following git repo with the starter-branch (using the -b branch flag) into your colcon workspace:

git clone -b starter-branch https://github.com/MOGI-ROS/Week-7-8-ROS2-Navigation

Let’s take a look what’s inside the bme_ros2_navigation package with the tree command!

.
├── CMakeLists.txt
├── package.xml
├── config
│   ├── amcl_localization.yaml
│   ├── ekf.yaml
│   ├── gz_bridge.yaml
│   ├── navigation.yaml
│   ├── slam_toolbox_localization.yaml
│   ├── slam_toolbox_mapping.yaml
│   └── waypoints.yaml
├── launch
│   ├── check_urdf.launch.py
│   ├── spawn_robot.launch.py
│   └── world.launch.py
├── maps
│   ├── my_map.pgm
│   ├── my_map.yaml
│   ├── serialized.data
│   └── serialized.posegraph
├── meshes
│   ├── lidar.dae
│   ├── mogi_bot.dae
│   └── wheel.dae
├── rviz
│   ├── localization.rviz
│   ├── mapping.rviz
│   ├── navigation.rviz
│   ├── rviz.rviz
│   └── urdf.rviz
├── urdf
│   ├── materials.xacro
│   ├── mogi_bot.gazebo
│   └── mogi_bot.urdf
└── worlds
    ├── empty.sdf
    └── home.sdf

Let’s see what will we do with the existing files and folders:

• config: As we saw previously, we usually store parameters and large configuration files for ROS packages which aren’t comfortable to handle from the launchfiles directly. In this lesson we will use more configuration files from this folder.
• launch: Default launch files are already part of the starting package, we can test the package with spawn_robot.launch.py.
• maps: Offline map files for the Gazebo world
• meshes: this folder contains the 3D models in dae format (collada mesh) that we use for our robot’s body, wheels and lidar sensor.
• rviz: Pre-configured RViz2 layouts
• urdf: The URDF models of our robot, we’ll extend the mogi_bot.urdf and gazebo files through this lesson
• worlds: default Gazebo worlds that we’ll use in the simulations.

We have another package bme_ros2_navigation_py for our python scripts:

.
├── bme_ros2_navigation_py
│   ├── __init__.py
│   ├── send_initialpose.py
│   └── slam_toolbox_load_map.py
├── package.xml
├── resource
│   └── bme_ros2_navigation_py
├── setup.cfg
└── setup.py

Test the starter package

After we downloaded the starter-branch from GitHub, let’s rebuild the workspace and source the install/setup.bash file to make sure ROS and its tools are aware about the new package. Before we test it let’s take a look on the spawn_robot.launch.py, we’ll notice a few important changes!

• This package uses ekf sensor fusion by default, we don’t forward the tf transformations from Gazebo, because it’s done by the robot_localization package.
• We moved all the parameter_bridge topics into a yaml config file gz_bridge.yaml, we don’t describe them in the launch file anymore, this is more comfortable when we forward many topics.
• There is a new node in the launch file marker_server from the interactive-marker-twist-server this can be used to move and rotate our robot directly from RViz. Let’s install it with sudo apt install ros-jazzy-interactive-marker-twist-server!

Now we are ready to test the package with the usual launch file:

ros2 launch bme_ros2_navigation spawn_robot.launch.py

Let’s see the rqt_graph tool to see the connections between the nodes:

And also try the rqt_tf_tree:

ros2 run rqt_tf_tree rqt_tf_tree

Mapping

Let’s learn how to create the map of the robot’s surrounding. In practice we are using SLAM algorithms, SLAM stands for Simultaneous Localization and Mapping. It is a fundamental technique in robotics (and other fields) that allows a robot to:

1. Build a map of an unknown environment (mapping).
2. Track its own pose (position and orientation) within that map at the same time (localization).

Usually SLAM algorithms consists of 4 core functionalities:

1. Sensor inputs
   SLAM typically uses sensor data (e.g., LIDAR scans, camera images, or depth sensor measurements) to detect features or landmarks in the environment.
2. State estimation
   An internal state (the robot’s pose, including x, y, yaw) is estimated using algorithms like Extended Kalman Filters, Particle Filters, or Graph Optimization.
3. Map building
   As the robot moves, it accumulates new sensor data. The SLAM algorithm integrates that data into a global map (2D grid map, 3D point cloud, or other representations).
4. Loop closure
   When the robot revisits a previously mapped area, the SLAM algorithm detects that it’s the same place (loop closure). This knowledge is used to reduce accumulated drift and refine both the map and pose estimates.

SLAM Toolbox

In this lesson we will use the slam_toolbox package that has to be installed first:

sudo apt install ros-jazzy-slam-toolbox

After installing it we will create a new launch file for mapping, the slam_toolbox parameters are already in the config folder (slam_toolbox_mapping.yaml) we’ll use these parameters. Let’s also move the interactive_twist_marker and RViz related functions into the new launch file from spawn_robot.launch.py.

    #launchDescriptionObject.add_action(rviz_launch_arg)
    #launchDescriptionObject.add_action(rviz_config_arg)
    launchDescriptionObject.add_action(world_arg)
    launchDescriptionObject.add_action(model_arg)
    launchDescriptionObject.add_action(x_arg)
    launchDescriptionObject.add_action(y_arg)
    launchDescriptionObject.add_action(yaw_arg)
    launchDescriptionObject.add_action(sim_time_arg)
    launchDescriptionObject.add_action(world_launch)
    #launchDescriptionObject.add_action(rviz_node)
    launchDescriptionObject.add_action(spawn_urdf_node)
    launchDescriptionObject.add_action(gz_bridge_node)
    launchDescriptionObject.add_action(gz_image_bridge_node)
    launchDescriptionObject.add_action(relay_camera_info_node)
    launchDescriptionObject.add_action(robot_state_publisher_node)
    launchDescriptionObject.add_action(trajectory_node)
    launchDescriptionObject.add_action(ekf_node)
    #launchDescriptionObject.add_action(interactive_marker_twist_server_node)

Also change the reference_frame_id of mogi_trajectory_server from odom to map because this will be our new reference frame when we have a map!

    trajectory_node = Node(
        package='mogi_trajectory_server',
        executable='mogi_trajectory_server',
        name='mogi_trajectory_server',
        parameters=[{'reference_frame_id': 'map'}]
    )

Let’s create mapping.launch.py:

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation')

    gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation)
    os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path

    rviz_launch_arg = DeclareLaunchArgument(
        'rviz', default_value='true',
        description='Open RViz'
    )

    rviz_config_arg = DeclareLaunchArgument(
        'rviz_config', default_value='mapping.rviz',
        description='RViz config file'
    )

    sim_time_arg = DeclareLaunchArgument(
        'use_sim_time', default_value='True',
        description='Flag to enable use_sim_time'
    )

    # Generate path to config file
    interactive_marker_config_file_path = os.path.join(
        get_package_share_directory('interactive_marker_twist_server'),
        'config',
        'linear.yaml'
    )

    # Path to the Slam Toolbox launch file
    slam_toolbox_launch_path = os.path.join(
        get_package_share_directory('slam_toolbox'),
        'launch',
        'online_async_launch.py'
    )

    slam_toolbox_params_path = os.path.join(
        get_package_share_directory('bme_ros2_navigation'),
        'config',
        'slam_toolbox_mapping.yaml'
    )

    # Launch rviz
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])],
        condition=IfCondition(LaunchConfiguration('rviz')),
        parameters=[
            {'use_sim_time': LaunchConfiguration('use_sim_time')},
        ]
    )

    interactive_marker_twist_server_node = Node(
        package='interactive_marker_twist_server',
        executable='marker_server',
        name='twist_server_node',
        parameters=[interactive_marker_config_file_path],
        output='screen',
    )

    slam_toolbox_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(slam_toolbox_launch_path),
        launch_arguments={
                'use_sim_time': LaunchConfiguration('use_sim_time'),
                'slam_params_file': slam_toolbox_params_path,
        }.items()
    )

    launchDescriptionObject = LaunchDescription()

    launchDescriptionObject.add_action(rviz_launch_arg)
    launchDescriptionObject.add_action(rviz_config_arg)
    launchDescriptionObject.add_action(sim_time_arg)
    launchDescriptionObject.add_action(rviz_node)
    launchDescriptionObject.add_action(interactive_marker_twist_server_node)
    launchDescriptionObject.add_action(slam_toolbox_launch)

    return launchDescriptionObject

Build the workspace and we’ll need 2 terminals, in the first we launch the simulation like before (but this time it won’t open RViz):

ros2 launch bme_ros2_navigation spawn_robot.launch.py

And in another terminal we launch the new mapping.launch.py:

ros2 launch bme_ros2_navigation mapping.launch.py

Let’s take a look first on rqt_tf_tree:

ros2 run rqt_tf_tree rqt_tf_tree

We can see an additional frame map over the odom odometry frame. We can also visualize this transformation in RViz:

The difference between the odom and map frames show the accumulated drift of our odometry over time. don’t forget that this is already improved a lot thanks to the sensor fusion between the raw odometry and the IMU.

With SLAM Toolbox we can also save the maps, we have two options:

1. Save Map: The map is saved as a .pgm file and a .yaml file. This is a black and white image file that can be used with other ROS nodes for localization as we will see later. Since it’s only an image file it’s impossible to continue the mapping with such a file because SLAM Toolbox handles the map in the background as a graph that cannot be restored from an image.
2. Serialize Map: With this feature we can serialize and later deserialize SLAM Toolbox’s graph, so it can be loaded and the mapping can be continued. Although other ROS nodes won’t be able to read or use it for localization.

After saving a serialized map next time we can load (deserialize it):

And we can also load the map that is in the starter package of this lesson:

We can also use a custom node to deserialize the already saved map, it implements the same functionality as the Deserialize Map button in RViz:

ros2 run bme_ros2_navigation_py slam_toolbox_load_map

Another way to save the .pgm and .yaml map for later use is the map_saver_cli tool of the navigation stack.

ros2 run nav2_map_server map_saver_cli -f my_map

To use it, map_server has to be installed:

sudo apt install ros-jazzy-nav2-map-server

Localization

While mapping was the process of creating a representation (a map) of an environment. Localization is the process by which a robot determines its own position and orientation within a known environment (map). In other words:

• The environment or map is typically already available or pre-built.
• The robot’s task is to figure out “Where am I?” or “Which direction am I facing?” using sensor data, often by matching its current perceptions to the known map.

Localization with AMCL

AMCL (Adaptive Monte Carlo Localization) is a particle filter–based 2D localization algorithm. The robot’s possible poses (position + orientation in 2D) are represented by a set of particles. It adaptively samples the robot’s possible poses according to sensor readings and motion updates, converging on an accurate estimate of where the robot is within a known map.

AMCL is part of the ROS2 navigation stack, let’s install it first:

sudo apt install ros-jazzy-nav2-bringup 
sudo apt install ros-jazzy-nav2-amcl

And then let’s create a new launch file localization.launch.py:

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation')

    gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation)
    os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path

    rviz_launch_arg = DeclareLaunchArgument(
        'rviz', default_value='true',
        description='Open RViz'
    )

    rviz_config_arg = DeclareLaunchArgument(
        'rviz_config', default_value='localization.rviz',
        description='RViz config file'
    )

    sim_time_arg = DeclareLaunchArgument(
        'use_sim_time', default_value='True',
        description='Flag to enable use_sim_time'
    )

    # Generate path to config file
    interactive_marker_config_file_path = os.path.join(
        get_package_share_directory('interactive_marker_twist_server'),
        'config',
        'linear.yaml'
    )

    # Path to the Slam Toolbox launch file
    nav2_localization_launch_path = os.path.join(
        get_package_share_directory('nav2_bringup'),
        'launch',
        'localization_launch.py'
    )

    localization_params_path = os.path.join(
        get_package_share_directory('bme_ros2_navigation'),
        'config',
        'amcl_localization.yaml'
    )

    map_file_path = os.path.join(
        get_package_share_directory('bme_ros2_navigation'),
        'maps',
        'my_map.yaml'
    )

    # Launch rviz
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])],
        condition=IfCondition(LaunchConfiguration('rviz')),
        parameters=[
            {'use_sim_time': LaunchConfiguration('use_sim_time')},
        ]
    )

    interactive_marker_twist_server_node = Node(
        package='interactive_marker_twist_server',
        executable='marker_server',
        name='twist_server_node',
        parameters=[interactive_marker_config_file_path],
        output='screen',
    )

    localization_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(nav2_localization_launch_path),
        launch_arguments={
                'use_sim_time': LaunchConfiguration('use_sim_time'),
                'params_file': localization_params_path,
                'map': map_file_path,
        }.items()
    )

    launchDescriptionObject = LaunchDescription()

    launchDescriptionObject.add_action(rviz_launch_arg)
    launchDescriptionObject.add_action(rviz_config_arg)
    launchDescriptionObject.add_action(sim_time_arg)
    launchDescriptionObject.add_action(rviz_node)
    launchDescriptionObject.add_action(interactive_marker_twist_server_node)
    launchDescriptionObject.add_action(localization_launch)

    return launchDescriptionObject

Build the workspace and let’s try it, we’ll need 2 terminals again, one for the simulation:

ros2 launch bme_ros2_navigation spawn_robot.launch.py

And another for the new launch file:

ros2 launch bme_ros2_navigation localization.launch.py

To start using AMCL we have to provide an initial pose to the /initialpose topic. We can use RViz’s built in tool for that.

This will initialize AMCL’s particles around the initial pose which can be displayed in RViz as a particle cloud where each particle rerpresnts a pose (position + orientation in 2D).

The initial distribution and spread of the partciles depends on the covariance matrix of the initial pose that cannot be changed in RViz, but we can publish our own /initialpose topic by a custom node which is in the package of this lesson, send_initialpose.py.

The main purpose of the localization algorithm is establishing the transformation between the fixed map and the robot’s odometry frame based on real time sensor data. We can visualize this in RViz as we saw it during mapping:

Localization with SLAM toolbox

It’s possible to use SLAM toolbox in localization mode, it requires a small adjustment on the parameters which is already part of this lesson, slam_toolbox_localization.yaml. This requires the path to the serialized map file.

Make sure the map_file_name parameter is changed to the path on your machine to the serialized map file!

Let’s create the launch file for localization, localization_slam_toolbox.launch.py, it’s very similar to the SLAM toolbox mapping launch file:

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation')

    gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation)
    os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path

    rviz_launch_arg = DeclareLaunchArgument(
        'rviz', default_value='true',
        description='Open RViz'
    )

    rviz_config_arg = DeclareLaunchArgument(
        'rviz_config', default_value='mapping.rviz',
        description='RViz config file'
    )

    sim_time_arg = DeclareLaunchArgument(
        'use_sim_time', default_value='True',
        description='Flag to enable use_sim_time'
    )

    # Generate path to config file
    interactive_marker_config_file_path = os.path.join(
        get_package_share_directory('interactive_marker_twist_server'),
        'config',
        'linear.yaml'
    )

    # Path to the Slam Toolbox launch file
    slam_toolbox_launch_path = os.path.join(
        get_package_share_directory('slam_toolbox'),
        'launch',
        'localization_launch.py'
    )

    slam_toolbox_params_path = os.path.join(
        get_package_share_directory('bme_ros2_navigation'),
        'config',
        'slam_toolbox_localization.yaml'
    )

    # Launch rviz
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])],
        condition=IfCondition(LaunchConfiguration('rviz')),
        parameters=[
            {'use_sim_time': LaunchConfiguration('use_sim_time')},
        ]
    )

    interactive_marker_twist_server_node = Node(
        package='interactive_marker_twist_server',
        executable='marker_server',
        name='twist_server_node',
        parameters=[interactive_marker_config_file_path],
        output='screen',
    )

    slam_toolbox_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(slam_toolbox_launch_path),
        launch_arguments={
                'use_sim_time': LaunchConfiguration('use_sim_time'),
                'slam_params_file': slam_toolbox_params_path,
        }.items()
    )

    launchDescriptionObject = LaunchDescription()

    launchDescriptionObject.add_action(rviz_launch_arg)
    launchDescriptionObject.add_action(rviz_config_arg)
    launchDescriptionObject.add_action(sim_time_arg)
    launchDescriptionObject.add_action(rviz_node)
    launchDescriptionObject.add_action(interactive_marker_twist_server_node)
    launchDescriptionObject.add_action(slam_toolbox_launch)

    return launchDescriptionObject

Rebuild the workspace and try it, we’ll see that the map keeps updating unlike with AMCL, this is the normal behavior of the localization with SLAM toolbox. According to this paper, the localization mode does continue to update the pose-graph with new constraints and nodes, but the updated map expires over some time. SLAM toolbox describes it as “elastic”, which means it holds the updated graph for some amount of time, but does not add it to the permanent graph.

Navigation

Navigation in robotics is the overall process that enables a robot to move from one location to another in a safe, efficient, and autonomous manner. It typically involves:

1. Knowing where the robot is (localization or SLAM),
2. Knowing where it needs to go (a goal pose or waypoint),
3. Planning a path to reach that goal (path planning), and
4. Moving along that path while avoiding dynamic and static obstacles (motion control and obstacle avoidance).

ROS’s nav2 navigation stack implements the above points 2 to 4, for the first point we already met several possible solutions.

Let’s create a launch file that uses both AMCL and the nav2 navigation stack, navigation.launch.py:

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation')

    gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation)
    os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path

    rviz_launch_arg = DeclareLaunchArgument(
        'rviz', default_value='true',
        description='Open RViz'
    )

    rviz_config_arg = DeclareLaunchArgument(
        'rviz_config', default_value='navigation.rviz',
        description='RViz config file'
    )

    sim_time_arg = DeclareLaunchArgument(
        'use_sim_time', default_value='True',
        description='Flag to enable use_sim_time'
    )

    # Generate path to config file
    interactive_marker_config_file_path = os.path.join(
        get_package_share_directory('interactive_marker_twist_server'),
        'config',
        'linear.yaml'
    )

    # Path to the Slam Toolbox launch file
    nav2_localization_launch_path = os.path.join(
        get_package_share_directory('nav2_bringup'),
        'launch',
        'localization_launch.py'
    )

    nav2_navigation_launch_path = os.path.join(
        get_package_share_directory('nav2_bringup'),
        'launch',
        'navigation_launch.py'
    )

    localization_params_path = os.path.join(
        get_package_share_directory('bme_ros2_navigation'),
        'config',
        'amcl_localization.yaml'
    )

    navigation_params_path = os.path.join(
        get_package_share_directory('bme_ros2_navigation'),
        'config',
        'navigation.yaml'
    )

    map_file_path = os.path.join(
        get_package_share_directory('bme_ros2_navigation'),
        'maps',
        'my_map.yaml'
    )

    # Launch rviz
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])],
        condition=IfCondition(LaunchConfiguration('rviz')),
        parameters=[
            {'use_sim_time': LaunchConfiguration('use_sim_time')},
        ]
    )

    interactive_marker_twist_server_node = Node(
        package='interactive_marker_twist_server',
        executable='marker_server',
        name='twist_server_node',
        parameters=[interactive_marker_config_file_path],
        output='screen',
    )

    localization_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(nav2_localization_launch_path),
        launch_arguments={
                'use_sim_time': LaunchConfiguration('use_sim_time'),
                'params_file': localization_params_path,
                'map': map_file_path,
        }.items()
    )

    navigation_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(nav2_navigation_launch_path),
        launch_arguments={
                'use_sim_time': LaunchConfiguration('use_sim_time'),
                'params_file': navigation_params_path,
        }.items()
    )

    launchDescriptionObject = LaunchDescription()

    launchDescriptionObject.add_action(rviz_launch_arg)
    launchDescriptionObject.add_action(rviz_config_arg)
    launchDescriptionObject.add_action(sim_time_arg)
    launchDescriptionObject.add_action(rviz_node)
    #launchDescriptionObject.add_action(interactive_marker_twist_server_node)
    launchDescriptionObject.add_action(localization_launch)
    launchDescriptionObject.add_action(navigation_launch)

    return launchDescriptionObject

We’ll need 2 terminals as before, one for the simulation:

ros2 launch bme_ros2_navigation spawn_robot.launch.py

And in another terminal we launch the new navigation.launch.py:

ros2 launch bme_ros2_navigation navigation.launch.py

We are using AMCL, so first we’ll have to publish an initial pose, then we have to tell the pose goal to the navigation stack. For that we can also use RViz’s other built-in feature:

As soon as the pose goal is received the navigation stack plans a global path to the goal and the controller ensures locally that the robot follows the global path while it avoids dynamic obstacles. The controller calculates a cost map around the robot that determines the ideal trajectory of the robot. If there aren’t any obstacles around the robot this cost map weighs the global plan.

If obstacles are detected around the robot those can be visualized as a cost map too:

Waypoint navigation

We can use the navigation stack for waypoint navigation, this can be done through the GUI of RViz or writing a custom node. Let’s start with the first one.

We’ll need 2 terminals as before, one for the simulation:

ros2 launch bme_ros2_navigation spawn_robot.launch.py

And in another terminal we launch the navigation.launch.py:

ros2 launch bme_ros2_navigation navigation.launch.py

First, we have to make sure that the Nav2 Goal toolbar is added to RViz! If not, we can add it under the + sign.

Then we have to switch nav2 to waypoint following mode:

Using the Nav2 Goal tool we can define the waypoints:

And when we are done with the waypoints we can start the navigation through them:

It’s possible to run multiple loops through the waypoints and it’s also possible to save and load the waypoints. One example is already in the config folder: waypoints.yaml. You can see a video about it here:

It’s also possible to follow waypoints through the nav2 navigation stack’s API with a custom node. Let’s create follow_waypoints.py in the bme_ros2_navigation_py package:

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import FollowWaypoints
from rclpy.action import ActionClient
from tf_transformations import quaternion_from_euler

class WaypointFollower(Node):
    def __init__(self):
        super().__init__('waypoint_follower')
        self._action_client = ActionClient(self, FollowWaypoints, 'follow_waypoints')

    def define_waypoints(self):
        waypoints = []

        # Waypoint 1
        wp1 = PoseStamped()
        wp1.header.frame_id = 'map'
        wp1.pose.position.x = 6.0
        wp1.pose.position.y = 1.5
        q = quaternion_from_euler(0, 0, 0)
        wp1.pose.orientation.x = q[0]
        wp1.pose.orientation.y = q[1]
        wp1.pose.orientation.z = q[2]
        wp1.pose.orientation.w = q[3]
        waypoints.append(wp1)

        return waypoints

    def send_goal(self):
        waypoints = self.define_waypoints()
        goal_msg = FollowWaypoints.Goal()
        goal_msg.poses = waypoints

        self._action_client.wait_for_server()
        self._send_goal_future = self._action_client.send_goal_async(
            goal_msg, feedback_callback=self.feedback_callback
        )
        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('Goal rejected.')
            return

        self.get_logger().info('Goal accepted.')
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        current_waypoint = feedback.current_waypoint
        self.get_logger().info(f'Navigating to waypoint {current_waypoint}')

    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info('Waypoint following completed.')
        rclpy.shutdown()

def main(args=None):
    rclpy.init(args=args)
    waypoint_follower = WaypointFollower()
    waypoint_follower.send_goal()
    rclpy.spin(waypoint_follower)

if __name__ == '__main__':
    main()

The definition of the waypoints are identical to the config file we have, we define the x, y and z position and the orientation in a quaternion.

Let’s add the entry point to the setup.py:

    entry_points={
        'console_scripts': [
            'send_initialpose = bme_ros2_navigation_py.send_initialpose:main',
            'slam_toolbox_load_map = bme_ros2_navigation_py.slam_toolbox_load_map:main',
            'follow_waypoints = bme_ros2_navigation_py.follow_waypoints:main',
        ],
    },

Build the workspace, and we’ll need 3 terminals this time, one for the simulation:

ros2 launch bme_ros2_navigation spawn_robot.launch.py

Another terminal to launch the navigation.launch.py:

ros2 launch bme_ros2_navigation navigation.launch.py

And in the third one:

ros2 run bme_ros2_navigation_py follow_waypoints

We can add more waypoints easily:

...
        # Waypoint 2
        wp2 = PoseStamped()
        wp2.header.frame_id = 'map'
        wp2.pose.position.x = -2.0
        wp2.pose.position.y = -8.0
        q = quaternion_from_euler(0, 0, 1.57)
        wp2.pose.orientation.x = q[0]
        wp2.pose.orientation.y = q[1]
        wp2.pose.orientation.z = q[2]
        wp2.pose.orientation.w = q[3]
        waypoints.append(wp2)

        # Waypoint 3
        wp3 = PoseStamped()
        wp3.header.frame_id = 'map'
        wp3.pose.position.x = 0.0
        wp3.pose.position.y = 0.0
        q = quaternion_from_euler(0, 0, 0)
        wp3.pose.orientation.x = q[0]
        wp3.pose.orientation.y = q[1]
        wp3.pose.orientation.z = q[2]
        wp3.pose.orientation.w = q[3]
        waypoints.append(wp3)

        # Add more waypoints as needed
...

Navigation with SLAM

As we saw to navigate a mobile robot we need to know 2 things:

1. Knowing where the robot is (localization or SLAM),
2. Knowing where it needs to go (a goal pose or waypoint)

In the previous examples we used localization on a know map, but it’s also possible to navigate together with an online SLAM. It means we don’t know the complete environment around the robot but we can already navigate in the known surrounding.

Let’s create a navigation_with_slam.launch.py launch file where we start both the SLAM toolbox and the navigation stack:

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    pkg_bme_ros2_navigation = get_package_share_directory('bme_ros2_navigation')

    gazebo_models_path, ignore_last_dir = os.path.split(pkg_bme_ros2_navigation)
    os.environ["GZ_SIM_RESOURCE_PATH"] += os.pathsep + gazebo_models_path

    rviz_launch_arg = DeclareLaunchArgument(
        'rviz', default_value='true',
        description='Open RViz'
    )

    rviz_config_arg = DeclareLaunchArgument(
        'rviz_config', default_value='navigation.rviz',
        description='RViz config file'
    )

    sim_time_arg = DeclareLaunchArgument(
        'use_sim_time', default_value='True',
        description='Flag to enable use_sim_time'
    )

    # Generate path to config file
    interactive_marker_config_file_path = os.path.join(
        get_package_share_directory('interactive_marker_twist_server'),
        'config',
        'linear.yaml'
    )

    nav2_navigation_launch_path = os.path.join(
        get_package_share_directory('nav2_bringup'),
        'launch',
        'navigation_launch.py'
    )

    navigation_params_path = os.path.join(
        get_package_share_directory('bme_ros2_navigation'),
        'config',
        'navigation.yaml'
    )

    slam_toolbox_params_path = os.path.join(
        get_package_share_directory('bme_ros2_navigation'),
        'config',
        'slam_toolbox_mapping.yaml'
    )

    # Launch rviz
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', PathJoinSubstitution([pkg_bme_ros2_navigation, 'rviz', LaunchConfiguration('rviz_config')])],
        condition=IfCondition(LaunchConfiguration('rviz')),
        parameters=[
            {'use_sim_time': LaunchConfiguration('use_sim_time')},
        ]
    )

    interactive_marker_twist_server_node = Node(
        package='interactive_marker_twist_server',
        executable='marker_server',
        name='twist_server_node',
        parameters=[interactive_marker_config_file_path],
        output='screen',
    )

    # Path to the Slam Toolbox launch file
    slam_toolbox_launch_path = os.path.join(
        get_package_share_directory('slam_toolbox'),
        'launch',
        'online_async_launch.py'
    )

    slam_toolbox_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(slam_toolbox_launch_path),
        launch_arguments={
                'use_sim_time': LaunchConfiguration('use_sim_time'),
                'slam_params_file': slam_toolbox_params_path,
        }.items()
    )

    navigation_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(nav2_navigation_launch_path),
        launch_arguments={
                'use_sim_time': LaunchConfiguration('use_sim_time'),
                'params_file': navigation_params_path,
        }.items()
    )

    launchDescriptionObject = LaunchDescription()

    launchDescriptionObject.add_action(rviz_launch_arg)
    launchDescriptionObject.add_action(rviz_config_arg)
    launchDescriptionObject.add_action(sim_time_arg)
    launchDescriptionObject.add_action(rviz_node)
    #launchDescriptionObject.add_action(interactive_marker_twist_server_node)
    launchDescriptionObject.add_action(slam_toolbox_launch)
    launchDescriptionObject.add_action(navigation_launch)

    return launchDescriptionObject

Build the workspace and let’s try it!

We’ll need 2 terminals as before, one for the simulation:

ros2 launch bme_ros2_navigation spawn_robot.launch.py

And in another terminal we launch the new navigation_with_slam.launch.py:

ros2 launch bme_ros2_navigation navigation_with_slam.launch.py

Exploration

Exploration is a process by which a robot operating in an unknown or partially known environment actively searches the space to gather new information. This is a real life use case to use SLAM together with the navigation stack.

There is a simple exploration package that we will use in this lesson, you can download it from the GitHub:

git clone https://github.com/MOGI-ROS/m-explore-ros2.git

Build the workspace and source the setup.bash to make sure ROS is aware about the new package!

We’ll need 3 terminals, one for the simulation:

ros2 launch bme_ros2_navigation spawn_robot.launch.py

In another terminal we launch the navigation_with_slam.launch.py:

ros2 launch bme_ros2_navigation navigation_with_slam.launch.py

And in the third one we launch the exploration:

ros2 launch explore_lite explore.launch.py

The exploration node will identify the boundaries of the know surrounding and will navigate the robot until it finds all the physical boundaries of the environment.

We can take a look on the rqt_graph of the simulation, but we’ll see it’s quite big! We can find the exploration node and we can see that it subscribes to the /map and to the action status and feedback of the navigation stack.

And finally here is a video about exploration: