Part 4 Move Service-Server¶
Copy all the code below into your move_server.py
file and review the annotations to understand how it all works.
#!/usr/bin/env python3
import rospy
from {BLANK}.msg import Twist # (1)!
from tuos_msgs.srv import SetBool, SetBoolResponse # (2)!
class moveService():
def __init__(self):
service_name = "move_service"
rospy.init_node(f"{service_name}_server") # (14)!
self.service = rospy.Service(service_name, SetBool, self.srv_callback) # (15)!
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) # (3)!
rospy.loginfo(f"the '{service_name}' Server is ready to be called...") # (16)!
def srv_callback(self, request_from_client): # (4)!
vel = Twist()
response_from_server = SetBoolResponse() # (5)!
if request_from_client.request_signal == True: # (6)!
print(f"Server received a 'true' request and the robot will now move for 5 seconds...") # (7)!
StartTime = rospy.get_rostime() # (8)!
vel.linear.x = 0.1
self.pub.publish(vel) # (9)!
rospy.loginfo('Published the velocity command to /cmd_vel')
while (rospy.get_rostime().secs - StartTime.secs) < 5: # (10)!
continue
rospy.loginfo('5 seconds have elapsed, stopping the robot...')
vel.linear.x = 0.0
self.pub.publish(vel) # (11)!
response_from_server.response_signal = True # (12)!
response_from_server.response_message = "Request complete."
else: # (13)!
response_from_server.response_signal = False
response_from_server.response_message = "Nothing happened, set request_signal to 'true' next time."
return response_from_server
def main(self):
rospy.spin() # (17)!
if __name__ == '__main__':
server = moveService()
server.main()
-
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 theTwist
message from the correct message package as well. -
We also need to 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 customtuos_msgs
package that we've created for you.Here, we import two different things from the
tuos_msgs
package:- A definition of the full service message:
SetBool
, which we need to use when we create the service later. - The Response portion of the service message:
SetBoolResponse
, which we will use to issue a response to the service caller.
- A definition of the full service message:
-
Here, we set up a publisher to the
/cmd_vel
topic, so that we can publish velocity commands to the robot (usingTwist
messages). Hopefully this part is starting to become familiar to you by now! -
Here we define a callback function for the server called
srv_callback
. Any code within this function will be executed whenever the service is called.The function can take one input argument only, in this case we are calling it
request_from_client
. This is where therospy.Service
instance that we set up earlier will put the data that it obtains from a/move_service
call, whenever a Request is made. -
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). -
We then analyse the service Request data (this is the data that is passed to the Server node, whenever a call to the service is made by a caller, or client). We know how to access the data within the service request from using the
rossrv info
command, which provides us with the following information:The Request message will therefore contain a boolean value called
request_signal
, so we can call this value from the input to our callback function (which we calledrequest_from_client
). Using anif
statement, we check if this value isTrue
orFalse
, and then define some actions for each situation accordingly... -
Print a status message to tell the Service caller that a
True
value has been received. -
Get the current ROS time.
-
Set a linear velocity for the robot, publish this to the
/cmd_vel
topic using the publisher that we set up in the__init__()
method earlier (self.pub
). -
Here, we use a while loop to act as a 5-second timer (by keeping an eye on the current ROS time using
get_rostime()
). Once 5 seconds have elapsed, this while loop will end. -
Once the time has elapsed, we publish another velocity command to make the robot stop.
-
Finally, we can format a service Response using the
SetBoolResponse
instance that we set up earlier (response_from_server
). Again, we know the names of the attributes in the service response from therossrv info
command: -
If the value of the
request_from_client.request_signal
was actually found to beFalse
by ourif
statement earlier, then we do nothing other than send a service response, to indicate that nothing has happened! -
Initialise our ROS Node with a name (
"move_service_server"
). -
Create a
rospy.Service
instance where we define:- The name of the service that this node will launch (
service_name = "move_service"
at the beginning of the__init__()
method). - The full service message format that the service will use, in this case:
SetBool
, which we imported earlier. - A callback function, in this case called
srv_callback
, which will define what we want this service Server to do once the service is called.
- The name of the service that this node will launch (
-
Send some information to the terminal to indicate that the node has been launched successfully, and that the Service is ready to be called.
-
The
rospy.spin()
function keeps our node running indefinitely (so that the callback function can continue to execute, whenever the service is called).
Fill in the Blank!
Which message package does the Twist
message belong to?