ROS1 PythonでServiceのコードを書いてみる

はじめに

初めてROSのサービスコードを書くことになったので，サービスの追加手順やPythonコードの書き方をメモしておきたいと思います．

チュートリアルを丁寧になぞる

ROS公式チュートリアルのページにサービスについてのチュートリアルがあるのですが，
他のチュートリアルを読まないといけなかったり，書いてないところがあったりするので，
チュートリアルを丁寧になぞって理解していきたいと思います．

そもそもサービスって何？

ROSのサービスとは，ノードが他のノードとお互いに通信するための方法です．
一般的に使用されるトピック通信とは異なり,ノード(クライアント側)が他のノード(サーバ側)にリクエストを送り,他のノードが処理をした後のレスポンスを受取るような通信を行います．
サーバとクライアントの1対1通信になっており，同期・非同期の通信を行うことができるそうです．

実践

では，実際にチュートリアルをやっていきます．
まずは，ワークスペースを作成します．

$ mkdir -p catkin_ws_test/src
# src内へ移動
$ cd catkin_ws_test/src/

ROSパッケージを作る

ワークスペースの中にROSパッケージを作成します．
この際に，message_generationを含めたパッケージを作成します．

# パッケージ作成コマンド
$ catkin_create_pkg beginner_tutorials std_msgs rospy roscpp message_generation
# 作成したパッケージに入る
$ cd beginner_tutorials/
# Python用ディレクトリの作成
$ mkdir scripts
# サービス用のディレクトリ作成
$ mkdir srv
# ここまでの設定でビルドできるか確認
$ cd ../..
$ catkin build

ビルドした結果，エラー無く終わればワークスペースの作成完了です．

[build] Found 1 packages in 0.0 seconds.
[build] Package table is up to date.
Starting  >>> beginner_tutorials
Finished  <<< beginner_tutorials         [ 0.2 seconds ]
[build] Summary: All 1 packages succeeded!
[build]   Ignored:   None. 
[build]   Warnings:  None. 
[build]   Abandoned: None. 
[build]   Failed:    None. 
[build] Runtime: 0.2 seconds total.  

Serviceの追加

サービスに使用するsrvファイルを追加します.
このsrvファイルには送受信の変数フォーマットを記載します．

# パッケージまで移動する
$ roscd beginner_tutorials/
# サービスのフォーマットを記載するファイルを作成する
$ touch srv/AddTwoInts.srv

今回のチュートリアルの内容では、a,bの2つの整数を送信し，a,bの合計である整数を受け取るプログラムを書きます．
---の上側に送信する変数を記載し，下側に受信する変数を記載します．

AddTwoInts.srv

int64 a
int64 b
---
int64 sum

CMakeLists.txtの編集

作成したsrvファイルをROSで使用するためにCMakeLists.txtを編集し，ビルドします．
まずは，パッケージ内のCMakeLists.txtをエディタで開き，下記のadd_service_files,generate_messages部分のコメントアウトを解除して、AddTwoInts.srvを追加します．

CMakeLists.txt

## Generate services in the 'srv' folder
add_service_files(
  FILES
  AddTwoInts.srv
)
 
## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs
)

一旦、ビルドしてサービスが読み込まれているか確認します．

$ catkin build
$ rossrv show beginner_tutorials/AddTwoInts

rossrv showの結果、以下のようにsrvファイルで設定した項目が表示されれば、読み込まれています．

~/catkin_ws_test$ rossrv show beginner_tutorials/AddTwoints
int64 a
int64 b
---
int64 sum

Pythonコードの追加

処理の内容を記載します．

サーバ側

サーバ側には，受け取ったリクエストを処理して返す関数を用意します．

add_two_ints_server.py

#!/usr/bin/env python3

# srvディレクトリ内のファイルを読み込めるようにする
from beginner_tutorials.srv import *
import rospy

# 合計値を計算して返す関数
def handle_add_two_ints(req):
    print(f"Returning [{req.a} + {req.b} = {(req.a + req.b)}]")
    return AddTwoIntsResponse(req.a + req.b)

# main関数
def add_two_ints_server():
    rospy.init_node('add_two_ints_server')
    # サービスを起動する[引数：サービス名, サービスフォーマット, 使用する関数名]
    s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints)
    print("Ready to add two ints.")
    rospy.spin()

if __name__ == "__main__":
    add_two_ints_server()

クライアント側

クライアント側にはサーバーに対してリクエストを送り，返ってきたレスポンスを処理する関数を作成します．

add_two_ints_client.py

#!/usr/bin/env python3

import sys
import rospy
from beginner_tutorials.srv import *

def add_two_ints_client(x, y):
    rospy.wait_for_service('add_two_ints')
    try:
        # add_two_intsのサービスが使用可能になるまで待機する関数
        add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)
        # サーバー側の処理を普通の関数のように呼び出せる
        resp1 = add_two_ints(x, y)
        return resp1.sum
    except rospy.ServiceException as e:
        print(f"Service call failed: {e}")

def usage():
    return(f"{sys.argv[0]} [x y]")

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(f"Requesting {x} + {y}")
    print(f"{x} + {y} = {add_two_ints_client(x, y)}")

処理を実行する

ROSの処理を実行するために，roscoreを起動し，サーバ側，クライアント側とノードを実行しました．

# 1つ目のターミナル
$ roscore
# 2つ目のターミナル：サーバ起動
$ cd catkin_ws
$ source ./devel/setup.bash
$ rosrun beginner_tutorials add_two_ints_server.py
# 3つ目のターミナル：クライアント側の起動
$ cd catkin_ws
$ source ./devel/setup.bash
$ rosrun beginner_tutorials add_two_ints_client.py 4 5

実行すると、サーバ側のターミナル表示は、以下のようになります。

server side

Ready to add two ints.
Returning [4 + 5 = 9]

クライアント側のターミナル表示は、以下のようになります。

client size

Requesting 4 + 5
4 + 5 = 9

ざっくりとした処理の流れは以下の表の通り。

サーバ側	クライアント側
起動
待機中	起動
待機中	(4,5)のデータを送信
(4,5)のデータを受取り， 足し算結果の9を返す	待機中
待機中	9を受取り表示する

起動順所をクライアント側から起動しても,サーバ側が起動されるまで待機するので正常に実行されます．

以上，サービスのチュートリアルを実行してみました．

参考サイト