ROS入門筆記（十一）：編寫與測試簡單的Service和Client (Python)
01 導讀
C++代碼必須通過編譯生成可執行文件；
python代碼是可執行文件，不需要編譯；
- 開發的功能包都放在catkin_ws這樣一個工作空間里；
- 新建的功能包取名為service_example，實現兩個整數求和為例，client端節點向server端節點發送a、b的請求，server端節點返回響應sum=a+b給client端節點；
- 通信網絡結構如圖所示：
服務編程流程
- 創建服務器
- 創建客戶端
- 添加編譯選項
- 運行可執行程序
02 功能包的創建
在catkin_ws/src/目錄下新建功能包service_example，並在創建時顯式的指明依賴rospy和std_msgs，依賴std_msgs將作為基本數據類型用於定義我們的服務類型。打開命令行終端，輸入命令：
$ cd ~/catkin_ws/src
#創建功能包topic_example時，顯式的指明依賴rospy和std_msgs，
#依賴會被默認寫到功能包的CMakeLists.txt和package.xml中
$ catkin_create_pkg service_example rospy std_msgs
03 在功能包中創建自定義服務類型
-
服務(srv): 一個srv文件描述一項服務。它包含兩個部分：請求和響應。
-
服務類型的定義文件都是以*.srv為擴展名，srv文件則存放在功能包的srv目錄下。
-
服務通信過程中服務的數據類型需要用戶自己定義，與消息不同，節點並不提供標准服務類型。
3.1 定義srv文件
srv文件分為請求和響應兩部分，由'---'分隔。
在功能包service_example目錄下新建srv目錄，然后在service_example/srv/目錄中創建AddTwoInts.srv文件
int64 a
int64 b
---
int64 sum
其中 a 和 b 是請求, 而sum 是響應。
3.2 在package.xml中添加功能包依賴
srv文件被轉換成為C++，Python和其他語言的源代碼：
查看package.xml, 確保它包含一下兩條語句:
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
如果沒有，添加進去。 注意，在構建的時候，我們只需要"message_generation"。然而，在運行的時候，我們只需要"message_runtime"。
3.3 在CMakeLists.txt添加編譯選項
第一步，增加message_generation
打開功能包中的CMakeLists.txt文件，利用find_packag函數，增加對message_generation的依賴，這樣就可以生成消息了。 你可以直接在COMPONENTS的列表里增加message_generation，就像這樣：
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
)
有時候你會發現，即使你沒有調用find_package,你也可以編譯通過。這是因為catkin把你所有的功能包都整合在一起，因此，如果其他的功能包調用了find_package，你的功能包的依賴就會是同樣的配置。但是，在你單獨編譯時，忘記調用find_package會很容易出錯。
第二步，刪掉#，去除對下邊語句的注釋:
找到如下代碼塊:
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
用你自己定義的srv文件名（AddTwoInts.srv）替換掉那些Service*.srv文件，修改好后的代碼如下：
add_service_files(
FILES
AddTwoInts.srv
)
第三步，msg和srv都需要的步驟
在CMakeLists.txt中找到如下部分:
# generate_messages(
# DEPENDENCIES
# # std_msgs # Or other packages containing msgs
# )
去掉注釋並附加上所有你消息文件所依賴的那些含有.msg文件的功能包（這個例子是依賴std_msgs,不要添加roscpp,rospy)，結果如下:
generate_messages(
DEPENDENCIES
std_msgs
)
原因：generate_messages的作用是自動創建我們自定義的消息類型 .msg與服務類型 .srv相對應的 .h，由於我們定義的服務類型使用了std_msgs中的int64基本類型，所以必須向generate_messages指明該依賴。
第四步，由於增加了新的消息，所以我們需要重新編譯我們的功能包：
目的：查看配置是否有問題
$ cd ~/catkin_ws
$ catkin_make -DCATKIN_WHITELIST_PACKAGES="service_example"
所有在msg路徑下的.msg文件都將轉換為ROS所支持語言的源代碼。生成的C++頭文件將會放置在~/catkin_ws/devel/include/service_example/。 Python腳本語言會在 ~/catkin_ws/devel/lib/python2.7/dist-packages/service_example/msg 目錄下創建。
04 查看自定義的服務消息
通過<功能包名/服務類型名>找到該服務，打開命令行終端，輸入命令：
$ source ~/catkin_ws/devel/setup.bash
$ rossrv show service_example/AddTwoInts
05 功能包的源代碼編寫
功能包中需要編寫兩個獨立可執行的節點，一個節點用來作為client端發起請求，另一個節點用來作為server端響應請求，所以需要在新建的功能包service_example/scripts目錄下新建兩個文件server.py和client.py，並將下面的代碼分別填入。
5.1 編寫Service節點（server.py）
將創建一個簡單的service節點("server")，該節點將接收到兩個整形數字，並返回它們的和。
如何實現一個服務器
- 初始化ROS節點；
- 創建Server實例；
- 循環等待服務請求，進入回調函數；
- 在回調函數中完成服務功能的處理，並反饋應答數據。
在service_example包中創建scripts / server.py文件：
#!/usr/bin/env python
from service_example.srv import AddTwoInts,AddTwoIntsResponse
import rospy
def handle_add_two_ints(req):
print "Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b))
#因為我們已經將服務的類型聲明為AddTwoInts，所以它會為您生成AddTwoIntsRequest對象（可以自由傳遞）
return AddTwoIntsResponse(req.a + req.b) # AddTwoIntsResponse由服務生成的返回函數
def add_two_ints_server():
rospy.init_node('add_two_ints_server') # 聲明節點為add_two_ints_server
#定義服務器節點名稱，服務類型，處理函數
#處理函數調用實例化的AddTwoIntsRequest接收請求和返回實例化的AddTwoIntsResponse
s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints)
print "Ready to add two ints."
rospy.spin() # 就像訂閱者示例一樣，rospy.spin()使代碼不會退出，直到服務關閉；
if __name__ == "__main__":
add_two_ints_server()
在~/catkin_ws/src/service_example下，讓節點可執行:
$ chmod +x scripts/server.py
5.2 編寫Client節點（client.py）
如何實現一個客戶端
- 初始化ROS節點；
- 創建一個Client實例；
- 發布服務請求數據；
- 等待Server處理之后的應答結果。
在service_example包中創建scripts / client.py文件，並在其中粘貼以下內容：
#!/usr/bin/env python
"""
導入sys模塊，sys.argv的功能是在外部向程序的內部傳遞參數。sys.argv(number)，number=0的時候是腳本的名稱
"""
import sys
import rospy
from service_example.srv import *
def add_two_ints_client(x, y):
# 等待接入服務節點
# 第二句是調用wait_for_service，阻塞直到名為“add_two_ints”的服務可用。
rospy.wait_for_service('add_two_ints')
try:
# 創建服務的處理句柄,可以像調用函數一樣，調用句柄
add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)
resp1 = add_two_ints(x, y)
return resp1.sum
#如果調用失敗，可能會拋出rospy.ServiceException
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def usage():
return "%s [x y]"%sys.argv[0]
if __name__ == "__main__":
if len(sys.argv) == 3:
x = int(sys.argv[1])
y = int(sys.argv[2])
else:
print usage()
sys.exit(1)
print "Requesting %s+%s"%(x, y)
print "%s + %s = %s"%(x, y, add_two_ints_client(x, y))
在~/catkin_ws/src/service_example節點可執行：
$ chmod +x scripts/client.py
代碼解析：
我們可以像普通函數一樣使用這個句柄並調用它：
resp1 = add_two_ints(x, y)
return resp1.sum
因為我們已經將服務的類型聲明為AddTwoInts，所以它會為你生成AddTwoIntsRequest對象(可以自由傳遞)。返回值是AddTwoIntsResponse對象。如果調用失敗，可能會拋出rospy.ServiceException，因此你應該設置適當的try/except塊。
06 功能包的編譯
我們使用CMake作為構建系統，是的，即使是Python節點也必須使用它。這是為了確保創建消息和服務時自動生成Python代碼。
$ cd ~/catkin_ws
$ catkin_make -DCATKIN_WHITELIST_PACKAGES="service_example"
07 測試service和client
7.1 運行Service
第一步，打開一個命令行終端：
$ roscore
第二步，打開第二個命令行終端：
# 用rosrun <package_name> <node_name>啟動功能包中的發布節點。
$ source ~/catkin_ws/devel/setup.bash # 激活catkin_ws工作空間（必須有，必不可少）
$ rosrun service_example server.py # (python版本)
你將看到如下的輸出信息:
Ready to add two ints. # Server節點啟動后的日志信息
7.2 運行Client
現在，運行Client並附帶一些參數：
打開第三個命令行客戶端：
$ source ~/catkin_ws/devel/setup.bash # 激活catkin_ws工作空間（必須有，必不可少）
$ rosrun service_example client.py 1 3 # (Python)
你將會看到如下的輸出信息:
# Client啟動后發布服務請求，並成功接收到反饋結果
Requesting 1+3
1 + 3 = 4
# Server接收到服務調用后完成加法求解，並將結果反饋給Client
Returning [1 + 3 = 4]
現在，你已經成功地運行了你的第一個Service和Client程序。