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

03/12/2021

What we are going to learn

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

List of resources used in this post

ROS Development Studio (ROSDS) —▸ http://rosds.online
Robot Ignite Academy –▸ https://www.robotigniteacademy.com
ROS2 Cookbook –▸ https://github.com/mikeferguson/ros2_cookbook/blob/main/pages/launch.md
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()

Now let’s open also our subscriber_member_function.py file and modify the listener_callback method so that it will have the following content:

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.

Topics: