Pynote

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

ROS - C++ によるサービスクライアントの作成方法

試した環境

コード

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 nodeHandler;

ros::NodeHandle は service server/client、subscriber、publisher を作成するためのインタフェースクラスである。

サービスクライアントの作成

ros::NodeHandle クラスの serviceClient() 関数でサービスクライアントを作成する。

// (1)
template <class MReq, class MRes>
ServiceClient serviceClient(const std::string &service_name, bool persistent = false,
                            const M_string &header_values = M_string())

// (2)
template <class Service>
ServiceClient serviceClient(const std::string &service_name, bool persistent = false,
                            const M_string &header_values = M_string())

テンプレートでサービスの型を指定し、引数にサービス名を指定する。
(2) のオーバーロード関数を使えばよい。

(1)

    ros::ServiceClient client =
        node_handler.serviceClient<
            service_cpp::AddTwoInts::Request,
            service_cpp::AddTwoInts::Response>("add_two_ints");

(2)

    ros::ServiceClient client =
        node_handler.serviceClient<service_cpp::AddTwoInts>("add_two_ints");
サービスの呼び出し

ros::ServiceClient クラスの call() 関数でサービスを呼び出す。

// (1)
template<class MReq, class MRes>
bool call(MReq& req, MRes& res)

// (2)
template<class Service>
bool call(Service& service)

(1) サービスオブジェクトを渡して呼び出す。

service_cpp::AddTwoInts::Request req;
service_cpp::AddTwoInts::Response res;
// リクエストを作成する
req.a = 1;
req.b = 2;

// サービスを呼び出す
if (client.call(req, res))
{
    ROS_INFO("Sum: %ld", (long int)res.sum);
}
else
{
    ROS_ERROR("Failed to call service add_two_ints");
    return 1;
}

(2) リクエスト、レスポンスオブジェクトを別々に渡して呼び出す。

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

サービス自体が存在しない場合のほか、サービスは呼び出しせたがサーバー側のコールバック関数が false を返した場合も call()
は false を返すことに注意する。