rospy-编写简单的服务端和客户端

最近一直在实习公司做课程，记录一些笔记和自己写的代码。基础知识和代码解释参考wiki、创客制造和其他博客，侵删。

背景知识

服务service

ROS中的通信方式有四种,主题topic、服务service、参数服务器param、动作库action。当我们需要直接与节点通信并以RPC（远程过程调用RemoteProcedureCall）方式获得应答时，将无法通过主题实现，而需要使用服务。Service通信是双向的,它不仅可以发送消息,同时还会有反馈。所以service包括两部分,一部分是请求方(Clinet),另一部分是应答方/服务提供方(Server)。这时请求方(Client)就会发送一个request，要等待server处理,反馈回一个reply,这样通过类似“请求-应答”的机制完成整个服务通信。

这
种通信方式的示意图如下:

NodeB是server(应答方),提供了一个服务的接口,叫做/Service,我们一般都会用string类型来指定service的名称,类似于topic。Node A向NodeB发起了请求,经过处理后得到了反馈。服务需由用户开发，节点并不提供标准服务。包含消息源代码的文件存储在srv文件中。像主题一样，服务关联一个以包中.srv文件名称来命名的服务类型。与其他基于ROS文件系统的类型一样，服务类型是包名和.srv文件名的组合。

时间

ROS具有内置的时间和持续的原始类型，在rospy由rospy.Time和rospy.Duration实现。

• 获取当前时间：rospy.Time.now(),rospy.get_rostime()两个是相同的。

1. now = rospy.get_rostime( )  

2. rospy.loginfo("Current time %i %i", now.secs, now.nsecs)  

获取当前时间：rospy.get_time()，获取浮点值的秒数

1. seconds= rospy.get_time( )  

使用模拟时钟的时间，直到在/clock上收到第一条消息，否则get_rostime() 会得到0值。0值意味客户端还不知道时间，需要区别对待，循环获取get_rostime()直到非0值。

• Sleepingand Rates（睡眠和速率）

rospy.sleep(duration)

duration可以是rospy.Duration或秒。会睡眠指定的时间。

1. # sleep for 10 seconds  

2. rospy.sleep(10.)  

3.   

4. # sleep for duration  

5. d = rospy.Duration(10, 0)  

6. rospy.sleep(d)  

rospy.sleep()如果出现错误，会抛出rospy.ROSInterruptException

rospy.Rate(hz)，可以保持一定的速率来进行循环。

1. r = rospy.Rate(10) # 10hz  

2. while not rospy.is_shutdown(): 

3.  #rospy.is_shutdown()用于检测程序是否退出，是否按Ctrl-C或其他

4.     pub.publish("hello")  

5.     r.sleep()  

Rate.sleep()出现错误，抛出rospy.ROSInterruptException

这两个函数经常用来实现机器人移动过程中的停顿等功能。

异常

异常类型

ROSException，ROS客户端基本异常类

ROSSerializationException，信息序列化的错误异常

ROSInitException，初始化ROS状态的错误异常

ROSInterruptException，操作中断的错误异常，经常在rospy.sleep()and rospy.Rate 中用到

ROSInternalException，rospy内部错误的异常(i.e.bugs).

ServiceException，ROS服务通讯相关错误的异常

更详细信息可以参考http://docs.ros.org/api/rospy/html/rospy.exceptions-module.html

我们只需要知道它的用法即可。

按照学习的知识，编了一个顾客点餐的KFC_demo.

任务描述

写一个简单的服务端(server)和客户端(client)，模拟点餐功能，加深对服务通信方式的理解。服务请求包括菜品，单价和数量，服务回复需支付的总价。

• 子任务1创建service_rospy_demo功能包

首先切换到之前通过创建catkin工作空间教程创建的catkin工作空间中的src目录下：

1. cd ~/catkin_ws/src    

现在使用catkin_create_pkg命令来创建一个名为service_rospy_demo的功能包，这个程序包依赖于std_msgs、roscpp和rospy：

1. catkin_create_pkg service_rospy_demo std_msgs rospy roscpp  

这将会创建一个名为service_rospy_demo的文件夹，这个文件夹里面包含一个package.xml文件和一个CMakeLists.txt文件，这两个文件都已经自动包含了部分你在执行catkin_create_pkg命令时提供的信息。

回到catkin工作空间下，编译：

1. cd ..  

2. catkin_make  

得到如下图所示的结果则编译通过。

配置环境变量:

1. sourcedevel/setup.bash //设置环境变量  

2. echo$ROS_PACKAGE_PATH //检查环境变量  

我们习惯把source~/catkin_ws/devel/setup.bash 命令追加到~/.bashrc文件中,这样每次打开终端,系统就会刷新工作空间环境，不需要每次都手动配置环境变量。可以通过

echo"source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc

命令来追加。

• 子任务2定义srv服务

  • 创建新的服务

在service_rospy_demo包下新建服务KFC_demo.srv内容如下，我们定义了服务请求的内容，菜单menu，单价price，和数量number，以及服务应答的内容账单（总价bill)。

1. string menu  

2. float64 price  

3. int64 number  

4. ---  

5. float64 bill  

• • 配置package.xml和CMakeLists.txt文件

创建好之后，我们需要确保srv文件能够被转换为c++/python或者其他语言的代码。打开service_rospy_demo文件夹里的package.xml文件（rosed、gedit、vim等都可以，rosed命令可以用来调用系统的编辑器直接打开ros相关的文件进行编辑，比起cd到相应目录下面打开更加快捷，可以设置它调用的编辑器，默认是vim），在构建阶段我们需要 "message_generation",而在运行时我们需要 "message_runtime"，所以需要在package.xml文件里添加相应的依赖项。确保里面存在这两行且去掉它们的注释：

1. <!--  <build_depend>message_generation</build_depend> -->    

2. <!-- <exec_depend>message_runtime</exec_depend> -->  

然
后修改CMakeLists.txt文件。ROS的catkin编译系统会将自定义的msg、srv(甚至还有action)文件自动编译构建,生成对应的C++、Python、LISP等语言下可用的库或模块。许多初学者错误地以为,只要建立了一个msg或srv文件,就可以直接在程序中使用,这是不对的,必须在CMakeLists.txt中添加关于消息创建、指定消息/服务文件那几个宏命令。

然后打开包目录下的CMakeLists.txt文件，在find_package调用中添加message_generation依赖，让你可以生成ROS信息。如下所示，括号里添加一项message_generation即可：

find_package(catkinREQUIRED COMPONENTS

roscpp

rospy

std_msgs

message_generation

)

在CMakeLists.txt文件，增加服务文件，取消#，并修改为：

add_service_files(
  FILES
  KFC_demo.srv
 )

在CMakeLists.txt文件，增加消息生成包，取消#，并修改为：
generate_messages(
  DEPENDENCIES
   std_msgs
 )

回到工作空间，catkin_make一下

现在，srv目录下的所有.srv文件，都会生成ROS支持的语言的源代码，C++的头文件在~/catkin_ws/devel/include/service_rospy_demo/里，Python基本在~/catkin_ws/devel/lib/python2.7/dist-packages/service_rospy_demo/srv/里，lisp文件则在~/catkin_ws/devel/share/common-lisp/ros/service_rospy_demo/srv/里。

• 子任务3 编写服务端节点

  • 编写KFC_server_demo.py文件

在service_rospy_demo/src/script文件夹下创建KFC_server_demo.py文件。内容代码如下：

1. #!/usr/bin/env python  

2. # coding:utf-8  

3. # 上面指定编码utf-8，使python能够识别中文  

4. import rospy  

5. from service_rospy_demo.srv import * #导入定义的服务  

6. def kfc_server_srv():  

7.     # 初始化节点，命名为 "kfc_server"  

8.     rospy.init_node("kfc_server")  

9.     #定义服务节点名称，服务的类型，处理函数  

10.     s = rospy.Service("kfc_order", KFC_demo, handle_order_function)  

11.   

12.     print "ready to order:"  

13.     rospy.spin()  

14. #定义处理函数  

15. def handle_order_function(req):  

16.     print "The guest wants %s %s. The unit price of the %s is %s."%(req.number,req.menu,req.menu,req.price)  

17.     #计算客人的账单  

18.     bill = req.price * req.number  

19.     return KFC_demoResponse(bill)  

20.   

21. # 如果单独运行此文件，则将上面定义的kfc_server_srv作为主函数运行     

22. if __name__=="__main__":  

23.     kfc_server_srv()  

• 代码分析

• #!/usr/bin/envpython

指定通过python解释代码，这句话是所有Python脚本必须有的。

• #coding:utf-8

上面指定编码utf-8，使python能够识别中文，如果不加这个，编译Python脚本时会出现警告或者错误。

• importrospy

导入rospy包，rospy是ROS的python客户端。参考rospyAPI接口

• fromservice_rospy_demo.srv import *

导入定义的服务，这里我们定义的服务为service_rospy_demo.srv。

• defkfc_server_srv():

定义函数

• rospy.init_node("kfc_server")

初始化节点，命名为 "kfc_server"，服务端必须是节点，所以必须有节点初始化语句，但客户端可以不是节点，所以不用必须加这个语句。

• s= rospy.Service("kfc_order", KFC_demo,handle_order_function) 

定义服务节点名称，服务的类型，处理函数。处理函数handle_order_function的具体实现将在后面进行定义。

• print "ready to order:"

当我们启动服务端节点后，将在终端看到readyto order: 的输出信息，加这句话的好处是可以清楚的知道我们的服务节点是否已经成功准备。

• rospy.spin()

保持节点运行，直到节点关闭。不像roscpp,rospy.spin不影响订阅的回调函数，因为回调函数有自己的线程。

• defhandle_order_function(req):

print "The guest wants %s %s. The unit price of the %s is%s."%(req.number,req.menu,req.menu,req.price)

#计算客人的账单

bill = req.price * req.number

return KFC_demoResponse(bill)

在任务一中，处理函数相对简单，只是简单的打印输出，在这里我们对请求部分的数据进行了简单处理，bill= req.price * req.number，账单等于请求输入的单价乘以数量。

• returnKFC_demoResponse("Hi%s"%req.name)

由服务生成的返回函数，这个函数是自动生成的。

• if__name__=="__main__":

kfc_server_srv()

如果单独运行此文件，则将上面定义的kfc_server_srv作为主函数运行

• • 用命令行的方式调用服务

在编写客户端Python脚本文件之前，我们先用命令行的方式调用服务。回到工作空间catkin_ws,编译代码。

$cd ~/catkin_ws
$ catkin_make

打开终端，运行roscore

打开新的终端，启动服务节点：

$rosrun service_rospy_demo KFC_server_demo.py

打开新终端，列出服务
$rosservice list
查看服务参数
$rosservice args /kfc_order
调用服务
$rosservice call /kfc_order hamburger 11.5 3

知
识点：

$rosservice list 列出目前正运行的服务

$rosservice args /service_name 查看服务的参数

$rosservice call /service_name service-args 从命令行调用服务。

在此例子中，hamburger11.5 3 是我们输入的参数，当然也可以输入别的值。

• 子任务4编写客户端节点

在scripts目录新建KFC_client_demo.py文件。代码如下：

1. #!/usr/bin/env python  

2. # coding:utf-8   

3. #sys模块包含了与Python解释器和它的环境有关的函数。

4. import sys  

5. import rospy  

6. from service_rospy_demo.srv import *  

7. def kfc_client_srv(m,p,n):  

8.     # 服务客户端不必是节点，所以不用调用rospy.init_node  

9.     # 等待有可用的服务 "kfc_order"  

10.     rospy.wait_for_service("kfc_order")  

11.   

12.     #调用服务求解结果并将结果返回   

13.     try:  

14.         # 定义service客户端，创建服务处理句柄.service名称为“kfc_order”，service类型为KFC_demo  

15.         kfc_client = rospy.ServiceProxy("kfc_order",KFC_demo)       

16.         resp = kfc_client(m,p,n)  

17.   

18.         #上述为简化风格，也可用正式的。  

19.         #resp = kfc_client.call(KFC_demoRequest(m,p,n)  

20.         return resp.bill  

21.   

22.     except rospy.ServiceException, e:  

23.         print "Service call failed: %s"%e  

24. def usage():  

25.         return "%s [m p n]"%sys.argv[0]  

26. 
    

27. # 如果单独运行此文件，则将上面函数kfc_client_srv()作为主函数运行  

28. if __name__=="__main__":  

29.   

30.     #判断客户端输入的参数是否符合条件   

31.   

32.     if len(sys.argv)==4:  

33.        m=str(sys.argv[1])  

34.        p=float(sys.argv[2])  

35.        n=int(sys.argv[3])  

36.        print "The guest wants %s %s. The unit price of the %s is %s."%(n,m,m,p)  

37.        bill = kfc_client_srv(m,p,n)  

38.     else:  

39.        print usage()  

40.        sys.exit(1)  

41.   

42.     print "The guest has to pay %s yuan."% bill 

• 代码分析

• #!/usr/bin/envpython

指定通过python解释代码，这句话是所有Python脚本必须有的。

• #coding:utf-8

上面指定编码utf-8，使python能够识别中文，如果不加这个，编译Python脚本时会出现警告或者错误。

• importsys

导入sys模块，sys模块包含了与Python解释器和它的环境有关的函数。本demo里我们将使用sys.argv方法外部输出参数。

• importrospy

导入rospy包，rospy是ROS的python客户端。

• fromservice_rospy_demo.srv import *

导入定义的服务，这里我们定义的服务为service_rospy_demo.srv

• defkfc_client_srv(m,p,n)

定义客户端函数。

• rospy.wait_for_service("kfc_order") 

等待接入服务节点。

• kfc_client= rospy.ServiceProxy("kfc_order",KFC_demo)

定义service客户端，创建服务处理句柄.service名称为“kfc_order”，service类型为KFC_demo

• resp= kfc_client(m,p,n)

上述为简化风格，也可用正式的。

resp= kfc_client.call(KFC_demoRequest(m,p,n)

• try:  

……

exceptrospy.ServiceException,e:

print"Service call failed: %s"%e

如果尝试运行try服务请求失败的话，将报告异常rospy.ServiceException。将打印Servicecall failed语句到终端显示。

• defusage():

return "%s [m p n]"%sys.argv[0]

sys模块中的argv变量通过使用点号指明——sys.argv——这种方法的一个优势是这个名称不会与任何在你的程序中使用的argv变量冲突。另外，它也清晰地表明了这个名称是sys模块的一部分。

sys.argv变量是一个字符串的列表，特别地，sys.argv包含了命令行参数的列表，即使用命令行传递给你的程序的参数。脚本的名称总是sys.argv列表的第一个参数。这个函数的作用是返回你输出入的参数。

• if__name__=="__main__":

iflen(sys.argv)==4:

m=str(sys.argv[1])

p=float(sys.argv[2])

n=int(sys.argv[3])

print "The guest wants %s %s. The unit price of the %s is%s."%(n,m,m,p)

bill = kfc_client_srv(m,p,n)

else:

print usage()

sys.exit(1)

print"The guest has to pay %s yuan."% bill

单独运行此文件时，先判断输出的参数是否符合要求，是否是三个参数。是的话将输出客户的请求信息并输出计算的bill值，不是的话（例如输入了四个参数），将输出当时输入的参数是什么，便于查看。

• 子任务5编译代码并测试结果

打开终端运行roscore

打开一个新的终端，运行服务端节点

1. rosrunservice_rospy_demo KFC_server_demo.py 

打开一个新的终端，运行客户端节点

1. rosrunservice_rospy_demo KFC_client_demo.py hamburger 17.5 3 

我们将得到返回的信息