The Complete Guide to Parameters – ROS 2 C++

In this tutorial, we’ll explore parameters in ROS 2 using C++. Parameters are important for creating flexible and dynamic robot software. 

You’ll learn how to define, access, and use parameters within your nodes, enabling you to customize your robot’s behavior without altering its code. 

Join me as we dive into the essentials of parameters, setting the foundation for more complex robotics applications.

Real-World Applications

Here are some real-world examples of how ROS 2 parameters can be used in robotic arm applications:

  • Joint Limits and Velocities: Parameters can be used to define the safe operating range (minimum and maximum) for each joint of the arm in terms of angles or positions. Additionally, they can set the maximum allowed speed for each joint during movement. This ensures safe and controlled operation, preventing damage to the robot or its surroundings.
  • Payload Capacity: This parameter specifies the maximum weight the arm can safely carry while maintaining stability and avoiding damage to its motors or gears. This parameter is important for tasks like picking and placing objects or performing assembly operations.
  • Path Planning Parameters: These parameters influence how the arm plans its trajectory to reach a target point. They can include factors like joint acceleration/deceleration rates, smoothness of the path, and collision avoidance settings. 
  • End-effector Calibration: Parameters can store calibration data for the gripper or any tool attached to the end of the arm. This data can include offsets, transformations, or specific configurations for different tools, ensuring accurate tool positioning during tasks.

As you can see, pretty much any variable that applies to your robot can be declared as a parameter in ROS 2. Parameters are used to configure nodes at startup (and during runtime), without changing the code.

Remember, a ROS 2 node is a mini-program (written in either Python or C++) that performs a specific task within a robot system. 

Prerequisites

Here is the GitHub repository that contains the code we will develop in this tutorial.

Write the Code

Let’s start by creating a ROS 2 node that declares two parameters (a floating-point value and a string) and includes a function that executes anytime one of the parameters changes.

Open a terminal, and type these commands to open VS Code.

cd ~/ros2_ws
code .

Right-click on src/cobot_arm_examples/src folder, and create a new file called “minimal_cpp_parameters.cpp”.

Type the following code inside minimal_cpp_parameters.cpp:

/**
 * @file minimal_cpp_parameters.cpp
 * @brief Demonstrates the basics of declaring parameters in ROS 2.
 *
 * Description: Demonstrates the basics of declaring parameters in ROS 2.
 * 
 * -------
 * Subscription Topics:
 *   None
 * -------
 * Publishing Topics:
 *   String message
 *   /topic_cpp - std_msgs/String
 * -------
 * @author Addison Sears-Collins
 * @date 2024-03-06
 */
 
#include <memory> // Include for smart pointer utilities
#include <rclcpp/rclcpp.hpp> // ROS 2 C++ client library
#include <rcl_interfaces/msg/parameter_descriptor.hpp> // Enables parameters' metadata such as their names and types.
#include <rclcpp/parameter.hpp> // Include for manipulating parameters within a node.

/**
 * @class MinimalParameter
 * @brief Defines a minimal ROS 2 node that declares parameters.
 *
 * This class inherits from rclcpp::Node and demonstrates the process of declaring
 * parameters within a ROS 2 node. It showcases how to declare parameters with
 * default values and descriptions, and how to handle parameter change requests
 * via a callback mechanism.
 */
 class MinimalParameter : public rclcpp::Node
{
public:
    /**
     * @brief Constructs a MinimalParameter node.
     *
     * Initializes the node, declares two parameters (`velocity_limit` and `robot_name`)
     * with default values and descriptions, and registers a callback for handling
     * parameter change requests.
     */
    MinimalParameter() : Node("minimal_parameter_cpp_node")
    {
        // Describe parameters
        rcl_interfaces::msg::ParameterDescriptor velocity_limit_descriptor;
        velocity_limit_descriptor.description = "Maximum allowed angular velocity in radians per second (ignored for fixed joints)";

        rcl_interfaces::msg::ParameterDescriptor robot_name_descriptor;
        robot_name_descriptor.description = "The name of the robot";

        // Declare parameters
        this->declare_parameter("velocity_limit", 2.792527, velocity_limit_descriptor);
        this->declare_parameter("robot_name", "Automatic Addison Bot", robot_name_descriptor);
    
        // Register a callback function that will be called whenever there is an attempt 
        // to change one or more parameters of the node.  
        parameter_callback_handle = this->add_on_set_parameters_callback(
            std::bind(&MinimalParameter::parameter_change_callback, this, std::placeholders::_1));		
    }
	
private:
    /**
     * @brief Callback function for handling parameter change requests.
     *
     * This method is invoked automatically whenever there is an attempt to change
     * one or more of the node's parameters. It checks each parameter change request,
     * logs the new value of recognized parameters, and validates the change.
     *
     * @param params A vector containing the parameters attempted to be changed.
     * @return An instance of rcl_interfaces::msg::SetParametersResult indicating
     *         whether the parameter changes were successful or not.
     */

    // Member variable to store the callback handle
    rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle;

    rcl_interfaces::msg::SetParametersResult parameter_change_callback(const std::vector<rclcpp::Parameter> &params)
    {
        // Create a result object to report the outcome of parameter changes.
	    auto result = rcl_interfaces::msg::SetParametersResult();

        // Assume success unless an unsupported parameter is encountered.
        result.successful = true;

        // Iterate through each parameter in the change request.    
        for (const auto &param : params) 
        {
            // Check if the changed parameter is 'velocity_limit' and of type double.
            if (param.get_name() == "velocity_limit" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) 
            {
                RCLCPP_INFO(this->get_logger(), "Parameter velocity_limit has changed. The new value is: %f", param.as_double());
            } 
            // Check if the changed parameter is 'robot_name' and of type string.
            else if (param.get_name() == "robot_name" && param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) 
            {
                RCLCPP_INFO(this->get_logger(), "Parameter robot_name has changed. The new value is: %s", param.as_string().c_str());
            } 
            // Handle any unsupported parameters.
            else 
            {
                // Mark the result as unsuccessful and provide a reason.
                result.successful = false;
                result.reason = "Unsupported parameter";
            }
        } 
        // Return the result object, indicating whether the parameter change(s) were successful or not.		
        return result;
    }
};

/**
 * @brief Main function.
 *
 * This function initializes the ROS 2 system, creates and runs an instance of the
 * MinimalParameter node. It keeps the node running until it is manually terminated,
 * ensuring that the node can react to parameter changes or perform its intended
 * functions during its lifecycle. Finally, it shuts down the ROS 2 system before
 * terminating the program.
 *
 * @param argc The number of command-line arguments.
 * @param argv The array of command-line arguments.
 * @return int Returns 0 upon successful completion of the program.
 */
int main(int argc, char * argv[])
{
    // Initialize ROS 2.
    rclcpp::init(argc, argv); 
  
    // Create an instance of the MinimalParameter node and keep it running.
    auto minimal_parameter_cpp_node = std::make_shared<MinimalParameter>();
    rclcpp::spin(minimal_parameter_cpp_node);

    // Shutdown ROS 2 upon node termination.
    rclcpp::shutdown(); 

    // End of program.
    return 0; 
}

Modify the CMakeLists.txt File

Now let’s configure the CMakeLists.txt file. Here is what it should look like:

cmake_minimum_required(VERSION 3.8)
project(cobot_arm_examples)

# Check if the compiler being used is GNU's C++ compiler (g++) or Clang.
# Add compiler flags for all targets that will be defined later in the 
# CMakeLists file. These flags enable extra warnings to help catch
# potential issues in the code.
# Add options to the compilation process
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# Locate and configure packages required by the project.
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(rcl_interfaces REQUIRED)
find_package(std_msgs REQUIRED)

# Define a CMake variable named dependencies that lists all
# ROS 2 packages and other dependencies the project requires.
set(dependencies
  rclcpp
  rcl_interfaces
  std_msgs
)

# Add the specified directories to the list of paths that the compiler
# uses to search for header files. This is important for C++
# projects where you have custom header files that are not located
# in the standard system include paths.
include_directories(
  include
)

# Tells CMake to create an executable target named minimal_cpp_publisher
# from the source file src/minimal_cpp_publisher.cpp. Also make sure CMake
# knows about the program's dependencies.
add_executable(minimal_cpp_publisher src/minimal_cpp_publisher.cpp)
ament_target_dependencies(minimal_cpp_publisher ${dependencies})

add_executable(minimal_cpp_subscriber src/minimal_cpp_subscriber.cpp)
ament_target_dependencies(minimal_cpp_subscriber ${dependencies})

add_executable(minimal_cpp_parameters src/minimal_cpp_parameters.cpp)
ament_target_dependencies(minimal_cpp_parameters ${dependencies})

# Copy necessary files to designated locations in the project
install (
  DIRECTORY cobot_arm_examples scripts
  DESTINATION share/${PROJECT_NAME}
)

install(
  DIRECTORY include/
  DESTINATION include
)

# Install cpp executables
install(
  TARGETS
  minimal_cpp_publisher
  minimal_cpp_subscriber
  minimal_cpp_parameters
  DESTINATION lib/${PROJECT_NAME}
)

# Install Python modules for import
ament_python_install_package(${PROJECT_NAME})

# Install Python executables
install(
  PROGRAMS
  scripts/minimal_py_publisher.py
  scripts/minimal_py_subscriber.py
  scripts/minimal_py_parameters.py
  #scripts/example3.py
  #scripts/example4.py
  #scripts/example5.py
  #scripts/example6.py
  #scripts/example7.py
  DESTINATION lib/${PROJECT_NAME}
)

# Automates the process of setting up linting for the package, which
# is the process of running tools that analyze the code for potential
# errors, style issues, and other discrepancies that do not adhere to
# specified coding standards or best practices.
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

# Used to export include directories of a package so that they can be easily
# included by other packages that depend on this package.
ament_export_include_directories(include)

# Generate and install all the necessary CMake and environment hooks that 
# allow other packages to find and use this package.
ament_package()

Modify the package.xml File

Add the rcl_interfaces dependency to package.xml:

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>cobot_arm_examples</name>
  <version>0.0.0</version>
  <description>Basic examples demonstrating ROS 2</description>
  <maintainer email="automaticaddison@example.com">Addison Sears-Collins</maintainer>
  <license>Apache-2.0</license>

  <!--Specify build tools that are needed to compile the package-->
  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>ament_cmake_python</buildtool_depend>

  <!--Declares package dependencies that are required for building the package-->
  <depend>rclcpp</depend>
  <depend>rclpy</depend>
  <depend>std_msgs</depend>
  <depend>rcl_interfaces</depend>

  <!--Specifies dependencies that are only needed for testing the package-->
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

Build the Workspace

Open a new terminal window, and type the following commands:

cd ~/ros2_ws/
colcon build
source ~/.bashrc

Run the Node 

In this section, we will finally run our node. Open a terminal window, and type:

ros2 run cobot_arm_examples minimal_cpp_parameters

Now, press Enter.

Open a new terminal window.

What are the currently active nodes?

ros2 node list

List all the parameters for all nodes in the ROS2 system.

ros2 param list

Retrieve the value of a specific parameter for a node.

ros2 param get /minimal_parameter_cpp_node robot_name
1-get-min-param-cpp-node-robot-name
ros2 param get /minimal_parameter_cpp_node velocity_limit
2-get-min-param-cpp-node-vel-limit

Set the value of a specific parameter for a specified node.

ros2 param set /minimal_parameter_cpp_node robot_name '!!str New Automatic Addison Bot'

You should see “Set parameter successful”.

Double check the new value:

ros2 param get /minimal_parameter_cpp_node robot_name
3-get-new-node-name
ros2 param set /minimal_parameter_cpp_node velocity_limit 1.0

Now go back to the terminals where your scripts are running and press CTRL + C to stop the execution.

To clear the terminal window, type:

clear

That’s it! Keep building!

The Complete Guide to Parameters – ROS 2 Python

In this tutorial, we’ll explore the power of parameters in ROS 2. Parameters are important for creating flexible and dynamic robot software. 

You’ll learn how to define, access, and use parameters within your nodes, enabling you to customize your robot’s behavior without altering its code. 

Join me as we dive into the essentials of parameters, setting the foundation for more complex robotics applications.

Real-World Applications

Here are some real-world examples of how ROS 2 parameters can be used in robotic arm applications:

  • Joint Limits and Velocities: Parameters can be used to define the safe operating range (minimum and maximum) for each joint of the arm in terms of angles or positions. Additionally, they can set the maximum allowed speed for each joint during movement. This ensures safe and controlled operation, preventing damage to the robot or its surroundings.
  • Payload Capacity: This parameter specifies the maximum weight the arm can safely carry while maintaining stability and avoiding damage to its motors or gears. This parameter is important for tasks like picking and placing objects or performing assembly operations.
  • Path Planning Parameters: These parameters influence how the arm plans its trajectory to reach a target point. They can include factors like joint acceleration/deceleration rates, smoothness of the path, and collision avoidance settings. 
  • End-effector Calibration: Parameters can store calibration data for the gripper or any tool attached to the end of the arm. This data can include offsets, transformations, or specific configurations for different tools, ensuring accurate tool positioning during tasks.

As you can see, pretty much any variable that applies to your robot can be declared as a parameter in ROS 2. Parameters are used to configure nodes at startup (and during runtime), without changing the code.

Remember, a ROS 2 node is a mini-program (written in either Python or C++) that performs a specific task within a robot system. 

Prerequisites

Here is the GitHub repository that contains the code we will develop in this tutorial.

Write the Code

Let’s start by creating a ROS 2 node that declares two parameters (a floating-point value and a string) and includes a function that executes anytime one of the parameters changes.

Open a terminal, and type these commands to open VS Code.

cd ~/ros2_ws
code .

Right-click on src/cobot_arm_examples/scripts folder, and create a new file called “minimal_py_parameters.py”.

Type the following code inside minimal_py_parameters.py:

#! /usr/bin/env python3

"""
Description:
    This ROS 2 node demonstrates how to declare parameters.
-------
Publishing Topics:
    None
-------
Subscription Topics:
    None    
-------
Author: Addison Sears-Collins
Date: March 4, 2024
"""

import rclpy # Import the ROS 2 client library for Python
from rclpy.node import Node # Import the Node class for creating ROS 2 nodes
from rcl_interfaces.msg import ParameterDescriptor # Enables the description of parameters
from rcl_interfaces.msg import SetParametersResult # Handles responses to parameter change requests
from rclpy.parameter import Parameter # Handles parameters within a node

class MinimalParameter(Node):
    """Create MinimalParameter node.

    """
    def __init__(self):
        """ Create a custom node class for declaring parameters.

        """

        # Initialize the node with a name
        super().__init__('minimal_parameter_node')

        # Describe parameters
        velocity_limit_descriptor = ParameterDescriptor(
            description='Maximum allowed angular velocity in radians per second (ignored for fixed joints)')        
        robot_name_descriptor = ParameterDescriptor(description='The name of the robot')

        # Declare parameters
        self.declare_parameter('velocity_limit', 2.792527, velocity_limit_descriptor)
        self.declare_parameter('robot_name', 'Automatic Addison Bot', robot_name_descriptor)

        # Register a callback function that will be called whenever there is an attempt to
        # change one or more parameters of the node.
        self.add_on_set_parameters_callback(self.parameter_change_callback)

    def parameter_change_callback(self, params):
        """Gets called whenever there is an attempt to change one or more parameters.

        Args:
            params (List[Parameter]): A list of Parameter objects representing the parameters that are 
                being attempted to change.
        
        Returns:
            SetParametersResult: Object indicating whether the change was successful.
        """
        result = SetParametersResult()

        # Iterate over each parameter in this node
        for param in params:
            # Check the parameter's name and type
            if param.name == "velocity_limit" and param.type_ == Parameter.Type.DOUBLE:
                # This parameter has changed. Display the new value to the terminal.
                self.get_logger().info("Parameter velocity_limit has changed. The new value is: %f" % param.value)
                # The parameter change was successfully handled.
                result.successful = True
            if param.name == "robot_name" and param.type_ == Parameter.Type.STRING:
                self.get_logger().info("Parameter robot_name has changed. The new value is: %s" % param.value)
                result.successful = True

        return result

def main(args=None):
    """Main function to start the ROS 2 node.

    Args:
        args (List, optional): Command-line arguments. Defaults to None.
    """

    # Initialize ROS 2 communication
    rclpy.init(args=args)

    # Create an instance of the MinimalParameter node
    minimal_parameter_node = MinimalParameter()

    # Keep the node running and processing events.
    rclpy.spin(minimal_parameter_node)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_parameter_node.destroy_node()

    # Shutdown ROS 2 communication
    rclpy.shutdown()

if __name__ == '__main__':
    # Execute the main function if the script is run directly
    main()

Modify the CMakeLists.txt File

Now let’s configure the CMakeLists.txt file. Here is what it should look like:

cmake_minimum_required(VERSION 3.8)
project(cobot_arm_examples)

# Check if the compiler being used is GNU's C++ compiler (g++) or Clang.
# Add compiler flags for all targets that will be defined later in the 
# CMakeLists file. These flags enable extra warnings to help catch
# potential issues in the code.
# Add options to the compilation process
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# Locate and configure packages required by the project.
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(rcl_interfaces REQUIRED)
find_package(std_msgs REQUIRED)

# Define a CMake variable named dependencies that lists all
# ROS 2 packages and other dependencies the project requires.
set(dependencies
  rclcpp
  rcl_interfaces
  std_msgs
)

# Add the specified directories to the list of paths that the compiler
# uses to search for header files. This is important for C++
# projects where you have custom header files that are not located
# in the standard system include paths.
include_directories(
  include
)

# Tells CMake to create an executable target named minimal_cpp_publisher
# from the source file src/minimal_cpp_publisher.cpp. Also make sure CMake
# knows about the program's dependencies.
add_executable(minimal_cpp_publisher src/minimal_cpp_publisher.cpp)
ament_target_dependencies(minimal_cpp_publisher ${dependencies})

add_executable(minimal_cpp_subscriber src/minimal_cpp_subscriber.cpp)
ament_target_dependencies(minimal_cpp_subscriber ${dependencies})

# Copy necessary files to designated locations in the project
install (
  DIRECTORY cobot_arm_examples scripts
  DESTINATION share/${PROJECT_NAME}
)

install(
  DIRECTORY include/
  DESTINATION include
)

# Install cpp executables
install(
  TARGETS
  minimal_cpp_publisher
  minimal_cpp_subscriber
  DESTINATION lib/${PROJECT_NAME}
)

# Install Python modules for import
ament_python_install_package(${PROJECT_NAME})

# Install Python executables
install(
  PROGRAMS
  scripts/minimal_py_publisher.py
  scripts/minimal_py_subscriber.py
  scripts/minimal_py_parameters.py
  #scripts/example3.py
  #scripts/example4.py
  #scripts/example5.py
  #scripts/example6.py
  #scripts/example7.py
  DESTINATION lib/${PROJECT_NAME}
)

# Automates the process of setting up linting for the package, which
# is the process of running tools that analyze the code for potential
# errors, style issues, and other discrepancies that do not adhere to
# specified coding standards or best practices.
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

# Used to export include directories of a package so that they can be easily
# included by other packages that depend on this package.
ament_export_include_directories(include)

# Generate and install all the necessary CMake and environment hooks that 
# allow other packages to find and use this package.
ament_package()

Build the Workspace

Open a new terminal window, and type the following commands:

cd ~/ros2_ws/
colcon build
source ~/.bashrc

Run the Node 

In this section, we will finally run our node. Open a terminal window, and type:

ros2 run cobot_arm_examples minimal_py_parameters.py

Now, press Enter.

Open a new terminal window.

What are the currently active nodes?

ros2 node list
1-ros2-node-list

List all the parameters for all nodes in the ROS2 system.

ros2 param list
2-ros2-param-list

Retrieve the value of a specific parameter for a node.

ros2 param get /minimal_parameter_node robot_name
ros2 param get /minimal_parameter_node velocity_limit
3-ros2-param-get

Set the value of a specific parameter for a specified node.

ros2 param set /minimal_parameter_node robot_name '!!str New Automatic Addison Bot'

You should see “Set parameter successful”.

4-name-change-successful

Double check the new value:

ros2 param get /minimal_parameter_node robot_name
ros2 param set /minimal_parameter_node velocity_limit 1.0
5-parameter-has-changed
6-new-velocity-limit

Now go back to the terminals where your scripts are running and press CTRL + C to stop the execution.

To clear the terminal window, type:

clear

That’s it! Keep building!

Sensor Fusion Using the Robot Localization Package – ROS 2

In this tutorial, I will show you how to set up the robot_localization ROS 2 package on a simulated mobile robot. We will use the robot_localization package to fuse odometry data from the /wheel/odometry topic with IMU data from the /imu/data topic to provide locally accurate, smooth odometry estimates. Wheels can slip, so using the robot_localization package can help correct for this.

This tutorial is the third 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 Galactic or newer, you can get the code here.

Let’s get started!

Prerequisites 

You have completed the first two tutorials of this series:

  1. How to Create a Simulated Mobile Robot in ROS 2 Using URDF
  2. Set Up the Odometry for a Simulated Mobile Robot in ROS 2

About the Robot Localization Package

We will configure the robot_localization package to use an Extended Kalman Filter (ekf_node) to fuse the data from sensor inputs. These sensor inputs come from the IMU Gazebo plugin and the differential drive Gazebo plugin that are defined in our SDF file.

In a real robotics project, instead of simulated IMU and odometry data, we would use data from an IMU sensor like the BNO055 and wheel encoders, respectively.

 The ekf_node will subscribe to the following topics (ROS message types are in parentheses):

  • /wheel/odometry :  Position and velocity estimate based on the information from the differential drive Gazebo plugin (in a real robotics project this would be information drawn from wheel encoder tick counts). The orientation is in quaternion format. (nav_msgs/Odometry)
  • /imu/data : Data from the Inertial Measurement Unit (IMU) sensor Gazebo plugin (sensor_msgs/Imu.msg)

This node will publish data to the following topics:

  • /odometry/filtered : The smoothed odometry information (nav_msgs/Odometry)
  • /tf : Coordinate transform from the odom frame (parent) to the base_footprint frame (child). To learn about coordinate frames in ROS, check out this post.

Install the Robot Localization Package

Let’s begin by installing the robot_localization package. Open a new terminal window, and type the following command:

sudo apt install ros-foxy-robot-localization

If you are using a newer version of ROS 2 like ROS 2 Humble, type the following:

 sudo apt install ros-humble-robot-localization 

Instead of the commands above, you can also type the following command directly into the terminal. It will automatically detect your ROS 2 distribution:

sudo apt install ros-$ROS_DISTRO-robot-localization

If you are using ROS 2 Galactic and want to build from the source instead of using the off-the-shelf packages above, you will need to download the robot_localization package to your workspace.

cd ~/dev_ws/src
git clone -b fix/galactic/load_parameters https://github.com/nobleo/robot_localization.git

The reason you need to download that package above is because the Navigation Stack might throw the following exception if you don’t:

[ekf_node-1] terminate called after throwing an instance of ‘rclcpp::ParameterTypeException’ [ekf_node-1] what(): expected [string] got [not set]

cd ..
colcon build

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:

colcon_cd basic_mobile_robot
cd config
gedit ekf.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. Also you can check out this link on GitHub for a sample ekf.yaml file.

Create a Launch File

Now go to your launch folder. Open a new terminal window, and type:

colcon_cd basic_mobile_robot
cd launch
gedit basic_mobile_bot_v3.launch.py

Copy and paste this code into the file.

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

Save the file, and close it.

Move to the package.

colcon_cd basic_mobile_robot

Open the package.xml file.

gedit package.xml file.

Copy and paste this code into the file.

Save the file, and close it.

Open the CMakeLists.txt file.

gedit CMakeLists.txt

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

Launch the Robot

Open a new terminal, and launch the robot.

cd ~/dev_ws/
ros2 launch basic_mobile_robot basic_mobile_bot_v3.launch.py

It might take a while for Gazebo and RViz to load, so be patient.

To see the active topics, open a terminal window, and type:

ros2 topic list

To see more information about the topics, execute:

ros2 topic info /imu/data
ros2 topic info /wheel/odometry

You should see an output similar to below:

16-ros2-topic-info-imu-wheel

Both topics have 1 publisher and 1 subscriber.

To see the output of the robot localization package (i.e. the Extended Kalman Filter (EKF)), type:

ros2 topic echo /odometry/filtered
16-zz-odometry-filtered

I will move my robot in the reverse direction using the rqt_robot_steering GUI. Open a new 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

Move the sliders to move the robot.

17-move-the-sliders

We can see the output of the odom -> base_footprint transform by typing the following command:

ros2 run tf2_ros tf2_echo odom base_footprint

Let’s see the active nodes.

ros2 node list
18-ros2-node-list

Let’s check out the ekf_node (named ekf_filter_node).

ros2 node info /ekf_filter_node
19-filter-node-info

Let’s check out the ROS node graph.

rqt_graph

Click the blue circular arrow in the upper left to refresh the node graph. Also select “Nodes/Topics (all)”.

20-ros-node-graph

To see the coordinate frames, type the following command in a terminal window.

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:

21-view-coordinate-frames

You can see that the parent frame is the odom frame. The odom frame is the initial position and orientation of the robot. Every other frame below that is a child of the odom frame.

Later, we will add a map frame. The map frame will be the parent frame of the odom frame.

Finally, in RViz, under Global Options, change Fixed Frame to odom.

22-fixed-frame-to-odom

Open the steering tool again.

rqt_robot_steering

If you move the robot around using the sliders, you will see the robot move in both RViz and Gazebo.

gazebo-and-rviz-1

That’s it!

In the next tutorial, I will show you how to add LIDAR to your robot so that you can map the environment and detect obstacles.