ROS2の勉強　第5弾：parameterを使う

# #プログラミング ROS2< parameterを使う >

##はじめに
ROS2を難なく扱えるようになることが目的である．その第5弾として，「parameterを使う」を扱う．

##環境
#####仮想環境
|||
|:-:|:--|
|ソフト|VMware Workstation 15|
|実装RAM|3 GB|
|OS|Ubuntu 64 ビット|
|isoファイル|ubuntu-20.04.3-desktop-amd64.iso|

#####コンピュータ
|||
|:-:|:--|
|デバイス|MSI|
|プロセッサ|Intel(R) Core(TM) i5-7300HQ CPU @ 2.50GHz 2.50GHz|
|実装RAM|8.00 GB (7.89 GB 使用可能)|
|OS|Windows (Windows 10 Home, バージョン：21H1)|

#####ROS2
|||
|:-:|:--|
|Distribution|foxy|

## parameter
### 概要
https://docs.ros.org/en/foxy/Tutorials/Parameters/Understanding-ROS2-Parameters.html

"You can think of parameters as node settings"
「parameterはノードの設定として考えられる」とある．実際にlaunchファイルを作成する際にノード立ち上げの部分でのタグにparameterを設定するものが存在した．一度ここで，第2弾で作ったlaunchファイルを事項したとき，つまりturtlesim_nodeとturtle_teleop_keyを立ち上げた時のparameterを確認してみる．

![params.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/432d0eb8-d906-9f6a-dcd8-c4d5f4abb224.png)

今回はturtlesimの背景の部分を操作してみる．

### コマンドでの操作
プログラムから操作する前にまず，ros2のparamコマンドについて触れておく．

#### param list
```bash:command
ros2 param list
```
```bash:result
ros2 param list
```

#### param get
```bash:command
ros2 param get /turtlesim/turtlesim/background_r
```
```bash:result

```

#### ros2 set
```bash:command
ros2 param set /turtlesim/turtlesim/background_r 150
```

#### ros2 dump
```bash:command
ros2 param get /turtlesim/turtlesim/background_r
```
```bash:result
Saving to:  ./turtlesim__turtlesim.yaml
```
```yaml:result
/turtlesim/turtlesim:
  ros__parameters:
    background_b: 0
    background_g: 255
    background_r: 150
    use_sim_time: false
```

#### ros2 load
```bash:command
ros2 param load /turtlesim/turtlesim ./turtlesim__turtlesim.yaml
```
```bash:result
Set parameter background_b successful
Set parameter background_g successful
Set parameter background_r successful
Set parameter use_sim_time successful
```

###全体の流れ
<iframe width="560" height="315" src="https://www.youtube.com/embed/EgF5FJilL1E" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>


### プログラム: turtlesimの背景を変える
#### parameterを操作するサービス
parameter操作にserviceを使うにあたって，どのようなserviceを使うのかということを知っておく必要がある．そこで，parameterを操作するserviceを``ros2 interface list``で確認した．その時の様子を以下に示す．今回の対象を赤線で示している．

![rcl_interfaces.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/0ccdeb02-2310-7192-7ee1-b87cd7b38c03.png)

そうやら，rcl_interfaceが提供するserviceでparameterを操作することができそうである．

<details><summary>**turtlesimの背景を変えるプログラム**</summary><div>

```python:bg_paramTurtle.py
import rclpy
from rclpy.node import Node

from rcl_interfaces.msg import Parameter, ParameterType, ParameterValue # msg for treating interfaces(params)
from rcl_interfaces.srv import GetParameters                            # srv to get parameters
from rcl_interfaces.srv import SetParameters                            # srv to set parameters     # you can write "from rcl_interfaces.srv import GetParameters, SetParameters"

class Bg_Param(Node):
    def __init__(self):
        super().__init__('cg_turtle')   # take over __init__ from Node class ... 'cg_turtle' is node_name
    
    def setParam(self, red, green, blue):
        client = self.create_client(
            SetParameters,
            '/turtlesim/turtlesim/set_parameters'#.format_map(locals())
        )

        ready = client.wait_for_service(timeout_sec=5.0)

        if not ready:
            raise RuntimeError('Wiat for service timed out')    # generate runtime exception
        
        req_for_set = SetParameters.Request()

        # setting for red
        param = Parameter()                                 # prepare instance to access values
        param.name = "background_r"                         # name
        param.value.type = ParameterType.PARAMETER_INTEGER  # type
        param.value.integer_value = red                     # value
        req_for_set.parameters.append(param)                # add this to request_list to request

        # setting for green
        param = Parameter()                                 # prepare instance to access values
        param.name = "background_g"                         # name
        param.value.type = ParameterType.PARAMETER_INTEGER  # type
        param.value.integer_value = green                   # value
        req_for_set.parameters.append(param)                # add this to request_list to request

        # setting for blue
        param = Parameter()                                 # prepare instance to access values
        param.name = "background_b"                         # name
        param.value.type = ParameterType.PARAMETER_INTEGER  # type
        param.value.integer_value = blue                    # value
        req_for_set.parameters.append(param)                # add this to request_list to request

        future = client.call_async(req_for_set)             # call service with request Asynchronously

    def getParam(self):
        client = self.create_client(
            GetParameters,                          # type
            '/turtlesim/turtlesim/get_parameters'   # service name
        )

        ready = client.wait_for_service(timeout_sec=5.0)

        if not ready:
            raise RuntimeError('Wiat for service timed out')    # generate runtime exception
        
        req_for_get = GetParameters.Request()

        req_for_get.names = ["background_r", "background_g", "background_b"]    # set names for parameters to request
        
        future = client.call_async(req_for_get)         # call service with request Asynchronously
        rclpy.spin_until_future_complete(self, future)  # loop until receive the response

        response = future.result()
        if response is None:
            e = future.exception()
            raise RuntimeError(                                 # display error message clearly
                'Exception while calling service of node'
                "'{args.node_name}':{e}".format_map(locals())
            )
        
        print('background_r:', response.values[0].integer_value)
        print('background_g:', response.values[1].integer_value)
        print('background_b:', response.values[2].integer_value)

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

    param_client = Bg_Param()

    param_client.setParam(0, 10, 100)   # set red, green, blue
    param_client.getParam()             # get parameter

if __name__ == '__main__':
    main()
```
</details></div>

またしてもpythonで見慣れないものがあったため，記載しておく．

### 見慣れなかったもの
##### 1つ目: format_map()
```python:
~.format_map()
```
辞書型の配列を文字列に組み込むようなものである．辞書型の配列を扱うのと同じようにキーで指定できる．使用例を以下に示す．

```python:使用例
s = {'a':0,'b':1,'c':2}
print('{a}, {b}, {c}'.format_map(s))
```
```bash:結果
0, 1, 2
```
``format``と非常に似ているが，``format_map``は辞書型で扱え，文字列に組み込むのも順番ではなく，キーで指定することができるため，直感的に文字列に埋め込むことができる．

##### 2つ目: locals()
```python:2つ目
locals()
```
これは，関数のような閉じたところでの活用が見られる．簡単に言うと，閉じられた中で用いられた変数などを自動ですべて辞書型配列にまとめることができる．使用例を以下に示す．

```python:使用例
a = 1
def test(b=2, c=3):
    d = True
    print(locals())
test()
```
```bash:結果
{'b': 2, 'c': 3, 'd': True}
```
何も辞書型配列に格納するような操作はしていないが，``locals()``を使うだけで変数とその中身を辞書型配列にまとめることができる．最後に実際にプログラム内で使われていたように，``format_map()``と``locals()``を両方用いた例を示してみる．

##### format_mapとlocalsの利用
```python:format_mapとlocals
a = 1
def test(b=2, c=3):
    d = True
    print('TEST_VALUE : {b}, {c}, TEST_BOOL : {d}'.format_map(locals()))
test()
```
```bash:結果
TEST_VALUE : 2, 3, TEST_BOOL : True
```

確かにこれも便利かもしれないが，これぐらいの程度であればf文字列でもよいかなと個人的には思う．ただ，``locals()``はうまく通ことができれば非常な便利なツールとなりそうである．

### 実行
#### setup.pyの編集
基本的にビルドに関する情報はsetup.pyの中に記述されている．

<details><summary>**setup.pyの編集**</summary><div>

```python:setup.py
from setuptools import setup

package_name = 'param_lesson'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Yuya Shimizu',
    maintainer_email='yuya@example.com',
    description='a package for practice params',
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'bg_color = param_lesson.bg_paramTurtle:main'
        ],
    },
)
```
</details></div>

#### ビルド
```bash:ビルド
cd ~/ros2_ws
colcon build
```

#### 実行コマンド
```bash:terminal_1
ros2 launch turtlesim_test myTurtlesim.launch.py
```

```bash:terminal_2
ros2 run param_lesson bg_color
```
#### 実行の様子
<iframe width="560" height="315" src="https://www.youtube.com/embed/R6MJTAVaxFE" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

##感想
ROS1でもparameterはあったが，実はあまり勉強できていなかった．今回を通して，parameterを学ぶことができた．またプログラム側からparameterが操作できるようになり，条件に応じてparameterを変えることも自在となった．今回は実装していないが，例えば，カメが壁に近づいたら背景を赤に近づけるとか，複数のカメや障害物を配置して，衝突しそうになったり衝突したりする状況に応じて背景を変えることもできるはずで，ゲームのようなことも簡単にできると思う．次回は，アクションを使うプログラムの作成について学ぶ．

##参考
- ロボットプログラミングROS2入門　玉川大学　岡田浩之　著，科学情報出版株式会社