Robot Ignite3: ROS基础--服务(service)

ros services client

services vs actions vs topic

topic 因为话题对整个ros系统可见，当某种信息流需要总是能被其他节点获知，那么可以将其发布到话题中去
services: 服务是一种同步消息处理机制，只有当services中结果处理完毕返回值之后才会继续执行下面的程序。
actions: 是一种异步处理机制。就好比开了另外一个线程，我们可以将一个任务放到actions中去，在执行该任务的同时可以继续执行其他任务。

可见，当需要串行处理时使用services，可以并行处理时使用actions.

例子1

我们启动一个机械臂仿真环境

roslaunch iri_wam_aff_demo start_demo.launch

这个launch文件如下:

<launch>

  <include file="$(find iri_wam_reproduce_trajectory)/launch/start_service.launch"/>

  <node pkg ="iri_wam_aff_demo"
        type="iri_wam_aff_demo_node"
        name="iri_wam_aff_demo"
        output="screen">
  </node>

</launch>

可以看到其中包含了一个launch文件引用，也就是说我们可以使用include 标签实现一个launch文件启动多个节点。
接下来主要考察rosservice的使用方法及service类型数据结构。
使用命令查看service消息信息：

user ~ $ rosservice info /execute_trajectory
Node: /iri_wam_reproduce_trajectory
URI: rosrpc://ip-172-31-17-169:35175
Type: iri_wam_reproduce_trajectory/ExecTraj
Args: file

可以看出service也是依附于某一个节点的，具有URI和Args两个有意思的属性值。URI用于确定service的发出地址，Args是该项service所需要的参数。
需要注意的是，service虽然也有type类型，并且看上去跟msg挺像，但是并不是msg，也就是说使用rosmsg show是看不到具体结构的，这是因为service的type对应的是srv文件(service缩写)。srv文件跟msg文件都被放在xxx_msgs包中，因为他两的结构与功能确实很像。
查看service的type文件使用如下命令：

user:~$ rossrv show DeleteModel
[gazebo_msgs/DeleteModel]:
string model_name
---
bool success
string status_message

srv文件的结构如下：

REQUEST
---
RESPONSE

#example
string model_name
---
bool success
string status_message

REQUEST 是service所需要的传入参数，RESPONSE 为返回参数

例子2：删除仿真环境中模型

使用如下命令可以删除环境中模型

rosservice call /gazebo/delete_model [TAB]+[TAB]

其中[TAB]+[TAB] 意思是可以通过短暂时间内双击自动补全可输入参数名称。
要删除环境中物体首先需要知道环境中都有那些物体，这通过如下命令实现：

 rostopic echo /gazebo/model_states -n1

说白了就是输出一次模型状态topic的值，这样返回来一个字典，关键子name对应的list中就是环境中物体的名字。要删除哪个敲入哪个就可以。

那如何使用脚本删除物体呢？

 #! /usr/bin/env python

import rospy
from gazebo_msgs.srv import DeleteModel, DeleteModelRequest # Import the service message used by the service /gazebo/delete_model
import sys 

rospy.init_node('service_client') # Initialise a ROS node with the name service_client
rospy.wait_for_service('/gazebo/delete_model') # Wait for the service client /gazebo/delete_model to be running
delete_model_service = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) # Create the connection to the service
kk = DeleteModelRequest() # Create an object of type DeleteModelRequest
kk.model_name = "bowl_1" # Fill the variable model_name of this object with the desired value
result = delete_model_service(kk) # Send through the connection the name of the object to be deleted by the service
print result # Print the result given by the service called

其返回结果如下

success: True
status_message: DeleteModel: successfully deleted model

这里seccess跟status_message就是返回的值，注意到这些值是函数delete_model_service(kk) 的返回值。 而该函数对象为rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) 实例。

练习1：操作机械臂

操作机械臂任务目标:

给定各个关节广义坐标随时变化值，将其传递给ExecTrajectory service，从而实现特定轨迹

相应节点代码如下：

#! /usr/bin/env python

import rospy
import rospkg
from iri_wam_reproduce_trajectory.srv import ExecTraj,ExecTrajRequest
import sys

rospy.init_node('service_client')
rospy.wait_for_service('/execute_trajectory')#等待/execute_trajectory服务启动
exec_traj_srv=rospy.ServiceProxy('/execute_trajectory',ExecTraj)#建立与服务的连接代理
f=ExecTrajRequest()#创建输入参数对象

rospack=rospkg.RosPack()
f.file=rospack.get_path('iri_wam_reproduce_trajectory') + "/config/get_food.txt"#具体传入的目标
exec_traj_srv(f)#与服务端通讯，上传参数

notice: 每一个service消息类型(这里是ExecTraj），都有对应的Request或Response类(这里的ExecTraj)没有Response类，具体service结构可通过rossrv show命令查看。

ROS Service: make a service

前面讲如何接受使用其他节点发布的service，现在着重讲述如何发布service以及创建自己的service类型。

首先运行如下代码

#! /usr/bin/env python

import rospy
from std_srvs.srv import Empty, EmptyResponse # you import the service message python classes generated from Empty.srv.


def my_callback(request):
    print "My_callback has been called"
    return EmptyResponse() # the service Response class, in this case EmptyResponse
    #return MyServiceResponse(len(request.words.split())) 

rospy.init_node('service_server') 
my_service = rospy.Service('/my_service', Empty , my_callback) # create the Service called my_service with the defined callback
rospy.spin() # mantain the service open.

注意到service发布者的代码跟topic发布者代码很相似，rospy.Publisher与rospy.Service 输入参数对比[(消息名称，服务名称),(消息类型，服务类型),(回调函数，回调函数)]。

在运行上述代码后，可以使用rosservice list 命令查看新服务

$rosservice list
...
/my_service
/robot_state_publisher/get_loggers
/robot_state_publisher/set_logger_level
...

而在使用命令

rosservice call /my_service [Tab]+[Tab]#这里的tab在前面有解释

后，我们就call了上述service:/my_service ,回调函数将会执行，可在屏幕上看到My_callback has been called.

练习2：让小车走方形轨迹

在理想条件下，要让小车走方形轨迹，只需要输入所走形状及对应时间，就能自动计算出来线速度与角速度的变化值了。但是实际上，球形小车具有惯性，无法即刻停止。同时如果如上述那样直接输入速度指令/cmd_vel 由于对自身实际位置未知，实际就是一个开环控制，会导致误差进一步积累，从而实际轨迹可能偏离目标轨迹很多。而里程计/odom 对位置记录的准确度更高，如果能以其作为参考控制调整速度值，会获得更精确的路径。

1.开环版本

• 用len变量表示正方形边长，ang变量表示转角，v表示线速度，w表示角速度，控制频率为R，则有：
• 设起始点就是小车当前位置.那么直线部分控制次数为$len/v\cdot R=T_1 \cdot R$,转角部分$ang/w\cdot R=T_2\cdot R$
• 大致算法如下：

vel=0
for i in range(4):
    for j in range(T1*R):
        vel.lin.x=v
        pub.publish(vel)
        rate.sleep()
    vel.lin.x=0
    for k in range(T2*R):
        vel.ang.z=w
        pub.publish(vel)
        rate.sleep()
    vel.ang.z=0 

后来我把代码换成如下形式，发现小车只走直线，emmmmm，说明对于这个小车y方向与x方向默认是一个方向，需要转向。

#! /usr/bin/env python

import rospy
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from math import pi

if __name__=='__main__':
    rospy.init_node('move_square')
    pub=rospy.Publisher('/cmd_vel',Twist,queue_size=4)

    length=1.0
    angle=pi/4
    v=0.2
    w=0.1

    r=10
    t1r=int((length/v)*r)
    t2r=int((angle/w)*r)
    my_vel=Twist()
    my_vel.linear.x=0
    rate = rospy.Rate(r)


    for i in range(4):
        for j in range(t1r):
            my_vel.linear.x=v
            pub.publish(my_vel)
            rate.sleep()

        my_vel=Twist()
        pub.publish(my_vel)

        #for k in range(t2r):
        #    my_vel.angular.z=w
        #    pub.publish(my_vel)
        #    rate.sleep()

        #my_vel.angular.z=0.0
        #pub.publish(my_vel)

        for k in range(t1r):
            my_vel.linear.y=v
            pub.publish(my_vel)
            rate.sleep()

        my_vel=Twist()
        pub.publish(my_vel)
    rospy.spin()

2.使用/odom 作为反馈控制

一次失败的尝试
我首先将反馈控制的脚本写成如下这样:

#! /usr/bin/env python

import rospy
import tf
from geometry_msgs.msg import Twist, Point, Quaternion
from move_bb8 import MoveBB8

if __name__ == '__main__':
    try:
        MoveBB8()
        rospy.spin()

    except rospy.ROSInterruptException:
        rospy.loginfo(" navigation finished")

其中MoveBB8类如下:

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from math import radians, copysign, sqrt, pow, pi
import PyKDL

class MoveBB8():
    def __init__(self):
        rospy.init_node('square_odom',anonymous=False)
        rospy.on_shutdown(self.shutdown)
        rate=20
        r=rospy.Rate(rate)
        goal_distance=rospy.get_param("~goal_distance",1.0)
        goal_angle=radians(rospy.get_param("~goal_angle",90))
        linear_speed=rospy.get_param("~linear_speed",0.2)
        angular_speed=rospy.get_param("~angular_speed",0.7)
        angular_tolerance=radians(rospy.get_param("~angular_tolerance",2))

        self.cmd_vel=rospy.Publisher('/cmd_vel',Twist,queue_size=5)
        self.base_frame=rospy.get_param('~base_frame','/base_link')
        self.odom_frame=rospy.get_param('~odom_frame','/odom')
        self.tf_listener=tf.TransformListener()
        rospy.sleep(2)

        self.odom_frame='/odom'
        try:
            self.tf_listener.waitForTransform(self.odom_frame,'/base_link',rospy.Time(),rospy.Duration(1.0))
            self.base_frame='/base_link'
        except(tf.Exception,tf.ConnectivityException,tf.LookupException):
            rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
            rospy.signal_shutdown("tf Exception")


        pos=Point()

        for i in range(4):
            move_cmd=Twist()
            move_cmd.linear.x=linear_speed

            (pos,rotation)=self.get_odom()

            x_start=pos.x
            y_start=pos.y

            dist=0

            while dist<goal_distance and not rospy.is_shutdown():
                self.cmd_vel.publish(move_cmd)

                r.sleep()
                (pos,rotation)=self.get_odom()

                dist=sqrt(pow((pos.x-x_start),2)+
                        pow((pos.y-y_start),2))

            move_cmd=Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1.0)

            move_cmd.angular.z=angular_speed
            last_angle=rotation

            turn_angle=0

            while abs(turn_angle+angular_tolerance)< abs(goal_angle) and not rospy.is_shutdown():
                self.cmd_vel.publish(move_cmd)

                r.sleep()

                (pos,rotation)=self.get_odom()

                delta_angle=self.normalize_angle(rotation-last_angle)

                turn_angle +=delta_angle
                last_angle=rotation

            move_cmd=Twist()
            self.cmd_vel.publish((move_cmd))
            rospy.sleep(1.0)

        self.cmd_vel.publish(Twist())

    def get_odom(self):
        try:
            (trans,rot)=self.tf_listener.lookupTransform(self.odom_frame,self.base_frame,rospy.Time(0))
        except(tf.Exception,tf.ConnectivityException,tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans),self.quat_to_angle(Quaternion(*rot)))

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1.0)

    def quat_to_angle(self,quat):
        rot=PyKDL.Rotation.Quaternion(quat.x,quat.y,quat.z,quat.w)
        return rot.GetRPY()[2]

    def normalize_angle(self,angle):
        res=angle
        while res>pi:
            res -=2.0*pi
        while res< -pi:
            res +=2.0*pi

        return res

上面引入了一个新的库tf，重新编译后发现出现如下错误(之前没在package,CMakeLists里面加入tf，结果加了还是一样错误):

core service [/rosout] found
process[iri_wam_reproduce_trajectory-1]: started with pid [18477]
ERROR: cannot launch node of type [unit3_services/square_odom.py]: can't locate node [square_odom.py] in package [unit3_services]
[ INFO] [1522023755.133377718]: Waiting for action server to start.
[ INFO] [1522023755.160859301]: Ready execute trajectories

后来参考了如下问题
https://github.com/mavlink/mavros/issues/353
发现应该将py文件直接当成节点，不需要对应的launch来启动它

rosrun unit3_services square_odom.py

emmmm,竟然真的可以动了！！虽然动的跟理想效果差的不是一般的远。看来还需要进一步分析/odom 消息。

PS: can’t locate node 的问题可参见https://answers.ros.org/question/145801/cant-locate-node-in-package/#233094
我这里是因为py文件没有可执行权限，使用chmod修改一下就好了。