ROS2講座16 ROS2 Parameterを使う1

# 環境

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

| 項目 |	値 |
|:--|:--|
| CPU | Core i5-8250U |
| Ubuntu | 22.04 |
| ROS2 | Humble |

# 概要

## 変化点

ROS1からROS2の変更でROSパラメーターには大幅な変更が入っています。

* 保持主体の変更
    * ROS1では'/node_A/parameter'のような名前のパラメーターでも、その値はroscoreが持っていて、各ノードはroscoreが持っているパラメーターを読んだり、変更のリクエストを送ったりしていました。
    * ROS2では各ノード毎にパラメーターの実態を持ちます。もちろんROS1の時のようにほかのノードからパラメーターを読んだり、変更のリクエストを送ることが出来ます。
* 事前宣言の必須化
    * ROS1ではどのノードからでも好きな時に好きなキーのパラメーターを書くことが出来ました。
    * ROS2ではあらかじめ`declare_parameter()`で宣言したパラメーターしか使えません。宣言していないパラメーターを読み書きしようとするよ削除になります。
* 辞書型の削除
    * ROS2ではパラメーターとして使えるものとしてbool、int64、float64、stringとこれら4つの配列、byte配列の9つの型しか使えません。ROS1の時のようにパラメーターの値を辞書型として取得することはできなくなり、これによりパラメーターを厳格に運用できます。一方、例えばパラメーターでプラグインを指定して、プラグインごとに異なるパラメーターを指定するといったROS1でよく行われていた実装をするにはテクニックが必要となります。
* 変更時のコールバックの追加
    * ROS1ではパラメーターが変更されたタイミングを知ることが出来ないために、ポーリングでパラメーターの値を常に取得するという必要がありました。
    * ROS2ではパラメーターの変更時にフックするコールバックを登録することもできます。また自ノードで管理しているパラメーターについては、変更時のcallbackで拒否の返答をすることにより、パラメーター値の変更requestを拒否することが出来ます。

## 接続

パラメーターの通信はすべてserviceとtopicで行われています。

パラメーターサーバー（≒ROSノード）毎に以下の6つのサービスが生成されます（param_serverはROSノード名になります）。ROSノードが持っているパラメーターにアクセスするときはこれらのserviceを用います。

* /param_server/describe_parameters
* /param_server/get_parameter_types
* /param_server/get_parameters
* /param_server/list_parameters
* /param_server/set_parameters
* /param_server/set_parameters_atomically

またこれとは別にトピックが1つ生成されます。ROSノードが持つパラメーターが変更されたときに、その通知を行います。これによりあるROSノードのパラメーターの変更を他のノードでも検知できます。

* /parameter_events

以後のパラメーターの操作関数は内部でこれらのservice/topicを使用しています。

```plantuml
@startuml
hide empty members

object param_client
object param_server

param_client-->param_server: /param_server\n/set_parameters
param_client-->param_server: /param_server\n/get_parameters
param_client<..param_server: /parameter_events

@enduml
```

# ROSパラメーターを使用するノードの記述

c++のノードでパラメーターを扱うソースコード例を提示します。

## サーバー側

```c++:rclcpp_param_lecture/src/server.cpp
#include <rclcpp/rclcpp.hpp>

using namespace std::chrono_literals;
using namespace std::placeholders;

class ParamServer : public rclcpp::Node
{
public:
  ParamServer()
  : Node("param_server")
  {
    this->declare_parameter("int_param", 1);
    parameter_handle_ =
      this->add_on_set_parameters_callback(
      std::bind(
        &ParamServer::parametersCallback, this,
        std::placeholders::_1));
    interval_timer_ =
      this->create_wall_timer(2000ms, std::bind(&ParamServer::onIntervalTimer, this));
  }

private:
  void onIntervalTimer()
  {
    int param = this->get_parameter("int_param").as_int();
    RCLCPP_INFO(this->get_logger(), "value %d", param);
  }

  rcl_interfaces::msg::SetParametersResult parametersCallback(
    const std::vector<rclcpp::Parameter> & parameters)
  {
    rcl_interfaces::msg::SetParametersResult result;
    result.successful = true;
    result.reason = "success";
    for (const auto & parameter : parameters) {
      if (parameter.get_name() == "int_param" &&
        parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
      {
        int param = parameter.as_int();
        if (0 <= param) {
          RCLCPP_INFO(this->get_logger(), "changed: %s %d", parameter.get_name().c_str(), param);
        } else {
          RCLCPP_INFO(this->get_logger(), "reject: %s %d", parameter.get_name().c_str(), param);
          result.successful = false;
          result.reason = "negative value";
          return result;
        }
      }
    }
    return result;
  }

private:
  rclcpp::TimerBase::SharedPtr interval_timer_{nullptr};
  OnSetParametersCallbackHandle::SharedPtr parameter_handle_{nullptr};
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto param_server = std::make_shared<ParamServer>();
  rclcpp::spin(param_server);
  rclcpp::shutdown();
  return 0;
}
```

* `this->declare_parameter("int_param", 1);`でこの`param_sever`ノード内の`int_param`というパラメーターをint64型、デフォルト値1で宣言しています。
    * `get_parameter("int_param")`でこのパラメーターの値を読み込みます。
* `add_on_set_parameters_callback()`ではパラメーターが変更されたときのcallbackに`parametersCallback`を登録しています。この関数の実行はオプションで、必須ではありません
    * このROSノードが持つパラメーターに(自ノード内からも含め)変更のリクエストがあった時にcallbackが呼ばれます。この関数がresult.successful==falseを返した時は、そのリクエストは無効となりパラメーターの更新がされません。今回の例では負の値に変更することはできません。

## クライアント側

```c++:rclcpp_param_lecture/src/client.cpp
#include <rclcpp/rclcpp.hpp>
#include <termios.h>

using namespace std::chrono_literals;
using namespace std::placeholders;

class ParamClient : public rclcpp::Node
{
public:
  ParamClient()
  : Node("param_client")
  {
    param_client_ = std::make_shared<rclcpp::AsyncParametersClient>(this, "param_server");
    parameter_sub_ =
      param_client_->on_parameter_event(
      std::bind(
        &ParamClient::ParameterEventCallback, this,
        std::placeholders::_1));
    interval_timer_ =
      this->create_wall_timer(1000ms, std::bind(&ParamClient::onIntervalTimer, this));
  }

private:
  void onIntervalTimer()
  {
    static int counter = 0;
    if (counter == 5) {
      if (!param_client_->service_is_ready()) {
        RCLCPP_WARN(this->get_logger(), "service not ready");
      } else {
        param_client_->set_parameters({rclcpp::Parameter("int_param", 5)});
        RCLCPP_INFO(this->get_logger(), "set param: %s %d", "int_param", 5);
      }
    } else if (counter == 10) {
      if (!param_client_->service_is_ready()) {
        RCLCPP_WARN(this->get_logger(), "service not ready");
      } else {
        param_client_->set_parameters({rclcpp::Parameter("int_param", -1)});
        RCLCPP_INFO(this->get_logger(), "set param: %s %d", "int_param", 2);
      }
    }
    counter++;
  }

  void ParameterEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg)
  {
    RCLCPP_INFO(this->get_logger(), "node: %s", msg->node.c_str());
    for (const auto & p : msg->changed_parameters) {
      RCLCPP_INFO(this->get_logger(), "change: %s", p.name.c_str());
    }
  }

  rclcpp::TimerBase::SharedPtr interval_timer_{nullptr};
  rclcpp::AsyncParametersClient::SharedPtr param_client_{nullptr};
  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_sub_{nullptr};
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto param_server = std::make_shared<ParamClient>();
  rclcpp::spin(param_server);
  rclcpp::shutdown();
  return 0;
}
```

* 他のノードのパラメーターにアクセスするためのクラスが`AsyncParametersClient`です。Syncもあるのですが、Asyncの方が使いやすいです。
    * コンストラクターでアクセスする対象のROSノード名を記載します。
    * `param_client_->service_is_ready()`で対象のROSノードが起動しているかが分かります。
    * `param_client_->set_parameters({rclcpp::Parameter("int_param", 5)})`でパラメーターの変更を行います。
* `param_client_->on_parameter_event()`で対象のROSノードのパラメーター更新のイベントを取得できます。

## CmakeList

```cmake:rclcpp_param_lecture/CMakeLists.txtの一部
add_executable(param_server 
  src/server.cpp
)
ament_target_dependencies(param_server
  rclcpp
)

add_executable(param_client 
  src/client.cpp
)
ament_target_dependencies(param_client
  rclcpp
)

install(
  TARGETS param_client
  DESTINATION lib/${PROJECT_NAME}
)

```

# ビルド

```shell:ビルド
source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build
```

# 実行

## コマンドからパラメーター操作

server側のROSノードを起動します。

```shell:サーバー側のROSノード起動（1つ目のターミナル）
source ~/ros2_ws/install/setup.bash
ros2 run rclcpp_param_lecture param_server 
```

`[param_server]: value 1`の様にパラメーターの値が定期的に出力されます。

### パラメーター一覧を取得します。

```shell:パラメーター一覧取得（2つ目のターミナル）
ros2 param list
```

```text:パラメーター一覧取得の結果
/param_server:
  int_param
  qos_overrides./parameter_events.publisher.depth
  qos_overrides./parameter_events.publisher.durability
  qos_overrides./parameter_events.publisher.history
  qos_overrides./parameter_events.publisher.reliability
  use_sim_time
```

### パラメーターを取得します。

```shell:パラメーター取得（2つ目のターミナル）
ros2 param get /param_server int_param
```

```shell:パラメーター取得の結果
Integer value is: 1
```

### パラメーターを設定します。

```shell:パラメーター取得（2つ目のターミナル）
ros2 param set /param_server int_param 4
```

```shell:パラメーター設定の結果
Set parameter successful: success
```

1つ目のターミナルでも`changed: int_param 4`と出力されます。

### 拒否される値でパラメーターを設定します。

```shell:パラメーター設定（2つ目のターミナル）
ros2 param set /param_server int_param -1
```

```shell:パラメーター設定の結果
Setting parameter failed: negative value
```

1つ目のターミナルでも`[param_server]: fail: int_param -1`を出力されます。

## ノードからパラメーター変更

server側のROSノードを起動します。

```shell:サーバー側のROSノード起動（1つ目のターミナル）
source ~/ros2_ws/install/setup.bash
ros2 run rclcpp_param_lecture param_server 
```

client側のROSノードを起動します。

```shell:サーバー側のROSノード起動（2つ目のターミナル）
source ~/ros2_ws/install/setup.bash
ros2 run rclcpp_param_lecture param_client 
```

client側のプログラムが時間経過で有効な値の設定、無効な値の設定をします。server側のパラメーターが変わるとclient側（2つ目のターミナル）でもそれを検知して

```text
[param_client]: node: /param_server
[param_client]: change: int_param
```

のような出力をします。

# 参考
* [Parameter API design in ROS](https://design.ros2.org/articles/ros_parameters.html)
* [ROS2 チュートリアル](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Using-Parameters-In-A-Class-CPP.html)
* [APIリファレンス](https://docs.ros2.org/latest/api/rclcpp/classrclcpp_1_1Node.html)

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