ROS2のcreate_serviceのコールバック登録が冗長すぎるからtemplateで簡略化した

概要

ROS 2でservice server作るときに一つのNodeがたくさんService提供している場合、Serviceを持つインスタンスがメンバ変数にその数分存在して鬱陶しいです。

class SomeNode : rclcpp::Node
{
public:
  explicit SomeNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
  // コールバック達
  void srv_cb1(const std::shared_ptr<rmw_request_id_t> request_handler,
    const std::shared_ptr<srv_pkgs::srv::Srv1::Request> request,
    const std::shared_ptr<srv_pkgs::srv::Srv1::Response> response);
  void srv_cb2(const std::shared_ptr<rmw_request_id_t> request_handler,
    const std::shared_ptr<srv_pkgs::srv::Srv2::Request> request,
    const std::shared_ptr<srv_pkgs::srv::Srv2::Response> response);
  void srv_cb3(const std::shared_ptr<rmw_request_id_t> request_handler,
    const std::shared_ptr<srv_pkgs::srv::Srv3::Request> request,
    const std::shared_ptr<srv_pkgs::srv::Srv3::Response> response);
  void srv_cb4(const std::shared_ptr<rmw_request_id_t> request_handler,
    const std::shared_ptr<srv_pkgs::srv::Srv4::Request> request,
    const std::shared_ptr<srv_pkgs::srv::Srv4::Response> response);
private:
  // 実用上、番号で名付けるのではなく、"add_point_srv_"みたいな固有名詞を付けることが多いですが、今回は簡略化のために番号を付けています。
  rclcpp::Service<srv_pkgs::srv::Srv1>::SharedPtr srv1_;
  rclcpp::Service<srv_pkgs::srv::Srv1>::SharedPtr srv2_;
  rclcpp::Service<srv_pkgs::srv::Srv1>::SharedPtr srv3_;
  rclcpp::Service<srv_pkgs::srv::Srv1>::SharedPtr srv4_;
};

これらのコールバックはコンストラクタ等の初期化処理の中で次のように登録されます。

using namespace std::placeholders;
srv1_ = create_service<srv_pkgs::srv::Srv1>("srv1",
  std::bind(&SomeNode::srv_cb1, this, _1, _2, _3));
srv2_ = create_service<srv_pkgs::srv::Srv2>("srv2",
  std::bind(&SomeNode::srv_cb2, this, _1, _2, _3));
...

このうち、コールバックはすべて別の動作を行うため、律義に宣言していく必要があります。一方、個別に作成したsrv1_ ~ srv4のインスタンスはコールバックを登録した後特に呼び出されないことが多く、実行中にメモリに存在させているだけであることが多いです。
それに対し、私は次のような要望が浮かび上がりました。

• std::vector等に放り投げて一つの変数で管理したい。
• std::bindの記述が冗長すぎる。変化する情報はサービス名, Typeとコールバックの関数であとは全部同じ

色々応用が利くAPIとはいえ、状況が限定されれば必要な共通処理はできるだけ簡略化したくなるのが人間です。
しかし、templateが絡む記述は(特に私は慣れていないため)難しいので、その説明をしていきます。

やり方

まず一つ目の要望をかなえるため、serviceの変数を格納するためのコンテナをメンバー変数として用意します。

  // ServiceBaseは自動生成される全てのServiceの親クラスなのでどのようなServiceも受け入れられる。
  std::vector<rclcpp::ServiceBase::SharedPtr> srvs_;

次にstd::bindの記載を簡略化すべく、新しくsimple_create_serviceというメンバ関数を宣言します。ただし、wrapする元の関数はtemplate関数なので次のように宣言しなければいけません。

template<typename T>
void simple_create_service(
  const std::string& service_name,
  void (SomeNode::*member_func)(
    const std::shared_ptr<rmw_request_id_t>,
    const std::shared_ptr<typename T::Request>,
    const std::shared_ptr<typename T::Response>)
  )
{
  srvs_.push_back(create_service<T>(service_name,
    std::bind(member_func, this,
      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3
    )
  );
}

この関数は第一引数にサービス名、第二引数にSomeNodeのクラスメソッドのアドレスを受け取ります(ただしvoid(const std::shared_ptr<... )の形である必要があります)。全てのサービスの返り値・引数が全て同様の形式なのでこれでいけるわけです。
これでcreate_service簡易版であるsimple_create_serviceの宣言ができました。この関数はtemplateなので必ず実装も含めヘッダファイルに書く必要があります。
impl形式等を用いる場合、std::bindするインスタンスはthis以外になるのでそこも注意してください。
あと、std::placeholdersを律義に書いていますが、ヘッダファイル内でusing namespaceを宣言してしまうと、そのヘッダを利用する全てのプログラムに影響を与えるためです。もしusing namespaceを使いたい場合、その辺も考慮する必要があります。

実際に使う場合、次のように呼び出します。

simple_create_service<srv_pkgs::srv::Srv1>("srv1", &SomeNode::srv_cb1);

変化する情報のみ記載されるようになったため、複数行並べてもかなり見やすいと思います。
冗長な記述は見るほうも書くほうも嫌なのでまとめていきましょう。

この呼び出しに対し、関数templateは次のようにtypename Tがsrv_pkgs::srv::Srv1に変換されたものが実行されます。
(templateの挙動を理解している人には不要な説明だとは思いますが)

void simple_create_service(
  const std::string& service_name,
  void (SomeNode::*member_func)(
    const std::shared_ptr<rmw_request_id_t>,
    const std::shared_ptr<srv_pkgs::srv::Srv1::Request>,
    const std::shared_ptr<tsrv_pkgs::srv::Srv1::Response>)
  )
{
  srvs_.push_back(create_service<srv_pkgs::srv::Srv1>(service_name,
    std::bind(member_func, this,
      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3
    )
  );
}