Dynamic publishers, subscribers and callbacks
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 ...