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

【[ROS2関係トップページへ](https://qiita.com/NeK/items/7ac0f4ec10d51dbca084)】
【[ROS2レクチャー:初級 -class style-](https://qiita.com/NeK/items/14db31ff784a0bfafe3d)】

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

[独自メッセージを用いたROS2 publisher/subscriber:初級 -class style-]:https://qiita.com/NeK/items/cd00705f7e473f83ca44
[ROS2の最小構成parameter:初級 -class type-]:https://qiita.com/NeK/items/c7710827c1676487473a

ここではserviceとclientのプログラムを作成する．やり取りするメッセージは[以前作った独自メッセージ](https://qiita.com/NeK/items/26751b8ed12976ff977d)を使用する．

service/clientを説明する[公式のアニメーション付き図](https://docs.ros.org/en/galactic/Tutorials/Services/Understanding-ROS2-Services.html#background)がすごく分かりやすい．

request headerを用いた書き方はROS2の[ROS2の最小構成service/client with request header:初級 -class style-](https://qiita.com/NeK/items/c1a860401bdb7db8fa94)を参考に．意味が分からないよ，という人は特に参考にしなくてよい．当該ページは，"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

# 準備

```Bash: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

## プログラム

```c++: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()
  );
};
```

```c++: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)
  );
}
```

```c++: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処理と非同期処理

```c++: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()
  );
};
```

```c++: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)
  );
}
```

```c++: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](http://docs.ros2.org/crystal/api/rclcpp/classrclcpp_1_1Client.html#ac7203d297201a83411a4274ccf9a1e2a)を参照．

<details>
<summary>serviceからのデータ受信に対する処理をcallback関数にする理由</summary>serviceに対してデータを送信したら，その結果としてserviceから返信がくる．この時受け取るデータはfutureオブジェクトであり，「まだ処理は終わってないかもだけど終わったかどうかはfutureオブジェクトの中をみるとわかるから」というものである．そこでasync_send_request関数はfutureオブジェクトを見ながら「serviceの処理が終わってデータが届いたか」を確認しながらcallback関数の実行タイミングを計っている．またfutureオブジェクトの中に処理結果のデータも入っているので，callback関数に渡す引数もfutureオブジェクトとなっている．具体的にはrclcpp::Client<メッセージ型>::SharedFutureの型となっている．
</details>

## プログラム:lambda関数による非同期処理

```c++: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()
  );
};
```

```c++: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)
  );
}
```

```c++: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
以下は追加したところに関係する部分のみ抜粋

```xml: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}
)
```

# ビルド・実行
## ビルド

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

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

```Bash:terminal1
$ cd ~/ros2_studies_ws/
$ . install/local_setup.bash
$ ros2 run my_srv_class srv_test
```

```Bash: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

## プログラム:

```c++: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()
  );
};
```

```c++: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{
      // 返信来なかった．悲しい．
      ...
    }
  }

}
```

```c++: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](https://docs.ros2.org/latest/api/rclcpp/index.html)にある[rclcpp::spin_until_future_complete()](https://docs.ros2.org/latest/api/rclcpp/namespacerclcpp.html#a702eef83e9bdbaabbc89e426668d808c)を使えってことで．

参考記事: [How to call spin_some or spin_until_future_complete in member functions](https://answers.ros.org/question/354132/how-to-call-spin_some-or-spin_until_future_complete-in-member-functions/)

さらに[FutureReturnCode](https://docs.ros2.org/latest/api/rclcpp/namespacerclcpp.html#a7b4ff5f1e516740d7e11ea97fe6f5532)を詳しく見ていると，rclcpp::FutureReturnCodeとして`SUCCESS`, `INTERRUPTED`, `TIMEOUT`あるので好きなように使え，と．


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