サービスとクライアントを作る(C++)

今回はC++でサービスとクライアントを作ります。

一度Pythonで作ったものと同じで２つの数字（int)を足したものを返すというつまらないサービスを作ります。
http://www.ros.org/wiki/ROS/Tutorials/WritingServiceClient(c%2B%2B)

では
$ roscd beginner_tutorials
して、ソースの編集をはじめましょう。

では以下を
src/add_two_ints_server.cpp
として保存してください。

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

bool add(beginner_tutorials::AddTwoInts::Request  &req,
         beginner_tutorials::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;
}

ではコードを見ていきます。

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

もうこれはお決まりですね。
一行目はいつものおまじない。
２行目はsrvファイルを以前作ったときに作られたヘッダファイルです。

bool add(beginner_tutorials::AddTwoInts::Request  &req,
         beginner_tutorials::AddTwoInts::Response &res )

これはコールバックの宣言ですね。
サービスの実体になります。
宣言したメッセージのRequestとResponseを引数にとります。
Requestがインプット、Responseがアウトプットだと思いましょう。

{
  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;
}

中身です。
res, reqのメンバ変数はsrvファイルに書いたものになっていますね。
成功したらtrueを返しましょう。

ros::ServiceServer service = n.advertiseService("add_two_ints", add);

これでサービスを登録します。
そしてross::spin()でコールバックを待ちます。

これでサービスは終了です。

次にこれを使うクライアントを作りましょう。

以下を
src/add_two_ints_client.cpp
として保存しましょう。

#include "ros/ros.h"
#include "beginner_tutorials/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<beginner_tutorials::AddTwoInts>("add_two_ints");
  beginner_tutorials::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;
}

ではまた、中身を見ていきます。

いつもと違うのはまず、

ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");

ですね。

beginner_tutorials::AdTwoIntsというサービス型で、add_two_intsという名前のサービスを使うクライアントをつくります。
返り値のros::ServiceClinetクラスのインスタンスclientを使ってサービスを実行していきます。

beginner_tutorials::AddTwoInts srv;
  srv.request.a = atoll(argv[1]);
  srv.request.b = atoll(argv[2]);

この辺でサービス用の入力を設定します。
requestというメンバ変数に入れます。
requestはbeginner_tutorials::AddTwoInts::Requestクラスですね。

if (client.call(srv))

で実際にサービスを呼び出します。
サービス呼び出しはブロックされるので、返り値がもらえるまでここで止まります。
返り値をちゃんとチェックしましょう。

ではビルドしましょう。

CMakeLists.txtの最後に
以下を足します。

rosbuild_add_executable(add_two_ints_server src/add_two_ints_server.cpp)
rosbuild_add_executable(add_two_ints_client src/add_two_ints_client.cpp)

そして
$ make
とすればバイナリができているはずです。

では

$ roscore
して、
$ rosrun beginner_tutorials add_two_ints_server
して、
$ rosrun beginner_tutorials add_two_ints_client 1 3

すると、
[ INFO] 1261731706.032448000: Sum: 4

と、でましたね。
試しにPythonで作ったクライアントでつないでみると、
$ rosrun beginner_tutorials2 add_two_ints_client.py 1 3

Requesting 1+3

1 + 3 = 4

とできました。

異言語間が簡単に繋がりますね。

これでサービスとクライアントができました。