ROS講座28 Dynamic Reconfigureを使う

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
Dynamic Reconfigureはパラメーターを動的に変更するROSの仕組みです。この仕組みではGUIでパラメーターを変更しながらプログラムを実行することができます。ここまでもシミュレーションでros_controlのPIDゲインを変更するところで使用しています。以下のようにPID制御の各値をプログラム実行中にGUIから変更できます。

![reconfig1.png](https://qiita-image-store.s3.amazonaws.com/0/254442/48122b92-d31a-ed1a-bba0-f8de6bdec61b.png)

# 手順
以下のような4つの手順で設定します。

* cfgファイルの製作
* ROSノードの製作
* CMakeFileの設定
* 実行

# cfgファイルの製作
ROSパッケージ下のcfgディレクトリにcfgファイルを製作します。
このファイルで動的に変更する変数の型と名前を決定します。

# ソースコード

## dynamic_reconfigureのmsgの定義
cfgなどのdynamic_reconfigureのメッセージ系は`ros_lecture_msgs`に記述します。

### cfgファイル
```python:ros_lecture_msgs/cfg/Sample1.cfg
#!/usr/bin/env python
PACKAGE = "ros_lecture_msgs"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("int_param", int_t, 0, "An Integer parameter", 50, 0, 100)
gen.add("double_param", double_t, 0, "A double parameter", .5, 0, 1)
gen.add("str_param", str_t, 0, "A string parameter", "Hello World")
gen.add("bool_param", bool_t, 0, "A Boolean parameter", True)

size_enum = gen.enum([ gen.const("Small", int_t, 0, "A small constant"),
                  gen.const("Medium", int_t, 1, "A medium constant"),
                  gen.const("Large", int_t, 2, "A large constant"),
                  gen.const("ExtraLarge", int_t, 3, "An extra large constant") ],
                  "An enum to set size")

gen.add("size", int_t, 0, "A size parameter which is edited via an enum", 1, 0, 3, edit_method=size_enum)

exit(gen.generate(PACKAGE, "ros_lecture_msgs", "Sample1"))
``` 

注意点を細かく説明します。

* 2行目の`PACKAGE = "ros_lecture_msgs"`ではパッケージ名を入れます。
* `gen.add(...)`では動的に設定する変数を1つ1つ設定していきます。
* 最終行の`exit(gen.generate(PACKAGE, "ros_lecture_msgs", "Sample1"))`では第2引数はパッケージ名、第3引数はこのファイル名から拡張子を除いた物にします。

### 実行フラグの追加
上記の.cfgファイルは実行フラグを立てる必要があります。

```shell:実行フラグの追加
roscd ros_lecture_msgs/cfg
chmod a+x sample1.cfg
```

### ビルドの設定

```ros_lecture_msgs/CMakeLists.txtに追加
# パッケージの依存を追加
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  dynamic_reconfigure　#この行を追加
)

# configファイルを追加
generate_dynamic_reconfigure_options(
  cfg/Sample1.cfg
)
```

## dynamic_reconfigureを使う側の記述

### ROSノード

```c++:adv_lecture/src/reconfigure1.cpp
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <ros_lecture_msgs/Sample1Config.h>

void callback(ros_lecture_msgs::Sample1Config& config, uint32_t level)
{
  ROS_INFO("Reconfigure Request: %d %f %s %s %d", config.int_param, config.double_param, config.str_param.c_str(),
           config.bool_param ? "True" : "False", config.size);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "reconfigure");
  dynamic_reconfigure::Server<ros_lecture_msgs::Sample1Config> server;
  dynamic_reconfigure::Server<ros_lecture_msgs::Sample1Config>::CallbackType f;
  f = boost::bind(&callback, _1, _2);
  server.setCallback(f);
  ros::spin();
  return 0;
}
```

* cfgで書いた設定をincludeします。`#include <ros_lecture_msgs/Sample1Config.h>`の様に「(パッケージ名)/(cfgファイル名)Config.h」という名前のファイルをincludeします。
* `  dynamic_reconfigure::Server<ros_lecture_msgs::Sample1Config> server;`の様にしてdynamic_reconfigureのserverをインスタンス化します。また`setCallback()`で値の変更が起きた時に呼ばれるcallback関数を登録します。

### ビルドの設定
元のCMakeListに追加する内容を下に示します。

```adv_lecture/CMakeLists.txtに追加
# パッケージの依存を追加
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  dynamic_reconfigure　#この行を追加
)

# c++ ROSノードの追加
add_executable(reconfigure1 src/reconfigure1.cpp)

# c++ ros_lecture_msgsの子会作成したReconfigureへの依存の追加
add_dependencies(reconfigure1 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

# c++ ROSノードのライブラリの追加
target_link_libraries(reconfigure1
  ${catkin_LIBRARIES}
)
```

## launch

```c++:adv_lecture/launch/dynamic_reconfigure.launch
<launch>
    <node name="reconf1" pkg="adv_lecture" type="adv_reconfigure" output="screen"/>
    <node name="reconf2" pkg="adv_lecture" type="adv_reconfigure" output="screen"/>
</launch>
```

同じnodeを2つ立ち上げます。

# 実行
各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

```shell:実行
roslaunch adv_lecture dynamic_reconfigure.launch 
```

```shell:rqtの実行
rosrun rqt_reconfigure rqt_reconfigure 
```

![adv_reconf.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/56ad9d43-1cf8-6835-2a08-121af0ffd790.png)

GUIで値の変更をすると以下のような出力が出ます。

```text
[ INFO] [1588428717.061003188]: Reconfigure Request: 73 0.500000 Hello World True 1
```

# 参考
[ROSwiki](http://wiki.ros.org/dynamic_reconfigure/Tutorials)
日本語版は古かったり不十分であったりします。

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)