【ROS2関係トップページへ】
【ROS2レクチャー:初級 -class style-】
【前:独自メッセージを用いたROS2 publisher/subscriber:初級 -class style-】
【次:ROS2の最小構成parameter:初級 -class type-】
ここではserviceとclientのプログラムを作成する.やり取りするメッセージは以前作った独自メッセージを使用する.
service/clientを説明する公式のアニメーション付き図がすごく分かりやすい.
こちらはrequest headerをきちんと書いたバージョンで,通常はROS2の最小構成service/client:初級 -class style-でよい.
再度掲示:service用メッセージ
パッケージ名:my_messages
収納ディレクトリ:srv
メッセージファイル名:CalcTwoFloats.msg
float64 a
float64 b
---
float64 sum
float64 diff
上記をビルドしたものなので,以下のように使用できる.
- インクルードファイル
- #include "my_messages/srv/calc_two_floats.msg"
- 型クラス
- my_messages::srv::CalcTwoFloats
準備
$ cd ~/ros2_studies_ws/
$ ros2 pkg create my_srv_class --dependencies rclcpp my_messages
クラス化したservice
作成物:
- src/my_srv.hpp
- src/my_srv.cpp
- src/my_srv_main.cpp
プログラム
#include <rclcpp/rclcpp.hpp>
#include "my_messages/srv/calc_two_floats.hpp"
class MySrv : public rclcpp::Node{
private:
rclcpp::Service<my_messages::srv::CalcTwoFloats>::SharedPtr srv_;
void handleService_(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<my_messages::srv::CalcTwoFloats::Request> request,
const std::shared_ptr<my_messages::srv::CalcTwoFloats::Response> response
);
public:
MySrv(
const std::string& name_space="",
const rclcpp::NodeOptions& options = rclcpp::NodeOptions()
);
};
#include <rclcpp/rclcpp.hpp>
#include "my_srv.hpp"
#include "my_messages/srv/calc_two_floats.hpp"
void MySrv::handleService_(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<my_messages::srv::CalcTwoFloats::Request> request,
const std::shared_ptr<my_messages::srv::CalcTwoFloats::Response> response
){
(void)request_header;
RCLCPP_INFO(this->get_logger(),"srv.request:%lf,%lf", request->a, request->b);
response->sum = request->a + request->b;
response->diff = request->a - request->b;
}
MySrv::MySrv(
const std::string& name_space,
const rclcpp::NodeOptions& options
): Node("minimal_service",name_space,options){
using namespace std::placeholders;
srv_ = this->create_service<my_messages::srv::CalcTwoFloats>(
"srv_test",
std::bind(&MySrv::handleService_, this, _1, _2, _3)
);
}
#include <rclcpp/rclcpp.hpp>
#include "my_srv.hpp"
int main(int argc, char * argv[]){
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MySrv>());
rclcpp::shutdown();
return 0;
}
概要
create_serviceでサービス設定を行い,callback関数を設定している.またmain関数の中でspinすることでnodeを実行している.
説明
privateなメンバ関数handleService_がcallback関数になっている.serviceでのcallback関数の引数は,request_header, request, responseとなる.
request_headerはミドルウェアの識別用で使用しないため,(void)request_header;で見かけ上使用してwarningが出ないようにしている.
requestがクライアント側からの入力であり値が入っているものである.これに処理をくわえてresponseに代入して返す.responseを返すタイミングは関数終了時 or return時.
std::bindを使用しているが,引数の数が多いためusing namespace std::placeholdersを使用している.使用していなければstd::bindの_1をstd::placeholders::_1と書かなければいけなくなる.
クラス化したclient
作成物:
callback関数によるTimer処理と非同期処理
- src/my_clnt.hpp
- src/my_clnt.cpp
- src/my_clnt_main.cpp
lambda関数による非同期処理
- src/my_clnt_with_lambda.hpp
- src/my_clnt_with_lambda.cpp
- src/my_clnt_main_with_lambda.cpp
プログラム:callback関数によるTimer処理と非同期処理
#include <rclcpp/rclcpp.hpp>
#include "my_messages/srv/calc_two_floats.hpp"
class MyClnt : public rclcpp::Node{
private:
rclcpp::Client<my_messages::srv::CalcTwoFloats>::SharedPtr clnt_;
rclcpp::TimerBase::SharedPtr timer_;
void callback_timer_();
void callback_response_(rclcpp::Client<my_messages::srv::CalcTwoFloats>::SharedFuture future);
public:
MyClnt(
const std::string& name_space="",
const rclcpp::NodeOptions& options = rclcpp::NodeOptions()
);
};
#include <rclcpp/rclcpp.hpp>
#include "my_clnt.hpp"
#include "my_messages/srv/calc_two_floats.hpp"
using namespace std::chrono_literals;
void MyClnt::callback_timer_(){
while(!clnt_->wait_for_service(1s)){
if(!rclcpp::ok()){
RCLCPP_ERROR(this->get_logger(), "Client interrupted while waiting for service");
return;
}
RCLCPP_INFO(this->get_logger(), "waiting for service...");
}
auto request = std::make_shared<my_messages::srv::CalcTwoFloats::Request>();
request->a = 10.0;
request->b = 11.2;
auto future_res = clnt_->async_send_request(request,
std::bind(&MyClnt::callback_response_,this,std::placeholders::_1));
}
void MyClnt::callback_response_(rclcpp::Client<my_messages::srv::CalcTwoFloats>::SharedFuture future){
RCLCPP_INFO(this->get_logger(), "res: %lf, %lf", future.get()->sum, future.get()->diff);
}
MyClnt::MyClnt(
const std::string& name_space,
const rclcpp::NodeOptions& options
): Node("minimal_client",name_space,options){
clnt_ = this->create_client<my_messages::srv::CalcTwoFloats>("srv_test");
timer_ = this->create_wall_timer(
1s,
std::bind(&MyClnt::callback_timer_, this)
);
}
#include <rclcpp/rclcpp.hpp>
#include "my_clnt.hpp"
int main(int argc, char * argv[]){
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MyClnt>());
rclcpp::shutdown();
return 0;
}
概要
クラス化したclientのプログラム.定期的にserviceにアクセスするためTimerを使用している(clientとしては必須ではない).そのためのcallback関数がcallback_timer_()である.
また非同期通信を行うときに,ROS風ではspin系関数であるspin_until_future_completeを使用した.しかしクラス化したnodeの場合,spin系を使用するのはクラスを実体化する時であり,クラス外(main関数の中でnodeを宣言し,それを実体化する時.例えばmy_clnt_main.cppの6行目)である.その為にクラス内でspin_until_future_completeは使用できない.
そこでspin系の関数をクラス内で使用する代わりに,非同期通信が終わりserviceから結果を得た場合の(futureオブジェクトの準備が出来た後の)処理をcallback関数としてasync_send_requestに引数として渡している.
説明
src/my_clnt.hppより
- 6行目がclient用の設定変数.
- 7,8行目がTimerの設定
- 9行目が非同期処理のためのcallback関数
src/my_clnt.cppより
コンストラクタ:
- 31行目がclient用の設定
- 32行目からがTimer系の設定で,1秒ごとにserviceにメッセージを送るためのcallback_timer_関数を割り当てている
callback_timer_関数
- 8行目からがserviceへの接続確認
- 16-18行目がserciceに送信するデータの準備
- 20行目がROS風との一番の違いで,async_send_request関数に対してserviceに送るデータ(request)に加えてserviceからのデータ受信に対する処理をcallback関数(callback_response_)を引数として持つ
- callback_response_への引数は一つでrclcpp::Client<メッセージ型>::SharedFuture(futureオブジェクトへのポインタ)となっている.
- 非同期処理に関するcallback関数に渡されるSharedFutureについてはrclcppのAPIを参照.
serviceからのデータ受信に対する処理をcallback関数にする理由
serviceに対してデータを送信したら,その結果としてserviceから返信がくる.この時受け取るデータはfutureオブジェクトであり,「まだ処理は終わってないかもだけど終わったかどうかはfutureオブジェクトの中をみるとわかるから」というものである.そこでasync_send_request関数はfutureオブジェクトを見ながら「serviceの処理が終わってデータが届いたか」を確認しながらcallback関数の実行タイミングを計っている.またfutureオブジェクトの中に処理結果のデータも入っているので,callback関数に渡す引数もfutureオブジェクトとなっている.具体的にはrclcpp::Client<メッセージ型>::SharedFutureの型となっている.プログラム:lambda関数による非同期処理
#include <rclcpp/rclcpp.hpp>
#include "my_messages/srv/calc_two_floats.hpp"
class MyClnt : public rclcpp::Node{
private:
rclcpp::Client<my_messages::srv::CalcTwoFloats>::SharedPtr clnt_;
rclcpp::TimerBase::SharedPtr timer_;
void callback_timer_();
public:
MyClnt(
const std::string& name_space="",
const rclcpp::NodeOptions& options = rclcpp::NodeOptions()
);
};
#include <rclcpp/rclcpp.hpp>
#include "my_clnt_with_lambda.hpp"
#include "my_messages/srv/calc_two_floats.hpp"
using namespace std::chrono_literals;
void MyClnt::callback_timer_(){
while(!clnt_->wait_for_service(1s)){
if(!rclcpp::ok()){
RCLCPP_ERROR(this->get_logger(), "Client interrupted while waiting for service");
return;
}
RCLCPP_INFO(this->get_logger(), "waiting for service...");
}
auto request = std::make_shared<my_messages::srv::CalcTwoFloats::Request>();
request->a = 10.0;
request->b = 11.2;
auto response_received_callback = [this](rclcpp::Client<my_messages::srv::CalcTwoFloats>::SharedFuture future) {
RCLCPP_INFO(this->get_logger(), "res: %lf, %lf", future.get()->sum, future.get()->diff);
};
auto future_res = clnt_->async_send_request(request, response_received_callback);
}
MyClnt::MyClnt(
const std::string& name_space,
const rclcpp::NodeOptions& options
): Node("minimal_client",name_space,options){
clnt_ = this->create_client<my_messages::srv::CalcTwoFloats>("srv_test");
timer_ = this->create_wall_timer(
1s,
std::bind(&MyClnt::callback_timer_, this)
);
}
#include <rclcpp/rclcpp.hpp>
#include "my_clnt_with_lambda.hpp"
int main(int argc, char * argv[]){
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MyClnt>());
rclcpp::shutdown();
return 0;
}
概要
callback_response_をlambda関数化しただけのもの.
ただ,lambda関数は「変数に代入できる(src/my_clnt_with_lambda.cppの20~22行目)」のだが,これを今まで使用していなかったので記す.
いったん変数に代入しているが,「その場所でそのままlambda関数を書く(同.cppの23行目で直接lambda関数を記述する)」のと同じなのでbindじゃない.
package.xmlとCMakeLists.txt
以下は追加したところに関係する部分のみ抜粋
<package format="3">
<depend>rclcpp</depend>
<depend>my_messages</depend>
find_package(rclcpp REQUIRED)
find_package(my_messages REQUIRED)
add_executable(srv_test
src/my_srv.cpp
src/my_srv_main.cpp
)
ament_target_dependencies(srv_test
rclcpp
my_messages
)
add_executable(clnt_test
src/my_clnt.cpp
src/my_clnt_main.cpp
)
ament_target_dependencies(clnt_test
rclcpp
my_messages
)
add_executable(clnt_test2
src/my_clnt_with_lambda.cpp
src/my_clnt_main_with_lambda.cpp
)
ament_target_dependencies(clnt_test2
rclcpp
my_messages
)
install(TARGETS
srv_test
clnt_test
clnt_test2
DESTINATION lib/${PROJECT_NAME}
)
ビルド・実行
ビルド
$ cd ~/ros2_studies_ws/
$ colcon build --symlink-install --packages-up-to my_srv_class
$ . install/local_setup.bash
実行
service用とclient用の端末を起動しテスト.下記ではクライアントとしてclnt_testを使用.
$ cd ~/ros2_studies_ws/
$ . install/local_setup.bash
$ ros2 run my_srv_class srv_test
$ cd ~/ros2_studies_ws/
$ . install/local_setup.bash
$ ros2 run my_srv_class clnt_test
【前:独自メッセージを用いたROS2 publisher/subscriber:初級 -class style-】
【次:ROS2の最小構成parameter:初級 -class type-】