You are either going to have the overhead of processing many topics with one subscriber, or processing one topic/subscriber with many subscribers. That's unavoidable in a single node, and I suspect you'd prefer not to have multiple node instances. Personally, I'd opt for many subscribers; one way you might handle this is with a yaml configuration file, a launch file, and a nifty boost::bind
trick.
I should say explicitly that I haven't run the actual code below, but I do use variations of it, so I know the fundamental idea is sound.
A sensors.yaml
file (in the root directory of the sensors
package, as specified in the launch file below) contains a list of topics:
# list of sensor topics (need to prepend '/sensor/' to each)
temperature_sensors: [temperature0, temperature1, ...]
Note that the following launch file enters the contents of sensors.yaml
into the parameter server in the node's namespace (which is handy to know if you need to get the information from another node). The temperature_sensor_set
is used to specify the initial topics to be processed (which can then be modified at run time).
<launch>
<!-- type 'myclass' executable name for 'MyClass' class -->
<node pkg="sensors" name="sensors" type="myclass" >
<rosparam command="load" file="$(find sensors)/sensors.yaml" />
<!-- empty for all or subset of items from those in yaml list -->
<param name="temperature_sensor_set" value="" />
</node>
</launch>
A subscriber is created for each sensor/topic; the temperature_sensor_set
parameter specifies some selection of sensors (an empty string will mean process all of them). When setting up the subscriptions, each subscriber is associated with a unique identifier (that happens to be its vector index). Through the 'magic' of boost::bind
, the identifier becomes part of the callback mechanism and allows message filtering.
Assume the following is found in class MyClass
; I leave it to you to set up the header file and other things:
typedef struct SubInfo {
ros::Subscriber sub;
bool process;
} SubInfo;
std::vector<SubInfo> m_tempSubs;
std::map<std::string, unsigned int> m_topicToIxMap;
...
MyClass::MyClass() {
// assumes ros::init() has been called; set up all subscribers
ros::NodeHandle nh;
// get all the topics
std::vector<std::string> tempTopics;
nh.getParam("temperature_sensors", tempTopics);
// get the topic filtering list
std::string temperatureSensorList;
nh.param("temperature_sensor_set", temperatureSensorList, "");
for (unsigned int i=0; i<tempTopics.size(); ++i) {
std::string topic("/sensor/");
topic += tempTopics[i];
unsigned int ix = m_tempSubs.size();
m_topicToIxMap[tempTopics[i]] = ix;
SubInfo si;
si.sub = nh.subscribe<TemperatureMsg>(topic, 1,
boost::bind(&MyClass::tempCb, this, _1, ix));
if (temperatureSensorList.empty() ||
temperatureSensorList.find(tempTopics[i]) > 0) {
si.process = true;
} else {
si.process = false;
}
m_tempSubs.push_back(si);
}
}
MyClass::tempCb(const TemperatureMsg::ConstPtr &msg, unsigned int ix) {
// 'ix' happily corresponds with the subscriber at 'tempSubs[ix]'
if (m_tempSubs[ix].process) {
// process the message
}
}
MyClass::setSensor(const std::string &ts, bool on) {
if (m_topicToIxMap.count(ts) > 0) {
m_tempSubs[m_topicToIxMap[ts]].process = on;
}
}
You can, of course, make it more clever and/or efficient:
- embed additional parameters in the
boost::bind
- maintain a
std ...
(more)