Pynote

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

ROS - C++ によるサービスサーバーの作成方法

概要


ROS の C++ によるサービスサーバーの作成について

試した環境

コード

#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();
}
ノードハンドラ
ros::NodeHandle nodeHandler;

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

サービスサーバーの作成

ros::NodeHandle クラスの advertiseService() 関数でサービスを作成する。

// (1)
template<class T, class MReq, class MRes>
ServiceServer advertiseService(
	const std::string& service,
	bool(T::*srv_func)(MReq &, MRes &), T *obj)
// (2)
template<class T, class MReq, class MRes>
ServiceServer advertiseService(
	const std::string& service,
	bool(T::*srv_func)(ServiceEvent<MReq, MRes>&), T *obj)
// (3)
template<class T, class MReq, class MRes>
ServiceServer advertiseService(
	const std::string& service,
	bool(T::*srv_func)(MReq &, MRes &),
	const boost::shared_ptr<T>& obj)
// (4)
template<class T, class MReq, class MRes>
ServiceServer advertiseService(
	const std::string& service,
	bool(T::*srv_func)(ServiceEvent<MReq, MRes>&),
	const boost::shared_ptr<T>& obj)
// (5)
template<class MReq, class MRes>
ServiceServer advertiseService(
	const std::string& service,
	bool(*srv_func)(MReq&, MRes&))
// (6)
template<class MReq, class MRes>
ServiceServer advertiseService(
	const std::string& service,
	bool(*srv_func)(ServiceEvent<MReq, MRes>&))
// (7)
template<class MReq, class MRes>
ServiceServer advertiseService(
	const std::string& service,
	const boost::function<bool(MReq&, MRes&)>& callback,
	const VoidConstPtr& tracked_object = VoidConstPtr())
// (8)
template<class S>
ServiceServer advertiseService(
	const std::string& service,
	const boost::function<bool(S&)>& callback,
	const VoidConstPtr& tracked_object = VoidConstPtr())

8個のオーバーロード関数の中でよく使うと思われる関数だけ紹介する。

コールバック関数はリクエスト、レスポンスオブジェクトの参照を受け取り、返り値はサービスの呼び出しが正常に行われた場合は true、そうでない場合は false を返す。
正常に行われたかどうかの基準は作成者が決めて良い。
例えば、service_cpp パッケージで AddTwoInts というサービス定義ファイルを作成した場合、コールバック関数は次のようになる。

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

(1) コールバック関数にオブジェクトのメソッドを指定する。

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

class MyClass
{
  public:
    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;

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

(3) boost::shard_ptr で管理しているオブジェクトのメソッドを指定する。
API が作成されたのが C++11 以前であるため、std::shard_ptr には対応していない。

#include <boost/shared_ptr.hpp>

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

class MyClass
{
  public:
    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;

    boost::shared_ptr<MyClass> objPtr = boost::make_shared<MyClass>();
    ros::ServiceServer service = nodeHandler.advertiseService(
        "add_two_ints", &MyClass::add, objPtr);
    ros::spin();
}

(5) グローバル関数を指定する

#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();
}