このページでは、ROSにおけるもう一つの重要な通信機能であるサービスを行うROSノードを作成していきます。

ここで実装するサービスは、ROSメッセージとサービスで定義した"AddTwoInts"というサービスで、2つの整数を受け取り、その和を戻り値として返すものです。

サービスを行うノードを add_two_ints_serverとし、サービスを利用するノードを add_two_ints_client として実装していきましょう。

サービスノードを実装するには、リクエストとレスポンスのメッセージ型を定義したsrvファイルを用意しなければいけません。

ここで使うsrvファイルは、ROSメッセージとサービスで定義したものを使いますので説明は省略します。

まず最初に、サービスを提供するサーバーを実装します。下のようなコマンドで mytut パッケージのsrcディレクトリに移動してください。

 > roscd mytut/src

ROSサービスの典型的なコードは、

1. 実際のサービスを行うコールバック関数定義 [(Request &, Response&)引数に持ちます]
2. main関数の定義
   1. ROSノードの初期化を行う
   2. ros::ServiceServerでサーバーインターフェースを作成
   3. ros::spin関数で外部からのサービスリクエストの処理（内部では、無限ループになっています）

となります。

したがって、add_two_ints_server.cppというファイル名で下のような実装コードを作成してください。

#include "ros/ros.h"
#include "mytut/AddTwoInts.h"

bool add(mytut::AddTwoInts::Request  &req,
         mytut::AddTwoInts::Response &res)
{
  res.sum = req.a + req.b;
  ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
  ROS_INFO("sending back response: [%ld]", (long int)res.sum);
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_server");
  ros::NodeHandle n;

  ros::ServiceServer service = n.advertiseService("add_two_ints", add);
  ROS_INFO("Ready to add two ints.");
  ros::spin();

  return 0;
}

次に、AddTwoIntsサービスを呼び出すROSノード（クライアント）を実装しましょう。

典型的なROSサービスクライアントは、main関数を

1. ROSノードの初期化
2. ROSサービスのクライアントインターフェースの作成
3. サービス呼出しのRequestの設定
4. kクライアントインターフェースのcallメソッドでサービスの呼出し

となります。

この場合には、ROSクライアントは1度の関数コール後、終了します。

add_two_ints_client.cppというファイル名で下のような実装コードを作成してください。

#include "ros/ros.h"
#include "mytut/AddTwoInts.h"
#include <cstdlib>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_client");
  if (argc != 3)
  {
    ROS_INFO("usage: add_two_ints_client X Y");
    return 1;
  }

  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<mytut::AddTwoInts>("add_two_ints");
  mytut::AddTwoInts srv;
  srv.request.a = atoll(argv[1]);
  srv.request.b = atoll(argv[2]);
  if (client.call(srv))
  {
    ROS_INFO("Sum: %ld", (long int)srv.response.sum);
  }
  else
  {
    ROS_ERROR("Failed to call service add_two_ints");
    return 1;
  }

  return 0;
}

先ほど作成した add_two_ints_server.cppとadd_two_ints_client.cppをビルドするために、トピック通信の時と同じようにCMakeLists.txtに、add_execubale, target_link_libraries, add_dependenciesを追加します。

パッケージのCMakeLists.txtに下記の記述を追加してください。

add_executable(add_two_ints_server src/add_two_ints_server.cpp)
target_link_libraries(add_two_ints_server ${catkin_LIBRARIES})
add_dependencies(add_two_ints_server beginner_tutorials_gencpp)

add_executable(add_two_ints_client src/add_two_ints_client.cpp)
target_link_libraries(add_two_ints_client ${catkin_LIBRARIES})
add_dependencies(add_two_ints_client beginner_tutorials_gencpp)

以上が終了すれば、最後にワークスペースのトップに戻って catkin_makeを実行します。

 > roscd
 > catkin_make --use-vc15

以上でROSサービスのサーバーとクライアントのビルドは終了です。

次に、Pythonでadd_two_ints_serverとadd_two_ints_clientの実装を行います。Pythonのコードは、scriptsというディレクトリの下に実装していきます。下のコマンドでディレクトリを移動します。

 > roscd mytut/scripts

では、サーバーノードを実装します。
Python版のサーバーの典型的なコードは、

1. コールバック関数の定義（Requestを引数とし、該当するResponseの返値を返す）
2. サーバーのメイン関数
   1. ROSノードの初期化
   2. rospy.Serice関数でサーバーインターフェースを定義する
   3. rospy.spin関数でサービスリクエストの処理を行う（内部では無限ループが回っています）

この処理を add_two_ints_server.py を下のように作成してください。下のコードは、Python3用に変更されていますので、オリジナルのROSノードの実装と少し異なります。

#
from mytut.srv import *
import rospy

def handle_add_two_ints(req):
    print("Returning [%s + %s = %s]" % (req.a, req.b, (req.a + req.b))) 
    return AddTwoIntsResponse(req.a + req.b)

def add_two_ints_server():
    rospy.init_node('add_two_ints_server')
    s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints)
    print( "Ready to add two ints.")
    rospy.spin()

if __name__ == "__main__":
    add_two_ints_server()

次に、ROSサービスクライアントを実装していきます。ROSサービスクライアントの典型的な処理は、

1. rospy.wait_for_service関数で呼び出したいサービスが起動するまで待つ
2. rospy.ServiceProy関数でサービス用のインターフェースを作成する
3. サービスの呼出し

となります。

そこで、add_two_ints_client.pyというファイル名で下のコードを実装してください。下のコードは、Python3用に変更されていますので、オリジナルのROSノードの実装と少し異なります。

#
import sys
import rospy
from mytut.srv import *

def add_two_ints_client(x, y):
    rospy.wait_for_service('add_two_ints')
    try:
        add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)
        resp1 = add_two_ints(x, y)
        return resp1.sum
    except rospy.ServiceException as e:
        print( "Service call failed: %s"%e)

def usage():
    return "%s [x y]"%sys.argv[0]

if __name__ == "__main__":
    if len(sys.argv) == 3:
        x = int(sys.argv[1])
        y = int(sys.argv[2])
    else:
        print( usage() )
        sys.exit(1)
    print( "Requesting %s+%s"%(x, y))
    print( "%s + %s = %s"%(x, y, add_two_ints_client(x, y)))

以上で、C++版とPython版のROSサービスのサーバーとクライアントの実装が終了しました。次は、ここで実装したサービスを起動して動作確認を行います。