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

【ROS2関係トップページへ】
メモ状態

概要

ROS2でパラメータが再設定された時に呼び出される関数(callback関数)をset_on_parameters_set_callbackで設定できる．
Crystal Clemmys以前ではregister_param_change_callback関数だったが，これは廃止予定．

set_on_parameters_set_callback関数について

公式githubを見ると当該関数は以下の宣言となる．

OnParametersSetCallbackType set_on_parameters_set_callback(OnParametersSetCallbackType callback)

で，このcallback関数を使うために必要な，OnParametersSetCallbackTypeは同じgithubのところで以下のように宣言されている．

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

で，callback関数の戻り値となるSetParametersResultは公式githubのrcl_interfaceの該当箇所をみると以下を持つ．

• bool successful
• string reason

というわけで，例えばパラメータをメンバ変数として持つ場合などでは，コンストラクタにて以下のように書く

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

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

      for(auto&& param : params){
        if(!this->has_parameter(param.get_name())){
          results->successful = false;
          results->reason = "No Param";
          return *results;
        }
        if(param.get_name() == "param1"){
          member_param1_ = param.as_double(); // double型メンバ変数に代入
        }....
        }else{
          // パラメータ設定がうまくいかなかったとき
          results->successful = false;
          results->reason = "Wrong Param";
          return *results;
        }
      }
      results->successful = true;
      results->reason = ""; 
      return *results;
   }
  );
...
}

ちなみにcallback関数の場合，以下の感じ

calss NodeA : public rclcpp::Node{
  rcl_interfaces::msg::SetParametersResult
    param_reset_callback(const std::vector<rclcpp::Parameter> params);
...
};

rcl_interfaces::msg::SetParametersResult
NodeA::param_reset_callback(const std::vector<rclcpp::Parameter> params){
  auto results = std::make_shared<rcl_interfaces::msg::SetParametersResult>();

  for(auto&& param : params){
    if(!this->has_parameter(param.get_name())){
      results->successful = false;
      results->reason = "No Param";
      return *results;
    }
    if(param.get_name() == "param1"){
      member_param1_ = param.as_double(); // double型メンバ変数に代入
    }....
    }else{
      // パラメータ設定がうまくいかなかったとき
      results->successful = false;
      results->reason = "Wrong Param";
    }
  }
  results->successful = true;
  results->reason = ""; 
  return *results;
}

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

  this->set_on_parameters_set_callback(
    std::bind(&NodeA::param_reset_callback, this, std::placeholders::_1)
  );
...
}

自作Nodeクラスを継承した場合のcallback関数

• 親クラス
  • NodeA
• 子クラス
  • NodeB

概要

クラスNodeAがあり，それを継承したNodeBがあるとする．
NodeBに追加でパラメータを設定した場合，NodeBは以下のパラメータの種類を持つ．

• NodeAのパラメータ
• NodeBのパラメータ

よってパラメータ変更に対するcallback関数もNodeAおよびNodeBに対して考えなければならない．

考慮すべき点

NodeBにパラメータ変更のためのcallback関数を設定した場合，その関数に与えられる入力"const std::vector<rclcpp::Parameter> params"には何が入っているのか？
答えは，NodeAのパラメータとNodeBのパラメータ両方である．
よって，NodeBは以下のようにcallback関数を考える必要がある．

1. param.get_name()からNodeBにかかわるパラメータであれば処理する
2. NodeBのものでないパラメータは，一時保存変数forward_paramに入れておく
3. すべてのparamについてloopを回す
4. 親クラスに，forward_paramを渡して処理をお願いする．
   • 親クラスでSetParametersResult resultの判定を行ってもらう．
     • 子クラスでは，「存在しないパラメータ」か「親クラスのパラメータ」か分からないため
5. 親クラスから帰ってきたresultを返す．

例えば以下のようなソースとなる．

rcl_interfaces::msg::SetParametersResult
NodeB::param_reset_callback(const std::vector<rclcpp::Parameter> params){
  std::vector<rclcpp::Parameter> forward_params;

  for(auto&& param : params){
    if(param.get_name() == "param1"){
      member_param1_ = param.as_double(); // double型メンバ変数に代入
    }....
    }else{
      // 親クラスに判定を任せる
      forward_params.push_back(param);
    }
  }
  return NodeA::param_reset_callback(forward_params);
}

その他

package.xml，CMakeLists.txtについて

rcl_interfacesに関わる項目が必要かと思いきや，不必要．
多分，rclcppのパッケージに含まれている．

results変数について

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