diff --git a/src/dynamic_graph/ros/__init__.py b/src/dynamic_graph/ros/__init__.py index 96018be4441493520b08266df51b5e609b7fd19b..7f58645523daba8ae5e7d790cd21a392378a0f9a 100644 --- a/src/dynamic_graph/ros/__init__.py +++ b/src/dynamic_graph/ros/__init__.py @@ -3,3 +3,4 @@ from .ros import RosPublish as RosImport from .ros import RosSubscribe as RosExport from .ros_publish import RosPublish from .ros_subscribe import RosSubscribe +from .ros_queued_subscribe import RosQueuedSubscribe diff --git a/src/ros_queued_subscribe-python-module-py.cc b/src/ros_queued_subscribe-python-module-py.cc index 9ee6b14f44064e5f03095f9d652ce2fc95d983a7..c14f8e638b81c54838b43bdc5f07468d3cc268e7 100644 --- a/src/ros_queued_subscribe-python-module-py.cc +++ b/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, diff --git a/src/ros_queued_subscribe.cpp b/src/ros_queued_subscribe.cpp index f9ecaf5a2a03b1420285c7d0a7ad30b837d72cd1..f5226afcd62678387ed7f222c7c9b633b3b5d4ef 100644 --- a/src/ros_queued_subscribe.cpp +++ b/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();) { diff --git a/src/ros_queued_subscribe.hh b/src/ros_queued_subscribe.hh index ff0eb9f4437b425005fe64545e0286cadb80af8a..a54b6831ad6c59a1d1e119c233ca6b62fd78dfee 100644 --- a/src/ros_queued_subscribe.hh +++ b/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_; diff --git a/src/ros_queued_subscribe.hxx b/src/ros_queued_subscribe.hxx index 99e6394fc26045af3fb702a48de21b9d5edecbbe..f8994a248ab3ab39cbb854d692f10177b934c081 100644 --- a/src/ros_queued_subscribe.hxx +++ b/src/ros_queued_subscribe.hxx @@ -54,6 +54,7 @@ struct Add { RosQueuedSubscribe::bindedSignal_t bindedSignal(bs); rosSubscribe.bindedSignal()[signal] = bindedSignal; + rosSubscribe.topics()[signal] = topic; } };