Dynamic Reconfigureを使ってmove_baseのパラメータを実行中に動的に変更する

# はじめに
はじめまして、porizouです。
普段はROSを用いた自律移動ロボット開発の仕事をしています。

今さらROS,Navigationの記事を投稿するかどうか迷ったのですが、まだ使っている人が多そうだったので記事を書くことにしました。
ROSのNavigationStackを使用していて、move_baseのパラメータをノード実行中に動的に変更する必要があったので、`Dynamic Reconfigure`を使用して`rqt_reconfigure`でGUIから変更する場合と他ノードから変更する場合それぞれについて方法をまとめました。

# Dynamic Reconfigureについて
Dynamic Reconfigureとはノード実行中にrosparamの値を動的に変更することができるROSの機能です。
Dynamic Reconfigure自体の説明については以下ROS講座の記事がわかりやすいので、本記事では詳細は省略します。

> [ROS講座28 Dynamic Reconfigureを使う](https://qiita.com/srs/items/3adcc5898955a6aa1631)

move_baseのパラメータは一部を除きDynamic ReconfigureのServerを使って実装されているため、ノード実行中に動的に変更することができます。

# テスト環境
以下の環境で動作確認しました。Noeticを使っていますがMelodicでも同様の手順でできます。

|項目  |値  |
|---|---|
|CPU  |Ryzen5  |
|OS |Ubuntu20.04LTS|
|ROS  |Noetic  |ubuntu20.04



move_baseの動作確認にはおなじみのTurtlebot3のGazeboシミュレーションを使用しました。
https://github.com/ROBOTIS-GIT/turtlebot3_simulations

# 変更可能なパラメータ
以下Local Plannerにdwa_local_plannerを使用した場合のDynamic Reconfigureで変更可能なmove_base関連パラメータです。
Global Plannerやmove_base本体のパラメータ（controller_frequencyなど）は動的に変更することができません。


|名前  |説明  |
|---|---|
|/move_base/DWAPlannerROS  |dwa_local_plannerのパラメータ |
|/move_base/global_costmap/inflation_layer  |Global CostmapのInflation Layerのパラメータ |
|/move_base/global_costmap/obstacle_layer   |Global CostmapのObstacle Layerのパラメータ  |
|/move_base/local_costmap/inflation_layer  |Local CostmapのInflation Layerのパラメータ |
|/move_base/local_costmap/obstacle_layer   |Local CostmapのObstacle Layerのパラメータ|

追記：LocalPlannerにbase_local_plannerを使用した場合は/move_base/TrajectryPlannerROSという名前で定義されています。

#rqt_reconfigureから変更する方法
rqt_reconfigureを使うことでGUI画面からパラメータを動的に変更することができます。

move_baseが起動している状態で以下のコマンドで実行。

```bash:rqt_reconfigureの実行
$ rosrun rqt_reconfigure rqt_reconfigure 
```
こんな感じの画面が出てきます。

![Screenshot from 2020-12-30 14-07-18.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/2192dd2c-c062-dfe3-2dae-bcb0eef3fb58.png)
各パラメータの青いバーを動かすことでパラメータが変更できます。

rqt_reconfigureを使うことで、わざわざ設定ファイルを書き換えてノードを再起動しなくてもロボットを動かしながらmove_baseのパラメータ調整をすることができます。

# 他ノードから動的に変更する方法（Python）
以下Pythonノードからパラメータを動的に変更するサンプルプログラムです。

rvizからパラメータが変更されたことが確認しやすいようにLocalCostmapのinflation_layerのinflation_radiusの値を1秒ごとに交互に切り替えるプログラムを作成しました。

2行目でdynamic_reconfigure.clientをインポートし、6行目でClentを作成しています。
client.update_configureationの引数で変更したいパラメータ名と値を指定します。

以下スクリプトを実行するとinflation_radiusの値が1秒ごとに0.5と1.0に切り替わります。


```python:sample.py
import rospy
import dynamic_reconfigure.client

if __name__ == '__main__':
    rospy.init_node("dynamic_reconfigure_client")
    client = dynamic_reconfigure.client.Client("/move_base/local_costmap/inflation_layer/")
    while not rospy.is_shutdown():
        client.update_configuration({"inflation_radius": 0.5})
        rospy.sleep(1)
        client.update_configuration({"inflation_radius": 1.0})
        rospy.sleep(1)
        
    rospy.spin()

```

# 他ノードから動的に変更する方法（C++）
以下C++ノードからパラメータを動的に変更するサンプルプログラムです。動作は上と同様。
C++の場合、それぞれのConfigファイルをincludeしてくる必要がありました。

```cpp
#include <ros/ros.h>
#include <dynamic_reconfigure/client.h>
#include "costmap_2d/InflationPluginConfig.h"

int main(int argc, char **argv){
  ros::init(argc, argv, "dynamic_reconfigure_client");
  dynamic_reconfigure::Client<costmap_2d::InflationPluginConfig> client("/move_base/local_costmap/inflation_layer/");
  costmap_2d::InflationPluginConfig config;

  while(ros::ok())
  {
    config.inflation_radius = 0.5;
    client.setConfiguration(config);
    ros::Duration(1).sleep();

    config.inflation_radius = 1.0;
    client.setConfiguration(config);
    ros::Duration(1).sleep();
  }
  return 0;
}
```

# 実行結果
以下でTurtlebot3のNavigationを起動

```bash:move_baseの実行
$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch 
```
別のターミナルで上記サンプルプログラム（Python版）を実行　（Melodicの場合は "python sample.py"）

```bash:サンプルの実行
$ python3 sample.py
```

以下rvizの画面（GlobalCostmapなどは非表示にしています）


####inflation_radius : 0.5![rviz_screenshot_2020_12_30-16_21_52.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/738bf0f2-33a3-5357-b75b-bfd0b4f69b84.png)

####inflation_radius : 1.0![rviz_screenshot_2020_12_30-16_21_36.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/6a6a847b-8b18-ebe8-6071-0f90df516188.png)

これら画面が1秒ごとに交互に切り替わっているかと思います。

# 最後に
今回はmove_baseのパラメータを変更する例を紹介しましたが、同様にamclのパラメータも動的に変更することが可能です。
またプログラムからパラメータを変更するサンプルはpythonで書きましたが、後日C++版も追記予定です。

追記：C++版のサンプルプログラムを追加しました。（2021/1/13)

追記：今回テスト用に作成したスクリプトを以下githubに上げました。（2021/1/31)
https://github.com/porizou/dynamic_reconfigure_test

# 参考


dynamic_reconfigureチュートリアル
http://wiki.ros.org/dynamic_reconfigure/Tutorials/UsingTheDynamicReconfigurePythonClient

How to dynamically reconfigure the hokuyo_node from the command line or code.
http://wiki.ros.org/hokuyo_node/Tutorials/UsingDynparamToChangeHokuyoLaserParameters

ROSのノードのパラメータをノード実行中に変更できるDynamic Reconfigure
http://urusulambda.com/2018/04/19/ros%E3%81%AE%E3%83%8E%E3%83%BC%E3%83%89%E3%81%AE%E3%83%91%E3%83%A9%E3%83%A1%E3%83%BC%E3%82%BF%E3%82%92%E3%83%8E%E3%83%BC%E3%83%89%E5%AE%9F%E8%A1%8C%E4%B8%AD%E3%81%AB%E5%A4%89%E6%9B%B4%E3%81%A7/


https://blog.csdn.net/u014610460/article/details/79531616