ROS2におけるパラメータ変更に対するcallback関数の設定

【[ROS2関係トップページへ](https://qiita.com/NeK/items/7ac0f4ec10d51dbca084)】

Humble Hawksbillまでのページは[こちら](https://qiita.com/NeK/items/37cbae948f8a4178044b)

# 概要
ROS2ではパラメータが設定・変更されたときに自動的に呼び出される関数を設定できる(callback関数)．
必要な数だけ設定できるが，その種類は大別すると以下の種類となる．

* add_pre_set_parameters_callbackに登録するもの
    * パラメータ変更の依頼が来たときに，実際に変更する前に色々処理する部分
    * 例えば変更依頼のあるパラメータに関連して変更するものがあるときに，合わせて変更するように処理するとか？
    * 返り値は `nothing`，つまりなにもなし
* add_on_set_parameters_callbackに登録するもの
    * パラメータ変更依頼が来たときに，実際に変更したり却下したりする部分
    * パラメータ自身は変更不可
    * 返り値は `rcl_interfaces/msg/SetParametersResult`
* add_post_set_parameters_callbackに登録するもの
    * パラメータ変更に対してしなければいけないことがある場合に処理する部分
    * パラメータ自身は変更不可
    * 返り値は `nothing`，つまりなにもなし
* ParameterEventHandler
    * 特に他のnodeのパラメータに変更があったときにお知らせ処理してくれるもの

[Parameters]や[Monitoring for parameter changes (C++)]を参考のこと．また[Pre and post set parameter callback support]にサポート開始の経緯が少しある．実際の使用例は[Demos:set_parameters_callback.cpp]あたりを参考に．

## 更新履歴
2024/12/29: Iron以降用に準備．

# callback関数概要
基本的には

1. パラメータ変更依頼に対応するcallback関数3種(pre,on,post}の作成
1. 主に「ノードA」が，「ノードB」のパラメータ変更を知るためのイベントハンドラの設定
    * これは「ノードA」での設定？
1. remove用？
    * まだあるのか？

を必要に応じて行う．

その他の情報としては以下のとおり．

* remove_on_set_parameters_callback
    * handlerを渡すことで対応するcallback関数を無効化する．
* add_{pre,on,post}_...の登録用関数からの返り値が，登録したcallback関数に対するhandler(割り当てた変数のようなもの)となる．
    * 注意点として，<font color="Red">handlerをちゃんと保持していないとcallbackが呼ばれない</font>

## callback関数が呼び出されるタイミング
パラメータ再設定のリクエストが出されたときにcallback関数が呼ばれる．では，そのリクエストがいつ出されるのかというと，以下の時である．

* declare_parameter*の実行
* set_parameter*の実行
* コンソールでのパラメータ値設定
* remove_on_set_parameters_callbackによるパラメータの削除

ちょっと注意なのが(当たり前だけど) callback関数が設定される前にdeclare_parameterやset_parameterされた場合，callback関数はよばれない．callback関数が設定された後のdeclare_parameterなどにはcallback関数がよばれる．

### yamlファイルとdeclare_parameterとcallbackt関数
yamlファイルを使用する場合について考える．

* declare_parameter("hoge", 0);
    * "hoge"を0として設定
* yamlファイルでは hoge: 1
    * "hoge"を1として設定

この時，yamlファイルを使用せず実行すると"hoge"は0，yamlファイルを使用すると"hoge"は1になる．
ではyamlファイルを使用している時にdeclare_parameterによってcallback関数が呼ばれると，"hoge"はどうなっているか？
答えは，"hoge"は1として扱われている．安心してcallback関数に初期設定などを任せられる．

# add_pre_set_parameters_callback用設定
## 概要
Parameterオブジェクトのリストを受け取り，返り値なし(`nothing`)とする．
実際にパラメータを変更する前に，

* Parameterオブジェクトのリスト中のパラメータ自体の変更
* 関係して変更しなければならないパラメータをParameterオブジェクトのリスト中へ追加
* Parameterオブジェクトのリスト中のパラメータ自体の削除

を行う．

## 作成すべきもの
### callback関数
[公式APIのparameter関係の関数](https://docs.ros.org/en/jazzy/p/rclcpp/generated/classrclcpp_1_1Node.html#classrclcpp_1_1Node)から

* 引数
    * std::vector\<rclcpp::Parameter\> &
* 返り値
    * なし

となる．つまり以下のような関数になり，これをadd_pre_on_set_parameters_callbackに渡すこととなる．

```
// 関数の宣言の場合
void modify_upcoming_parameters_callback(std::vector<rclcpp::Parameter>& parameters);

// lambda式の場合
auto modify_upcoming_parameters_callback =
  [](std::vector<rclcpp::Parameter>& parameters) {
    ...
  };
```

### 関数保持用のハンドラ
add_pre_on_set_parameters_callback関数からの返り値(ハンドラ)を保持しておかないと関数が有効にならない．

```
// ヘッダーのprivateなどで保持
...
private:
  rclcpp::node_interfaces::PreSetParametersCallbackHandle::SharedPtr
    pre_set_parameters_callback_handle_;

// cppなどの関数定義のところで使用
...
  pre_set_parameters_callback_handle_ = this->add_pre_set_parameters_callback(
      modify_upcoming_parameters_callback);
```

# add_on_set_parameters_callback用設定
## 概要
Parameterオブジェクトのリストを受け取り，変更するかしないかを返り値とする．

* Parameterオブジェクトを見てパラメータを変更するか否かを決定
    * result.successful = {true,false};
* 変更の可否についてコメントを付与
    * result.reason = "comment";
* パラメータ自体の演算は行わず，変更可であれば受け取った変更をそのまま使用
* あるパラメータの変更が不可であれば，その回に受け取った他のパラメータも変更不可となるっぽい？

を行う．

## 作成すべきもの
### callback関数
[公式APIのparameter関係の関数](https://docs.ros.org/en/jazzy/p/rclcpp/generated/classrclcpp_1_1Node.html#classrclcpp_1_1Node)から

* 引数
    * const std::vector\<rclcpp::Parameter\> &
    * 注意: [Demos:set_parameters_callback.cpp]とちょっと違う．あちらはconstでなく，また&(参照)でない．どちらが正しい？ちなみにどちらもコンパイルok，動作ok．
* 返り値
    * rcl_interfaces::msg::SetParametersResult

となる．つまり以下のような関数になり，これをadd_on_set_parameters_callbackに渡すこととなる．

```
// 必要ファイルの読み込み(必要そうな場所でinclude．無くてももいいかも？)
#include "rcl_interfaces/msg/set_parameters_result.hpp"

// 関数の宣言の場合
rcl_interfaces::msg::SetParametersResult validate_upcoming_parameters_callback(const std::vector<rclcpp::Parameter>& parameters);

// lambda式の場合
auto validate_upcoming_parameters_callback =
  [](const std::vector<rclcpp::Parameter>& parameters) {
    rcl_interfaces::msg::SetParametersResult result;
    ...
    return result;
  };
```

### 関数保持用のハンドラ
add_on_set_parameters_callback関数からの返り値(ハンドラ)を保持しておかないと関数が有効にならない．

```
// ヘッダーのprivateなどで保持
...
private:
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
    on_set_parameters_callback_handle_;

// cppなどの関数定義のところで使用
...
    on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(
      validate_upcoming_parameters_callback);
```

# add_post_set_parameters_callback用設定
## 概要
Parameterオブジェクトのリストを受け取り，返り値なし(`nothing`)とする．
実際にパラメータを変更した後の処理に使用される．例えば，

* 変更後のパラメータに合わせて内部変数をアップデート

などを行う．

## 作成すべきもの
### callback関数
[公式APIのparameter関係の関数](https://docs.ros.org/en/jazzy/p/rclcpp/generated/classrclcpp_1_1Node.html#classrclcpp_1_1Node)から

* 引数
    * const std::vector\<rclcpp::Parameter\> &
* 返り値
    * なし

となる．つまり以下のような関数になり，これをadd_post_on_set_parameters_callbackに渡すこととなる．

```
// 関数の宣言の場合
void react_upcoming_parameters_callback(const std::vector<rclcpp::Parameter>& parameters);

// lambda式の場合
auto react_upcoming_parameters_callback =
  [](const std::vector<rclcpp::Parameter>& parameters) {
    ...
  };
```

### 関数保持用のハンドラ
add_post_on_set_parameters_callback関数からの返り値(ハンドラ)を保持しておかないと関数が有効にならない．

```
// ヘッダーのprivateなどで保持
...
private:
  rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr
    post_set_parameters_callback_handle_;

// cppなどの関数定義のところで使用
...
  post_set_parameters_callback_handle_ = this->add_post_set_parameters_callback(
    react_to_updated_parameters_callback);
```

# あと書くもの
removeのやつ

event handlerのやつ


# 登録したcallback関数の挙動
## 返り値:resultsと挙動
複数のパラメータ再設定の関数を設定した場合，パラメータ再設定がリクエストされた時の挙動は以下のようになる．

* add_on_set_parameters_callbackで設定された<font color="Red">逆順</font>に呼び出される．
* 各パラメータ再設定の関数の返り値が<font color="Red">falseを返したところで終了</font>する．
    * 何か異常があればそこで終了，の考え方．

## 返り値:resultsの考え方(個人的私見)
パラメータ再設定成功(true)か失敗(false)かをresultsとして返す．その成功・失敗の考え方についてメモ書きをする．
特に以下の項目に着目する．

1. 対象のパラメータがあるかないか
- 対象のパラメータがある上で値が設定出来たか否か

2番目については[上のプログラム例](#パラメータ設定用関数のプログラム例)のように，例えば負の値をとりえない変数に負の値を代入しようとした時にfalseになる．

### Foxy以後
1番目の状況は起こりえない．宣言(declare_parameter)しないと使用できないので，「対象のパラメータが存在しない」場合にはcallback関数が呼ばれる前にエラーとなる．
またadd_on_set_parameterで複数のcallback関数が呼ばれること，返り値がfalseになった時点で処理が終わることから，

* 基本results.success=true
* 内容的に設定できなかったらresults.success=false (2.の判断基準)

とする．

### Eloquent以前
1番目の状況が起こりえる，一つのcallback関数で処理をすることから

* 基本results.success=false
* 設定できたらresults.success=true

とする．もう古い指標なので使うことはないと思うが．

## 自作Nodeクラスを継承した場合
add_on_set_params_callback以前では【[ROS2におけるパラメータ変更に対するcallback関数の設定 before Foxy Fitzroy](https://qiita.com/NeK/items/bda87749652b94524e56)】にあるようにクラスの継承などで気を遣わなければならなかった．
が，add_on_set_params_callbackでは複数のパラメータ再設定の関数を登録でき，独立性が高くなったので，継承したクラス内で新たにパラメータが増えても対応するパラメータ再設定の関数を普通に作成し，add_on_set_params_callbackで追加するだけでよい．


# 旧情報になるのか？:remove_on_set_parameters_callback関数
[公式API:rclcpp]の[rclcpp::Node::remove_on_set_parameters_callback()]では当該関数の宣言は以下のようになる．

```
void rclcpp::Node::remove_on_set_parameters_callback	(	const OnSetParametersCallbackHandle *const 	handler	)	
```

add_on_set_parameters_callback関数の返り値を[rclcpp::Node::remove_on_set_parameters_callback()]に渡してやればcallback関数ではなくなる．

# その他
## package.xml，CMakeLists.txtについて
rcl_interfacesに関わる項目が必要かと思いきや，不必要．
多分，rclcppのパッケージに含まれている．



<<details><summary>旧情報</summary><div>

# 旧情報:add_on_set_parameters_callback用のcallback関数
Parameterオブジェクトのリストを受け取り，`rcl_interfaces/msg/SetParametersResult`を返すものとして作成する．

このcallback関数は，declare_parameterで登録したときやパラメータの変更があったとき，「そのパラメータに対して変更がなされる<font color="Red">前</font>」に実行される．
また複数のcallback関数が登録されている場合，パラメータが変更されるのは全てのcallback関数から「変更OK(SetParametersResult.successful=true)」の場合だけ．
よって，この種類のcallback関数内では，変更が正しいか否かの判定のみで，変更に対するリアクションについてはon_set_...の方のcallback関数に任せるべきである．

## 作成すべきパラメータ設定用関数
### rclcppの場合
[公式API:rclcpp]の[rclcpp::Node::add_on_set_parameters_callback()]では当該関数の宣言は以下のようになる．

```
RCUTILS_WARN_UNUSED
OnSetParametersCallbackHandle::SharedPtr rclcpp::Node::add_on_set_parameters_callback	(
  OnParametersSetCallbackType callback
)	
```

よって自作するパラメータ設定用の関数はOnParametersSetCallbackType型となる．
次にそのOnParametersSetCallbackTypeを見ると以下のように宣言されている．

```
using OnParametersSetCallbackType =
  std::function<
  rcl_interfaces::msg::SetParametersResult(const std::vector<rclcpp::Parameter> &)
  >;
```

よって自作するパラメータ設定用関数は以下の型になる．

* 引数
    * const std::vector\<rclcpp::Parameter\> &
* 返り値
    * rcl_interfaces::msg::SetParametersResult

つまり以下のような関数になり，これをadd_on_set_params_callbackに渡すことになる．

```
rcl_interfaces::msg::SetParametersResult reset_params_callback(const std::vector<rclcpp::Parameter> & param);
```

### rclpyの場合
[公式API:rclpy]の[rclpy,Node.add_on_set_parameters_callback()]では当該関数の宣言は以下のようになる．

```
Parameters: callback (Callable[[List[Parameter]], SetParametersResult]) 
Return type: None
```

よって自作するパラメータ設定用関数は以下の型になる．

* 引数
    * List[Parameter]
* 返り値
    * rcl_interfaces::msg::SetParametersResult

つまり以下のような関数になり，これをadd_on_set_params_callbackに渡すことになる．

```
rcl_interfaces::msg::SetParametersResult reset_params_callback(self, params);
```

### パラメータ設定用関数の戻り値
callback関数の戻り値となるSetParametersResultは[公式API:rclcpp]の[rclcpp::Node::add_on_set_parameters_callback()]の説明にあるように以下の値を持つ．

- bool successful
    - 設定に成功したか(true)否か(false)
- string reason
    - 特に失敗した時の理由

## パラメータ設定用関数のプログラム例
### rclcpp : 関数名付与の場合
名前の後に"_(アンダースコア)"が付いているのは，個人的な名前付けルールに従っているだけなので気にせずに．

```c++:header
calss NodeA : public rclcpp::Node{
  rcl_interfaces::msg::SetParametersResult
    reset_params_callback_(const std::vector<rclcpp::Parameter>& params);
  OnSetParametersCallbackHandle::SharedPtr reset_param_handler_;
...
};
```

```c++:cpp
rcl_interfaces::msg::SetParametersResult
NodeA::reset_params_callback_(const std::vector<rclcpp::Parameter>& params){
  auto results = std::make_shared<rcl_interfaces::msg::SetParametersResult>();
  results->successful = true;

  for(auto&& param : params){
    if(param.get_name() == "param1"){
      member_param1_ = param.as_double(); // double型メンバ変数に代入
    }else if(param.get_name() == "param2"){
      // なんか変な時．例えば重力加速度にマイナスの値を代入しようとするなど．
      if(!some_considion){
        results->successful = false;
        results->reason = "Wrong operation"; 
        return *results;
      }
    }
  }
  return *results;
}

NodeA::NodeA()
: Node(...){
...

  reset_param_handler_ = this->add_on_set_parameters_callback(
    std::bind(&NodeA::reset_params_callback_, this, std::placeholders::_1)
  );
...
}
```

### rclcpp : 無名関数(ラムダ関数)使用の場合

```c++:header
calss NodeA : public rclcpp::Node{
  OnSetParametersCallbackHandle::SharedPtr reset_param_handler_;
...
};
```

```c++:cpp
NodeA::NodeA()
: Node(...){
...

  reset_param_handler_ = this->add_on_set_parameters_callback(
    [this](const std::vector<rclcpp::Parameter>& params) -> rcl_interfaces::msg::SetParametersResult {
      auto results = std::make_shared<rcl_interfaces::msg::SetParametersResult>();
      results->successful = true;

      for(auto&& param : params){
        if(param.get_name() == "param1"){
          member_param1_ = param.as_double(); // double型メンバ変数に代入
        }else if(param.get_name() == "param2"){
          // なんか変な時．例えば重力加速度にマイナスの値を代入しようとするなど．
          if(!some_considion){
            results->successful = false;
            results->reason = "Wrong operation"; 
            return *results;
          }
        }
      }
      return *results;
   }
  );
...
}
```

### rclpy
[ROS2 ANSWERSのこの記事](https://answers.ros.org/question/355508/ros2-using-dynamic-parameters-in-python/)が参考になる．
少しひねると以下のよう？

```
def cp_params(self, params):
  res = SetParametersResult(successful=True, reason='')

  return res
```

# 旧情報:on_set_parameter_event用のcallback関数
`rcl_interfaces/msg/ParameterEvent`オブジェクトを受け取り，返り値のないものとして作成する．

このcallback関数は，declare_parameterで登録したときやパラメータの変更があった時，削除された時に，「そのパラメータに対して変更がなされた<font color="Red">後</font>」に実行される．
つまり，パラメータの変更がなされた後に呼ばれる(add_on_parameters_callback関数に登録された全てのcallback関数から「変更OK(SetParametersResult.successful=true)」となった場合だけに呼ばれる)．
基本的な役割は，パラメータが変更されたことに対するリアクションをすることとなる．




</div></details>





[Pre and post set parameter callback support]:https://docs.ros.org/en/jazzy/Releases/Release-Iron-Irwini.html#pre-and-post-set-parameter-callback-support
[Parameters]:https://docs.ros.org/en/jazzy/Concepts/Basic/About-Parameters.html
[Monitoring for parameter changes (C++)]:https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Monitoring-For-Parameter-Changes-CPP.html
[Demos:set_parameters_callback.cpp]:https://github.com/ros2/demos/blob/jazzy/demo_nodes_cpp/src/parameters/set_parameters_callback.cpp
[公式API:rclcpp]:https://docs.ros.org/en/jazzy/p/rclcpp/index.html






[rclpy,Node.add_on_set_parameters_callback()]:https://docs.ros2.org/foxy/api/rclpy/api/node.html#rclpy.node.Node.add_on_set_parameters_callback
[rclcpp::Node::add_on_set_parameters_callback()]:http://docs.ros2.org/foxy/api/rclcpp/classrclcpp_1_1Node.html#a0c3ad2ca5c56eb54af0227f04e943679
[rclcpp::Node::remove_on_set_parameters_callback()]:http://docs.ros2.org/foxy/api/rclcpp/classrclcpp_1_1Node.html#a15d4f7fa0ef760941b6a78f42cccb7e0
[公式API:rclpy]:http://docs.ros2.org/foxy/api/rclpy/
[rclpy,Node.add_on_set_parameters_callback()]:https://docs.ros2.org/foxy/api/rclpy/api/node.html#rclpy.node.Node.add_on_set_parameters_callback