From 11aec9576849a659e9010767297f69cd4de47674 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Wed, 30 Jun 2021 15:09:24 +0000
Subject: [PATCH] [RosQueuedSubsribe] Add command listTopics
to get the name of topics corresponding to each signal.
---
src/dynamic_graph/ros/__init__.py | 1 +
src/ros_queued_subscribe-python-module-py.cc | 2 +-
src/ros_queued_subscribe.cpp | 8 ++++++++
src/ros_queued_subscribe.hh | 3 +++
src/ros_queued_subscribe.hxx | 1 +
5 files changed, 14 insertions(+), 1 deletion(-)
diff --git a/src/dynamic_graph/ros/__init__.py b/src/dynamic_graph/ros/__init__.py
index 96018be..7f58645 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 9ee6b14..c14f8e6 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 f9ecaf5..f5226af 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 ff0eb9f..a54b683 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 99e6394..f8994a2 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;
}
};
--
GitLab