Go to Qiita Advent Calendar 2024 Top
LoginSignup
4
3

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@srs(project_srs)

ROS2講座16 ROS2 Parameterを使う1

Posted at

環境

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

項目 値
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を使用しています。

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

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を返した時は、そのリクエストは無効となりパラメーターの更新がされません。今回の例では負の値に変更することはできません。

クライアント側

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

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}
)

ビルド

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

実行

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

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

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

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

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

パラメーター一覧取得（2つ目のターミナル）
ros2 param list
パラメーター一覧取得の結果
/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

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

パラメーター取得（2つ目のターミナル）
ros2 param get /param_server int_param
パラメーター取得の結果
Integer value is: 1

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

パラメーター取得（2つ目のターミナル）
ros2 param set /param_server int_param 4
パラメーター設定の結果
Set parameter successful: success

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

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

パラメーター設定（2つ目のターミナル）
ros2 param set /param_server int_param -1
パラメーター設定の結果
Setting parameter failed: negative value

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

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

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

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

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

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

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

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

のような出力をします。

参考

目次ページへのリンク

ROS2講座の目次へのリンク

4
3
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
4
3

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?