Week 4 Move Server

Week 4 ROS Services: Server Node

The Code

Copy all of the code below into your move_server.py file. Then, review the explainer below to understand how this all works.

#!/usr/bin/env python3

import rospy
from {BLANK}.msg import Twist
from com2009_msgs.srv import SetBool, SetBoolResponse

service_name = "move_service"

pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
vel = Twist()

def callback_function(service_request):

    service_response = SetBoolResponse()

    if service_request.request_signal == True:
        print(f"The '{service_name}' Server received a 'true' request and the robot will now move for 5 seconds...")

        StartTime = rospy.get_rostime()

        vel.linear.x = 0.1
        pub.publish(vel)

        rospy.loginfo('Published the velocity command to /cmd_vel')
        while (rospy.get_rostime().secs - StartTime.secs) < 5:
            continue

        rospy.loginfo('5 seconds have elapsed, stopping the robot...')

        vel.linear.x = 0.0
        pub.publish(vel)

        service_response.response_signal = True
        service_response.response_message = "Request complete."
    else:
        service_response.response_signal = False
        service_response.response_message = "Nothing happened, set request_signal to 'true' next time."
    return service_response

rospy.init_node(f"{service_name}_server")
my_service = rospy.Service(service_name, SetBool, callback_function)
rospy.loginfo(f"the '{service_name}' Server is ready to be called...")
rospy.spin()

FILL IN THE {BLANK}! Which message package does the Twist message belong to?

The Code Explained:

Firstly, don't forget the shebang:

#!/usr/bin/env python3

As you should know by now, in order to develop any ROS node in Python we first need to import the rospy library so that we can interact with ROS. We're also going to be issuing velocity commands to the robot, so we need to import the Twist message from the correct message package as well.

Then, we also import the Service Message that we want to use for the service that we will set up. This service will use the SetBool service message from a custom com2009_msgs package that we have created for you:

from com2009_msgs.srv import SetBool, SetBoolResponse

Here, we import two different things from the com2009_msgs package:

1. A definition of the full service message: SetBool, which we need to use when we create the service later.
2. The Response portion of the service message: SetBoolResponse, which we will use to issue a response to the service caller.

Then, we set up a publisher to the /cmd_vel topic, so that we can publish velocity commands to the robot (using Twist messages). By now you should be familiar with this from the work you have done so far on this course.

pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
vel = Twist()

Next, we define a callback_function:

def callback_function(service_request):

We'll come back to this in more detail in a bit and first jump to the part of the code after the callback_function(), since this next bit will actually execute first:

rospy.init_node(f"{service_name}_server")
my_service = rospy.Service(service_name, SetBool, callback_function)
rospy.loginfo(f"the '{service_name}' Server is ready to be called...")
rospy.spin()

Here, we initialise a new ROS Node and call it "move_service_server" (via the service_name = "move_service" variable inside an f-string). Then, we create a rospy.Service instance where we define:

1. The name of the service that this node will launch (service_name = "move_service" at the beginning of the code).
2. The full service message format that the service will use, in this case: SetBool, which we imported earlier.
3. A callback function, in this case called callback_function, which will define what we want this service Server to do once the service is called.

After this, we send some information to the terminal using rospy.loginfo to indicate that the node has been launched successfully and is ready to be called. Finally, we use the rospy.spin() tool to make sure that our node keeps running indefinitely.

Now let's return to the definition of the callback_function(). This contains the code that will be executed by this node when the service is called. The function can take one input argument only, in this case we are calling it service_request. This is where the rospy.Service instance that we set up earlier will put the data that it obtains from the /move_service whenever it is called (the service Request). Then, we create an instance of the Response portion of the SetBool service message, which we will populate with data later on (based on the outcome of the actions that the service server performs).

def callback_function(service_request):

    service_response = SetBoolResponse()

We then analyse the service Request data, which is passed to this node whenever a call to the service is made. We know how to access the data within the service request from using the rossrv info command, which provides us with the following information:

$ rossrv info com2009_msgs/SetBool:

bool request_signal
---
bool response_signal
string response_message

The Request message will contain a boolean value called request_signal, so we can call this value by name from the input variable to our callback function, which we called service_request. We will check if this value is True or False using an if statement, and then define some actions for each situation accordingly:

if service_request.request_signal == True:

If the service_request.request_signal value is True, then we perform the following actions:

1. Print a status message to tell the Service caller that a True value has been received:

   print(f"The '{service_name}' Server received a 'true' request and the robot will now move for 5 seconds...")

2. Get the current ROS time using the rospy.get_rostime() function:

   StartTime = rospy.get_rostime()

3. Set a linear velocity for the robot, publish this to the /cmd_vel topic using the publisher that we set up earlier and then issue another status message to indicate that this has been done:

   vel.linear.x = 0.1
   pub.publish(vel)
   
   rospy.loginfo('Published the velocity command to /cmd_vel')

4. Setup a while loop to act as a timer, and wait until 5 seconds has elapsed before doing anything else:

   while (rospy.get_rostime().secs - StartTime.secs) < 5:
       continue

5. Once the time has elapsed, issue another status message to indicate this, then publish another velocity command to make the robot stop:

   rospy.loginfo('5 seconds have elapsed, stopping the robot...')
   
   vel.linear.x = 0.0
   pub.publish(vel)

6. Finally, we can format a service Response using the SetBoolResponse instance that we set up earlier (called service_response). Again, we know how to access the fields within the service response from the rossrv info command as shown earlier, and thus we know that our Response should be constructed as follows:

       service_response.response_signal = True
       service_response.response_message = "Request complete."

   If, however, the value of the service_request.request_signal was actually found to be False by our if statement earlier then we format our service response differently:

   else:
       service_response.response_signal = False
       service_response.response_message = "Nothing happened, set request_signal to 'true' next time."