0
$\begingroup$

Rosanswers logo

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 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

    # Publishers
    self.pause_publishers[i] = self.create_publisher(Bool, '/robot'+str(i+1)+'/trajectory_controller/pause_or_resume', 50)

# 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)

def some_example_function_for_pause_pub(self):
    msg = Bool()
    msg.data = True 
    self.pause_publishers[0].publish(msg)

Originally posted by KimJensen on ROS Answers with karma: 55 on 2022-04-05

Post score: 2

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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

https://github.com/ros-planning/navigation2/tree/main/nav2_bringup/launch

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

https://github.com/neobotix/neo_simulation2/tree/feature/multi_robot_navigation/launch

or base it on this one:

https://www.theconstructsim.com/spawning-multiple-robots-in-gazebo-with-ros2/

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.


Originally posted by ljaniec with karma: 3064 on 2022-04-05

This answer was ACCEPTED on the original site

Post score: 3


Original comments

Comment by KimJensen on 2022-04-12:
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?

Comment by KimJensen on 2022-04-12:
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.

Comment by ljaniec on 2022-04-12:
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, )

Comment by KimJensen on 2022-04-12:
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 all are referring to the same odom_callback?

Comment by ljaniec on 2022-04-12:
I think I would use these suggestions here: https://answers.ros.org/question/346810/ros2-python-add-arguments-to-callback/ 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.

Comment by KimJensen on 2022-04-13:
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)

Comment by KimJensen on 2022-04-13:
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 get a "self.pause_pub_r2"?

Comment by ljaniec on 2022-04-13:
I think you can use an array of publishers (e.g. self.pause_publishers = []) for it, maybe?

Comment by KimJensen on 2022-04-14:
You did it Łukasz, thank you so much! :D I'll edit the question with the final version of the code.

Comment by ljaniec on 2022-04-14:
I am glad to help you :)

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.