ROSのコールバック関数、まとめたくない?
ROS は情報を受け取ろうと思うと基本的には Topic をコールバック関数で受け取ることになります。このコールバック関数はもともと定義されている型しか使えないから、ほとんど似たような処理でも Topic ごとに関数が必要になって煩雑！とか思ってませんか？
実はそれ、おまとめ出来るんですよ。そう、boost::bindならね！
※ 相変わらず筆者はkineticを使っているので、以降の文章はすべてkineticでのみ確認しています。
boost::bind
boost::bind は、他のboostライブラリと同じく多くの複雑なことが出来ますが、この記事で重要なのは「関数の形を他の形に置き換えられる」という点です。
この機能により、指定された形しか許されていないコールバック関数を別の形の関数で定義して、呼び出せるようになります。
詳しくは公式ドキュメントや、少し古いですが有志による日本語ドキュメントも参照してください。
なお、C++11以降ではC++の標準ライブラリにほぼ同等の機能が移植されており、std::bind として実装されております。ですが、ROSはC++11を要求しない仕様のため、標準ではboost::bindを使います。
ROSで使うためには、パッケージの CMakeLists.txt に find_package(Boost REQUIRED COMPONENTS system) を追加し、プログラム中で boost/bind.hpp をincludeします。
Topic のコールバック
サンプル
早速例を示しましょう。
#include <ros/ros.h>
#include <ros/console.h>
#include <std_msgs/String.h>
#include <boost/bind.hpp>
void commonCb(const std_msgs::String::ConstPtr& msg, const int num)
{
ROS_INFO("Callback. ID %d : %s", num, msg->data.c_str());
}
class hoge
{
public:
hoge()
{
sub_m1_ = nh.subscribe<std_msgs::String>("sub_m1",
1,
boost::bind(&hoge::methodCb, this, _1, 11));
sub_m2_ = nh.subscribe<std_msgs::String>("sub_m2",
1,
boost::bind(&hoge::methodCb, this, _1, 12));
}
void methodCb(const std_msgs::String::ConstPtr& msg, const int num)
{
ROS_INFO("Callback in class. ID %d : %s", num, msg->data.c_str());
}
private:
ros::NodeHandle nh;
ros::Subscriber sub_m1_;
ros::Subscriber sub_m2_;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "cb_bind_example");
ros::NodeHandle nh;
ros::Subscriber sub1 = nh.subscribe<std_msgs::String>("sub1",
1,
boost::bind(commonCb, _1, 1));
ros::Subscriber sub2 = nh.subscribe<std_msgs::String>("sub2",
1,
boost::bind(commonCb, _1, 2));
hoge h;
ros::Subscriber sub3 = nh.subscribe<std_msgs::String>("sub3",
1,
boost::bind(&hoge::methodCb, &h, _1, 3));
ros::spin();
return 0;
}
このコードは、文字列を扱う Topic を受け取ってコンソールに表示するというだけの例です。
コールバック関数がクラス内のメソッドとして定義されている場合と、そうでない場合とで若干呼び出し方が異なるので、それぞれの場合を実装してあります。
Boostのライブラリは心の目で見ると読める、などと偉大な先輩はおっしゃっておられましたが、これもなんとなく見ればわかるんじゃないかと思います。
普段、subscribe関数のコールバック関数を指定する引数ところにboost::bindがあります。そして、そのboost::bindの中で、今回作った普段とは違う形の関数を引数に与えています。
boost::bindのひとつ目の引数に呼び出す関数のポインタ等を渡すのですが、クラスのメソッドの場合にはふたつ目の引数としてクラスの実体へのポイント等が必要となります。sub3の例がわかりやすいと思います。クラス内で定義する場合にはthisポインタを利用します。
それ以降の引数が、関数に渡される引数となっており、_1 のように_から始まるものがPlaceholderと呼ばれる特殊な値であり、「もともとの呼び出し元の引数のN個目」が_Nとして表現されます。
簡単な例にすると
int f(int a, int b, int c)
{
return a + b + c;
}
という引数三個の関数を引数二個として呼びたい、与えた二個の引数はそのままに、引数の3番目には常に0を与えたい、という場合には
boost::bind(f, _1, _2, 0);
とします。バリエーションで、同じく引数二個の関数だけど、与えた引数1番目を実行関数の3番目に、2番目を1番目に入れ替えて、2番目には常に10を渡したい、という場合には
boost::bind(f, _2, 10, _1);
となります。このように、Placeholderは元の引数の順番や位置に依存せずに使うことができます。
と言っても無意味に順番を入れ替えても混乱するだけなので、今回のROSのサンプルでは通常のコールバック関数で受け取る引数のあとに追加のint引数を作り、subscriberを作るときに定数を与えています。
注意が必要なのは、boost::bindで生成した関数オブジェクトからは型の推定ができないため、subscribe関数にはテンプレートを明示する必要があるという点です。
サンプルを実行して、トピックを与えてやると、こんな感じの結果になります。
rosrun cb_bind_example cb_bind_example
[ INFO] [1576593517.940507359]: Callback. ID 1 : Hello
[ INFO] [1576593529.864747527]: Callback. ID 2 : World
[ INFO] [1576593548.404552068]: Callback in class. ID 11 : Hello method
$ rostopic list
/rosout
/rosout_agg
/sub1
/sub2
/sub3
/sub_m1
/sub_m2
$ rostopic pub /sub1 std_msgs/String "data: 'Hello'"
publishing and latching message. Press ctrl-C to terminate
$ rostopic pub /sub2 std_msgs/String "data: 'World'"
publishing and latching message. Press ctrl-C to terminate
$ rostopic pub /sub_m1 std_msgs/String "data: 'Hello method'"
publishing and latching message. Press ctrl-C to terminate
sub1とsub2でコールバック関数は一つなのに異なる処理ができていることがわかると思います。また、メソッドの場合も正しく呼ぶことができています。
Service のコールバック
当然ながらこの手法は Topic のコールバックだけに限りません。サービスでも利用可能です。
筆者が作ってる中華IMUのROSドライバを例にします。
https://github.com/strv/wit_imu_driver/blob/master/src/wit_imu_driver_node.cpp
このIMUは固定のバイナリ配列を送信することで各種キャリブレーションをトリガすることができます。それをROSのServiceとして実装したのですが、実際に必要なのはシリアルポートにデータを送るということが共通で、送る中身が異なるだけ、しかも送る内容は固定であるため、Topicの例のように関数はひとつにして、送る中身のデータを予め与えるようにしてみました。
送信に使うServiceのコールバック関数は下記のように定義しています。
bool cbSrvTrgWriteCommand(std_srvs::TriggerRequest& req
, std_srvs::TriggerResponse& res
, const std::vector<uint8_t>& bytes)
{
bool ret = sendBytes(bytes);
if (ret)
{
res.message = "Success";
res.success = true;
}
else
{
res.message = "Failed";
res.success = false;
}
return true;
}
これを登録している部分が下記のようになります。
srv_trg_yaw_clr_ = pnh_.advertiseService<std_srvs::TriggerRequest, std_srvs::TriggerResponse>(
"trigger_yaw_clear",
boost::bind(
&WitImuDriver::cbSrvTrgWriteCommand,
this,
_1,
_2,
ptr_imu_->genYawClr()));
srv_trg_acc_cal_ = pnh_.advertiseService<std_srvs::TriggerRequest, std_srvs::TriggerResponse>(
"trigger_acc_calibration",
boost::bind(
&WitImuDriver::cbSrvTrgWriteCommand,
this,
_1,
_2,
ptr_imu_->genAccCal()));
この他にも何種類か同じ関数を利用しており、関数をまとめるメリットが大きいのです。
まとめ
boost::bind 超便利。けど、「C++完全に理解した」じゃないとちゃんとは理解できない。
別にboost::bindはROSのコールバック用にあるわけではないので、他のところでも使ってみると良いでしょう。
参考文献
この記事を書き始めてから気がついたのですが、fuRoの原さんがこの辺も含めて超丁寧に説明されているスライドを公表してくださっておりました。P.68以降、数ページにわたって解説されているので、まずはそっちを読みましょう！！
https://www.slideshare.net/hara-y/ros-nav-rsj-seminar