ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

How to deal with a large number of (possibly variable) topic names

asked 2014-12-17 11:19:07 -0500

trianta2 gravatar image

Our system has many sensors, and in the future the number of a particular type of sensor may vary.

Currently our scheme is something like:

/sensor/temperature0
/sensor/temperature1

but this scales poorly with a large number of temperature sensors. Likewise, if there is a node which is supposed to log all temperature sensor data, it now has to be reconfigured each time the number of temperature sensors changes.

One way to deal with the explosion of sensor numbers is to include an "index" attribute inside the sensor message itself. This way we would only have a single topic

/sensor/temperature

but this would mean every subscriber which was interested in only one of the temperatures would have to discard all the other messages, resulting in unnecessary overhead and boilerplate code.

Has anyone come up with an elegant solution to this problem?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
4

answered 2014-12-17 15:04:41 -0500

kramer gravatar image

updated 2014-12-17 15:33:22 -0500

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)
edit flag offensive delete link more

Comments

BTW, having tried this myself, unless I'm missing something, using boost::bind isn't necessary. You can just use the address of the callback as normal.

M@t gravatar image M@t  ( 2020-07-09 18:44:07 -0500 )edit

Back when I wrote this, boost::bind was the accepted (only?) way to do this. Good to know it's no longer needed, thanks for the update!

kramer gravatar image kramer  ( 2022-06-23 22:26:12 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2014-12-17 11:19:07 -0500

Seen: 1,111 times

Last updated: Dec 17 '14