ros services
MOVE_BB8.py
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
import time
class MoveBB8():
def __init__(self):
self.bb8_vel_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.ctrl_c = False
rospy.on_shutdown(self.shutdownhook)
self.rate = rospy.Rate(10) # 10hz
def publish_once_in_cmd_vel(self, cmd):
"""
This is because publishing in topics sometimes fails teh first time you publish.
In continuos publishing systems there is no big deal but in systems that publish only
once it IS very important.
"""
while not self.ctrl_c:
connections = self.bb8_vel_publisher.get_num_connections()
if connections > 0:
self.bb8_vel_publisher.publish(cmd)
rospy.loginfo("Cmd Published")
break
else:
self.rate.sleep()
def shutdownhook(self):
# works better than the rospy.is_shut_down()
self.stop_bb8()
self.ctrl_c = True
def stop_bb8(self):
rospy.loginfo("shutdown time! Stop the robot")
cmd = Twist()
cmd.linear.x = 0.0
cmd.angular.z = 0.0
self.publish_once_in_cmd_vel(cmd)
def move_x_time(self, moving_time, linear_speed=0.2, angular_speed=0.2):
cmd = Twist()
cmd.linear.x = linear_speed
cmd.angular.z = angular_speed
rospy.loginfo("Moving Forwards")
self.publish_once_in_cmd_vel(cmd)
time.sleep(moving_time)
self.stop_bb8()
rospy.loginfo("######## Finished Moving Forwards")
def move_square(self):
i = 0
while not self.ctrl_c and i < 4:
# Move Forwards
self.move_x_time(moving_time=2.0, linear_speed=0.2, angular_speed=0.0)
# Stop
self.move_x_time(moving_time=4.0, linear_speed=0.0, angular_speed=0.0)
# Turn
self.move_x_time(moving_time=3.5, linear_speed=0.0, angular_speed=0.2)
# Stop
self.move_x_time(moving_time=0.1, linear_speed=0.0, angular_speed=0.0)
i += 1
rospy.loginfo("######## Finished Moving in a Square")
if __name__ == '__main__':
rospy.init_node('move_bb8_test', anonymous=True)
movebb8_object = MoveBB8()
try:
movebb8_object.move_square()
except rospy.ROSInterruptException:
pass
然后是创建一个service server,用于发布做正方形移动的service: bb8_move_in_square_service_server.py
#! /usr/bin/env python
import rospy
from std_srvs.srv import Empty, EmptyResponse # you import the service message python classes generated from Empty.srv.
from move_bb8 import MoveBB8
def my_callback(request):
rospy.loginfo("The Service move_bb8_in_square has been called")
movebb8_object = MoveBB8()
movebb8_object.move_square()
rospy.loginfo("Finished service move_bb8_in_square that was called called")
return EmptyResponse() # the service Response class, in this case EmptyResponse
#return MyServiceResponse(len(request.words.split()))
rospy.init_node('service_move_bb8_in_square_server')
my_service = rospy.Service('/move_bb8_in_square', Empty , my_callback) # create the Service called move_bb8_in_square with the defined callback
rospy.loginfo("Service /move_bb8_in_square Ready")
rospy.spin() # mantain the service open.
service分为server跟client,server只是发布做方形移动的service,但实际进行方形移动还需要一个client来调用该service: bb8_move_in_square_service_client.py
#! /usr/bin/env python
import rospkg
import rospy
from std_srvs.srv import Empty, EmptyRequest # you import the service message python classes generated from Empty.srv.
rospy.init_node('service_move_bb8_in_square_client') # Initialise a ROS node with the name service_client
rospy.wait_for_service('/move_bb8_in_square') # Wait for the service client /move_bb8_in_square to be running
move_bb8_in_square_service_client = rospy.ServiceProxy('/move_bb8_in_square', Empty) # Create the connection to the service
move_bb8_in_square_request_object = EmptyRequest() # Create an object of type EmptyRequest
result = move_bb8_in_square_service_client(move_bb8_in_square_request_object) # Send through the connection the path to the trajectory file to be executed
print result # Print the result given by the service called
server跟client都有对应的launch文件:
$Launch Program: start_bb8_move_in_square_service_server.launch
<launch>
<!-- Start Service Server for move_bb8_in_square service -->
<node pkg="unit_3_services" type="bb8_move_in_square_service_server.py" name="service_move_bb8_in_square_server" output="screen">
</node>
</launch>
Launch Program: call_bb8_move_in_square_service_server.launch
<launch>
<!-- Start Service Server for move_bb8_in_square service -->
<node pkg="unit_3_services" type="bb8_move_in_square_service_client.py" name="service_move_bb8_in_square_client" output="screen">
</node>
</launch>
先启动server的launch文件,然后启动client的launch文件,就可以实现小车方形轨迹了。