Writing a Simple Publisher and Subscriber (Python)

Posted 2020-09-07

Writing a Service Node

Here we‘ll create the service ("add_two_ints_server") node which will receive two ints and return the sum.

Change directory into the beginner_tutorials package, you created in the earlier tutorial, creating a package:

$ roscd beginner_tutorials

Please make sure you have followed the directions in the previous tutorial for creating the service needed in this tutorial, creating the AddTwoInts.srv (be sure to choose the right version of build tool you‘re using at the top of wiki page in the link).

Create the scripts/add_two_ints_server.py file within the beginner_tutorials package and paste the following inside it:

#!/usr/bin/env python

from beginner_tutorials.srv import *
import rospy

def handle_add_two_ints(req):
    print "Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b))
    return AddTwoIntsResponse(req.a + req.b)

def add_two_ints_server():
    rospy.init_node(‘add_two_ints_server‘)
    s = rospy.Service(‘add_two_ints‘, AddTwoInts, handle_add_two_ints)
    print "Ready to add two ints."
    rospy.spin()

if __name__ == "__main__":
    add_two_ints_server()

Don‘t forget to make the node executable:

chmod +x scripts/add_two_ints_server.py

Code Explained

We declare our node using init_node() and then declare our service:

s = rospy.Service(‘add_two_ints‘, AddTwoInts, handle_add_two_ints)

This declares a new service named add_two_ints with the AddTwoInts service type. All requests are passed to handle_add_two_ints function. handle_add_two_ints is called with instances of AddTwoIntsRequest and returns instances of AddTwoIntsResponse.

Just like with the subscriber example, rospy.spin() keeps your code from exiting until the service is shutdown.

Writing the Client Node

#!/usr/bin/env python

import sys
import rospy
from beginner_tutorials.srv import *

def add_two_ints_client(x, y):
    rospy.wait_for_service(‘add_two_ints‘)
    try:
        add_two_ints = rospy.ServiceProxy(‘add_two_ints‘, AddTwoInts)
        resp1 = add_two_ints(x, y)
        return resp1.sum
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

def usage():
    return "%s [x y]"%sys.argv[0]

if __name__ == "__main__":
    if len(sys.argv) == 3:
        x = int(sys.argv[1])
        y = int(sys.argv[2])
    else:
        print usage()
        sys.exit(1)
    print "Requesting %s+%s"%(x, y)
    print "%s + %s = %s"%(x, y, add_two_ints_client(x, y))

Don‘t forget to make the node executable:

chmod +x scripts/add_two_ints_client.p

Code Explained

For clients you don‘t have to call init_node(). We first call:

 rospy.wait_for_service(‘add_two_ints‘)

 This is a convenience method that blocks until the service named add_two_ints is available. Next we create a handle for calling the service:

 add_two_ints = rospy.ServiceProxy(‘add_two_ints‘, AddTwoInts)

We can use this handle just like a normal function and call it:

        resp1 = add_two_ints(x, y)
        return resp1.sum

Because we‘ve declared the type of the service to be AddTwoInts, it does the work of generating the AddTwoIntsRequest object for you (you‘re free to pass in your own instead). The return value is an AddTwoIntsResponse object. If the call fails, a rospy.ServiceException may be thrown, so you should setup the appropriate try/except block.

Building your nodes

catkin_make

Try it out!

rosrun beginner_tutorials add_two_ints_server.py
rosrun beginner_tutorials add_two_ints_client.py
rosrun beginner_tutorials add_two_ints_client.py 4 5

Requesting 4+5
4 + 5 = 9

Returning [4 + 5 = 9]