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

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

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

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

ここでは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の最小構成service/client with request header:初級 -ROS1 style-](https://qiita.com/NeK/items/d5a4ebff90a1839a0433)を参考に．意味が分からないよ，という人は特に参考にしなくてよい．当該ページは，"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_ros1_like --dependencies rclcpp my_messages
```

# ROS風service

- src/ros1_like_srv_main.cpp


## プログラム:src/ros1_like_srv_main.cpp

```c++:src/ros1_like_srv_main.cpp
#include <rclcpp/rclcpp.hpp>
#include "my_messages/srv/calc_two_floats.hpp"

rclcpp::Node::SharedPtr node = nullptr;

void handleService(
  const std::shared_ptr<my_messages::srv::CalcTwoFloats::Request> request,
  const std::shared_ptr<my_messages::srv::CalcTwoFloats::Response> response
){
  RCLCPP_INFO(node->get_logger(),"srv.request:%lf,%lf", request->a, request->b);
  response->sum = request->a + request->b;
  response->diff = request->a - request->b;
}

int main(int argc, char * argv[]){
  using namespace std::placeholders;
  //using std::placeholders::_1;
  //using std::placeholders::_2;

  rclcpp::init(argc, argv);
  node = rclcpp::Node::make_shared("minimal_service");
  auto service = node->create_service<my_messages::srv::CalcTwoFloats>(
    "srv_test",
    std::bind(handleService, _1,_2)
  );
  rclcpp::spin(node);
  rclcpp::shutdown();
  node = nullptr;
  return 0;
}
```

### 概要
create_serviceでサービス設定を行う．その中でトピック名とcallback関数(handleService)を設定し，nodeを実行している．

### 説明
6行目からがcallback関数になっている．serviceの場合callback関数の引数は二つあり，request, responseの変数を割り当てている．
requestがクライアント側からの入力であり値が入っているものである．これに処理をくわえてresponseに代入して返す．responseを返すタイミングは関数終了時 or return時．

22～25行目がserviceの設定で，トピック名の設定やcallback関数の設定を行っている．
その後26行目でnodeを実行している．

ちなみに16～18行目がbindのための設定である．丁寧に書くと17,18行目のようになる．引数の数が多くなると面倒になってくるので，16行目のように書く．

# ROS風client
作成物:

- src/ros1_like_clnt_main.cpp

## プログラム:src/ros1_like_clnt_main.cpp

```c++:src/ros1_like_clnt_main.cpp
#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include "my_messages/srv/calc_two_floats.hpp"

int main(int argc, char * argv[]){
  using namespace std::chrono_literals;
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("minimal_client");
  auto client = node->create_client<my_messages::srv::CalcTwoFloats>("srv_test");

  while(!client->wait_for_service(1s)){
    if(!rclcpp::ok()){
      RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear.");
      return 1;
    }
    RCLCPP_INFO(node->get_logger(), "waiting for service...");
  }

  auto request = std::make_shared<my_messages::srv::CalcTwoFloats::Request>();
  request->a = 10.0;
  request->b = 11.2;

  auto res_future = client->async_send_request(request);
  if(rclcpp::spin_until_future_complete(node, res_future) != rclcpp::executor::FutureReturnCode::SUCCESS){
    RCLCPP_ERROR(node->get_logger(), "Service call failed.");
    return 1;
  }

  auto res = res_future.get();
  RCLCPP_INFO(node->get_logger(), "res: %lf, %lf", res->sum, res->diff);

  rclcpp::shutdown();
  return 0;
}
```

### 概要
他と同じようにcreate_client<メッセージ型>でクライアントとして設定を行う．
他と異なるところは以下のとおり

- serviceへの接続確認をする
- 非同期通信でメッセージを送信する
    - 送り方はFutureモデルを使用
    - 同期通信もあるのかもしれないけど非同期通信があればいらなさそう

### 説明
9行目でクライアントの準備
10～17行目でサーバとの通信を試みて接続できるまで待つ．
19行目から送信するデータの準備を行い，23行目で非同期通信としてデータを送信する設定を行っている．
24行目でFuture用のspinを行っておりnodeを実行してデータ処理の結果を得ている．データ処理の結果を受信する可能性もあるため(途中でネットワークが切れたり)，rclcpp::executor::FutureReturnCode::SUCCESSか判定することで正しくデータを受信できたか判定している．
29行目のget()でserviceからのデータを得ている．

# 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(ros1_like_srv_test
  src/ros1_like_srv_main.cpp
)
ament_target_dependencies(ros1_like_srv_test
  rclcpp
  my_messages
)

add_executable(ros1_like_clnt_test
  src/ros1_like_clnt_main.cpp
)
ament_target_dependencies(ros1_like_clnt_test
  rclcpp
  my_messages
)

install(TARGETS
  ros1_like_srv_test
  ros1_like_clnt_test
  DESTINATION lib/${PROJECT_NAME}
)
```

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

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

## 実行
service用とclient用の端末を起動しテスト．

```Bash:terminal1
$ cd ~/ros2_studies_ws/
$ . install/local_setup.bash
$ ros2 run my_srv_ros1_like ros1_like_srv_test
```

```Bash:terminal2
$ cd ~/ros2_studies_ws/
$ . install/local_setup.bash
$ ros2 run my_srv_ros1_like ros1_like_clnt_test
```

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