ROS2 service/client node(+service)の作成

ROS2でプログラミングするために勉強したメモです．
ここでは，serviceの作成方法を書きます．

## 準備

### workspaceを作成する．

workspaceの作成に関する詳細は[ココ](https://qiita.com/Dorebom/items/dbcefa143b8bc7f2a9c0)．

#### 1. メインのROS2をunderlayに指定して，ターミナルに反映(source)する．

```
source /opt/ros/dashing/setup.bash
```

#### 2. `dev_ws`という名前のworkspaceを作成する．

```
mkdir dev_ws
mkdir dev_ws/src
cd dev_ws
```

#### 3. 環境の依存関係を解決する．

今回は何も`git clone`してないのでやる必要はないけど，忘れないように手順に組み込んどく．

```
# (注意)workspaceのルートである，dev_wsフォルダで実行
sudo rosdep install -i --from-path src --rosdistro dashing -y
```

### packageを作成する．

packageの作成に関する詳細は[ココ](https://qiita.com/Dorebom/items/2f99fdb16106ee96f066)．

#### 1. `src`フォルダに移動する

```
cd src
```

#### 2. `py_pubsub`という名前のpython-packageを作成

```
ros2 pkg create --build-type ament_python py_srvcli --dependencies rclpy example_interfaces
```

ここで，
- `py_srvcli`は，今回作るpackageの名前．
- `--dependencies`は，必要な依存関係の行をpackage.xmlに自動的に追加する．
- `example_interfaces`は，リクエストとレスポンスを構造化するために必要な`.srv`ファイルを含むパッケージ．

(都合で実行していないのでたぶんになるけど，)`py_srvcli`とは別のpackageである`example_interfaces`の`srv`フォルダに`AddTwoInts.srv`というファイルができていて，中身は以下の内容になっている．

```AddTwoInts.srv
int64 b
---
int64 sum
```

### 依存関係をpackageを構成するファイルたちに追加． 

- 今回は，先にまとめて依存関係を追加しておく．
    - publsher/sescriber nodeの説明では追加する説明を行ったが，ビルドする前までに記述しておけば良い．
    - UML設計の観点からすると，先にまとめて書くほうが自然だと思う．

package作成時に自動生成された`setup.py`，`setup.cfg`および`package.xml`ファイルを編集する．

#### `package.xml`を編集

以下を編集．

```package.xml
<description>Python client server tutorial</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>
```

`<buildtool_depend>ament_python</buildtool_depend>`の後に以下を追加．

```package.xml
<exec_depend>rclpy</exec_depend>
<exec_depend>std_msgs</exec_depend>
```
これは，コードが実行されるときに`rclpy`と`std_msgs`がこのpackageにとって必要であることを記述している．

##### `setup.py`を編集 

`package.xml`を編集したときと全く同じ内容で`setup.py`を編集

```setup.py
maintainer='Your Name',
maintainer_email='you@email.com',
description='Python client server tutorial',
license='Apache License 2.0',
```

後ろの方にエントリーポイントの記述を追加．

```setup.py
entry_points={
    'console_scripts': [
        'service = py_srvcli.service_member_function:main',
        'client = py_srvcli.client_member_function:main',
    ],
},
```

ここで，今回作るnodeは，以下の2つとなる．

- service node: service_member_function.py
- client node: client_member_function.py

##### `setup.cfg`を編集

自動的に生成されるので，特に編集することはない．

## service nodeの作成

#### 1. service nodeのpythonファイルを作成．

`dev_ws/src/py_srvcli`に移動し，以下を実行．

```
touch service_member_function.py
```

#### 2. 以下を内容として記述．

```service_member_function.py

# example_interfaces packageからサービスタイプ'AddTwoInts'をインポート
from example_interfaces.srv import AddTwoInts

import rclpy
from rclpy.node import Node


class MinimalService(Node):

    def __init__(self):
        super().__init__('minimal_service')
        # サービスを作成し，タイプ，名前，およびコールバックを定義．
        self.srv = self.create_service(AddTwoInts, 'add_two_ints', self.add_two_ints_callback)

    # サービスコールバックの定義
    # 要求データを受け取り，それを合計し，その合計を応答として返す．
    def add_two_ints_callback(self, request, response):
        response.sum = request.a + request.b
        self.get_logger().info('Incoming request\na: %d b: %d' % (request.a, request.b))

        return response


def main(args=None):
    # ROS 2 Pythonクライアントライブラリを初期化
    rclpy.init(args=args)
    # MinimalServiceクラスをインスタンス化してサービスノードを作成
    minimal_service = MinimalService()
    # ノードをスピンしてコールバックを処理
    rclpy.spin(minimal_service)

    rclpy.shutdown()


if __name__ == '__main__':
    main()
```

## client nodeの作成

#### 1. client nodeのpythonファイルを作成．

以下を実行．

```
touch client_member_function.py
```


#### 2. 以下を内容として記述．

```client_member_function.py
# sys.argvを使用して，リクエストのコマンドライン入力引数にアクセスするためにインポート
import sys

from example_interfaces.srv import AddTwoInts
import rclpy
from rclpy.node import Node


class MinimalClientAsync(Node):

    def __init__(self):
        super().__init__('minimal_client_async')
        # コンストラクター定義は，service nodeと同じタイプと名前のclient nodeを作成する．
        # タイプと名前は，clientとserviceが通信できるように一致する必要がある．
        self.cli = self.create_client(AddTwoInts, 'add_two_ints')
        # clientのタイプと名前に一致するserviceが利用可能かどうか，1秒に1回チェック．
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        # リクエスト定義
        self.req = AddTwoInts.Request()

    def send_request(self):
        self.req.a = int(sys.argv[1])
        self.req.b = int(sys.argv[2])
        self.future = self.cli.call_async(self.req)


def main(args=None):
    rclpy.init(args=args)

    minimal_client = MinimalClientAsync()
    minimal_client.send_request()

    while rclpy.ok():
        # ループは，futureシステムが実行されている限り，serviceからの応答があるかどうかを確認．
        # serviceが応答を送信した場合，結果をログメッセージに残す．
        rclpy.spin_once(minimal_client)
        if minimal_client.future.done():
            try:
                response = minimal_client.future.result()
            except Exception as e:
                minimal_client.get_logger().info(
                    'Service call failed %r' % (e,))
            else:
                minimal_client.get_logger().info(
                    'Result of add_two_ints: for %d + %d = %d' %
                    (minimal_client.req.a, minimal_client.req.b, response.sum))
            break

    minimal_client.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
```

## ビルドと実行

[publisher/subscriber node](https://qiita.com/Dorebom/items/47fb67e5e47a205f1395)と全く同様の手続き．

### ビルド

#### 1. dev_wsフォルダに移動し，以下を実行．
```
sudo rosdep install -i --from-path src --rosdistro <distro> -y
```

#### 2. `py_srvcli`packageをcolconでビルド．
```
colcon build --packages-select py_srvcli
```

### 実行

#### 1. 新しいターミナルを開き，dev_wsに移動して以下を実行．
```
. install/setup.bash
```

#### 2. service nodeを実行
```
ros2 run py_srvcli service
```

#### 3. client nodeを実行
同様に，別ターミナルで. install/setup.bashを行い，nodeを実行．

```
ros2 run py_srvcli client 2 3
```
ここで，
引数に`2`と`3`を与えた場合，client nodeは合計した`5`を受け取る．

##### 4. `Ctrl+C`で終了．



## 参考サイト
- https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Py-Service-And-Client/
- https://index.ros.org/doc/ros2/Tutorials/Custom-ROS2-Interfaces/