How to Replay ros2 bags with changed Quality of Service

How to Replay ros2 bags with changed Quality of Service

In this post, you will learn how to replay ros2 bags with changed Quality of Service (QoS) setting. You’ll discover how to set the QoS before recording the bag, and how to change the QoS when playing back the bag file.

Step 1: Get a Copy of the ROS package containing the code used in the post

Click here to copy the project. It would be copied to your cloud account at The Construct. That done, open the project using the Run button. This might take a few moments, please be patient.

Run rosject

PS: If you don’t have an account on the The Construct, you would need to create one. Once you create an account or log in, you will be able to follow the steps to read and write parameters in ros1 and ros2.

You might also want to try this on a local PC if you have ROS installed. In that case you need to read on and duplicate the source code of the package in your own local workspace. However, please note that we cannot support local PCs and you will have to fix any errors you run into on your own.

Step 2: Start a simulation and the Quality of Service publisher

Open Code Editor
Open a web shell

Open a web shell (1) and run the following commands to start a simulation:

export GAZEBO_RESOURCE_PATH=/home/user/ros2_ws/src/turtlebot/turtlebot3_simulations/turtlebot3_gazebo:${GAZEBO_RESOURCE_PATH}
export GAZEBO_MODEL_PATH=/home/user/ros2_ws/src/turtlebot/turtlebot3_simulations/turtlebot3_gazebo/models:${GAZEBO_MODEL_PATH}
export TURTLEBOT3_MODEL=waffle
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

You should see a simulation like this. If the robot does not show up, shutdown using Ctrl+C and run the above command again.

TurtleBot3 simulation
TurtleBot3 Simulation

Now make the robot move by publishing to the /cmd_vel topic, in another web shell (2). You should see the robot move.

ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.0}}"

Now start the QoS publisher in the same web shell (2) where you published to /cmd_vel. Note that we set the reliability to reliable. The QoS publisher publishes to the /robot_pose topic.

ros2 run qos_pose_publisher qos_pose_publisher -reliability reliable

Now let’s look at the reliability of the /robot_pose topic in a new web shell (3):

ros2 topic info /robot_pose --verbose

You should see something like this as part of the output:

QoS profile:
  Reliability: RELIABLE
  Durability: VOLATILE
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

Step 3: Record and play back a ros2 bag file of a topic

Now let’s record a ros2 bag file of a topic in web shell (3). We are using the /robot_pose topic.

ros2 bag record -o ros2bag_qos_as_published /robot_pose

Let it run for 5 seconds and then kill it, in web shell (3). You should have a new file created:

ros2bag_qos_as_published

Now let’s play the ros2 bag. But before that kill the qos_pose_publisher node in web shell (2) by pressing Ctrl+C. In the same web shell (2), run the following command:

ros2 bag play ros2bag_qos_as_published

Now let’s examine the QoS of the /robot_pose topic (now being published to from the ros2 bag), in web shell (3):

ros2 topic info /robot_pose --verbose

You should see something like we saw before:

QoS profile:
  Reliability: RELIABLE
  Durability: VOLATILE
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

Step 4: Change the Quality of Service profile for ros2 bag playback

Create a profile file for ros2 bag playback. In web shell (3):

touch override.yaml

Open override.yaml in the code editor and paste in the following content:

/robot_pose:
  history: keep_last
  depth: 10
  reliability: best_effort
  durability: volatile
  deadline:
    # unspecified/infinity
    sec: 0
    nsec: 0
  lifespan:
    # unspecified/infinity
    sec: 0
    nsec: 0
  liveliness: system_default
  liveliness_lease_duration:
    # unspecified/infinity
    sec: 0
    nsec: 0
  avoid_ros_namespace_conventions: false

Now go to web shell (2), stop the ros2 bag playback (if it’s still running), and run the following command instead:

ros2 bag play --qos-profile-overrides-path override.yaml ros2bag_qos_as_published 

Now, let’s see the quality of service of the /robot_pose topic. In web shell (3):

ros2 topic info /robot_pose --verbose

You should now see something similar to the following in the output:

QoS profile:
  Reliability: BEST_EFFORT
  Durability: VOLATILE
  Lifespan: 9223372036854775807 nanoseconds
  Deadline: 9223372036854775807 nanoseconds
  Liveliness: AUTOMATIC
  Liveliness lease duration: 9223372036854775807 nanoseconds

And that’s it! That’s how to replay ros2 bags with changed quality of service setting.

Step 5: Check your learning

  1. Do you understand how to set the QoS for the topic before recording a bag?
  2. Do you understand how to replay ros2 bags with changed quality of service setting?

If you didn’t get any of the points above, please go over the post again, more carefully this time.

(Extra) Step 6: Watch the video to understand how to create ros2 XML launch files

Here you go:

Feedback

Did you like this post? Do you have any questions about how to read and write parameters in ros1 and ros2? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other.

If you want to learn about other ROS topics, please let us know in the comments area and we will do a video or post about it.

How to create ros2 XML launch files

How to create ros2 XML launch files

In this post, you will learn how to create ros2 XML launch files. You’ll discover how ros2 XML launch files are similar to and different from their ros1 counterparts.

Step 1: Get a Copy of the ROS package containing the code used in the post

Click here to copy the project. It would be copied to your cloud account at The Construct. That done, open the project using the Run button. This might take a few moments, please be patient.

Run rosject

PS: If you don’t have an account on the The Construct, you would need to create one. Once you create an account or log in, you will be able to follow the steps to read and write parameters in ros1 and ros2.

You might also want to try this on a local PC if you have ROS installed. In that case you need to read on and duplicate the source code of the package in your own local workspace. However, please note that we cannot support local PCs and you will have to fix any errors you run into on your own.

Step 2: Explore the source code using the IDE

Open Code Editor

Open the IDE by clicking on the icon as shown above. You should now see something similar to the image below:

ros2 XML launch file

The main file we will work with in this post is the one highlighted in red in the image above:

  1. ~/turtlebot3_ws/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_world_action_server.launch.xml

Double-click on the file in the IDE to open and study the contents. We will discuss this file in the following steps.

Step 3: Understand how to create ros2 XML launch files

Wait first! I thought ros2 launch files are only written in Python! Yes, Python is one of the options when writing ros2 launch files; we can also use XML files, especially if we are writing simple launch files that do not need to leverage Python’s powerful API.

Let’s have a look at the XML file:

<launch>
    <arg name="use_sim_time" default="true"/>
    <include file="$(find-pkg-share turtlebot3_gazebo)/launch/turtlebot3_world.launch.py">
        <arg name="use_sim_time" value="$(var use_sim_time)"/>
    </include>
    <node pkg="patrol_action_server" exec="patrol_action_server_exe" name="patrol_action_server">
    </node>
</launch>

What exactly is happening in the launch file?

  • On line 2, we define an argument (variable) use_sim_time with a default value of true.
  • On line 3, we include another launch file, turtlebot3_world.launch.py for launching the TurtleBot3 world. The launch file can be found in the turtlebot3_gazebo package.
  • On line 4, we pass a required argument use_sim_time to the included launch file, assigning it the value of the use_sim_time defined on line 2.
  • On line 6, we define a node to be started by the launch file. This node can be found in the package patrol_action_server, the node executable is patrol_action_server_exe and the name of the node would appear as patrol_action_server.

In short, this launch file launches the TurtleBot3 world and starts the Patrol action server. The same launch file can be written in Python, but this XML looks simpler and easier to understand. And it does all we want it to do, and we can even include Python-based launch files!

Step 4: Understand how ros2 XML launch files are similar to/different from ros1 XML launch files

If you are familiar with ros1 launch files, you should already notice some similarities:

  • The same <launch> tag.
  • Similar <arg> tag.
  • Familiar <include> tag.
  • Similar <node> tag.

Now if we were writing the same launch file in ros1, it would be something like this (PS: we can’t include ros2 launch file in a ros1 launch file in reality):

<launch>
    <arg name="use_sim_time" default="true"/>
    <include file="$(find turtlebot3_gazebo)/launch/turtlebot3_world.launch.py">
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    </include>
    <node pkg="patrol_action_server" type="patrol_action_server_exe" name="patrol_action_server">
    </node>
</launch>

Can you spot the differences? Compare what you find with the list of differences below:

  • In file attribute of the the <include> tag on line 3, ros2 uses find-pkg-share while ros1 uses find.
  • In the value attribute of the <arg> tag on line 4, ros2 uses var to get the value of the argument while ros1 uses arg.
  • In the <node> tag, ros2 uses the exec attribute to specify the executable file to run while ros1 uses the type attribute.

Interesting, isn’t it? Now let’s see if the ros2 XML launch file works.

Step 5: Launch the ros2 XML launch file!

It’s time to see the ros2 XML launch file in action! Open a web shell and run the following commands:

cd ~/turtlebot3_ws
source install/setup.bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world_action_server.launch.xml

You should see the following simulation come up.

TurtleBot3 simulation
TurtleBot3 Simulation

Next, run the following command in the web another web shell

user:~$ ros2 node list

You should see /patrol_action_server (the node specified in the ros2 XML launch file) listed as one of the nodes.

Finally, let’s call the action server and see what happens to the simulation. Run the following in the last web shell used:

ros2 action send_goal --feedback /patrol custom_interfaces/action/Patrol radius:\ 0.2\

And that’s it – you have your ros2 XML launch file working!

Step 6: Check your learning

  1. Do you understand how to create ros2 XML launch files?
  2. Do you remember the similarities and differences in ros1 and ros2 XML launch files?

If you didn’t get any of the points above, please go over the post again, more carefully this time.

(Extra) Step 7: Watch the video to understand how to create ros2 XML launch files

Here you go:

https://www.youtube.com/video/yyUwKcWQ4DU

Feedback

Did you like this post? Do you have any questions about how to read and write parameters in ros1 and ros2? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other.

If you want to learn about other ROS topics, please let us know in the comments area and we will do a video or post about it.

How to read and write parameters in ros1 and ros2

How to read and write parameters in ros1 and ros2

In this post, you will learn how to read and write parameters in ros1 and ros2, using C++ nodes. You will see the slight differences in the ros1 and ros2 nodes and parameter files.

Step 1: Get a Copy of the ROS package containing the code used in the post

Click here to copy the project. It would be copied to your cloud account at The Construct. That done, open the project using the Run button. This might take a few moments, please be patient.

Run rosject

PS: If you don’t have an account on the The Construct, you would need to create one. Once you create an account or log in, you will be able to follow the steps to read and write parameters in ros1 and ros2.

You might also want to try this on a local PC if you have ROS installed. In that case you need to read on and duplicate the source code of the package in your own local workspace. However, please note that we cannot support local PCs and you will have to fix any errors you run into on your own.

Step 2: Explore the source code using the IDE

Open Code Editor

Open the IDE by clicking on the icon as shown above. You should now see something similar to the image below:

Yaml Params

The six main files we will work with in this post are highlighted in red in the image above. These files are:

ROS1:

  1. catkin_ws/src/yaml_parameters_ros1/config/params_demo_ros1.yaml
  2. catkin_ws/src/yaml_parameters_ros1/launch/ros1_params_cpp_demo.launch
  3. catkin_ws/src/yaml_parameters_ros1/src/yaml_params_ros1.cpp

ROS2:

  1. ros2_ws/src/yaml_parameters/config/params_demo_ros2.yaml
  2. ros2_ws/src/yaml_parameters/launch/yaml_parameters.launch.py
  3. ros2_ws/src/yaml_parameters/src/yaml_params_ros2.cpp

Double-click on each of the files in the IDE to open and study the contents. We will examine some of these files in the following steps.

Step 3: Understand how to read and write (load) parameters in ROS1

Now it’s time to see how to read and write parameters in ros1, working in the ros1 workspace.

Open a web shell and run the following commands:

Open webshell
cd ~/catkin_ws
source /opt/ros/noetic/setup.bash
source devel/setup.bash
roscore

The code block above changes to the ros1 workspace, sources it, and then starts the roscore (needed for ros1). Now let’s see a list of the current ros1 parameters available. Open another web shell and type the following:

user:~/catkin_ws$ rosparam list

Your output should be similar to the following.

/rosdistro
/roslaunch/uris/host_1_xterm__41731
/rosversion
/run_id

On the same web shell, run the following command to print out the ros parameters:

rosrun yaml_parameters_ros1 yaml_parameters_ros1_node

Your output should be the following:

[ INFO] [1657670307.494093666]: Integer parameter: 1
[ INFO] [1657670307.495689754]: Double parameter: 0.100000
[ INFO] [1657670307.495741256]: String parameter: default
[ INFO] [1657670307.495773876]: Nested integer parameter: 1
[ INFO] [1657670307.495797588]: Nested string parameter: default
[ INFO] [1657670307.495819891]: Boolean parameter: 0

The logic that produced the output above in contained in the catkin_ws/src/yaml_parameters_ros1/src/yaml_params_ros1.cpp file. Let’s see its content.

#include "ros/ros.h"
#include <string>

int main(int argc, char **argv) {
  ros::init(argc, argv, "my_node");

  ros::NodeHandle nh;

  int param0 = 1;
  double param1 = 0.1;
  std::string param2 = "default";
  int p3weight = 1;
  std::string p3name = "default";
  bool param4 = false;
  std::vector<bool> param5{false, false, false};
  std::vector<int> param6{1, 1, 1};
  std::vector<double> param7{0.1, 0.1, 0.1};
  std::vector<std::string> param8{"default", "default", "default"};

  nh.getParam("param0", param0);
  nh.getParam("param1", param1);
  nh.getParam("param2", param2);
  nh.getParam("param3/weight", p3weight);
  nh.getParam("param3/name", p3name);
  nh.getParam("param4", param4);
  nh.getParam("param5", param5);
  nh.getParam("param6", param6);
  nh.getParam("param7", param7);
  nh.getParam("param8", param8);

  ROS_INFO("Integer parameter: %d", param0);
  ROS_INFO("Double parameter: %f", param1);
  ROS_INFO("String parameter: %s", param2.c_str());
  ROS_INFO("Nested integer parameter: %d", p3weight);
  ROS_INFO("Nested string parameter: %s", p3name.c_str());
  ROS_INFO("Boolean parameter: %d", param4);
  ROS_INFO("Boolean vector parameter [0]: %d", static_cast<int>(param5[0]));
  ROS_INFO("Integer vector parameter [0]: %d", static_cast<int>(param6[0]));
  ROS_INFO("Double vector parameter [0]: %f", static_cast<double>(param7[0]));
  ROS_INFO("String vector parameter [0]: %s", param8[0].c_str());

  return 0;
}

But wait…are we getting the parameters in the YAML file (catkin_ws/src/yaml_parameters_ros1/config/params_demo_ros1.yaml) and their correct values? Let’s see what’s the in there!

# interger array
param0: 2
# double
param1: 0.2
# string
param2: "R2-D2"
# nested parameters
param3: 
  weight: 2
  name: "wood"
# boolean
param4: true
# boolean array
param5: [true, true, true]
# interger array
param6: [5,6,7,8]
# double array
param7: [0.2, 0.3, 0.4, 0.5]
# string array
param8: ["Bedroom", "Bathroom", "Laundry room", "Kitchen", "Living room"]

Gosh, we are not getting these parameters nor their values, and you probably know why! So far we have been reading the parameters but have loaded them. Now let’s get that done: enter the launch file catkin_ws/src/yaml_parameters_ros1/launch/ros1_params_cpp_demo.launch.

<launch>
    <rosparam file="$(find yaml_parameters_ros1)/config/params_demo_ros1.yaml" />
</launch>

This file, when launched, loads the YAML parameter file. Let’s see that in action. Run the following command in the open web shell:

roslaunch yaml_parameters_ros1 ros1_params_cpp_demo.launch

You should get something like the following:

... logging to /home/user/.ros/log/c5f51462-023c-11ed-bb5c-0242ac180007/roslaunch-1_xterm-13868.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://1_xterm:45437/

SUMMARY
========

PARAMETERS
 * /param0: 2
 * /param1: 0.2
 * /param2: R2-D2
 * /param3/name: wood
 * /param3/weight: 2
 * /param4: True
 * /param5: [True, True, True]
 * /param6: [5, 6, 7, 8]
 * /param7: [0.2, 0.3, 0.4, 0.5]
 * /param8: ['Bedroom', 'Bath...
 * /rosdistro: noetic
 * /rosversion: 1.15.11

NODES

ROS_MASTER_URI=http://1_xterm:11311

No processes to monitor
shutting down processing monitor...
... shutting down processing monitor complete

Well we have some fancy output there, but what has changed? Let’s see that by running two previous commands:

rosparam list
rosrun yaml_parameters_ros1 yaml_parameters_ros1_node

Your output should now look like this:

user:~/catkin_ws$ rosparam list
/param0
/param1
/param2
/param3/name
/param3/weight
/param4
/param5
/param6
/param7
/param8
/rosdistro
/roslaunch/uris/host_1_xterm__41731
/roslaunch/uris/host_1_xterm__45437
/rosversion
/run_id
user:~/catkin_ws$ rosrun yaml_parameters_ros1 yaml_parameters_ros1_node
[ INFO] [1657671513.558912623]: Integer parameter: 2
[ INFO] [1657671513.560352415]: Double parameter: 0.200000
[ INFO] [1657671513.560386238]: String parameter: R2-D2
[ INFO] [1657671513.560404606]: Nested integer parameter: 2
[ INFO] [1657671513.560420530]: Nested string parameter: wood
[ INFO] [1657671513.560435749]: Boolean parameter: 1
[ INFO] [1657671513.560450679]: Boolean vector parameter [0]: 1
[ INFO] [1657671513.560465819]: Integer vector parameter [0]: 5
[ INFO] [1657671513.560484622]: Double vector parameter [0]: 0.200000
[ INFO] [1657671513.560497372]: String vector parameter [0]: Bedroom

Can you spot the differences between the formal and the latter outputs of these commands? Sure you can! So, well, that’s how to read and load parameter in ros1!

Step 4: Understand how to read and write (load) parameters in ROS2

Now let’s change to the ros2 workspace.

cd ~/ros2_ws
source /opt/ros/foxy/setup.bash
source install/setup.bash

In ros2 we need to have a node running before we can check for parameters, because there is no parameter server in ros2. Let’s try running the node then. The logic behind this node is contained in the ros2_ws/src/yaml_parameters/src/yaml_params_ros2.cpp file:

#include <rclcpp/rclcpp.hpp>

class MainNode : public rclcpp::Node {
public:
  MainNode() : rclcpp::Node("node", rclcpp::NodeOptions()) {

    // example: declare parameters, default value given
    declare_parameter("param0", 1);
    declare_parameter("param1", 0.1);
    declare_parameter("param2", "default");
    declare_parameter("param3.weight", 1);
    declare_parameter("param3.name", "default");
    declare_parameter("param4", false);
    // example: declare a variable when declaring a parameter
    declare_parameter("param5", std::vector<bool>(3, false));
    declare_parameter("param6", std::vector<int64_t>(4, 1));
    declare_parameter("param7", std::vector<double>(4, 0.1));
    declare_parameter("param8", std::vector<std::string>(5, "default"));

    // Get parameter values one by one
    auto p0 = get_parameter("param0").as_int();
    auto p1 = get_parameter("param1").as_double();
    auto p2 = get_parameter("param2").as_string();
    auto p3weight = get_parameter("param3.weight").as_int();
    auto p3name = get_parameter("param3.name").as_string();
    auto p4 = get_parameter("param4").as_bool();
    auto p5 = get_parameter("param5").as_bool_array();
    auto p6 = get_parameter("param6").as_integer_array();
    auto p7 = get_parameter("param7").as_double_array();
    auto p8 = get_parameter("param8").as_string_array();

    // Print parameters
    RCLCPP_INFO(get_logger(), "Integer parameter: %ld", p0);
    RCLCPP_INFO(get_logger(), "Double parameter: %f", p1);
    RCLCPP_INFO(get_logger(), "String parameter: %s", p2.c_str());
    RCLCPP_INFO(get_logger(), "Nested integer parameter: %ld", p3weight);
    RCLCPP_INFO(get_logger(), "Nested string parameter: %s", p3name.c_str());
    RCLCPP_INFO(get_logger(), "Boolean parameter: %d", p4);
    RCLCPP_INFO(get_logger(), "Boolean vector parameter [0]: %d",
                static_cast<int>(p5[0]));
    RCLCPP_INFO(get_logger(), "Integer vector parameter [0]: %d",
                static_cast<int>(p6[0]));
    RCLCPP_INFO(get_logger(), "Double vector parameter [0]: %f",
                static_cast<double>(p7[0]));
    RCLCPP_INFO(get_logger(), "String vector parameter [0]: %s", p8[0].c_str());
  }
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);

  rclcpp::spin(std::make_shared<MainNode>());
  rclcpp::shutdown();
  return 0;
}

Go for it: run the node:

ros2 run yaml_parameters main_node

The output will be something like:

INFO] [1657672243.344585940] [node]: Integer parameter: 1
[INFO] [1657672243.344674910] [node]: Double parameter: 0.100000
[INFO] [1657672243.344704157] [node]: String parameter: default
[INFO] [1657672243.344720646] [node]: Nested integer parameter: 1
[INFO] [1657672243.344730700] [node]: Nested string parameter: default
[INFO] [1657672243.344745410] [node]: Boolean parameter: 0
[INFO] [1657672243.344755485] [node]: Boolean vector parameter [0]: 0
[INFO] [1657672243.344769947] [node]: Integer vector parameter [0]: 1
[INFO] [1657672243.344780509] [node]: Double vector parameter [0]: 0.100000
[INFO] [1657672243.344795534] [node]: String vector parameter [0]: default

Next, let’s get the list of ROS2 parameters:

user:~$ ros2 param list
/node:
  param0
  param1
  param2
  param3.name
  param3.weight
  param4
  param5
  param6
  param7
  param8
  use_sim_time

Buff…are we getting the values of the parameters in the YAML file ros2_ws/src/yaml_parameters/config/params_demo_ros2.yaml?

# if a namespace is specified
# ns_name: 
# node name
parameter_types_example: 
  ros__parameters:
    # int
    param0: 2
    # double
    param1: 0.2
    # string
    param2: "R2-D2"
    # nested parameters
    param3: 
      weight: 2
      name: "wood"
    # boolean
    param4: true
    # boolean array
    param5: [true, true, true]
    # interger array
    param6: [5,6,7,8]
    # double array
    param7: [0.2, 0.3, 0.4, 0.5]
    # string array
    param8: ["Bedroom", "Bathroom", "Laundry room", "Kitchen", "Living room"]

No, we are not :(. But not to worry, the launch file ros2_ws/src/yaml_parameters/launch/yaml_parameters.launch.py comes to the rescue! Let’s examine its content.

#!/usr/bin/env python3

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='yaml_parameters',
            executable='main_node',
            name='parameter_types_example',
            parameters=[os.path.join(
                get_package_share_directory('yaml_parameters'),
                'config', 'params_demo_ros2.yaml')],
            output='screen'),
    ])

Oh my, it’s a Python! Let’s set it loose and see what happens! Stop the currently running program with Ctrl + C and run the following in its place and check that your output is similar.

user:~/ros2_ws$ ros2 launch yaml_parameters yaml_parameters.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2022-07-13-00-40-35-558545-1_xterm-18432
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [main_node-1]: process started with pid [18434]
[main_node-1] [INFO] [1657672835.736702673] [parameter_types_example]: Integer parameter: 2
[main_node-1] [INFO] [1657672835.736792219] [parameter_types_example]: Double parameter: 0.200000
[main_node-1] [INFO] [1657672835.736810397] [parameter_types_example]: String parameter: R2-D2
[main_node-1] [INFO] [1657672835.736842588] [parameter_types_example]: Nested integer parameter: 2
[main_node-1] [INFO] [1657672835.736847188] [parameter_types_example]: Nested string parameter: wood
[main_node-1] [INFO] [1657672835.736855303] [parameter_types_example]: Boolean parameter: 1
[main_node-1] [INFO] [1657672835.736863129] [parameter_types_example]: Boolean vector parameter [0]: 1
[main_node-1] [INFO] [1657672835.736870819] [parameter_types_example]: Integer vector parameter [0]: 5
[main_node-1] [INFO] [1657672835.736878422] [parameter_types_example]: Double vector parameter [0]: 0.200000
[main_node-1] [INFO] [1657672835.736887246] [parameter_types_example]: String vector parameter [0]: Bedroom

The launch file simply loads the parameters in the YAML file and also runs the node we run earlier.

Well, that’s it!

Step 5: Check your learning

  1. Do you understand how to read and write parameters in ros1 and ros2, using C++ nodes?
  2. Did you notice the slight differences in the format of the YAML files for ros1 and ros2?
  3. Did you notice that you the ros2 parameters are tied to specific nodes vs existing in a parameter server in ros1?

If you didn’t get any of the points above, please go over the post again, more carefully this time.

Extra Step: Watch the video to understand how to read and write parameters in ros1 and ros2

Here you go:

Feedback

Did you like this post? Do you have any questions about how to read and write parameters in ros1 and ros2? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other.

If you want to learn about other ROS topics, please let us know in the comments area and we will do a video or post about it.

[ROS2 How-to] #2 – Create a ROS2 action server

[ROS2 How-to] #2 – Create a ROS2 action server

What we are going to learn

  1. – How to create a custom action message
  2. How to create an action server

List of resources used in this post

  1. Use the rosject: https://app.theconstructsim.com/#/l/4a1c58c5/
  2. The Construct: https://app.theconstructsim.com/
  3. ROS2 Courses –▸
    1. ROS2 Basics in 5 Days (Python): https://app.theconstructsim.com/#/Course/73
    2. ROS2 Basics in 5 Days (C++): https://app.theconstructsim.com/#/Course/61
    3. ROS2 Navigation training: https://www.theconstruct.ai/ros2-navigation-training/

What is a ROS2 Action

Let’s assume you wish to wash your clothing. There are two possible ways you could go about it:

  1. Go to the Laundry service provider
    1. Put your clothes to wash.
    2. Wait until the clothes are washed.
    3. Get your clothes.
  2. If you have a washing machine at home:
    1. Put your clothes to wash
    2. Instead of waiting, you can do other things and leave the watching machine doing its jobs
    3. Check once in a while if the clothes are finished
    4. Do other things.
    5. Clothes are washed.

Option 1 is a blocking activity because you have to wait (in theory not able to do anything else) for the clothes to be washed, while option 2 is non-blocking because you can do some other things while your clothes are being washed.

This non-blocking is what defines an Action. If ROS2 Services are for instant request-responses, an Action is a task that may take a lot of time to be finished, and in the meantime, a robot (or you) is free to do other things and is also able to constantly check the status of the action.

Opening the rosject

In order to learn how to create an and use an Action Server in ROS2, we need to have ROS2 installed in our system, and it is also useful to have some simulations. To make your life easier, we already prepared a rosject with a simulation for that: https://app.theconstructsim.com/#/l/4a1c58c5/.

You can download the rosject on your own computer if you want to work locally, but just by copying the rosject (clicking the link), you will have a setup already prepared for you.

After the rosject has been successfully copied to your own area, you should see a Run button. Just click that button to launch the rosject (below you have a rosject example).

Learn ROS2 Parameters - Run rosject

Learn ROS2 – Run rosject (example of the RUN button)

After pressing the Run button, you should have the rosject loaded. Let’s now head to the next section to really get some real practice.

Launching the simulation

The rosject we provided contains the packages needed to run a TurtleBot3 simulation in ROS2. The TurtleBot3 has the following sensors:

  • Lidar
  • IMU

Feel free to use this rosject to test your mobile robot programs.

The rosject is structured the following way:

  • turtlebot3_ws: this workspace contains the TurtleBot3 packages provided by ROBOTIS. Don’t modify this unless you know what you are doing and want to change something from the simulation
  • Use this workspace to develop your programs

Assuming you have opened the rosject by clicking the Run button, we can launch the simulation with:

cd
source turtlebot3_ws/install/setup.bash
source turtlebot3_ws/install/setup.bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
After a few seconds, the simulation should have opened automatically:

[ROS2 How-to] #2 - Create a ROS2 action server - Simulation running

[ROS2 How-to] #2 – Create a ROS2 action server – Simulation running

 

In case the simulation does not pop up automatically, you can easily click the Open Gazebo button like in the example below (bear in mind that the simulation below is not the one used in this tutorial. Its main purpose is to show the Open Gazebo button):

Open Gazebo by clicking Open Gazebo

Open Gazebo by clicking Open Gazebo

 

Creating our ROS2 package (later used to create our Action Server)

Let’s create our ROS2 Package. For that, let’s start by opening a new terminal:

Open a new Terminal

Open a new Terminal

 

In the terminal that was just open, by running the “ls”  command you can see that we have at least the following folders:

ros2_ws  turtlebot3_ws

 

The turtlebot3_ws contains the simulation, and the ros2_ws is where we are going to place our code.

Before you continue, it is worth mentioning that in the rosject that we shared with you, the custom_interfaces package that we are going to create here already exists. We are going to create it here basically for learning purposes. You would actually not need it since the package was already created for you:

Let’s create a package named custom_interfaces with the action_msgs std_msgs rosids_default_generators  packages as dependencies:

cd ~/ros2_ws/src/

ros2 pkg create custom_interfaces2 --build-type ament_cmake --dependencies action_msgs std_msgs rosidl_default_generators

If everything went ok, you should see the following:

going to create a new package
package name: custom_interfaces
destination directory: /home/user/ros2_ws/src
package format: 3
version: 0.0.0
description: TODO: Package description
maintainer: ['user <user@todo.todo>']
licenses: ['TODO: License declaration']
build type: ament_cmake
dependencies: ['action_msgs', 'std_msgs', 'rosidl_default_generators']
creating folder ./custom_interfaces
creating ./custom_interfaces/package.xml
creating source and include folder
creating folder ./custom_interfaces/src
creating folder ./custom_interfaces/include/custom_interfaces2
creating ./custom_interfaces/CMakeLists.txt

 

After the package was created, let’s create a folder called action:

mkdir -p custom_interfaces/action/

and also create the action/Patrol.action file.

touch custom_interfaces/action/Patrol.action

This is the file/Interface that we will use in our Action Server for patrolling.

Let’s now open that Patrol.action file. You can open it in the Code Editor. If you do not know how to open the Code Editor, please check the image below:

Open the IDE - Code Editor

Open the IDE – Code Editor

You can now open the custom_interfaces/action/Patrol.action file and paste the following content on it:

#Goal
float32 radius
---
#Result
bool success
---
#Feedback
float32 time_left

 

Now, to be able to compile our message file, we have to open the custom_interfaces/CMakeLists.txt file and paste the following content around line 14:

set(action_files
    "action/Patrol.action"
)

rosidl_generate_interfaces(${PROJECT_NAME}
    ${action_files}
    DEPENDENCIES action_msgs std_msgs
)
In the end, the final CMakeLiss.txt file would look like the following:
cmake_minimum_required(VERSION 3.8)
project(custom_interfaces)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(action_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)

set(action_files
  "action/Patrol.action"
)

rosidl_generate_interfaces(${PROJECT_NAME}
  ${action_files}
  DEPENDENCIES action_msgs std_msgs
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

 

And for the file custom_interfaces/package.xml we also have to add the following code before the <export> tag:

<depend>builtin_interfaces</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

In the end, our package.xml would look like the following:

<?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>custom_interfaces</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="user@todo.todo">user</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>action_msgs</depend>
  <depend>std_msgs</depend>
  <depend>rosidl_default_generators</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <depend>builtin_interfaces</depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

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

Please make sure you save the files with Ctrl+S after making the modifications.

Compiling our custom Action interface

Now that we defined our Custom Action interface, let’s compile it.

Let’s go to the first terminal we opened previously and run the following commands:

cd ~/ros2_ws/

colcon build

The package should have been compiled with no errors:

Starting >>> custom_interfaces
Finished <<< custom_interfaces [9.53s]

Summary: 1 package finished [9.53s]

 

Let’s now make sure ROS2 can find our Action interface:

source install/setup.bash

ros2 interface show custom_interfaces/action/Patrol

It should show:

#Goal
float32 radius
---
#Result
bool success
---
#Feedback
float32 time_left

 

So far so good. ROS is able to find our custom interface.

The time has now come for us to create the Action Server.

Creating our ROS2 Action Server

Let’s create a different package for the Action Server, just to keep things separated. Since we are not doing to create Interfaces in this new package, just use existing interfaces, let’s use the ament_python build type. Again, bear in mind that if you are using the rosject that we provided, the package already exists in the ~/ros2_ws/src folder:

cd ~/ros2_ws/src/
ros2 pkg create --build-type ament_python patrol_action_server --dependencies rclpy geometry_mgs custom_interfaces

The logs should be similar to the following:

going to create a new package
package name: patrol_action_server
destination directory: /home/user/ros2_ws/src
package format: 3
version: 0.0.0
description: TODO: Package description
maintainer: ['user <user@todo.todo>']
licenses: ['TODO: License declaration']
build type: ament_python
dependencies: ['rclpy', 'geometry_mgs', 'custom_interfaces']
creating folder ./patrol_action_server
creating ./patrol_action_server/package.xml
creating source folder
creating folder ./patrol_action_server/patrol_action_server2
creating ./patrol_action_server/setup.py
creating ./patrol_action_server/setup.cfg
creating folder ./patrol_action_server/resource
creating ./patrol_action_server/resource/patrol_action_server
creating ./patrol_action_server/patrol_action_server/__init__.py
creating folder ./patrol_action_server/test
creating ./patrol_action_server/test/test_copyright.py
creating ./patrol_action_server/test/test_flake8.py
creating ./patrol_action_server/test/test_pep257.py

Now that our package is created, let’s create a file patrol_action_server.py that will have the code of our Action Server:

touch patrol_action_server/patrol_action_server/patrol_action_server.py

Let’s now open that file using the Code Editor, and paste the following content to it:

#!/usr/bin/env python3
#
# Copyright 2019 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Authors: Ryan Shim, Gilbert

import math
import time

import rclpy

from geometry_msgs.msg import Twist

from rclpy.action import ActionServer
from rclpy.action import CancelResponse
from rclpy.action import GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.duration import Duration
from rclpy.node import Node
from rclpy.qos import QoSProfile

from rclpy.executors import MultiThreadedExecutor

from custom_interfaces.action import Patrol


class Turtlebot3PatrolServer(Node):

    def __init__(self):
        super().__init__('turtlebot3_patrol_server')


        self.goal = Patrol.Goal()


        qos = QoSProfile(depth=10)

        # Initialise publishers
        self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', qos)

        # Initialise servers
        self._action_server = ActionServer(
            self,
            Patrol,
            'patrol',
            execute_callback=self.execute_callback,
            callback_group=ReentrantCallbackGroup(),
            goal_callback=self.goal_callback,
            cancel_callback=self.cancel_callback)

        self.get_logger().info("Turtlebot3 patrol action server has been initialised.")


    def destroy(self):
        self._action_server.destroy()
        super().destroy_node()

    def goal_callback(self, goal_request):
        # Accepts or rejects a client request to begin an action
        self.get_logger().info('Received goal request :)')
        self.goal = goal_request
        return GoalResponse.ACCEPT

    def cancel_callback(self, goal_handle):
        # Accepts or rejects a client request to cancel an action
        self.get_logger().info('Received cancel request :(')
        return CancelResponse.ACCEPT

    async def execute_callback(self, goal_handle):
        self.get_logger().info('Executing goal...')

        radius = self.goal.radius  # unit: m
        speed = 0.5  # unit: m/s

        feedback_msg = Patrol.Feedback()
        total_driving_time = 2 * math.pi * radius / speed
        feedback_msg.time_left = total_driving_time
        last_time = self.get_clock().now()

        # Start executing an action
        while (feedback_msg.time_left > 0):
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                self.get_logger().info('Goal canceled')
                return Patrol.Result()

            curr_time = self.get_clock().now()
            duration = Duration()
            duration = (curr_time - last_time).nanoseconds / 1e9  # unit: s

            feedback_msg.time_left = total_driving_time - duration
            self.get_logger().info('Time left until the robot stops: {0}'.format(feedback_msg.time_left))
            goal_handle.publish_feedback(feedback_msg)

            # Give vel_cmd to Turtlebot3
            twist = Twist()
            twist = self.drive_circle(radius, speed)
            self.cmd_vel_pub.publish(twist)

            # Process rate
            time.sleep(0.010)  # unit: s

        # When the action is completed
        twist = Twist()
        self.cmd_vel_pub.publish(twist)

        goal_handle.succeed()
        result = Patrol.Result()
        result.success = True
        self.get_logger().info('Returning result: {0}'.format(result.success))

        return result

    def drive_circle(self, radius, velocity):
        self.twist = Twist()
        self.linear_velocity = velocity  # unit: m/s
        self.angular_velocity = self.linear_velocity / radius  # unit: rad/s

        self.twist.linear.x = self.linear_velocity
        self.twist.angular.z = self.angular_velocity

        return self.twist

def main(args=None):
    rclpy.init(args=args)

    patrol_action_server = Turtlebot3PatrolServer()

    # Use a MultiThreadedExecutor to enable processing goals concurrently
    executor = MultiThreadedExecutor()

    rclpy.spin(patrol_action_server, executor=executor)

    patrol_action_server.destroy()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

The code used above is just an adaptation of a code already provided by ROBOTIS.
Before we compile our code, we also have to open the patrol_action_server/setup.py file and modify the entry_points section to define our executable called patrol_action_server_exe in the following way:
entry_points={
        'console_scripts': [
            'patrol_action_server_exe = patrol_action_server.patrol_action_server:main',
        ],
    },

 

In the end, the complete ~/ros2_ws/src/patrol_action_server/setup.py would be as follows:

from setuptools import setup

package_name = 'patrol_action_server'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='user',
    maintainer_email='user@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'patrol_action_server_exe = patrol_action_server.patrol_action_server:main',
        ],
    },
)

 

Compiling our ROS2 Action Server

With everything in place, we compile our package just as before:

cd ~/ros2_ws/

colcon build
Starting >>> custom_interfaces
Finished <<< custom_interfaces [9.53s]
Starting >>> patrol_action_server
Finished <<< patrol_action_server [5.33s]

Summary: 2 packages finished [15.2s]
We can now run our server with the following commands:
source install/setup.bash

ros2 run patrol_action_server patrol_action_server_exe
The server should start with no problems:
[INFO] [1651528559.914166370] [turtlebot3_patrol_server]: Turtlebot3 patrol action server has been initialised

Calling our ROS2 Action Server

Ok, if you did not kill the Action Server launched in the previous section, please open a second terminal that we will use to call the Action Server.

With “ros2 node list” we should be able to find our node running:

ros2 node list

/turtlebot3_patrol_server
And with ros2 action list, we should be able to see the /patrol action:
ros2 action list

/patrol
We can now call our Action Server. If you remember when we created the Patrol.action, we defined a radius. Let’s them call the Action Server passing a radius of 0.5. The robot will be basically rotating:
ros2 action send_goal --feedback /patrol custom_interfaces/action/Patrol radius:\ 0.5\
You should now see the feedback sent by the action server:
Waiting for an action server to become available...
Sending goal:
     radius: 0.5

Goal accepted with ID: dd32bc835d7a4ef5ae854d0bfb4b119f

Feedback:
    time_left: 6.2831525802612305

Feedback:
    time_left: 6.271763801574707

Feedback:
    time_left: 6.260392665863037

Feedback:
    time_left: 6.2484917640686035

Feedback:
    time_left: 6.237414836883545

Feedback:
    time_left: 6.2265496253967285

Feedback:
    time_left: 6.215761661529541

...
^CCanceling goal...
Feedback:
time_left: 5.634908676147461
Goal canceled.

Remember that you can easily cancel the call to the action server by pressing CTRL+C.

If you look at the simulation after sending a goal to the Action Server, you should see the robot spinning around 0.5 meters.

Congratulations. You now know how to create a ROS2 Action Server from scratch. If you want more details about the code of the Action Server, please check the video in the next section.

Youtube video

So this is the post for today. Remember that we have the live version of this post on YouTube. If you liked the content, please consider subscribing to our youtube channel. We are publishing new content ~every day.

Keep pushing your ROS Learning.

Related Courses & Training

If you want to learn more about ROS and ROS2, we recommend the following courses:

Program Drones using ROS2 – Episode 1

Program Drones using ROS2 – Episode 1

In this post, we will see how to program drones using ROS2. Perhaps you have programmed drones using ROS1, found that programming drones using ROS2 is not as straightforward, and have been wondering, “how the heck do you program drones using ROS2?” You have come to the right place!

Step 1: Grab the required source code

You can program drones using ROS2 on your local PC by following the instructions in this repository step by step. However if you use the rosject you copied below, we have already done most of the heavy lifting for you and you just need to run a few commands to get your drone flying!

Click here to get your own copy of the project (PS: If you don’t have an account on the ROS Development Studio, you would need to create one. Once you create an account or log in, we will copy the project to your workspace).

Run rosject

That done, open the project using the Run button. This might take a few moments, please be patient.

Step 2: Understand the basic components

The ros2 drone system used in this post consists of three main parts:

  1. A ros2 simulation containing the simulation and the plugin that connects to the rest of the system.
  2. The PX4-Autopilot system.
  3. A Ground Control system for controlling the drone.

You’ll have to start these systems in order, as we’ll see in the next step.

PS: If you are doing the setup from the LS2N-Drone repository directly and running on your local PC, please adapt the next step for your local setup.

Step 3: Get the ROS2 drone flying!

Open a web shell and run the following commands:

Open webshell

sudo rm -rf /etc/apt/sources.list.d/husarnet.list && sudo apt update
sudo apt-get remove modemmanager -y
sudo apt install gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl -y
pip3 install pyros-genmsg transforms3d guizero scipy qtm pymavlink mttkinter jinja2 inputs toml pyqtgraph

Next, start the ros2 simulation:

cd ~/ros2_ws
source install/setup.bash
ros2 launch ls2n_drone_simulation single_drone_trajectory_sitl.launch.py

By this time, you should have the simulation running. Open the Gazebo app (if not opened automatically) to see it. Right click on the drone model on the left pane and select “Follow” to keep seeing the drone when it takes off.

Open Gazebo

LS2N-Drone Simulation: program drones using ROS2

Now, open another web shell and start the PX-4 system:

cd ~
cd px4-autopilot/Tools/
./gazebo_sitl_multiple_run_only_px4.sh

Finally, start the ground control system in another web shell:

user:~$ cd squashfs-root/
user:~/squashfs-root$ ./AppRun

The ground control GUI should load shortly. Open the Graphical Tools app to see it, if not opened automatically:

Open graphical tools

ros2 drone ground control

Time to take off the drone!

  1. Ensure the top-left corner of the controller says “Ready to Fly” in a green background.
  2. Click the Takeoff button on the top left.
  3. In the bottom middle, drag the slider to the right to confirm takeoff. You should see something similar to the image below.
  4. If the takeoff does not work, repeat 2 & 3.
  5. After takeoff, the button changes to “Land”. Use it to land the drone.

ros2 drone flying

Step 4: Consolidate your learning

Do you understand how to program drones using ROS2 after watching the video? If not, please review the material again and perhaps go over the video again. Let us know any problems you are seeing in the comments.

Extra Step: Watch the video for the sights and sounds version of how to program drones using ROS2

Here you go:

Related Resources

Feedback

Did you like this post? Do you have any questions about the explanations? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other. If you want to learn about other ROS2 topics, please let us know in the comments area and we will do a video or post about it.

Pin It on Pinterest