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

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

Eloquent Elusorまでのページは[こちら](https://qiita.com/NeK/items/bda87749652b94524e56)

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

* パラメータが変更される前に呼ぶcallback関数(設定前の確認などなど)
    * add_on_set_parameters_callbackに登録
* パラメータが変更されたあとに呼ぶcallback関数(設定後のリアクション用などなど)
    * on_set_parameter_eventに登録
        * [このページ](https://docs.ros.org/en/humble/Concepts/About-ROS-2-Parameters.html#parameter-callbacks)や[このページ](https://docs.ros.org/en/humble/Tutorials/Intermediate/Monitoring-For-Parameter-Changes-CPP.html)？未整理状態
        * rclcppにはAsyncParametersClientの中にこの関数を見つけることができる．しかしpythonでは見つからなかった(2023/01/19現在)

詳しくは[公式のここ](https://docs.ros.org/en/humble/Concepts/About-ROS-2-Parameters.html)を参考に．ページはhumble版だけどfoxyでもそう．

## 更新履歴
2023/01/19: すこし整理
2020/6/20: add_on_set_parameters_callback関数だけど，"add"から分かるように複数のcallback関数を追加できる模様．それに気が付いて，めちゃくちゃ便利になる！と気が付いた．
nodeを継承した場合なども下記に記しているが，これらを特別に考えなくて済むようになる．

# callback関数概要
基本的には，

1. callback関数の作成
1. add_on_set_parameters_callback, on_set_parameter_event関数で自作のcallback関数を登録

の流れとなる．
その他の情報としては以下のとおり．

* remove_on_set_parameters_callback
    * handlerを渡すことで対応するcallback関数を無効化する．
* add_..., on_set_...の登録用関数からの返り値が，登録した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_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
```

## 登録した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で追加するだけでよい．

# 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)」となった場合だけに呼ばれる)．
基本的な役割は，パラメータが変更されたことに対するリアクションをすることとなる．



# 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のパッケージに含まれている．

## results変数について
resultsなくても動きます．
その場合，すっきりするかもしれません．
が，ビルド時にwarningが出てすっきりしません．

[公式API:rclcpp]:http://docs.ros2.org/foxy/api/rclcpp/
[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