[RosQueuedSubsribe] Add command listTopics

  to get the name of topics corresponding to each signal.

Author: florent-lamiraux

src/dynamic_graph/ros/__init__.py

@@ -3,3 +3,4 @@
 from .ros import RosSubscribe as RosExport
 from .ros_publish import RosPublish
 from .ros_subscribe import RosSubscribe
+from .ros_queued_subscribe import RosQueuedSubscribe

src/ros_queued_subscribe-python-module-py.cc

@@ -13,7 +13,7 @@ BOOST_PYTHON_MODULE(wrap)
     .def("rm", &dg::RosQueuedSubscribe::rm, "Remove a signal reading data from a ROS topic",
         bp::args("signal_name"))
     .def("list", &dg::RosQueuedSubscribe::list, "List signals reading data from a ROS topic")
-
+    .def("listTopics", &dg::RosQueuedSubscribe::listTopics, "List subscribed topics from ROS in the same order as list command")
     .def("clearQueue", &dg::RosQueuedSubscribe::clearQueue,
         "Empty the queue of a given signal", bp::args("signal_name"))
     .def("queueSize", &dg::RosQueuedSubscribe::queueSize,

src/ros_queued_subscribe.cpp

@@ -99,6 +99,14 @@ std::vector<std::string> RosQueuedSubscribe::list() {
   return result;
 }
 
+std::vector<std::string> RosQueuedSubscribe::listTopics()
+{
+  std::vector<std::string> result(topics_.size());
+  std::transform(topics_.begin(), topics_.end(),
+      result.begin(), [](const auto& pair) { return pair.second; });
+  return result;
+}
+
 void RosQueuedSubscribe::clear() {
   std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
   for (; it != bindedSignal_.end();) {

src/ros_queued_subscribe.hh

@@ -137,6 +137,7 @@ class RosQueuedSubscribe : public dynamicgraph::Entity {
 
   void rm(const std::string& signal);
   std::vector<std::string> list();
+  std::vector<std::string> listTopics();
   void clear();
   void clearQueue(const std::string& signal);
   void readQueue(int beginReadingAt);
@@ -146,6 +147,7 @@ class RosQueuedSubscribe : public dynamicgraph::Entity {
   void add(const std::string& type, const std::string& signal, const std::string& topic);
 
   std::map<std::string, bindedSignal_t>& bindedSignal() { return bindedSignal_; }
+  std::map<std::string, std::string>& topics() { return topics_; }
 
   ros::NodeHandle& nh() { return nh_; }
 
@@ -162,6 +164,7 @@ class RosQueuedSubscribe : public dynamicgraph::Entity {
   static const std::string docstring_;
   ros::NodeHandle& nh_;
   std::map<std::string, bindedSignal_t> bindedSignal_;
+  std::map<std::string, std::string> topics_;
 
   int readQueue_;
   // Signal<bool, int> readQueue_;

src/ros_queued_subscribe.hxx

@@ -54,6 +54,7 @@ struct Add {
 
     RosQueuedSubscribe::bindedSignal_t bindedSignal(bs);
     rosSubscribe.bindedSignal()[signal] = bindedSignal;
+    rosSubscribe.topics()[signal] = topic;
   }
 };