4.3. Sample Subscriber NodesĀ¶

The subscriber node is very similar to the Publisher node, and many of my nodes are both publishers and subscribers.

The key difference is that the publisher and subscriber functions have a one to many relationship: there can be multiple subscribers subscribed to a topic, but there can only be one publisher.

A subscriber node receives the messages published by the subscriber nodes, up to the number of messages allowed in its queue.

import inspect
import rospy
from viper_toolkit import Dissect
from std_msgs.msg import String

4.3.1. The Example Subscriber Node and Class DefinitionĀ¶

In each package, located in the ā€œviper/srcā€ director of my git repository, there is a ā€˜scriptā€™ folder. Within this folder are the various nodes for that package (which usually are also a class definition of that node). We can take a look at the sample subscriber node I built within the ā€œviper_toolkitā€ package.

from scripts import subscriber
subscriber_source = inspect.getsource(subscriber)
print (subscriber_source)
#!/usr/bin/env python3

import rospy
from std_msgs.msg import String

class SubscriberNode(object):
    
    def __init__(self):
        self.setup_ros()
        self.setup_subscriber()
        self.main_loop()
    
    def setup_ros(self):
        rospy.init_node('sample_subscriber')
        self.message = String()
        
    def setup_subscriber(self):
        rospy.Subscriber(
            name = 'topic', 
            data_class = String, 
            callback = self.callback,
            queue_size = 1)
        
    def callback(self, msg):
        self.message = msg
        self.action_loop()
        
    def action_loop(self):
        text = self.message.data
        rospy.loginfo(f'I heard {text}')
        
    def main_loop(self):
        rospy.spin()

if __name__ == '__main__':
    SubscriberNode()

This is what the basic barebones subscriber looks likeā€¦ to better understand what is happening lets look at each of the parts.

4.3.2. Detailed Look at Class Attributes, Properties, and MethodsĀ¶

Each of my class definitions will look similar, although will greatly vary in complexity.

Lets take a look at the different parts of this node. To do this I have created a dissection tool Dissect() which you can find in my Viper Toolbox. It allows us to look at the different methods and properties within our class.

from scripts.subscriber import SubscriberNode

4.3.2.1. Initialize the Class ObjectĀ¶

The init class function executes at the time that the class in instantiated. We are telling the interpreter to first run the setup_ros() function, then run the setup_publisher() function, and finally to execute the main_loop() function.

Dissect('__init__', SubscriberNode)
class SubscriberNode(object):

    def __init__(self):
        self.setup_ros()
        self.setup_subscriber()
        self.main_loop()

4.3.2.2. Setup ROSĀ¶

We need to tell ROS that we would like to bring this node online; we are registering this kernel as ā€œsample_subscriberā€.

During this step we will also set the class ā€œmessageā€ attribute using the class String.This allows us to retrieve the previous message upon failure so that we do not disrupt the system loop in the event of a dropped message. Note that the message types between the subscribers and the publishers must be the same.

Dissect('setup_ros', SubscriberNode)
class SubscriberNode(object):

    def setup_ros(self):
        rospy.init_node('sample_subscriber')
        self.message = String()

4.3.2.3. Setting up the PublisherĀ¶

Next we will setup the subscriber API which allows a node to subscribe to a ā€œTopic.ā€ This node will be receive all messages of the topic ā€œtopicā€, but will only hold on to the last message (queue_size=1).

When it receives a new message it will execute its class callback() function

Dissect('setup_subscriber', SubscriberNode)
class SubscriberNode(object):

    def setup_subscriber(self):
        rospy.Subscriber(
            name = 'topic', 
            data_class = String, 
            callback = self.callback,
            queue_size = 1)

4.3.2.4. The Callback FunctionĀ¶

The callback() function tells the node what to do once it receives a message. In this example we are asking the node to save the incoming message to the container we set up earlier. Rather than perform an action in the main loop, which is activated cyclically, we will have our action originate from this callback.

Thoughout my modules I use a variety of types of messages and callbacks to not only pass on data such as images and inference results, but to also publish ā€œflagsā€ or indicators that a node wants something.

Dissect('callback', SubscriberNode)
class SubscriberNode(object):

    def callback(self, msg):
        self.message = msg
        self.action_loop()

We can take a look at the message received:

print ("Sample Message Received")
print (instance.message)
Sample Message Received
---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
/tmp/ipykernel_25647/3098139531.py in <module>
      1 print ("Sample Message Received")
----> 2 print (instance.message)

NameError: name 'instance' is not defined

4.3.2.5. The Action LoopĀ¶

In our example action loop we will take the most recent message and transform its case into capital letters. We access the data in the message through it message.data class property. We then output the results to the terminal, as well as return the results when this function is called.

Dissect('action_loop', SubscriberNode)
class SubscriberNode(object):

    def action_loop(self):
        text = self.message.data
        rospy.loginfo(f'I heard {text}')

4.3.2.6. Main LoopĀ¶

Like the publisher node, the main loop is the loop which will continually run while the node is online. Since all of our actions are occuring within the callback loop we do not have much in our main loop, other than the rospy.spin() function which keeps the kernal alive in the case that it is not actively processing data (but still waiting for messages).

Dissect('main_loop', SubscriberNode)
class SubscriberNode(object):

    def main_loop(self):
        rospy.spin()

4.3.3. Final OutputĀ¶

Our resulting output looks like this.

print ('''
if __name__ == '__main__':
    SubscriberNode()

[INFO] [1638136507.446916]: I heard The time is now 1638136507.4424667
[INFO] [1638136507.549606]: I heard The time is now 1638136507.542628
[INFO] [1638136507.645125]: I heard The time is now 1638136507.6427877
[INFO] [1638136507.744053]: I heard The time is now 1638136507.7424889
''')
if __name__ == '__main__':
    SubscriberNode()

[INFO] [1638136507.446916]: I heard The time is now 1638136507.4424667
[INFO] [1638136507.549606]: I heard The time is now 1638136507.542628
[INFO] [1638136507.645125]: I heard The time is now 1638136507.6427877
[INFO] [1638136507.744053]: I heard The time is now 1638136507.7424889