Robot Ignite4: ROS基础--服务(service)续：对小车走方形轨迹代码分析

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>
    
    <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>
    
    <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文件，就可以实现小车方形轨迹了。