【ROS2関係トップページへ】
【ROS2レクチャー:初級 -class style-】

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

ここではserviceとclientのプログラムを作成する．やり取りするメッセージは以前作った独自メッセージを使用する．

service/clientを説明する公式のアニメーション付き図がすごく分かりやすい．

request headerを用いた書き方はROS2のROS2の最小構成service/client with request header:初級 -class style-を参考に．意味が分からないよ，という人は特に参考にしなくてよい．当該ページは，"request header"に関すること以外本ページと同一であり，こちらが最新版となる．

再度掲示: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<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<my_messages::srv::CalcTwoFloats::Request> request,
  const std::shared_ptr<my_messages::srv::CalcTwoFloats::Response> response
){
  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)
  );
}

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, responseとなる．
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

追加 : クラス化したclient2 : callback関数を使わない場合

async_send_requestでリクエストを送ったら，そのまま返信を待って，そのまま返信の値を使って作業したいことって多い．
そんな時の方法．
とりあえずtimerも使わず，永久ループで人がリクエストの値を入力する例を記載．

作成物:

src/my_clnt.hpp
src/my_clnt.cpp
src/my_clnt_main.cpp

プログラム:

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_;

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;

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");

  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>();
  while(1){ // 良い子はwhile(1)なんてしてはいけません
    std::cout << "input a:";
    std::cin >> request->a;
    std::cout << "input b:";
    std::cin >> request->b;

    // ここからが重要なところ
    auto future_res = clnt_->async_send_request(request);
    if(
      rclcpp::spin_until_future_complete(this->get_node_base_interface(), res_future) == rclcpp::FutureReturnCode::SUCCESS
    ){
      // 返信が来た場合
      RCLCPP_INFO(this->get_logger(), "res: %lf, %lf", future_res.get()->sum, future_res.get()->diff);
    }else{
      // 返信来なかった．悲しい．
      ...
    }
  }

}

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

概要

要はrclcppのapiにあるrclcpp::spin_until_future_complete()を使えってことで．

参考記事: How to call spin_some or spin_until_future_complete in member functions

さらにFutureReturnCodeを詳しく見ていると，rclcpp::FutureReturnCodeとしてSUCCESS, INTERRUPTED, TIMEOUTあるので好きなように使え，と．

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