LoginSignup
0
0

More than 1 year has passed since last update.

@NeK(くら けん)

ROS2の最小構成service/client with request header:初級 -class style-

Posted at

【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

TwoInts.msg
float64 a
float64 b
---
float64 sum
float64 diff

上記をビルドしたものなので，以下のように使用できる．

  • インクルードファイル
    • #include "my_messages/srv/calc_two_floats.msg"
  • 型クラス
    • my_messages::srv::CalcTwoFloats

準備

terminal
$ 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

プログラム

src/my_srv.hpp
#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()
  );
};
src/my_srv.cpp
#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)
  );
}
my_srv_main.cpp
#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処理と非同期処理

src/my_clnt.hpp
#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()
  );
};
src/my_clnt.cpp
#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)
  );
}
src/my_clnt_main.cpp
#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関数による非同期処理

src/my_clnt_with_lambda.hpp
#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()
  );
};
src/my_clnt_with_lambda.cpp
#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)
  );
}
src/my_clnt_main_with_lambda.cpp
#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.xml
<package format="3">
  <depend>rclcpp</depend>
  <depend>my_messages</depend>
CMakeLists.txt
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}
)

ビルド・実行

ビルド

terminal
$ cd ~/ros2_studies_ws/
$ colcon build --symlink-install --packages-up-to my_srv_class
$ . install/local_setup.bash

実行

service用とclient用の端末を起動しテスト．下記ではクライアントとしてclnt_testを使用．

terminal1
$ cd ~/ros2_studies_ws/
$ . install/local_setup.bash
$ ros2 run my_srv_class srv_test
terminal2
$ cd ~/ros2_studies_ws/
$ . install/local_setup.bash
$ ros2 run my_srv_class clnt_test

【前:独自メッセージを用いたROS2 publisher/subscriber:初級 -class style-】
【次:ROS2の最小構成parameter:初級 -class type-】

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