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

ROS2関係トップページへ

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

2019/6/13

dashing diademataになってparameterの扱い方が変わった(Crystal Clemmysの情報はこちら).

変更点は自由に(適当に)パラメータを設定できなくなったこと.

getやsetでパラメータを扱う前に,まず以下の関数で"宣言"することとなった.


  • declare_parameter()

これにより名前の打ち間違いなどで似たようなパラメータが勝手に作られたりしなくなる.

関係して増えた関数は以下の通り.

-declare_parameter

- パラメータを宣言

- undeclare_parameter

- パラメータを亡かったことに

- has_parameter

- 前からあった気がするがパラメータの存在確認

2019/6/13現在,公式のAPIには無いようである.

公式のRoadmapに詳しく書いてある.

もしくは公式githubのrcl_interfaces



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プログラムを作成した.



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


準備


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 param1 = node->declare_parameter("foo");
auto param2 = node->declare_parameter("bar", "ok");
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~23行目:設定するparameterの宣言.名前だけでも良いし,デフォルト値を設定してもよい.

20~23行目:設定するparameterを準備.Async(非同期)なのでspin(もしくはspin_until_future_complete)まで実際には設定されない.

24行目:parameterを設定するとともにnodeを実行.


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行目).


クラス化したparameter

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


  • src/minimal_parameter.hpp


    • パラメータを保持するnodeのヘッダ

    • node名: param_holder



  • src/minimal_parameter.cpp


    • パラメータを保持するnodeのヘッダ



  • src/minimal_parameter_main.cpp


    • パラメータを保持するnodeのターゲット




プログラム : src/minimal_parameter.hpp


minimal_parameter.h

#include <rclcpp/rclcpp.hpp>


class MinimalParameter : public rclcpp::Node{
private:
rclcpp::AsyncParametersClient::SharedPtr param_;
public:
MinimalParameter();
};


概要

ROS1風と異なるところはAyncParametersClientのポインタを宣言しているところ.


プログラム : src/minimal_parameter.cpp


minimal_parameter.h

#include <rclcpp/rclcpp.hpp>

#include "minimal_parameter.hpp"

MinimalParameter::MinimalParameter()
: Node("param_holder"){
using namespace std::chrono_literals;
param_ = std::make_shared<rclcpp::AsyncParametersClient>(this);

while(!param_->wait_for_service(1s)){
RCLCPP_INFO(this->get_logger(), "waiting for service");
}

auto param1 = declare_parameter("foo");
auto param2 = declare_parameter("bar", "ok");
auto results = param_->set_parameters({
rclcpp::Parameter("foo",2),
rclcpp::Parameter("bar","hello")
});
}



概要

ROS1風と同様に,declareして設定.


プログラム : src/minimal_parameter_main.cpp


minimal_parameter.h

#include <rclcpp/rclcpp.hpp>

#include "minimal_parameter.hpp"

int main(int argc, char * argv[]){
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MinimalParameter>());
rclcpp::shutdown();
return 0;
}


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(minimal_parameter_test
src/minimal_parameter_main.cpp
src/minimal_parameter.cpp
)
ament_target_dependencies(minimal_parameter_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
minimal_parameter_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を起動.

param_holder用はROS1でもクラス化したものでも結果は変わらないので,ここではROS1風を実行.

また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回実行させると結果はどうなるか?