Ch4. 用 Python 写一个 ROS Service Client

0. 预热

注意，这篇博客的目的是在于写一段 Service Client，而非 Service。在开始我们今天的博客之前，笔者想先在这里记录下 ROS 中 Service 与 Topic 的异同：

1. Service 可以看作是一个 Publisher，多个 Subscriber 的Topic
2. Service 占用更多的内存，因此我们不能像 Subscriber 一样频繁地调用它
3. Service 的数据类型和服务名称与 Topic 有一些不同

1. 查看 Service

在 Ch.3 用Python写一个ROS 闭环控制 代码的基础上，我们开始今天的尝试：

首先我们运行 rosrun 和小乌龟：

rosrun roscore

rosrun turtlesim turtlesim_node

使用rosservice list查看可选service：

这里我们选择turtle1/set_pen查看服务信息，其作用是改变小乌龟轨迹颜色：

rosservice info /turtle1/set_pen

观察到该服务的Type类型是turtlrsim/SetPen，再查看该类数据信息：

rossrv show turtlesim/SetPen

2. 编写代码

我们的目标是在小乌龟经过某片区域时线变成红色，另一片区域时线变成绿色。因此主体代码是：

global previous_x    #设定比较值，让service仅在穿过区域时监听到
if pose.x >= 5.5 and previous_x <= 5.5:  #设定边界和条件
    previous_x = pose.x                  #记录位置
    rospy.loginfo("Set color to red!")
    call_set_pen_service(255, 0, 0, 3, 0)#变色
elif pose.x < 5.5 and previous_x >= 5.5:
    previous_x = pose.x
    rospy.loginfo("Set color to green!")
    call_set_pen_service(0, 255, 0, 3,0)#变色

还需要定义 call_service 的函数，注意 Service 名称和数据类型：

def call_set_pen_service(r, g, b, width, off):
    try:
        set_pen = rospy.ServiceProxy("/turtle1/set_pen", SetPen)
        response = set_pen(r, g, b, width, off)
        rospy.loginfo(response)
    except rospy.ServiceException as e:
        rospy.logwarn(e)

—————————————————————————————————
将新添的部分放进原来的代码中，并做一些细节上的调整：

#!/usr/bin/env python3
import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from turtlesim.srv import SetPen         #Service中的数据类型

previous_x = 0                           #定义全局变量以供判断

def call_set_pen_service(r, g, b, width, off):
    try:
        set_pen = rospy.ServiceProxy("/turtle1/set_pen", SetPen)
        response = set_pen(r, g, b, width, off) #定义颜色变量
        rospy.loginfo(response)
    except rospy.ServiceException as e:         #报错
        rospy.logwarn(e)


def pose_callback(pose: Pose):
    cmd = Twist()
    if pose.x > 9.0 or pose.x < 1.0 or pose.y > 10.0 or pose.y < 2.0:
        cmd.linear.x = 1.0
        cmd.angular.z = 2
    else:
        cmd.linear.x = 4.0
        cmd.linear.y = 0.0
        cmd.angular.z = 1.0
    pub.publish(cmd)

    global previous_x
    if pose.x >= 5.5 and previous_x <= 5.5:
        previous_x = pose.x 
        rospy.loginfo("Set color to red!")
        call_set_pen_service(255, 0, 0, 3, 0)
    elif pose.x < 5.5 and previous_x >= 5.5:
        previous_x = pose.x
        rospy.loginfo("Set color to green!")
        call_set_pen_service(0, 255, 0, 3,0)


if __name__ == '__main__':
    rospy.init_node("turtle_controller")
    rospy.wait_for_service("/turtle1/set_pen")   #等待Service
    pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)
    sub = rospy.Subscriber("/turtle1/pose", Pose, pose_callback)
    rospy.loginfo("Node has been started.")

    rospy.spin()

ctrl+s保存后退出，运行代码：

运行成功，我们下期再见！