LoginSignup
2
2

More than 3 years have passed since last update.

@NeK(くら けん)

ROS2の最小構成parameterプログラムの作成 for Crystal_Clemmys

Last updated at Posted at 2019-05-06

ROS2関係トップページへ

ここではparameterに関するプログラムを作成する．parameterとして非同期に取得・設定を行うAsyncParameterClientと同期的に行うSyncParameterClientを紹介する．

また，ここではROS風しか紹介しない．SyncParameterClientであればspin系を使用しない．そのためSyncParameterClientを使用すればコンストラクタの中でパラメータの取得や設定が出来るので，ROS風のnodeでもクラス化したnodeでもプログラムの書き方はあまり変わらない．

この他，parameter serviceや参照しているparameterが変更された場合に処理を行うcallback関数の機能，パラメータの権限に関するあれやこれやなどあるが，future worksで．


Parameter nodeに対するroughな解釈
Parameter nodeも他の種類のnodeと同じくservice側とclient側がある．ここから個人的な感想だが(以降間違っているかもしれないので注意)，違うのは，nodeがservice側とclient側に分かれているのではなく，nodeの中にある機能がservice側とclient側に分かれているのではないかと思う．nodeにはparameterのデータを保持する機能と参照する機能が必要だが，parameterのデータを保持し他へ提供するのがservice，参照するのがclient．nodeがservice側とclient側に分かれているのであれば，serviceのnodeとclientのnodeが存在し，通信を行うことになる．だけど，parameterの場合，上記のような機能としてservice側とclient側に分かれているので，一つのnodeの中にserviceの機能とclientの機能があってもいい(例えば公式のdemosのparameter関連)．自分のイメージとしてはLinuxのX Windowのような感じ．一つのPCの中でX Windowのserverとclientが動いてもいいし別々のPCで動いていてもいい(X window system 3)．ここでは複数のnode間でparameterを共有することを考えて最低限のparameterプログラムを作成した．

準備

terminal
$ cd ~/ros2_studies_ws/
$ ros2 pkg create minimal_parameter

ROS風 parameter

作成物:github.comの以下のファイル

  • src/ros1_like_parameter_holder_main.cpp
    • パラメータを保持するnode
    • node名: param_holder
  • src/ros1_like_parameter_async_user_main.cpp
    • param_holderに接続し，値を参照したり設定したりするnode
  • src/ros1_like_parameter_sync_user_main.cpp
    • param_holderに接続し，値を参照したり設定したりするnode

プログラム : src/ros1_like_parameter_holder_main.cpp

src/ros1_like_parameter_holder_main.cpp
#include <rclcpp/rclcpp.hpp>
#include <chrono>

int main(int argc, char * argv[]){
  using namespace std::chrono_literals;
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("param_holder");
  auto param_client = std::make_shared<rclcpp::AsyncParametersClient>(node);

  while(!param_client->wait_for_service(1s)){
    if(!rclcpp::ok()){
      RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service.");
      return 1;
    }
    RCLCPP_INFO(node->get_logger(),"waiting for service...");
  }

  auto results = param_client->set_parameters({
    rclcpp::Parameter("foo",2),
    rclcpp::Parameter("bar","hello")
  });
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

概要

パラメータを保持するnodeとして作成．
nodeを作り，保持するパラメータを準備し，spinでnodeを実行したままにする．
特に8行目でparameterの設定を行っているが，通常引数は二つである．一つ目がAsyncParametersClientの機能を持たせるnodeへのポインタ，二つ目がparameterのデータを保持しているparameter nodeの名前(serviceとなるnodeの名前)である．二つめが省力されることで，自身(node)がAsyncParametersClientの機能を持つとともにserviceでもあることになる．

説明

7行目:nodeの設定．名前をparam_holderとする．
8行目:param_holderのnodeをAsyncParametersClientとして設定．また自分自身のparameterをいじると設定(自分がserviceともなる)．
10～14行目:serviceへの接続(自分のparameterを扱う機能の部分にアクセス)
18～21行目:設定するparameterを準備．Async(非同期)なのでspin(もしくはspin_until_future_complete)まで実際には設定されない．
22行目:parameterを設定するとともにnodeを実行．
ros1_like_parameter_async_user_main.cpp

Parameter

set_parameters関数の中でParameterを使用している．またrclcppのAPIにも記述があるが補足．

パラメータはキー(要素名?)と値の対にて表される．辞書型の変数．ここで，ROS2ではParameterクラスとParameterValueクラスが存在し，両方ともパラメータを表すのに使えそう．rclcppのAPIによるとParameterValueはパラメータを保持するためのクラスで，Parameterも同じ．ただParameterクラスはget, setに関する関数テンプレートを使っていて任意のパラメータを扱えるよう．

プログラム : src/ros1_like_parameter_async_user_main.cpp

src/ros1_like_parameter_async_user_main.cpp
#include <rclcpp/rclcpp.hpp>
#include <chrono>

int main(int argc, char * argv[]){
  using namespace std::chrono_literals;
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("param_async_user");
  auto param_client = std::make_shared<rclcpp::AsyncParametersClient>(node, "param_holder");
  std::stringstream ss;

  while(!param_client->wait_for_service(1s)){
    if(!rclcpp::ok()){
      RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service.");
      return 1;
    }
    RCLCPP_INFO(node->get_logger(), "waiting for service...");
  }

  auto parameters_future = param_client->get_parameters({"foo", "bar"});
  if(rclcpp::spin_until_future_complete(node, parameters_future) != rclcpp::executor::FutureReturnCode::SUCCESS){
    RCLCPP_ERROR(node->get_logger(), "Failed to get param list.");
    return 1;
  }
  for(auto &param : parameters_future.get()){
    ss << "\nParameter name:" << param.get_name();
    ss << "\nParameter type:" << param.get_type_name();
    ss << "\nParameter value:" << param.value_to_string();
  }
  RCLCPP_INFO(node->get_logger(), ss.str().c_str());
  ss.str("");
  ss.clear(std::stringstream::goodbit);

  auto list_future = param_client->list_parameters({"foo","bar"},10);
  if(rclcpp::spin_until_future_complete(node, list_future) != rclcpp::executor::FutureReturnCode::SUCCESS){
    RCLCPP_ERROR(node->get_logger(), "Failed to get param list.");
    return 1;
  }
  for(auto &param_name : list_future.get().names){
    ss << "\n" << param_name;
  }
  for(auto &prefix : list_future.get().prefixes){
    ss << "\n" <<  prefix;
  }
  RCLCPP_INFO(node->get_logger(), ss.str().c_str());
  ss.str("");
  ss.clear(std::stringstream::goodbit);

  auto results_future = param_client->set_parameters({
    rclcpp::Parameter("foo",3),
    rclcpp::Parameter("bar","welcome")
    //rclcpp::Parameter("bar",10) // OK:別種のものを代入することは可能
    //rclcpp::Parameter("hoge","hige") // NG:パラメータを増やすことは無理
  });
  if(rclcpp::spin_until_future_complete(node, results_future) != rclcpp::executor::FutureReturnCode::SUCCESS){
    RCLCPP_ERROR(node->get_logger(), "Failed to get param list.");
    return 1;
  }
  for(auto &result : results_future.get()){
    if(!result.successful){
      RCLCPP_ERROR(node->get_logger(), "Failed to set parameter: %s", result.reason.c_str());
    }
  }

  parameters_future = param_client->get_parameters({"foo", "bar"});
  if(rclcpp::spin_until_future_complete(node, parameters_future) != rclcpp::executor::FutureReturnCode::SUCCESS){
    RCLCPP_ERROR(node->get_logger(), "Failed to get param list.");
    return 1;
  }
  for(auto &param : parameters_future.get()){
    ss << "\nParameter name:" << param.get_name();
    ss << "\nParameter type:" << param.get_type_name();
    ss << "\nParameter value:" << param.value_to_string();
  }
  RCLCPP_INFO(node->get_logger(), ss.str().c_str());
  ss.str("");
  ss.clear(std::stringstream::goodbit);

  rclcpp::shutdown();
  return 0;
}

概要

param_holderに接続し，そのnodeが持っているパラメータを参照・変更・また参照するプログラム．
Async(非同期)型となっているので，参照・変更を有効にするときにspin_until_future_completeを使用している．このspinによって返ってくるのはFutureオブジェクトなので，処理結果はget関数で行っている．

説明

8行目:AsyncParametersClientとして設定．二つ目の引数で参照するparameter nodeの名前を渡している．
19行目:取得するparameterの設定．キーで指定．
20～23行目:spinによってparameter取得を実行．できなかったら(rclcpp::executor::FutureReturnCode::SUCCESSでなかったら)エラー処理となっている．
24～28行目:spinによって返ってくるのはFutureオブジェクト．よってparameterの取得結果(処理結果)はget関数で得る．複数のparameterを取得しているのでfor文で回している．
33～46行目:parameterのlist．詳細情報？33行目の二つ目の引数10は深さで，対象parameterの名前を深さ10まで取得するらしい．
48～62行目:paramter設定．値を変えることは可能(49,50行目)．また値の種類を超えて変えることも可能(51行目)．しかし新しいキーを設定することはできない模様(エラーにはならない)．
64～76行目:parameter取得．

プログラム : src/ros1_like_parameter_sync_user_main.cpp

src/ros1_like_parameter_sync_user_main.cpp
#include <rclcpp/rclcpp.hpp>
#include <chrono>

int main(int argc, char * argv[]){
  using namespace std::chrono_literals;
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("param_sync_user");
  auto param_client = std::make_shared<rclcpp::SyncParametersClient>(node, "param_holder");
  std::stringstream ss;

  while(!param_client->wait_for_service(1s)){
    if(!rclcpp::ok()){
      RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service.");
      return 1;
    }
    RCLCPP_INFO(node->get_logger(), "waiting for service...");
  }

  auto parameters_get_results = param_client->get_parameters({"foo", "bar"});
  for(auto &param : parameters_get_results){
    ss << "\nParameter name:" << param.get_name();
    ss << "\nParameter type:" << param.get_type_name();
    ss << "\nParameter value:" << param.value_to_string();
  }
  RCLCPP_INFO(node->get_logger(), ss.str().c_str());
  ss.str("");
  ss.clear(std::stringstream::goodbit);

  auto list_results = param_client->list_parameters({"foo","bar"},10);
  for(auto &param_name : list_results.names){
    ss << "\n" << param_name;
  }
  for(auto &prefix : list_results.prefixes){
    ss << "\n" <<  prefix;
  }
  RCLCPP_INFO(node->get_logger(), ss.str().c_str());
  ss.str("");
  ss.clear(std::stringstream::goodbit);

  auto parameter_set_results = param_client->set_parameters({
    rclcpp::Parameter("foo",3),
    rclcpp::Parameter("bar","welcome")
    //rclcpp::Parameter("bar",10) // OK:別種のものを代入することは可能
//    rclcpp::Parameter("hoge","hige") // NG:パラメータを増やしても無効?
  });
  for(auto &result : parameter_set_results){
    if(!result.successful){
      RCLCPP_ERROR(node->get_logger(), "Failed to set parameter: %s", result.reason.c_str());
    }
  }

  parameters_get_results = param_client->get_parameters({"foo", "bar"});
  for(auto &param : parameters_get_results){
    ss << "\nParameter name:" << param.get_name();
    ss << "\nParameter type:" << param.get_type_name();
    ss << "\nParameter value:" << param.value_to_string();
  }
  RCLCPP_INFO(node->get_logger(), ss.str().c_str());
  ss.str("");
  ss.clear(std::stringstream::goodbit);

  rclcpp::shutdown();
  return 0;
}

概要

Sync(同期)型のプログラム．Async型と異なり，spin関数を用いなくても，get_parameters, set_parametersなどを実行したらすぐに取得・設定が行われる．そのためクラス化したnodeの場合にはSync型を使用することになる(コンストラクタの中でパラメータの取得・設定を行うためにはAsyncでは無理っぽい)．

Async型との違いは以下のとおり

  • spin_until_future_completeがなく，get_parameters, set_parametersがすぐに実行される
  • 結果がFutureオブジェクトでなく，処理結果そのままとなる．そのためAsyncでは返り値.get()で処理結果を得ていたが，Syncでは返り値をそのまま使用している(19,20行目，29,30行目，40,41行目，52,53行目)．

package.xmlとCMakeLists.txt

<package format="2">
  <build_depend>rclcpp></build_depend>
  <test_depend>rclcpp></test_depend>
  <exec_depend>rclcpp></exec_depend>
CMakeLists.txt
find_package(rclcpp REQUIRED)

add_executable(ros1_like_param_holder_test
  src/ros1_like_parameter_holder_main.cpp
)
ament_target_dependencies(ros1_like_param_holder_test
  rclcpp
)

add_executable(ros1_like_param_async_user_test
  src/ros1_like_parameter_async_user_main.cpp
)
ament_target_dependencies(ros1_like_param_async_user_test
  rclcpp
)

add_executable(ros1_like_param_sync_user_test
  src/ros1_like_parameter_sync_user_main.cpp
)
ament_target_dependencies(ros1_like_param_sync_user_test
  rclcpp
)

install(TARGETS
  ros1_like_param_holder_test
  ros1_like_param_async_user_test
  ros1_like_param_sync_user_test
  DESTINATION lib/${PROJECT_NAME}
)

ビルド・実行

ビルド

terminal
$ cd ~/ros2_studies_ws/
$ colcon build --symlink-install --packages-select minimal_parameter
$ . install/setup.bash

実行

param_holderとuser用のterminalを起動．
user用はAsyncでもSyncでも結果は変わらないので，ここではAsyncを実行．

terminal1
$ cd ~/ros2_studies_ws/
$ . install/setup.bash
$ ros2 run minimal_parameter ros1_like_param_holder_test
terminal2
$ cd ~/ros2_studies_ws/
$ . install/setup.bash
$ ros2 run minimal_parameter ros1_like_param_async_user_test

terminal2に出てくるメッセージを見て考える．
またterminal1のparam_holderを終了させないまま，async_userを2回，3回実行させると結果はどうなるか？

その他

クラス化するときには，param_clientをメンバ変数として持つことになる．
この時は
rclcpp::AsyncParametersClient::SharedPtr param_client;
かな．

2
2
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
2
2