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

# はじめに

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

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

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

https://wiki.ros.org/ja/ROS/Tutorials/WritingServiceClient%28python%29

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

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

# 実践

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

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

## ROSパッケージを作る

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

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

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

```bash
[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ファイルには送受信の変数フォーマットを記載します．

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

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

```text:AddTwoInts.srv
int64 a
int64 b
---
int64 sum
```

## CMakeLists.txtの編集

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

```cmake: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
)
```

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

```bash
$ 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コードの追加

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

### サーバ側

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

```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()
```

### クライアント側

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

```python: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を起動し，サーバ側，クライアント側とノードを実行しました．

```bash
# 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
```

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

```bash:server side
Ready to add two ints.
Returning [4 + 5 = 9]
```

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

```bash:client size
Requesting 4 + 5
4 + 5 = 9
```

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

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

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

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

# 参考サイト

https://hara-jp.com/_default/ja/Topics/ROSE381AEE382B5E383BCE38393E382B9E381A8E38391E383A9E383A1E383BCE382BF.html#:~:text=ROS%E3%82%B5%E3%83%BC%E3%83%93%E3%82%B9,%E8%A1%8C%E3%81%86%E3%81%93%E3%81%A8%E3%81%8C%E3%81%A7%E3%81%8D%E3%81%BE%E3%81%99%E3%80%82