ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Reading data from different topics, using the same callback

asked 2020-07-29 15:16:40 -0500

adry gravatar image

updated 2020-07-29 17:35:45 -0500

Thomas D gravatar image

I have to read data from topics having same name but different namespaces. e.g. node0/battery, node1/battery etc. so, my code is:

f  = open("./Nodelist.txt", "r")
nlist = f.read().splitlines()
for self.x in nlist:
     battery_lvl_listener = self.create_subscription(Float64,'' + str(self.x) + '/battery_lvl', self.battery_listener_callback,10)

def battery_listener_callback(self, msg):
        print("data read from: %s", self.x)

The problem is that self.x is always the same.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2020-07-29 19:38:32 -0500

Geoff gravatar image

self.x is always the same because it is set for each round of the for loop, then remains at the value of the final round of the for loop. It won't be updated after that, not even when the callback is called.

You can have multiple subscriptions using a single callback, so long as that callback is re-entrant (safe to be called multiple times in parallel). I don't think there is any way to know which particular topic the message came from, however. The assumption in the design of ROS 1 is that you use one callback per subscription, therefore you know implicitly which topic the data came from in a callback.

If you want to use "one" callback for many topics and know which topic sent the message, you can do it using a closure. Something like this should work:

import rclpy
from rclpy.node import Node

from std_msgs.msg import String


class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subs = []
        with open('nodelist.txt', 'r') as f:
            nlist = f.read().splitlines()
            for x in nlist:
                self.subs.append(self.create_subscription(String, x + '/battery_lvl', self.make_callback(x), 10))

    def make_callback(self, x):
        def callback(msg):
            self.get_logger().info('data read from: ' + x)
        return callback


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

However it is important to remember that this is technically three different callbacks at run time. If you actually need one method that is called no matter the topic then you will need to make a separate method and call it from the closure.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2020-07-29 15:16:40 -0500

Seen: 629 times

Last updated: Jul 29 '20