Pynote

Python、機械学習、画像処理について

ROS - C++ でサービスを使う

試した環境

手順

ファイルを用意する。

フォルダ構成

service_cpp/
├── srv
│   └── AddTwoInts.srv
├── src
│   ├── server.cpp
│   └── client.cpp
├── package.xml
└── CMakeLists.txt

パッケージの雛形を作成する。

依存パッケージとして roscpp を指定し、service_cpp というパッケージを作成する。

$ cd ~/catkin_ws/src
$ catkin_create_pkg service_cpp roscpp

すると、パッケージの雛形が作成される。

service_cpps
├── CMakeLists.txt
├── include
│   └── service_cpps
├── package.xml
└── src

サービス定義ファイルを作成する。

AddTwoInts.srv

int64 a
int64 b
---
int64 sum
サービスサーバーを作成する。

server.cpp

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

bool add(service_cpp::AddTwoInts::Request &req,
         service_cpp::AddTwoInts::Response &res)
{
    res.sum = req.a + req.b;
    return true;
}

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

    ros::ServiceServer service = nodeHandler.advertiseService(
        "add_two_ints", add);
    ros::spin();
}

サービスに対して、リクエストがあったときに呼ばれるコールバック関数 add() を定義する。
また、ros::NodeHandle::advertiseService() でサービスサーバーを作成する。

クライアントを作成する。

client.cpp

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

int main(int argc, char **argv)
{
    ros::init(argc, argv, "add_two_ints_client");

    ros::NodeHandle nodeHandler;
    ros::ServiceClient client =
        nodeHandler.serviceClient<service_cpp::AddTwoInts>("add_two_ints");

    service_cpp::AddTwoInts srv;
    // リクエストを作成する
    srv.request.a = 1;
    srv.request.b = 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::NodeHandle::serviceClient() でサービスクライアントを作成する。
srv.request に値を設定し、ros::ServiceClient::call() を呼ぶ。
呼び出しに成功した場合は srv.response に結果が格納されている。

CMakeLists.txt を編集する。

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(service_cpp)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)

## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
   roscpp
   message_generation
)

################################################
## Declare ROS messages, services and actions ##
################################################

## Generate services in the 'srv' folder
add_service_files(FILES
    AddTwoInts.srv
)

generate_messages()

###################################
## catkin specific configuration ##
###################################

catkin_package(
   CATKIN_DEPENDS roscpp message_runtime
)

###########
## Build ##
###########

## Specify additional locations of header files
include_directories(
  ${catkin_INCLUDE_DIRS}
)

# server
add_executable(server_node src/server.cpp)
target_link_libraries(server_node ${catkin_LIBRARIES})
add_dependencies(server_node ${PROJECT_NAME}_gencpp)

# client
add_executable(client_node src/client.cpp)
target_link_libraries(client_node ${catkin_LIBRARIES})
add_dependencies(client_node ${PROJECT_NAME}_gencpp)
package.xml を編集する。

package.xml

<?xml version="1.0"?>
<package>
    <name>service_cpp</name>
    <version>0.0.0</version>
    <description>The service_cpp package</description>
    <maintainer email="root@todo.todo">root</maintainer>
    <license>TODO</license>
    <buildtool_depend>catkin</buildtool_depend>
    <build_depend>roscpp</build_depend>
    <build_depend>message_generation</build_depend>
    <run_depend>roscpp</run_depend>
    <run_depend>message_runtime</run_depend>
    <export>
    </export>
</package>

以下の依存関係を追加する。

<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>
ビルドする。
$ cd ~/catkin_ws
$ catkin_make
実行する。

ROS マスターが起動していない場合は起動する。

$ roscore &

サービスサーバーを先に立ち上げる。

$ rosrun rosrun service_cpp server_node &

サービスクライアントを起動する。

$ rosrun service_cpp client_node
[ INFO] [1505731739.568536225]: Sum: 3

サービスサーバーに問い合わせ、1 + 2 の結果、3 が返ってきたのが確認できた。