Dynamic publishers, subscribers and callbacks

asked 2022-04-05 07:45:39 -0500

55 ●6 ●8 ●12

updated 2022-04-14 07:06:22 -0500

I am trying to create a multi-robot system (MRS), where multiple robots are communicating with each other over ROS2, Galactic, Ubuntu 20.04. Currently, I'm only running this in simulation, but I have multiple robots in Gazebo, each running on their own independent custom version of a navigation stack.

One of the nodes that I'm working on, is collision avoidance between the robots. Here, based on the number of robots, then I'm trying to ensure collision free operation between the robots. For starters this is simple start-stop mechanisms based on priority, but this isn't really relevant for this. Based on the number of robots, then I need to subscribe to e.g. the position of each of the robots, and of cause have a callback for each of the subscribers. Currently, I've just created N hardcoded subscribers and callbacks, assuming that I am spawning at most N robots. If I spawn more than N robots, then I'm unable to listen to the position of the N+i'th robot.

Currently, the topics of the different robots are encapsulated within a namespace, e.g. robot1/..

Is there a way for me to dynamically create the publisher, subscribers, callback, etc. that I need based on the number of robots that I have in the system? I'm essentially just trying to create identical publishers/subscribers that run on different namespaces. I'm very interested in a general solution to this problem, assuming one exists, as I have other nodes in which I would like to use the same principles.

I've attached a piece of sample code, presenting the way that I'm doing it right now.

self.declare_parameter('number_of_robots', 1)
self.param_number_of_robots

    # Creating publishers
    for i in range(self.param_number_of_robots):
        if i == 0:
            self.pause_pub_r1= self.create_publisher(Bool, '/robot1/pause_or_resume', 10)
        if i == 1:
            self.pause_pub_r2= self.create_publisher(Bool, '/robot2/pause_or_resume', 10)
        if i == 2:
            self.pause_pub_r3= self.create_publisher(Bool, '/robot3/pause_or_resume', 10)
        if i == 3:
            self.pause_pub_r4= self.create_publisher(Bool, '/robot4/pause_or_resume', 10)

    # Creating subscribers
    for i in range(self.param_number_of_robots):     
        if i == 0:
            #Create subscribers - ROBOT 1        
            self.subscription_heading_r1 = self.create_subscription(
                DebugTrajectoryController,
                '/robot1/odom',
                self.heading_callback_r1,
                qos.qos_profile_sensor_data
            )
            self.subscription_heading_r1  # prevent unused variable warning

        if i == 1:
            #Create subscribers - ROBOT 2        
            self.subscription_heading_r2 = self.create_subscription(
                DebugTrajectoryController,
                '/robot2/odom',
                self.heading_callback_r2,
                qos.qos_profile_sensor_data
            )
            self.subscription_heading_r2  # prevent unused variable warning

        if i == 2:
            #Create subscribers - ROBOT 3        
            self.subscription_heading_r3 = self.create_subscription(
                DebugTrajectoryController,
                '/robot3/odom',
                self.heading_callback_r3,
                qos.qos_profile_sensor_data
            )
            self.subscription_heading_r3  # prevent unused variable warning

        if i == 3:
            #Create subscribers - ROBOT 4       
            self.subscription_heading_r4 = self.create_subscription(
                DebugTrajectoryController,
                '/robot4/odom',
                self.heading_callback_r4,
                qos.qos_profile_sensor_data
            )
            self.subscription_heading_r4  # prevent unused variable warning

EDIT (Solution):

# Include library
from functools import partial

# Declare input variables
self.declare_parameter('number_of_robots', 1)
self.param_n_robots = self.get_parameter('number_of_robots').get_parameter_value().integer_value

# Declare variables
self.current_robots_poses = []
self.pause_publishers = []

# Dynamically append robots to arrays
for robot in range(self.param_n_robots):
    self.current_robots_poses.append([np.nan, np.nan])
    self.pause_publishers.append('pause_pub_r'+str(robot+1))

# Subscribers
for i in ...

(more)

edit retag flag offensive close merge delete

add a comment

answered 2022-04-05 08:11:24 -0500

ljaniec

3064 ●14 ●297 ●50 https://www.linkedin.c...

updated 2022-04-05 09:09:26 -0500

I think you should use a modified launcher similar to multi_tb3_simulation_launch.py from Nav2:

https://github.com/ros-planning/navig...

or Neobotix's (branch: feautre/multi_robot_navigation):

https://github.com/neobotix/neo_simul...

or base it on this one:

https://www.theconstructsim.com/spawn...

Use a ROS2 parameters in your nodes so you can setup them in the launchers of N robots as in these examples (so N nodes launched or a node with N publishers/subscribers given from the setup parameter). You can try to use a system variable too.

In general, self.create_subscription and self.create_publisher can use a parameterized topic name (e.g. '/robot'+str(i)+'/odom') so it can be easily written in a loop.

edit flag offensive delete link

Comments

Thank you for your answer.

For launching the different nodes, then I've made a system based on a combination of the Launch files from Nav2, and those from The Construct. Within this system, then each of the independent nodes are all spawned within their separate namespaces. In this scenario one node contains all relevant information for only one robot.

My problem arises when I have one node, that then has to receive/send information from/to multiple different robots, hence it will have to create pubs, subs and callbacks that correspond to the different namespaces of the different robots.

I'm already using the ROS2 parameters, which you're referring to. However, I've only used one setup parameter being the number of robots, and then I'm trying to create the dynamic pub/sub/callbacks within the node. Do you believe there's another, better, way to do this?

KimJensen ( 2022-04-12 07:35:58 -0500 )edit

Didn't know you could make parameterized topic names for pub/sub, that's nice! How do you do with dynamic callbacks then? I can't really see how one would make the declaration of the callbacks dynamic, and I can't seem to pass an index to the callback, hence using one callback for all of the subscribers seems to not be possible.

KimJensen ( 2022-04-12 07:40:05 -0500 )edit

To make sure I understand - do you want parameterized number of subscribers/publishers in your node and want to do different things with topic (and thus different callbacks each time)? First I would rather check if it is 100% needed - maybe you can group these callbacks based on their work (e.g. odometry callback, camera feedback callback etc.) and have a different callback function for each group E.g.

self.odometry_subscriber = self.create_subscription( Odometry, self.topic_prefix + "/odom", self.odometry_callback, 10, )

ljaniec ( 2022-04-12 09:28:44 -0500 )edit

Assuming I have 4 robots, encapsulated with /robot1, .., namespaces. What I then in essence want, is to have e.g. 4 subs, one to each of the robot's odom, and then inside the callback store the position of each of the robots.

If I then want to suddenly spawn 5 robots instead, then I'd like for my code to create 5 subs, and store the pos of 5 different robots, using the setup parameter (param_number_of_robots = 5) - so do it without more hardcoding.

My callbacks now:

def odom_callback_r1(self, msg):
            self.current_robots_poses[0][0] = round(msg.pose.pose.position.x, 2)
            self.current_robots_poses[0][1] = round(msg.pose.pose.position.y, 2)
def odom_callback_r2(self, msg):
            self.current_robots_poses[1][0] = round(msg.pose.pose.position.x, 2)
            self.current_robots_poses[1][1] = round(msg.pose.pose.position.y, 2)

How would you handle the msgs if four different odom topics ...(more)

KimJensen ( 2022-04-12 09:59:01 -0500 )edit

I think I would use these suggestions here: https://answers.ros.org/question/3468... to create a callback with argument (functools.partial or lambda function), then rewrite the code to work with loops over elements and an array of size param_number_of_robots.

ljaniec ( 2022-04-12 12:15:57 -0500 )edit

This combination of parameterized topic names and passing index to only one callback completely resolved my issue around subscribers and callbacks. Thank you so much!

For those interested, here is how I ended up doing it:

        # Subscribers
    for i in range(self.param_n_robots):
        self.subscription_odom = self.create_subscription(
                    Odometry,
                    '/robot'+str(i+1)+'/odom/filtered',
                    partial(self.odom_callback, index=i),
                    qos.qos_profile_sensor_data
                )
        self.subscription_odom  # prevent unused variable warning

# Callbacks
def odom_callback(self, msg, index):
    self.current_robots_poses[index][0] = round(msg.pose.pose.position.x, 2)
    self.current_robots_poses[index][1] = round(msg.pose.pose.position.y, 2)

KimJensen ( 2022-04-13 03:11:36 -0500 )edit

When it comes to the publishers, then I get that I can now use parameterized topic names, which partly solves my problem with dynamic publishers. When defining the publishers, then I have to declare a variable name (in example below self.pause_pub_r1 and self.pause_pub_r2), which is later used to publish using that publisher, e.g. "node.pause_pub_r1.publish(msg)". Using the parameterized topic names doesn't allow me to create different publisher variables names, but instead assigns different topics to the same publisher variable. Is it possible to create the pub variable name dynamically as well? So I basically want to ensure, that when I use e.g. a robot1 publisher, then I only publish to one topic, the /robot1/..., robot 2 publisher only publish on /robot2/.., etc.

for i in range(self.param_number_of_robots): 
    self.pause_pub_r1= self.create_publisher(Bool, '/robot'+str(i+1)+'/trajectory_controller/pause_or_resume', 50)
    # How do I ...

(more)

KimJensen ( 2022-04-13 03:23:21 -0500 )edit

I think you can use an array of publishers (e.g. self.pause_publishers = []) for it, maybe?

ljaniec ( 2022-04-13 04:01:51 -0500 )edit

see more comments

Dynamic publishers, subscribers and callbacks

EDIT (Solution):

1 Answer

Comments

Question Tools

Stats

Related questions

Dynamic publishers, subscribers and callbacks edit

EDIT (Solution):

1 Answer

Comments

Question Tools

Stats

Related questions

Dynamic publishers, subscribers and callbacks