一つのコールバック関数に複数のSubscriberを割り当てる方法

タイトルの通りです。

別々のTopicをSubscribeしている複数のSubscriberで、同じコールバック関数を呼び出す方法です。

準備

rostopic pub -r 10 chatter_0 std_msgs/String "message of chatter 0" &
rostopic pub -r 15 chatter_1 std_msgs/String "message of chatter 1" &

cpp

コールバック関数の型 ros::MessageEventがミソです。

ros::MessageEventにいくつかメソッドが用意されており、publisherの名前や topic名を取得できます。

#include "ros/ros.h"
#include "std_msgs/String.h"

void chatterCallback(const ros::MessageEvent<std_msgs::String const>& event)
{
    const ros::M_string& header = event.getConnectionHeader();
    std::string topic = header.at("topic");
    const std_msgs::StringConstPtr& msg = event.getMessage();

    ROS_INFO("[%s]:%s", topic.c_str(), msg->data.c_str());
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");
  ros::NodeHandle n;
  ros::Subscriber sub_0 = n.subscribe("/chatter_0", 1000, chatterCallback);
  ros::Subscriber sub_1 = n.subscribe("/chatter_1", 1000, chatterCallback);

  ros::spin();

  return 0;
}

結果

[ INFO] [1476365549.987671394]: [/chatter_1]:message of chatter 1
[ INFO] [1476365550.027815654]: [/chatter_0]:message of chatter 0
[ INFO] [1476365550.054415232]: [/chatter_1]:message of chatter 1
[ INFO] [1476365550.121278541]: [/chatter_1]:message of chatter 1
[ INFO] [1476365550.127638866]: [/chatter_0]:message of chatter 0

Python

rospy.Subscriber()のコンストラクタ引数 "callback_args"がミソです。 "callback_args"に指定した値が、コールバック関数の第二引数として渡されます。

#!/usr/bin/env python
import rospy
import std_msgs.msg

def callback(data, id):
  rospy.loginfo("[ID:"+str(id)+"] : " + data.data)

def listener():
    rospy.init_node('listener', anonymous=True)

    sub_0 = rospy.Subscriber("/chatter_0", std_msgs.msg.String, callback, callback_args=0)
    sub_1 = rospy.Subscriber("/chatter_1", std_msgs.msg.String, callback, callback_args=1)

    rospy.spin()

if __name__ == '__main__':
    listener()

結果

[INFO] [WallTime: 1476365751.387734] [ID:1] : message of chatter 1
[INFO] [WallTime: 1476365751.427946] [ID:0] : message of chatter 0
[INFO] [WallTime: 1476365751.454669] [ID:1] : message of chatter 1
[INFO] [WallTime: 1476365751.521102] [ID:1] : message of chatter 1
[INFO] [WallTime: 1476365751.527823] [ID:0] : message of chatter 0