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

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

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

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

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

こちらはrequest headerをきちんと書いたバージョンで，通常はROS2の最小構成service/client:初級 -ROS1 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_ros1_like --dependencies rclcpp my_messages

ROS風service

• src/ros1_like_srv_main.cpp

プログラム:src/ros1_like_srv_main.cpp

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<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(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;
  //using std::placeholders::_3;

  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,_3)
  );
  rclcpp::spin(node);
  rclcpp::shutdown();
  node = nullptr;
  return 0;
}

概要

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

説明

6行目からがcallback関数になっている．serviceの場合callback関数の引数は三つあり，request_header, request, responseの変数を割り当てている．
request_headerはミドルウェアの識別用だと思う．ここでは特に使用しない．使用しないままだとビルド時にwarningが出るので，11行目のように書いている．
requestがクライアント側からの入力であり値が入っているものである．これに処理をくわえてresponseに代入して返す．responseを返すタイミングは関数終了時 or return時．

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

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

ROS風client

作成物:

• src/ros1_like_clnt_main.cpp

プログラム:src/ros1_like_clnt_main.cpp

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

以下は重要な部分のみ抜粋

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

ビルド・実行

ビルド

terminal

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

実行

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

terminal1

$ cd ~/ros2_studies_ws/
$ . install/local_setup.bash
$ ros2 run my_srv_ros1_like ros1_like_srv_test

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