ROS のDynamic Reconfigureの機能をROS 2に移行する

概要　

ROS パッケージをROS 2移行する時に、
ROSの動的パラメータ設定機能　dynamic_reconfigureを使用している部分をROS2に移行する書き方のメモ

ROS2 ではdynamic_reconfigureは廃止され、動的パラメータ設定は標準のパラメータ機能に統合され
すべてのノードがパラメータサーバーを持つ

ROS 1での動的パラメータ設定

• cfg ファイルを作成して、動的に設定可能なパラメータを定義する
• cfg ファイルからコードを生成する
• コールバック関数を定義して、パラメータを更新する処理を書く

ROS 2での動的パラメータ設定

• パラメータをノードで定義する（declare_parameter メソッドを使用）
• add_on_set_parameters_callbackメソッドを使用して、パラメータが変更されたときに呼び出されるコールバック関数を登録する
• コールバック関数内で、新しいパラメータ値を更新する処理を書く

dynamic_reconfigure のチュートリアルのサンプルのROS2移行

こちらのチュートリアルのroscppのコードと同様の動作をするROS2のrclcppのコードを作成する。

元のチュートリアルは変更された値を表示するだけだが、移行後のコードは同じ名前のメンバ変数変数の値が動的に変わるようにした。

元のROS1のコード

#include <ros/ros.h>

#include <dynamic_reconfigure/server.h>
#include <dynamic_tutorials/TutorialsConfig.h>

void callback(dynamic_tutorials::TutorialsConfig &config, uint32_t level) {
  ROS_INFO("Reconfigure Request: %d %f %s %s %d", 
            config.int_param, config.double_param, 
            config.str_param.c_str(), 
            config.bool_param?"True":"False", 
            config.size);
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "dynamic_tutorials");

  dynamic_reconfigure::Server<dynamic_tutorials::TutorialsConfig> server;
  dynamic_reconfigure::Server<dynamic_tutorials::TutorialsConfig>::CallbackType f;

  f = boost::bind(&callback, _1, _2);
  server.setCallback(f);

  ROS_INFO("Spinning node");
  ros::spin();
  return 0;
}

ROS2 (rclcpp)移行後のコード例

#include <rclcpp/rclcpp.hpp>
#include <vector>
#include <rcl_interfaces/msg/set_parameters_result.hpp>

class DynamicTutorialsNode : public rclcpp::Node
{
public:
  DynamicTutorialsNode()
  : Node("dynamic_tutorials"),
    int_param(0),
    double_param(0.0),
    str_param(""),
    bool_param(false),
    size(1)
  {
    this->declare_parameter<int>("int_param", int_param);
    this->declare_parameter<double>("double_param", double_param);
    this->declare_parameter<std::string>("str_param", str_param);
    this->declare_parameter<bool>("bool_param", bool_param);
    this->declare_parameter<int>("size", size);

    auto parameter_change_cb = std::bind(&DynamicTutorialsNode::parameter_callback, this, std::placeholders::_1);
    reset_param_handler_ = this->add_on_set_parameters_callback(parameter_change_cb);
  }

rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector<rclcpp::Parameter> &parameters)
{
  auto result = rcl_interfaces::msg::SetParametersResult();
  result.successful = true;

  for (const auto &parameter : parameters) {
    if (parameter.get_name() == "int_param") {
      int_param = parameter.as_int();
      RCLCPP_INFO(this->get_logger(), "int_param changed to %d", int_param);
    } else if (parameter.get_name() == "double_param") {
      double_param = parameter.as_double();
      RCLCPP_INFO(this->get_logger(), "double_param changed to %f", double_param);
    } else if (parameter.get_name() == "str_param") {
      str_param = parameter.as_string();
      RCLCPP_INFO(this->get_logger(), "str_param changed to %s", str_param.c_str());
    } else if (parameter.get_name() == "bool_param") {
      bool_param = parameter.as_bool();
      RCLCPP_INFO(this->get_logger(), "bool_param changed to %s", bool_param ? "true" : "false");
    } else if (parameter.get_name() == "size") {
      size = parameter.as_int();
      RCLCPP_INFO(this->get_logger(), "size changed to %d", size);
    }
  }
  return result;
}

private:
  int int_param;
  double double_param;
  std::string str_param;
  bool bool_param;
  int size;
  OnSetParametersCallbackHandle::SharedPtr reset_param_handler_;
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<DynamicTutorialsNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

実行

dynamic_tutorialsという名前のパッケージを作成して上のコードを実行

ros2 run dynamic_tutorials dynamic_tutorials_node

コマンドからパラメータを変更する

ros2 param コマンドからいずれかのパラメータを変更する

ros2 param set  /dynamic_tutorials int_param 20

以下のように出力され中のメンバ変数変数が動的に変更できていることが分かる。

[INFO] [1694690437.662707074] [dynamic_tutorials]: int_param changed to 20

rqtからパラメータを変更する

rqt_reconfigureを実行

ros2 run rqt_reconfigure rqt_reconfigure

double_paramを変更

[INFO] [1695191380.645715349] [dynamic_tutorials]: double_param changed to 10.000000

おわりに

パラメータ変更に対するcallback関数の設定の詳しい内容はこちらの記事を参照

今回の記事で作成したサンプル

参考