How to transfer data from a set of topics to another based on name/prefix of topic?

Question

I currently using script which subscribes to 3 separate topics with different message types, converts the data from those topics into one message type, and publishes the combined data into a topic which controls a drone (i.e takes Pose, Twist, and Acceleration (Vector3) data and converts it into the message type "FullState" and publishes). This works well for one drone, but I need to expand the code to work for multiple drones.

The topics are named based on the ID of the drone, which is set in a .yaml file (cf1/pose, cf1/twist, cf1/acc, cf2/pose, cf2/twist, ..., cf1/cmd_full_state, cf2/cmd_full_state, ...)

I need to make sure that the data from the 'cf1' topics are being converted and published correctly to the 'cf1/cmd_full_state' topic, and the same thing to happen for cf2, cf3, etc.

Here is the code I have so far, which sets up the topics for each drone in the .yaml file.

#!/usr/bin/env python


import rospy
import yaml
from std_msgs.msg import String
from std_msgs.msg import Float32
from crazyswarm.msg import FullState
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Vector3

with open('crazyswarm/ros_ws/src/crazyswarm/launch/crazyflies.yaml', 'r') as file:
    cfs_yaml = yaml.safe_load(file)

class TranslationNode:
    def __init__ (self):
        
        for entry in cfs_yaml["crazyflies"]:
            id = entry['id']
            prefix = ("cf" + str(id))
        
            self.pose_sub = rospy.Subscriber(prefix +'/pose_cmd', Pose, self.pose_cb, callback_args = prefix, queue_size=10)
        self.twist_sub = rospy.Subscriber(prefix + '/twist_cmd', Twist, self.twist_cb, callback_args = prefix, queue_size=10)
        self.acc_sub = rospy.Subscriber(prefix + '/acc_cmd', Vector3, self.acc_cb, callback_args = prefix, queue_size=10)
    
        self.full_state_pub = rospy.Publisher(prefix + '/cmd_full_state', FullState, queue_size=10)
    
        self.pose = None
        self.twist = None
        self.acc = None
    
        self._full_state_timer = rospy.Timer(rospy.Duration(0.01),self.full_state_cb)
     
def pose_cb(self,data,args):
    prefix = args
    self.pose = data
    
def twist_cb(self,data,args):
    prefix = args
    self.twist = data
   
def acc_cb(self,data,args):
    prefix = args
    self.acc = data
    
def full_state_cb(self, event):
    if self.pose is not None and self.twist is not None and self.acc is not None:
        full_state_msg = FullState()
        full_state_msg.header.stamp = rospy.Time.now()
        full_state_msg.header.seq += 1
        full_state_msg.pose = self.pose
        full_state_msg.twist = self.twist
        full_state_msg.acc = self.acc
        self.full_state_pub.publish(full_state_msg)


def main():
    rospy.init_node('translation_node', anonymous=True)
    translation_node = TranslationNode()
    rospy.spin()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass
        

This is able to receive the messages from all of the Pose, Twist, and Acc topics and converts them to the FullState message type correctly. However, it only publishes the data to the topic with the last drone ID (i.e if we have 3 drones labeled cf1, cf2, and cf3, the script publishes all of the data to the 'cf3/cmd_full_state' topic)

I am relatively new to ROS so there's probably a simple fix or a better/cleaner method to use. Is there any way to make sure the data gets transferred to the correct topics without setting up each subscriber/publisher individually? Any help would be appreciated.

---

Originally posted by csanchez5 on ROS Answers with karma: 3 on 2023-07-07

Post score: 0

Answer 1

I would organize the code differently: have the TranslationNode class handle just one drone, and pass the name (or prefix) to the constructor. Then in main(), create a new instance of TranslationNode for each drone, storing the objects in an array (so the instance doesn't go out of scope & get garbage collected.)

With this alternate design, you do not need to pass an extra argument to the subscribe callback - the needed info is already available in self.

---

Originally posted by Mike Scheutzow with karma: 4903 on 2023-07-09

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by csanchez5 on 2023-08-10:
Apologies for late response. This method seemed to work. The messages are getting translated and published properly. Thanks!

Here are the parts of the code I adjusted:

class TranslationNode:
    def __init__ (self, name=None):
    
        self.prefix = name
    
        self.pose_sub = rospy.Subscriber(self.prefix +'/pose_cmd', Pose, self.pose_cb, queue_size=10)
        ...


def main():
    rospy.init_node('translation_node', anonymous=True)

    with open('crazyswarm/ros_ws/src/crazyswarm/launch/crazyflies.yaml', 'r') as file:
        cfs_yaml = yaml.safe_load(file)

    cf_list = []

    for entry in cfs_yaml["crazyflies"]:
         id = entry['id']
         prefix = ("cf" + str(id))
         cf_list.append(TranslationNode(prefix))
     
    rospy.spin()