service通信の作り方(python)

pythonでのService通信の作り方を示します．

公式のC++サービス通信の作成例はこちらにあります．

サービス通信の基本説明はこちら．

クライアントとサーバの関係でクライアントから要求されたときにサーバが応答する方式となります．

クライアントとサーバの用語ではなく，公式ページではclientとserviceとなっていて，clientから要求があったときserviceが応答することになります．

Serviceの作成

はじめにServiceを作成します．

~/ros2_ws/をワークスペースに設定しているものとします。C++と同じAddTwoInt.srvの時は--dependenciesにexample_interfacesを記載すればよいのですが、今回は簡単に標準で用意されているstd_srvsを指定します。

$ cd ~/ros2_ws/src

$ ros2 pkg create --build-type ament_python --license Apache-2.0 py_srvcli --dependencies rclpy std_srvs

--build-type でpythonを使うので「ament_python」を指定します。

std_srvsの中には、

ここまで。途中

下記のように、srcディレクトリにexample_interfacesをダウンロード。

$ cd ~/ros2_ws/src/

$ git clone https://github.com/ros2/example_interfaces.git

ダウンロードしたexample_interfacesの中のsrvディレクトリに AddTwoInts.srvがあるのでこれを使う。

AddTwoInts.srvの中身は下記で上記で記載したMyService.srvと同じ。

int64 a

int64 b

---

int64 sum

次に、package.xmlを修正。下記３行を自分の情報に変更（修正しなくてもコンパイルはできる。）

<description>service test program</description>

<maintainer email="my@email.com">My Name</maintainer>

<license>Apache License 2.0</license>

次に、service_node.cppを編集します。 

$ nano ~/ros2_ws/src/cpp_pkg/test_service/src/service_node.cpp

2つのint a, b の値を足して返信するためのサービスのプログラムは下記。（本家のソースから一部改変）

#include "rclcpp/rclcpp.hpp"                // dependencyで定義したものをここに書く

#include "example_interfaces/srv/add_two_ints.hpp"  // ここにダウンロードしたサンプルのインタフェースをbuildすることで生成される定義ファイルを記載する AddTwoInts.srv が add_two_ints.hpp になっている

#include <memory>

// 足し算の関数

void add(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,// 1つ目の引数はリクエストの変数。型は、example_interfaces::srv::AddTwoInts::Requestにする

          std::shared_ptr<example_interfaces::srv::AddTwoInts::Response>      response)// ２つ目の引数はレスポンスの変数。型は、example_interfaces::srv::AddTwoInts::Responseにする

{

  response->sum = request->a + request->b;

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld",

                request->a, request->b);

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);

}

int main(int argc, char **argv)

{

  rclcpp::init(argc, argv);  // 初期化

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("service_node"); //　ノード生成。「"」で囲まれた部分がノード名

  rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service =

    node->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add);// サービスの関数名．今回は「add_two_ints」

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");  //　ノードが準備できたらメッセージが表示されるようにしてある。

  rclcpp::spin(node);

  rclcpp::shutdown();

}

serviceノードがコンパイルされるように、CMakeLists.txtを修正しておく。

find_package(example_interfaces REQUIRED) #この宣言を追記。example_interfacesの依存を宣言

add_executable(service_node src/service_node.cpp) #これはすでにパッケージを作るときにノード名とパッケージ名を指定していれば記載されているはず。

ament_target_dependencies(server rclcpp example_interfaces) #後から追加した依存パッケージexample_interfacesを追記

依存に関連するところを２か所書かなくてはいけない。。。１回で済めばよいのに。。。

問題なくビルドができるか確認する。

$ cd ~/ros2_ws/&&colcon build && source ~/ros2_ws/install/setup.bash

Clientの作成

次に、サービスと通信するクライアントプログラムを作成する。

$ cd ~/ros2_ws/src/cpp_pkg/test_service/src

$ gedit client_node.cpp

クライアントのソースの内容は下記を記載．（本家から引用，一部改変）

#include "rclcpp/rclcpp.hpp"

#include "example_interfaces/srv/add_two_ints.hpp"

#include <chrono>

#include <cstdlib>

#include <memory>

using namespace std::chrono_literals;

int main(int argc, char **argv)

{

  rclcpp::init(argc, argv);

  if (argc != 3) {

      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_two_ints_client X Y");　//引数が２個記載されていないとエラーメッセージがターミナルには表示される．

      return 1;

  }

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("testservice_client");//クライアントノードの名前

  rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client =

    node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");//ここでサーバで提供されている名前を指定．今回はサーバで準備した「add_two_ints」を記載

  auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();//メッセージの定義で示したrequest側の変数を宣言

  request->a = atoll(argv[1]); //引数の１番目をaに

  request->b = atoll(argv[2]); //引数の２番めをｂに

  while (!client->wait_for_service(1s)) { //サーバとの接続待ち．１秒毎にメッセージ

    if (!rclcpp::ok()) {

      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");

      return 0;

    }

    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");

  }

  auto result = client->async_send_request(request);

  // Wait for the result.

  if (rclcpp::spin_until_future_complete(node, result) ==

    rclcpp::FutureReturnCode::SUCCESS)

  {

    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum);

  } else {

    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints");

  }

  rclcpp::shutdown();

  return 0;

}

クライアントをビルドするためにCMakeLists.txtを修正する必要がある．

$ gedit CMakeLists.txt

下記のCMakeLists.txtの追記と書いてある場所が追記した場所．

cmake_minimum_required(VERSION 3.8)

project(test_service)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

  add_compile_options(-Wall -Wextra -Wpedantic)

endif()

# find dependencies

find_package(ament_cmake REQUIRED)

find_package(rclcpp REQUIRED)

find_package(example_interfaces REQUIRED)

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}

  "srv/MyService.srv"

)

add_executable(service_node src/service_node.cpp)

add_executable(client_node src/client_node.cpp) #この行を追加

target_include_directories(service_node PUBLIC

  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

  $<INSTALL_INTERFACE:include>)

target_compile_features(service_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17

ament_target_dependencies(

  service_node

  "rclcpp"

  "example_interfaces"

)

ament_target_dependencies(client_node rclcpp example_interfaces) # 追加

install(TARGETS

  service_node

  client_node #追記

  DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)

  find_package(ament_lint_auto REQUIRED)

  # the following line skips the linter which checks for copyrights

  # comment the line when a copyright and license is added to all source files

  set(ament_cmake_copyright_FOUND TRUE)

  # the following line skips cpplint (only works in a git repo)

  # comment the line when this package is in a git repo and when

  # a copyright and license is added to all source files

  set(ament_cmake_cpplint_FOUND TRUE)

  ament_lint_auto_find_test_dependencies()

endif()

ament_export_dependencies(rosidl_default_runtime)

ament_package()

ビルド

次に、プログラムをビルドする。

まず依存関係を解決．

$ rosdep install -i --from-path src --rosdistro humble -y

下記でビルド．

$ cd ~/ros2_ws

$ colcon build

もしくは下記のようにtest_serviceパッケージだけを指定してビルド

$ colcon build --packages-select test_service

実行

次に、プログラムを実行してサービス通信を確認する。

まずはサーバ側を１つ目のターミナルで実行．

$ ros2 run test_service service_node

もう一つのターミナルでクライアントを実行．クライアントの実行には引数の数字を２つ入力すると合計した数が戻ってくる．

$ ros2 run test_service client_node 2 3

上記実行時に返ってくる表示例が下記

[INFO] [1715152391.630559375] [rclcpp]: Sum: 5

サーバー側は下記のように送信されてきた２つの数字と結果が表示されている．

[INFO] [1715152391.630340380] [rclcpp]: Incoming request

a: 2 b: 3

[INFO] [1715152391.630386790] [rclcpp]: sending back response: [5]

自分の定義型でサービス通信したいとき

サービス通信の型を管理するパッケージを作成して定義ファイルを置くだけ。

まずパッケージを作成してsrvディレクトリの中に定義ファイルを保存．．

$ cd ~/ros2_ws/src

$ ros2 pkg create --build-type ament_cmake my_interface

$ cd my_interface

$ mkdir srv

$ cd srv

$ nano MyService.srv

srvディレクトリ内にMyService.srvを作成して下記の用にファイル内容を記載．

int64 a

int64 b

---

int64 sum

CMakeLists.txtを下記のように修正

cmake_minimum_required(VERSION 3.8)

project(my_interface)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

  add_compile_options(-Wall -Wextra -Wpedantic)

endif()

# find dependencies

find_package(ament_cmake REQUIRED)

# uncomment the following section in order to fill in

# further dependencies manually.

# find_package(<dependency> REQUIRED)

#ここを追記

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}

  "srv/MyService.srv"

)

if(BUILD_TESTING)

  find_package(ament_lint_auto REQUIRED)

  # the following line skips the linter which checks for copyrights

  # comment the line when a copyright and license is added to all source files

  set(ament_cmake_copyright_FOUND TRUE)

  # the following line skips cpplint (only works in a git repo)

  # comment the line when this package is in a git repo and when

  # a copyright and license is added to all source files

  set(ament_cmake_cpplint_FOUND TRUE)

  ament_lint_auto_find_test_dependencies()

endif()

ament_export_dependencies(rosidl_default_runtime)#ここを追記

ament_package()

package.xmlも下記のように修正．

<?xml version="1.0"?>

<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>

<package format="3">

  <name>my_interface</name>

  <version>0.0.0</version>

  <description>TODO: Package description</description>

  <maintainer email="takubo@todo.todo">takubo</maintainer>

  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <test_depend>ament_lint_auto</test_depend>

  <test_depend>ament_lint_common</test_depend>

  <buildtool_depend>rosidl_default_generators</buildtool_depend>#ここを追記

  <exec_depend>rosidl_default_runtime</exec_depend>#ここを追記

  <member_of_group>rosidl_interface_packages</member_of_group>#ここを追記

  <export>

    <build_type>ament_cmake</build_type>

  </export>

</package>

以上で準備完了．

自分のメッセージ型を使いたいとき

使いたいサービスのサーバとクライアントのソースを修正．

今回は，上記で使っていたservice_node.cppとclient_node.cppを修正．

// service_node.cpp

#include "rclcpp/rclcpp.hpp"                // dependencyで定義したものをここに書く

#include "my_interface/srv/my_service.hpp"  // ここを修正

#include <memory>

// 足し算の関数

void add(const std::shared_ptr<my_interface::srv::MyService::Request> request,// ここを修正

          std::shared_ptr<my_interface::srv::MyService::Response>      response)// ここを修正

{

  response->sum = request->a + request->b;

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld",

                request->a, request->b);

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);

}

int main(int argc, char **argv)

{

  rclcpp::init(argc, argv);  // 初期化

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("service_node"); //　ノード生成。「"」で囲まれた部分がノード名

  rclcpp::Service<my_interface::srv::MyService>::SharedPtr service =// ここを修正

    node->create_service<my_interface::srv::MyService>("add_two_ints", &add);// ここを修正

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");  //　ノードが準備できたらメッセージが表示されるようにしてある。

  rclcpp::spin(node);

  rclcpp::shutdown();

}

// client_node.cpp

#include "rclcpp/rclcpp.hpp"

#include "my_interface/srv/my_service.hpp"  // ここを修正

#include <chrono>

#include <cstdlib>

#include <memory>

using namespace std::chrono_literals;

int main(int argc, char **argv)

{

  rclcpp::init(argc, argv);

  if (argc != 3) {

      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_two_ints_client X Y"); //引数が２個記載されていないとエラーメッセージがターミナルには表示される．

      return 1;

  }

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("testservice_client");//クライアントノードの名前

  rclcpp::Client<my_interface::srv::MyService>::SharedPtr client =// ここを修正

    node->create_client<my_interface::srv::MyService>("add_two_ints");// ここを修正

  auto request = std::make_shared<my_interface::srv::MyService::Request>();// ここを修正

  request->a = atoll(argv[1]); //引数の１番目をaに

  request->b = atoll(argv[2]); //引数の２番めをｂに

  while (!client->wait_for_service(1s)) { //サーバとの接続待ち．１秒毎にメッセージ

    if (!rclcpp::ok()) {

      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");

      return 0;

    }

    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");

  }

  auto result = client->async_send_request(request);

  // Wait for the result.

  if (rclcpp::spin_until_future_complete(node, result) ==

    rclcpp::FutureReturnCode::SUCCESS)

  {

    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum);

  } else {

    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints");

  }

  rclcpp::shutdown();

  return 0;

}

CMakeLists.txtも修正．

cmake_minimum_required(VERSION 3.8)

project(test_service)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

  add_compile_options(-Wall -Wextra -Wpedantic)

endif()

# find dependencies

find_package(ament_cmake REQUIRED)

find_package(rclcpp REQUIRED)

#find_package(example_interfaces REQUIRED) #この行を削除

find_package(my_interface REQUIRED) #この行を追加

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}

  "srv/MyService.srv"

)

add_executable(service_node src/service_node.cpp)

add_executable(client_node src/client_node.cpp) 

target_include_directories(service_node PUBLIC

  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

  $<INSTALL_INTERFACE:include>)

target_compile_features(service_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17

ament_target_dependencies(

  service_node

  "rclcpp"

  "my_interface" #この行を修正

)

ament_target_dependencies(client_node rclcpp my_interface) #この行を修正

install(TARGETS

  service_node

  client_node 

  DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)

  find_package(ament_lint_auto REQUIRED)

  # the following line skips the linter which checks for copyrights

  # comment the line when a copyright and license is added to all source files

  set(ament_cmake_copyright_FOUND TRUE)

  # the following line skips cpplint (only works in a git repo)

  # comment the line when this package is in a git repo and when

  # a copyright and license is added to all source files

  set(ament_cmake_cpplint_FOUND TRUE)

  ament_lint_auto_find_test_dependencies()

endif()

ament_export_dependencies(rosidl_default_runtime)

ament_package()