[ROS2 Q&A] 218 – How to Create Conditional Publisher in ROS2

What we are going to learn

1. How to create a simple publisher and subscriber
2. How to work with incoming messages
3. How to add condition to topic publication

List of resources used in this post

1. ROS Development Studio (ROSDS) —▸ http://rosds.online
2. Robot Ignite Academy –▸ https://www.robotigniteacademy.com
3. ROS2 Cookbook –▸ https://github.com/mikeferguson/ros2_cookbook/blob/main/pages/launch.md
4. Question asked on ROS Answers –▸ https://answers.ros.org/question/373178/conditional-statement-for-publisher-and-subscriber/

Creating a rosject

In order to learn how to create conditional publishers, let’s start by creating a publisher and a subscriber in Python. We are going to use The Construct (https://www.theconstruct.ai/) for this tutorial, but if you have ROS2 installed on your own computer, you should be able to do ~everything on your own computer, except this creating a rosject part.

Let’s start by opening The Construct (https://www.theconstruct.ai/) and logging in. You can easily create a free account if you still don’t have one.

Once inside, let’s create My Rosjects and then, Create a new rosject:

My Rosjects

Create a new rosject

For the rosject, let’s select ROS2 Foxy for the ROS Distro, let’s name the rosject as Battery Level Node. You can leave the rosject public.

Battery Level Node – Conditional Publisher in ROS2

If you mouse over the recently created rosject, you should see a Run button. Just click that button to launch the rosject.

Writing a simple publisher and subscriber (Python)

Once the rosject is open, we can now create our publisher and subscriber. For that, we are going to use ROS Docs as a reference.

Let’s open a new terminal by clicking on the Open a new shell window button:

Open a new shell

Once the terminal is open, we can list the files with the ls command:

user:~$ ls
ai_ws  catkin_ws  notebook_ws  ros2_ws  simulation_ws  webpage_ws

We can see a workspace named ros2_ws. Let’s enter that workspace using cd ros2_ws/:

user:~$ cd ros2_ws/
user:~/ros2_ws$

Let’s now source our workspace with:

source ~/ros2_ws/install/setup.bash

Let’s now enter our src folder:

 cd ~/ros2_ws/src/

And create a package named py_pubsub (python publisher and subscriber):

ros2 pkg create --build-type ament_python py_pubsub

The output should be similar to:

user:~/ros2_ws/src$ ros2 pkg create --build-type ament_python py_pubsub
going to create a new package
package name: py_pubsub
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: []
creating folder ./py_pubsub
creating ./py_pubsub/package.xml
creating source folder
creating folder ./py_pubsub/py_pubsub
creating ./py_pubsub/setup.py
creating ./py_pubsub/setup.cfg
creating folder ./py_pubsub/resource
creating ./py_pubsub/resource/py_pubsub
creating ./py_pubsub/py_pubsub/__init__.py
creating folder ./py_pubsub/test
creating ./py_pubsub/test/test_copyright.py
creating ./py_pubsub/test/test_flake8.py
creating ./py_pubsub/test/test_pep257.py

Be aware that in order to create this package, we basically used ROS Docs for reference.

If you now list your src folder using ls, you should be able to see our package:

user:~/ros2_ws/src$ ls
py_pubsub

We can now enter into this py_pubsub package using cd py_pubsub/. We will find a new folder named py_pubsub inside it. Let’s just enter into it. The full command would be as easy as cd ~/ros2_ws/src/py_pubsub/py_pubsub/ . But if you want to go step by step:

user:~/ros2_ws/src$ cd py_pubsub/
user:~/ros2_ws/src/py_pubsub$ ls
package.xml  py_pubsub  resource  setup.cfg  setup.py  test
user:~/ros2_ws/src/py_pubsub$ cd py_pubsub/
user:~/ros2_ws/src/py_pubsub/py_pubsub$ ls
__init__.py
user:~/ros2_ws/src/py_pubsub/py_pubsub$

Now inside that last py_pubsub folder, let’ download a publisher and a subscriber named publisher_member_function.py and subscriber_member_function.py respectively. Bear in mind that up to this point we are basically following the docs aforementioned:

cd ~/ros2_ws/src/py_pubsub/py_pubsub

wget https://raw.githubusercontent.com/ros2/examples/foxy/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_member_function.py

wget https://raw.githubusercontent.com/ros2/examples/foxy/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_member_function.py

We should now have three files inside our py_pubsub folder/package:

cd ~/ros2_ws/src/py_pubsub/py_pubsub; ls

__init__.py publisher_member_function.py subscriber_member_function.py

Let’s now open the publisher_member_function.py using the Code Editor:

ROS2 Publisher

Let’s modify the timer_callback method of the publisher_member_funcion.py, so that instead of increasing the initial value, let’s decrease it. Let’s also modify our constructor (__init__ method) to make the initial value of the i variable be 100.

What this means is that the i variable will initially have the value 100, then it is going to decrease it. The final content of the   publisher_member_funcion.py file should be similar to the following:

# Copyright 2016 Open Source Robotics Foundation, Inc.
#
# 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.

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('minimal_publisher')
        self.publisher_ = self.create_publisher(String, 'topic', 10)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 100

    def timer_callback(self):
        msg = String()
        msg.data = 'Battery Level: %d' % self.i
        self.publisher_.publish(msg)
        self.get_logger().info('Publishing: "%s"' % msg.data)
        self.i -= 1

def main(args=None):
    rclpy.init(args=args)
    minimal_publisher = MinimalPublisher()
    rclpy.spin(minimal_publisher)
    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

def listener_callback(self, msg):
        if msg.data in ['Battery Level: 70', 'Battery Level: 69', 'Battery Level: 68', 'Battery Level: 67', 'Battery Level: 66']:
            self.get_logger().info('WARNING!: "%s"' % msg.data)
            self.publisher_.publish(msg)

What the changes are going to do is: if the battery level is between 70 and 66 %, then a warning message will be printed, and that warning will be published into a topic named /battery.

We also have to create the publisher into the constructor (__init__ method) with:

 self.publisher_ = self.create_publisher(String, 'battery', 10)

So in the end, the final subscriber will look like this:

# Copyright 2016 Open Source Robotics Foundation, Inc.
#
# 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.

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(
            String,
            'topic',
            self.listener_callback,
            10)
        self.subscription  # prevent unused variable warning
        self.publisher_ = self.create_publisher(String, 'battery', 10)

    def listener_callback(self, msg):

        if msg.data in ['Battery Level: 70', 'Battery Level: 69', 'Battery Level: 68', 'Battery Level: 67', 'Battery Level: 66']:
            self.get_logger().info('WARNING!: "%s"' % msg.data)
            self.publisher_.publish(msg)

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

    minimal_subscriber = MinimalSubscriber()
    rclpy.spin(minimal_subscriber)

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

if __name__ == '__main__':
    main()

Now that we have our publisher and subscriber, ready where the subscriber contains a conditional publisher,  now open our package.xml file and add rclpy and std_msgs as dependencies, by adding the two lines below after ament_python:

<exec_depend>rclpy</exec_depend>
<exec_depend>std_msgs</exec_depend>

The final result should be similar to the image below:

exec_depend rcl_py and std_msgs

Let’s now open the setup.py file and add our publisher and subscriber (let’s call them talker and listener respectively) to the entry_points. Our final entry_points should look like:

entry_points={
        'console_scripts': [
                'talker = py_pubsub.publisher_member_function:main',
                'listener = py_pubsub.subscriber_member_function:main',
        ],
},

If you are wondering whether your final setup.py file is correct, yours should look like this:

from setuptools import setup

package_name = 'py_pubsub'

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': [
                'talker = py_pubsub.publisher_member_function:main',
                'listener = py_pubsub.subscriber_member_function:main',
        ],
    },
)

Building our workspace (publisher and subscriber)

Now that our publisher and subscriber are ready, let’s now build our workspace.

To make sure all dependencies are correct, let’s fist run rosdep install:

cd ~/ros2_ws/

rosdep install -i --from-path src --rosdistro foxy -y

If everything went ok, you should see something like this:

#All required rosdeps installed successfully

Now let’s build it with:

cd ~/ros2_ws/

colcon build

The output should be similar to the following:

Starting >>> py_pubsub [2.2s] 
[0/1 complete] [py_pubsub - 0.9s] 
Finished <<< py_pubsub [1.56s] 

Summary: 1 package finished [2.83s]

Running the publisher and subscriber

Now that our ros2_ws (ROS2 Workspace) is built, let’s run our publisher and subscriber to make sure they are working. Let’s start by running our publisher, that we named talker.

source ~/ros2_ws/install/setup.bash

ros2 run py_pubsub talker

If everything went ok, you should see something similar to this:

[INFO] [1636416737.394168909] [minimal_publisher]: Publishing: "Battery Level: 100"
[INFO] [1636416737.876938844] [minimal_publisher]: Publishing: "Battery Level: 99"
[INFO] [1636416738.377307008] [minimal_publisher]: Publishing: "Battery Level: 98"
[INFO] [1636416738.877402234] [minimal_publisher]: Publishing: "Battery Level: 97"
[INFO] [1636416739.377231791] [minimal_publisher]: Publishing: "Battery Level: 96"
[INFO] [1636416739.877032283] [minimal_publisher]: Publishing: "Battery Level: 95"
[INFO] [1636416740.377206630] [minimal_publisher]: Publishing: "Battery Level: 94"
[INFO] [1636416740.877335527] [minimal_publisher]: Publishing: "Battery Level: 93"
[INFO] [1636416741.377765577] [minimal_publisher]: Publishing: "Battery Level: 92"

But, if you got an error message like this one “No executable found“, then it means you need to make your publisher and subscriber executables. You can make them executable with:

chmod +x ~/ros2_ws/src/py_pubsub/py_pubsub/*

You can now open a different web shell and run the subscriber named listener with:

source ~/ros2_ws/install/setup.bash

ros2 run py_pubsub listener

If the battery level is already below 66%, you should see no messages being published by the listener. If that is the case, please kill and start the talker again in the first terminal. You can kill it with CTRL+C , and can start it again just as you did before:

CTRL+C

ros2 run py_pubsub talker

After having restarted the talker in the first terminal, you can now keep an eye on the terminal where the listener was executed. When the battery level goes between 70 and 66%, the following message should have been printed.

[INFO] [1636416915.455386963] [minimal_subscriber]: WARNING!: "Battery Level: 70"
[INFO] [1636416915.934977703] [minimal_subscriber]: WARNING!: "Battery Level: 69"
[INFO] [1636416916.435285872] [minimal_subscriber]: WARNING!: "Battery Level: 68"
[INFO] [1636416916.935092663] [minimal_subscriber]: WARNING!: "Battery Level: 67"
[INFO] [1636416917.434961817] [minimal_subscriber]: WARNING!: "Battery Level: 66"

And to make sure the message is being published, you can now open a third terminal and check the output of the /battery topic with:

ros2 topic echo /battery

The message printed should be similar to the one below:

$ ros2 topic echo /battery 

data: 'Battery Level: 70'
---
data: 'Battery Level: 69'
---
data: 'Battery Level: 68'
---
data: 'Battery Level: 67'
---
data: 'Battery Level: 66'

Be aware that in order to see the message being published in the third terminal, where the /battery topic is being echoed, you have to restart the talker again, since the battery level at this point is well below the 65%.

Again, you can kill the talker in the first terminal with CTRL+C , and can start it again just as you did before:

CTRL+C

ros2 run py_pubsub talker

Now you should be able to see the output in the third terminal (/battery topic)

Congratulations. You made it. You now know not only how to create conditional publishers, but you have also learned how to create packages,  and modify publishers and subscribers in ROS2 Python.

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.

[ROS2 Q&A] 217 – How to Mix ROS1 and ROS2 Packages

What we are going to learn

1. How to source ROS1 and ROS2 correctly
2. How to run ros1_bridge

List of resources used in this post

1. ROS Development Studio (ROSDS) —▸ http://rosds.online
2. Robot Ignite Academy –▸ https://www.robotigniteacademy.com
3. ros1_bridge –▸ https://github.com/ros2/ros1_bridge
4. Question asked on ROS Answers –▸ https://answers.ros.org/question/343680/can-i-mix-ros1-and-ros2-packages/

Creating a rosject

In order to learn how to make ROS1 interact with ROS2, we need to have ROS1 and ROS2 installed in our computers. Installing two different distros is not going to be covered in this topic because we are going to use The Construct (https://www.theconstruct.ai/) for this tutorial, where both ROS installations are already available. But if you want to test everything on your own computer, feel free to install ROS1 Noetic and install ROS2 Foxy on your Ubuntu 20.04 computer.

Let’s start by opening The Construct (https://www.theconstruct.ai/) and logging in. You can easily create a free account if you still don’t have one.

Once inside, let’s create My Rosjects and then, Create a new rosject:

My Rosjects

Create a new rosject

For the rosject, let’s select ROS2 Foxy for the ROS Distro, let’s name the rosject as ROS1/2 Node. You can leave the rosject public.

Interaction between ROS1 and ROS2 Nodes using ros1_bridge

If you mouse over the recently created rosject, you should see a Run button. Just click that button to launch the rosject.

Installing ros1_bridge

As we mentioned at the beginning of this post, you need to have ROS1 and ROS2 installed on the computer. Thank God we already have that on The Construct.

Let’s open a new terminal by clicking on the Open a new shell window button:

Open a new shell

Now, let’s check the ROS distributions that we have installed:

ls /opt/ros/

You should see at least foxy and noetic installed.

Let’s now install ros1_bridge, a ROS 2 package that provides bidirectional communication between ROS 1 and ROS 2.

sudo apt-get update

sudo apt-get install -y ros-foxy-ros1-bridge

The ros1-bridge package should have been installed. You can check it with:

sudo dpkg -l | grep 'ros1-bridge'

which should output something similar to:

ii  ros-foxy-ros1-bridge    0.9.6-1focal.20211014.191041   A simple bridge between ROS 1 and ROS 2

Starting a ROS1 Node

Now it is time to start a simple ROS1 publisher. You may be already familiar with this. For that, let’s first run roscore in the first terminal with:

source /opt/ros/noetic/setup.bash

roscore

Please be aware that we first sourced our ROS1 noetic distro.

Now, in another terminal, let’s launch a ros1 publisher:

source /opt/ros/noetic/setup.bash

rosrun rospy_tutorials talker

If everything went ok, you should have a /chatter topic now, and the talker should be printing something like the output below:

[INFO] [1636419517.414196]: hello world 1636419517.414014
[INFO] [1636419517.515074]: hello world 1636419517.5148714
[INFO] [1636419517.614426]: hello world 1636419517.614205
[INFO] [1636419517.714437]: hello world 1636419517.7142227
[INFO] [1636419517.814406]: hello world 1636419517.8142045
[INFO] [1636419517.914464]: hello world 1636419517.9142513

You can also confirm that there is a /chatter topic in ROS1 with the following commands in another (third) terminal:

source /opt/ros/noetic/setup.bash

rostolic list

The output should be similar to the following:

/chatter
/rosout
/rosout_agg

Starting a ROS2 Node

So far we have launched a ROS1 node. Let’s open a different terminal (a fourth one), source ROS2 and check the list of topics:

source /opt/ros/foxy/setup.bash 

ros2 topic list

The topics listed should contain no reference to /chatter, the topic being published by a ROS1 node. The list of topics in ROS2 should be:

/parameter_events
/rosout

Now let’s run ROS1 Bridge. For that, let’s first source both ros1 and ros2. Let’s open a fifth terminal for that:

source /opt/ros/noetic/setup.bash

source /opt/ros/foxy/setup.bash

ros2 run ros1_bridge dynamic_bridge --bridge-all-topics

If everything went ok after launching ROS1 Bridge, the output should be similar to this:

created 1to2 bridge for topic '/chatter' with ROS 1 type 'std_msgs/String' and ROS 2 type 'std_msgs/msg/String'
created 1to2 bridge for topic '/rosout' with ROS 1 type 'rosgraph_msgs/Log' and ROS 2 type 'rcl_interfaces/msg/Log'
created 1to2 bridge for topic '/rosout_agg' with ROS 1 type 'rosgraph_msgs/Log' and ROS 2 type 'rcl_interfaces/msg/Log'
created 2to1 bridge for topic '/chatter' with ROS 2 type 'std_msgs/msg/String' and ROS 1 type 'std_msgs/String'
created 2to1 bridge for topic '/rosout' with ROS 2 type 'rcl_interfaces/msg/Log' and ROS 1 type 'rosgraph_msgs/Log'
[INFO] [1636419930.466921521] [ros_bridge]: Passing message from ROS 1 std_msgs/String to ROS 2 std_msgs/msg/String (showing msg only once per type)
[INFO] [1636419930.467529301] [ros_bridge]: Passing message from ROS 2 rcl_interfaces/msg/Log to ROS 1 rosgraph_msgs/Log (showing msg only once per type)
[INFO] [1636419930.467995035] [ros_bridge]: Passing message from ROS 2 std_msgs/msg/String to ROS 1 std_msgs/String (showing msg only once per type)
removed 2to1 bridge for topic '/chatter'
[INFO] [1636419930.587503640] [ros_bridge]: Passing message from ROS 1 rosgraph_msgs/Log to ROS 2 rcl_interfaces/msg/Log (showing msg only once per type)

If we now go back to the fourth terminal and list the ROS2 topics again:

source /opt/ros/foxy/setup.bash 

ros2 topic list

Now we should see a /chatter topic.

/chatter
/parameter_events
/rosout
/rosout_agg

This is really awesome, isn’t it? We are running a publisher in ROS1, and we can see the topic in ROS2.

If we can see the topic, we also expect to see what is being published, right? Let’s try to subscribe to that topic using ROS2 to see what happens. We can run the command below in the same terminal where  we listed the ros2 topics:

ros2 topic echo /chatter

If everything worked for you as it should, you should be able to see in ROS2 the messages published in ROS1:

data: hello world 1636420166.362258
---
data: hello world 1636420166.4622662
---
data: hello world 1636420166.5622616
---
data: hello world 1636420166.662261
---
data: hello world 1636420166.7621603

Awesome. Congratulations. You made it. You now have seen, with hands-on, how to make ROS1 and ROS2 topics interact with one another. This is certainly a really basic example, but we hope you find it useful. Feel free to kill the nodes you launched in the different terminals if you want. You can also keep using The Construct to improve your ROS Skills.

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.

Exploring ROS2 with a wheeled robot – #4 – Obstacle Avoidance

In this post you’ll learn how to program a robot to avoid obstacles using ROS2 and C++.
Up to the end of the post, the Dolly robot moves autonomously in a scene with many obstacles, simulated using Gazebo 11.

You’ll learn:

• How to publish AND subscribe topics in the same ROS2 Node
• How to avoid obstacles
• How to implement your own algorithm in ROS2 and C++

1 – Setup environment – Launch simulation

Before anything else, make sure you have the rosject from the previous post, you can copy it from here.

Launch the simulation in one webshell and in a different tab, checkout the topics we have available. You must get something similar to the image below:

2 – Create the node

In order to have our obstacle avoidance algorithm, let’s create a new executable in the file ~/ros2_ws/src/my_package/obstacle_avoidance.cpp:

#include "geometry_msgs/msg/twist.hpp"    // Twist
#include "rclcpp/rclcpp.hpp"              // ROS Core Libraries
#include "sensor_msgs/msg/laser_scan.hpp" // Laser Scan

using std::placeholders::_1;

class ObstacleAvoidance : public rclcpp::Node {
public:
  ObstacleAvoidance() : Node("ObstacleAvoidance") {

    auto default_qos = rclcpp::QoS(rclcpp::SystemDefaultsQoS());
    subscription_ = this->create_subscription(
        "laser_scan", default_qos,
        std::bind(&ObstacleAvoidance::topic_callback, this, _1));
    publisher_ =
        this->create_publisher("cmd_vel", 10);
  }

private:
  void topic_callback(const sensor_msgs::msg::LaserScan::SharedPtr _msg) {
    // 200 readings, from right to left, from -57 to 57 degress
    // calculate new velocity cmd
    float min = 10;
    for (int i = 0; i < 200; i++) { float current = _msg->ranges[i];
      if (current < min) { min = current; } } auto message = this->calculateVelMsg(min);
    publisher_->publish(message);
  }
  geometry_msgs::msg::Twist calculateVelMsg(float distance) {
    auto msg = geometry_msgs::msg::Twist();
    // logic
    RCLCPP_INFO(this->get_logger(), "Distance is: '%f'", distance);
    if (distance < 1) {
      // turn around
      msg.linear.x = 0;
      msg.angular.z = 0.3;
    } else {
      // go straight ahead
      msg.linear.x = 0.3;
      msg.angular.z = 0;
    }
    return msg;
  }
  rclcpp::Publisher::SharedPtr publisher_;
  rclcpp::Subscription::SharedPtr subscription_;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared());
  rclcpp::shutdown();
  return 0;
}

In the main function we have:

• Initialize node rclcpp::init
• Keep it running rclcpp::spin

Inside the class constructor:

• Subcribe to the laser scan messages: subscription_
• Publish to the robot diff driver: publisher_

The obstacle avoidance intelligence goes inside the method calculateVelMsg. This is where decisions are made based on the laser readings. Notice that is depends purely on the minimum distance read from the message.

If you want to customize it, for example, consider only the readings in front of the robot, or even check if it is better to turn left or right, this is the place you need to work on! Remember to adjust the parameters, because the way it is, only the minimum value comes to this method.

3 – Compile the node

This executable depends on both geometry_msgs and sensor_msgs, that we have added in the two previous posts of this series. Make sure you have them at the beginning of the ~/ros2_ws/src/my_package/CMakeLists.txt file:

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)

And finally, add the executable and install it:

# obstacle avoidance
add_executable(obstacle_avoidance src/obstacle_avoidance.cpp)
ament_target_dependencies(obstacle_avoidance rclcpp sensor_msgs geometry_msgs)

...

install(TARGETS
  reading_laser
  moving_robot
  obstacle_avoidance
  DESTINATION lib/${PROJECT_NAME}/
)

Compile the package:

colcon build --symlink-install --packages-select my_package

4 – Run the node

In order to run, use the following command:

ros2 run my_package obstacle_avoidance

It will not work for this robot! Why is that? We are subscribing and publishing to generic topics: cmd_vel and laser_scan

We need a launch file to remap these topics, let’s create one at ~/ros2_ws/src/my_package/launch/obstacle_avoidance.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():

    obstacle_avoidance = Node(
        package='my_package',
        executable='obstacle_avoidance',
        output='screen',
        remappings=[
            ('laser_scan', '/dolly/laser_scan'),
            ('cmd_vel', '/dolly/cmd_vel'),
        ]
    )

    return LaunchDescription([obstacle_avoidance])

Recompile the package, source the workspace once more and launch it:

colcon build --symlink-install --packages-select my_package
source ~/ros2_ws/install/setup.bash
ros2 launch my_package obstacle_avoidance.launch.py


Related courses & tutorials:

• ROS2 Basics for C++
• Copy my rosject here
• Dolly robot – Official repo

[ROS2 Q&A] 216 – How to Use Static Transform Publisher in ROS2

What we are going to learn

1. How to create a package in ROS2
2. How to write a python launch file
3. How to define a static transform

List of resources used in this post

1. ROS Development Studio (ROSDS) —▸ http://rosds.online
2. Robot Ignite Academy –▸ https://www.robotigniteacademy.com
3. Question asked on ROS Answers –▸ https://answers.ros.org/question/372752/static_transform_publisher-in-ros2-launch-file/

Creating a rosject

In order to learn how to use a static transform, we need a ROS2 Python package. We are going to use The Construct (https://www.theconstruct.ai/) for this tutorial, but if you have ROS2 installed on your own computer, you should be able to do ~everything on your own computer, except this creating a rosject part.

Let’s start by opening The Construct (https://www.theconstruct.ai/) and logging in. You can easily create a free account if you still don’t have one.

Once inside, let’s create My Rosjects and then, Create a new rosject:

My Rosjects

Create a new rosject

For the rosject, let’s select ROS2 Foxy for the ROS Distro, let’s name the rosject as static_tf_publisher. You can leave the rosject public.

Static TF Publisher

If you mouse over the recently created rosject, you should see a Run button. Just click that button to launch the rosject.

Creating a ROS2 package named static_tf_transform

Once the rosject is open, we can now create a package that will be used to publish the static transform.

Let’s open a new terminal by clicking on the Open a new shell window button:

Open a new shell

Once the terminal is open, we can list the files with the ls command:

user:~$ ls
ai_ws  catkin_ws  notebook_ws  ros2_ws  simulation_ws  webpage_ws

We can see a workspace named ros2_ws. Let’s enter that workspace using cd ros2_ws/:

user:~$ cd ros2_ws/
user:~/ros2_ws$

Let’s now source our workspace with:

source ~/ros2_ws/install/setup.bash

Let’s now enter our src folder:

 cd ~/ros2_ws/src/

And create a package named static_tf :

ros2 pkg create --build-type ament_python static_tf

The output should be similar to:

going to create a new package
package name: static_tf
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: []
creating folder ./static_tf
creating ./static_tf/package.xml
creating source folder
creating folder ./static_tf/static_tf
creating ./static_tf/setup.py
creating ./static_tf/setup.cfg
creating folder ./static_tf/resource
creating ./static_tf/resource/static_tf
creating ./static_tf/static_tf/__init__.py
creating folder ./static_tf/test
creating ./static_tf/test/test_copyright.py
creating ./static_tf/test/test_flake8.py
creating ./static_tf/test/test_pep257.py

If you now list your src folder using ls, you should be able to see our package:

user:~/ros2_ws/src$ ls
static_tf

We can now enter into this static_tf package using cd static_tf/ and create a launch folder there, and a file named static_tf_launch.py on that folder:

cd ~/ros2_ws/src/static_tf/

mkdir launch

touch launch/static_tf_launch.py

We can now open that static_tf_launch.py file using the Code Editor, and add the following content to it:

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package="tf2_ros",
            executable="static_transform_publisher",
            output="screen" ,
            arguments=["0", "0", "0", "0", "0", "0", "odom", "laser"]
        )
    ])

Once the static_tf_launch.py file is ok, let’s now open again our setup.py file (~/ros2_ws/src/static_tf/setup.py), and add our static_tf_launch.py file to data_files, so that our launch file will be included in the install folder when we compile our workspace. By doing this we will be able to execute it:

The bit we have to add to data_files is (os.path.join(‘share’, package_name, ‘launch’), glob(‘launch/*.py’)), so that data_files looks like:

data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
    ],

Just to make sure you have everything correctly imported, the final setup.py file should look like this:

from setuptools import setup
import os
from glob import glob

package_name = 'static_tf'

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']),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
    ],
    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': [
        ],
    },
)

Launching our ROS2 static tf publisher

Awesome. Our files are all in place. Let’s now build our ros2 workspace with:

cd ~/ros2_ws

colcon build

If everything went ok, the compilation should have raised no errors:

Starting >>> static_tf 
Finished <<< static_tf [1.03s] 

Summary: 1 package finished [1.20s]

Now let’s source our workspace again and run our launch file that runs the static tf publisher.

source ~/ros2_ws/install/setup.bash

ros2 launch static_tf  static_tf_launch.py

After launching the static_tf_launch.py file, the output should be similar to the following:

[INFO] [launch]: All log files can be found below /home/user/.ros/log/2021-11-08-23-10-55-010925-2_xterm-6136
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [static_transform_publisher-1]: process started with pid [6138]
[static_transform_publisher-1] [INFO] [1636413056.712927342] [static_transform_publisher_uKqntZ5iDxRToFQR]: Spinning until killed publishing transform from 'odom' to 'laser'

As we can see in the logs, we have the static_transform_publisher publishing a static transform from the odom to the laser frame.

We can now go to a second terminal and check the transforms between the frames with:

ros2 run tf2_ros tf2_echo odom laser

Assuming everything went as expected, the output you should get should be similar to the following:

[INFO] [1636413489.330173729] [tf2_echo]: Waiting for transform odom ->  laser: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist
At time 0.0
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
At time 0.0
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
At time 0.0
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]

Congratulations. You now know how to publish a static transform between two frames in ROS2 using Python.  In addition to that, you have also learned how to create a python package as well as a launch file along the way. Feel free to terminate the scripts you have executed by pressing CTRL+C in the terminals. You can also for sure spend more time using this great online platform, The Construct.

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.