src/ros_publish.hh

 #ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HH
-# define DYNAMIC_GRAPH_ROS_PUBLISH_HH
-# include <iostream>
-# include <map>
+#define DYNAMIC_GRAPH_ROS_PUBLISH_HH
+#include <dynamic-graph/command.h>
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <ros/ros.h>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <fstream>
+#include <map>
+
+#include "converter.hh"
+#include "sot_to_ros.hh"
+
+namespace dynamicgraph {
+class RosPublish;
+
+namespace command {
+namespace rosPublish {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
+
+#define ROS_PUBLISH_MAKE_COMMAND(CMD)                      \
+  class CMD : public Command {                             \
+   public:                                                 \
+    CMD(RosPublish& entity, const std::string& docstring); \
+    virtual Value doExecute();                             \
+  }
+
+ROS_PUBLISH_MAKE_COMMAND(Add);
 
-# include <boost/shared_ptr.hpp>
-# include <boost/tuple/tuple.hpp>
+#undef ROS_PUBLISH_MAKE_COMMAND
 
-# include <dynamic-graph/entity.h>
-# include <dynamic-graph/signal-time-dependent.h>
-# include <dynamic-graph/command.h>
+}  // namespace rosPublish
+}  // end of namespace command.
 
-# include <ros/ros.h>
+/// \brief Publish dynamic-graph information into ROS.
+class RosPublish : public dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
 
-# include <realtime_tools/realtime_publisher.h>
+ public:
+  typedef boost::function<void(int)> callback_t;
 
-# include "converter.hh"
-# include "sot_to_ros.hh"
+  typedef boost::tuple<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
+                       callback_t>
+      bindedSignal_t;
 
-namespace dynamicgraph
-{
-  class RosPublish;
+  static const double ROS_JOINT_STATE_PUBLISHER_RATE;
 
-  namespace command
-  {
-    namespace rosPublish
-    {
-      using ::dynamicgraph::command::Command;
-      using ::dynamicgraph::command::Value;
+  RosPublish(const std::string& n);
+  virtual ~RosPublish();
 
-# define ROS_PUBLISH_MAKE_COMMAND(CMD)			\
-      class CMD : public Command			\
-      {							\
-      public:						\
-	CMD (RosPublish& entity,				\
-	     const std::string& docstring);		\
-	virtual Value doExecute ();			\
-      }
+  virtual std::string getDocString() const;
+  void display(std::ostream& os) const;
 
-      ROS_PUBLISH_MAKE_COMMAND(Add);
-      ROS_PUBLISH_MAKE_COMMAND(Clear);
-      ROS_PUBLISH_MAKE_COMMAND(List);
-      ROS_PUBLISH_MAKE_COMMAND(Rm);
+  void add(const std::string& signal, const std::string& topic);
+  void rm(const std::string& signal);
+  std::vector<std::string> list() const;
+  void clear();
 
-#undef ROS_PUBLISH_MAKE_COMMAND
+  int& trigger(int&, int);
+
+  template <typename T>
+  void sendData(
+      boost::shared_ptr<
+          realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> >
+          publisher,
+      boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time);
+
+  template <typename T>
+  void add(const std::string& signal, const std::string& topic);
+
+ private:
+  static const std::string docstring_;
+  ros::NodeHandle& nh_;
+  std::map<std::string, bindedSignal_t> bindedSignal_;
+  dynamicgraph::SignalTimeDependent<int, int> trigger_;
+  ros::Duration rate_;
+  ros::Time nextPublication_;
+  boost::mutex mutex_;
+  std::ofstream aofs_;
+  struct timespec nextPublicationRT_;
+};
+}  // end of namespace dynamicgraph.
 
-    } // end of namespace errorEstimator.
-  } // end of namespace command.
-
-
-  /// \brief Publish dynamic-graph information into ROS.
-  class RosPublish : public dynamicgraph::Entity
-  {
-    DYNAMIC_GRAPH_ENTITY_DECL();
-  public:
-    typedef boost::function<void (int)> callback_t;
-
-    typedef boost::tuple<
-      boost::shared_ptr<dynamicgraph::SignalBase<int> >,
-      callback_t>
-    bindedSignal_t;
-
-    static const double ROS_JOINT_STATE_PUBLISHER_RATE;
-
-    RosPublish (const std::string& n);
-    virtual ~RosPublish ();
-
-    virtual std::string getDocString () const;
-    void display (std::ostream& os) const;
-
-    void add (const std::string& signal, const std::string& topic);
-    void rm (const std::string& signal);
-    std::string list () const;
-    void clear ();
-
-    int& trigger (int&, int);
-
-    template <typename T>
-    void
-    sendData
-    (boost::shared_ptr
-     <realtime_tools::RealtimePublisher
-     <typename SotToRos<T>::ros_t> > publisher,
-     boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal,
-     int time);
-
-    template <typename T>
-    void add (const std::string& signal, const std::string& topic);
-
-  private:
-    static const std::string docstring_;
-    ros::NodeHandle& nh_;
-    std::map<std::string, bindedSignal_t> bindedSignal_;
-    dynamicgraph::SignalTimeDependent<int,int> trigger_;
-    ros::Duration rate_;
-    ros::Time lastPublicated_;
-  };
-} // end of namespace dynamicgraph.
-
-# include "ros_publish.hxx"
-#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HH
+#include "ros_publish.hxx"
+#endif  //! DYNAMIC_GRAPH_ROS_PUBLISH_HH

src/ros_publish.hxx

 #ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HXX
-# define DYNAMIC_GRAPH_ROS_PUBLISH_HXX
-# include <vector>
-# include <std_msgs/Float64.h>
+#define DYNAMIC_GRAPH_ROS_PUBLISH_HXX
+#include <std_msgs/Float64.h>
 
-# include "dynamic_graph_bridge/Matrix.h"
-# include "dynamic_graph_bridge/Vector.h"
+#include <vector>
 
-# include "sot_to_ros.hh"
+#include "dynamic_graph_bridge_msgs/Matrix.h"
+#include "dynamic_graph_bridge_msgs/Vector.h"
+#include "sot_to_ros.hh"
 
-# include <iostream>
-
-namespace dynamicgraph
-{
-  template <>
-  inline void
-  RosPublish::sendData
-  <std::pair<sot::MatrixHomogeneous, ml::Vector> >
-  (boost::shared_ptr
-   <realtime_tools::RealtimePublisher
-   <SotToRos
-   <std::pair<sot::MatrixHomogeneous, ml::Vector> >::ros_t> > publisher,
-   boost::shared_ptr
-   <SotToRos
-   <std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalIn_t> signal,
-   int time)
-  {
-    SotToRos
-      <std::pair
-      <sot::MatrixHomogeneous, ml::Vector> >::ros_t result;
-    if (publisher->trylock ())
-      {
-	publisher->msg_.child_frame_id = "/dynamic_graph/world";
-	converter (publisher->msg_, signal->access (time));
-	publisher->unlockAndPublish ();
-      }
+namespace dynamicgraph {
+template <>
+inline void RosPublish::sendData<std::pair<sot::MatrixHomogeneous, Vector> >(
+    boost::shared_ptr<realtime_tools::RealtimePublisher<
+        SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t> >
+        publisher,
+    boost::shared_ptr<
+        SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalIn_t>
+        signal,
+    int time) {
+  SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t result;
+  if (publisher->trylock()) {
+    publisher->msg_.child_frame_id = "/dynamic_graph/world";
+    converter(publisher->msg_, signal->access(time));
+    publisher->unlockAndPublish();
   }
+}
 
-  template <typename T>
-  void
-  RosPublish::sendData
-  (boost::shared_ptr
-   <realtime_tools::RealtimePublisher
-   <typename SotToRos<T>::ros_t> > publisher,
-   boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal,
-   int time)
-  {
-    typename SotToRos<T>::ros_t result;
-    if (publisher->trylock ())
-      {
-	converter (publisher->msg_, signal->access (time));
-	publisher->unlockAndPublish ();
-      }
+template <typename T>
+void RosPublish::sendData(
+    boost::shared_ptr<
+        realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> >
+        publisher,
+    boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time) {
+  typename SotToRos<T>::ros_t result;
+  if (publisher->trylock()) {
+    converter(publisher->msg_, signal->access(time));
+    publisher->unlockAndPublish();
   }
+}
 
-  template <typename T>
-  void RosPublish::add (const std::string& signal, const std::string& topic)
-  {
-    typedef typename SotToRos<T>::sot_t sot_t;
-    typedef typename SotToRos<T>::ros_t ros_t;
-    typedef typename SotToRos<T>::signalIn_t signal_t;
+template <typename T>
+void RosPublish::add(const std::string& signal, const std::string& topic) {
+  typedef typename SotToRos<T>::ros_t ros_t;
+  typedef typename SotToRos<T>::signalIn_t signal_t;
 
-    // Initialize the bindedSignal object.
-    bindedSignal_t bindedSignal;
+  // Initialize the bindedSignal object.
+  bindedSignal_t bindedSignal;
 
-    // Initialize the publisher.
-    boost::shared_ptr
-      <realtime_tools::RealtimePublisher<ros_t> >
-      pubPtr =
-      boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >
-      (nh_, topic, 1);
+  // Initialize the publisher.
+  boost::shared_ptr<realtime_tools::RealtimePublisher<ros_t> > pubPtr =
+      boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >(nh_, topic,
+                                                                    1);
 
-    // Initialize the signal.
-    boost::shared_ptr<signal_t> signalPtr
-      (new signal_t
-       (0,
-	MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal)));
-    boost::get<0> (bindedSignal) = signalPtr;
-    SotToRos<T>::setDefault(*signalPtr);
-    signalRegistration (*boost::get<0> (bindedSignal));
+  // Initialize the signal.
+  boost::shared_ptr<signal_t> signalPtr(new signal_t(
+      0, MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal)));
+  boost::get<0>(bindedSignal) = signalPtr;
+  SotToRos<T>::setDefault(*signalPtr);
+  signalRegistration(*boost::get<0>(bindedSignal));
 
-    // Initialize the callback.
-    callback_t callback = boost::bind
-      (&RosPublish::sendData<T>,
-       this,
-       pubPtr,
-       signalPtr,
-       _1);
-    boost::get<1> (bindedSignal) = callback;
+  // Initialize the callback.
+  callback_t callback =
+      boost::bind(&RosPublish::sendData<T>, this, pubPtr, signalPtr, _1);
+  boost::get<1>(bindedSignal) = callback;
 
-    bindedSignal_[signal] = bindedSignal;
-  }
+  bindedSignal_[signal] = bindedSignal;
+}
 
-} // end of namespace dynamicgraph.
+}  // end of namespace dynamicgraph.
 
-#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HXX
+#endif  //! DYNAMIC_GRAPH_ROS_PUBLISH_HXX

src/ros_queued_subscribe-python-module-py.cc

+#include <dynamic-graph/python/module.hh>
+
+#include "ros_queued_subscribe.hh"
+
+namespace dg = dynamicgraph;
+
+BOOST_PYTHON_MODULE(wrap) {
+  bp::import("dynamic_graph");
+
+  dg::python::exposeEntity<dg::RosQueuedSubscribe, bp::bases<dg::Entity>,
+                           dg::python::AddCommands>()
+      .def("clear", &dg::RosQueuedSubscribe::clear,
+           "Remove all signals reading data from a ROS topic")
+      .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,
+           "Return the queue size of a given signal", bp::args("signal_name"))
+      .def("readQueue", &dg::RosQueuedSubscribe::readQueue,
+           "Whether signals should read values from the queues, and when.",
+           bp::args("start_time"));
+}

src/ros_queued_subscribe.cpp

+//
+// Copyright (c) 2017-2018 CNRS
+// Authors: Joseph Mirabel
+//
+//
+
+#include "ros_queued_subscribe.hh"
+
+#include <dynamic-graph/factory.h>
+#include <ros/ros.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/UInt32.h>
+
+#include <boost/assign.hpp>
+#include <boost/bind.hpp>
+#include <boost/format.hpp>
+#include <boost/function.hpp>
+#include <boost/make_shared.hpp>
+
+#include "dynamic_graph_bridge/ros_init.hh"
+
+namespace dynamicgraph {
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosQueuedSubscribe, "RosQueuedSubscribe");
+
+namespace command {
+namespace rosQueuedSubscribe {
+Add::Add(RosQueuedSubscribe& entity, const std::string& docstring)
+    : Command(
+          entity,
+          boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
+          docstring) {}
+
+Value Add::doExecute() {
+  RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner());
+  std::vector<Value> values = getParameterValues();
+
+  const std::string& type = values[0].value();
+  const std::string& signal = values[1].value();
+  const std::string& topic = values[2].value();
+
+  if (type == "double")
+    entity.add<double>(type, signal, topic);
+  else if (type == "unsigned")
+    entity.add<unsigned int>(type, signal, topic);
+  else if (type == "matrix")
+    entity.add<Matrix>(type, signal, topic);
+  else if (type == "vector")
+    entity.add<Vector>(type, signal, topic);
+  else if (type == "vector3")
+    entity.add<specific::Vector3>(type, signal, topic);
+  else if (type == "matrixHomo")
+    entity.add<sot::MatrixHomogeneous>(type, signal, topic);
+  else if (type == "twist")
+    entity.add<specific::Twist>(type, signal, topic);
+  else
+    throw std::runtime_error("bad type");
+  return Value();
+}
+}  // namespace rosQueuedSubscribe
+}  // end of namespace command.
+
+const std::string RosQueuedSubscribe::docstring_(
+    "Subscribe to a ROS topics and convert it into a dynamic-graph signals.\n"
+    "\n"
+    "  Use command \"add\" to subscribe to a new signal.\n");
+
+RosQueuedSubscribe::RosQueuedSubscribe(const std::string& n)
+    : dynamicgraph::Entity(n),
+      nh_(rosInit(true)),
+      bindedSignal_(),
+      readQueue_(-1) {
+  std::string docstring =
+      "\n"
+      "  Add a signal reading data from a ROS topic\n"
+      "\n"
+      "  Input:\n"
+      "    - type: string among ['double', 'matrix', 'vector', 'vector3',\n"
+      "                          'matrixHomo', 'twist'],\n"
+      "    - signal: the signal name in dynamic-graph,\n"
+      "    - topic:  the topic name in ROS.\n"
+      "\n";
+  addCommand("add", new command::rosQueuedSubscribe::Add(*this, docstring));
+}
+
+RosQueuedSubscribe::~RosQueuedSubscribe() {}
+
+void RosQueuedSubscribe::display(std::ostream& os) const {
+  os << CLASS_NAME << std::endl;
+}
+
+void RosQueuedSubscribe::rm(const std::string& signal) {
+  std::string signalTs = signal + "Timestamp";
+
+  signalDeregistration(signal);
+  bindedSignal_.erase(signal);
+
+  if (bindedSignal_.find(signalTs) != bindedSignal_.end()) {
+    signalDeregistration(signalTs);
+    bindedSignal_.erase(signalTs);
+  }
+}
+
+std::vector<std::string> RosQueuedSubscribe::list() {
+  std::vector<std::string> result(bindedSignal_.size());
+  std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
+                 [](const auto& pair) { return pair.first; });
+  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();) {
+    rm(it->first);
+    it = bindedSignal_.begin();
+  }
+}
+
+void RosQueuedSubscribe::clearQueue(const std::string& signal) {
+  if (bindedSignal_.find(signal) != bindedSignal_.end()) {
+    bindedSignal_[signal]->clear();
+  }
+}
+
+std::size_t RosQueuedSubscribe::queueSize(const std::string& signal) const {
+  std::map<std::string, bindedSignal_t>::const_iterator _bs =
+      bindedSignal_.find(signal);
+  if (_bs != bindedSignal_.end()) {
+    return _bs->second->size();
+  }
+  return -1;
+}
+
+void RosQueuedSubscribe::readQueue(int beginReadingAt) {
+  // Prints signal queues sizes
+  /*for (std::map<std::string, bindedSignal_t>::const_iterator it =
+         bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
+    std::cout << it->first << " : " << it->second->size() << '\n';
+  }*/
+  readQueue_ = beginReadingAt;
+}
+
+std::string RosQueuedSubscribe::getDocString() const { return docstring_; }
+}  // end of namespace dynamicgraph.

src/ros_queued_subscribe.hh

+//
+// Copyright (c) 2017-2018 CNRS
+// Authors: Joseph Mirabel
+//
+//
+
+#ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
+#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
+#include <dynamic-graph/command.h>
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <ros/ros.h>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/thread/mutex.hpp>
+#include <map>
+#include <sot/core/matrix-geometry.hh>
+
+#include "converter.hh"
+#include "sot_to_ros.hh"
+
+namespace dynamicgraph {
+class RosQueuedSubscribe;
+
+namespace command {
+namespace rosQueuedSubscribe {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
+
+#define ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(CMD)                     \
+  class CMD : public Command {                                     \
+   public:                                                         \
+    CMD(RosQueuedSubscribe& entity, const std::string& docstring); \
+    virtual Value doExecute();                                     \
+  }
+
+ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Add);
+
+#undef ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND
+
+}  // end of namespace rosQueuedSubscribe.
+}  // end of namespace command.
+
+class RosQueuedSubscribe;
+
+namespace internal {
+template <typename T>
+struct Add;
+
+struct BindedSignalBase {
+  typedef boost::shared_ptr<ros::Subscriber> Subscriber_t;
+
+  BindedSignalBase(RosQueuedSubscribe* e) : entity(e) {}
+  virtual ~BindedSignalBase() {}
+
+  virtual void clear() = 0;
+  virtual std::size_t size() const = 0;
+
+  Subscriber_t subscriber;
+  RosQueuedSubscribe* entity;
+};
+
+template <typename T, int BufferSize>
+struct BindedSignal : BindedSignalBase {
+  typedef dynamicgraph::Signal<T, int> Signal_t;
+  typedef boost::shared_ptr<Signal_t> SignalPtr_t;
+  typedef std::vector<T> buffer_t;
+  typedef typename buffer_t::size_type size_type;
+
+  BindedSignal(RosQueuedSubscribe* e)
+      : BindedSignalBase(e),
+        frontIdx(0),
+        backIdx(0),
+        buffer(BufferSize),
+        init(false) {}
+  ~BindedSignal() {
+    signal.reset();
+    clear();
+  }
+
+  /// See comments in reader and writer for details about synchronisation.
+  void clear() {
+    // synchronize with method writer
+    wmutex.lock();
+    if (!empty()) {
+      if (backIdx == 0)
+        last = buffer[BufferSize - 1];
+      else
+        last = buffer[backIdx - 1];
+    }
+    // synchronize with method reader
+    rmutex.lock();
+    frontIdx = backIdx = 0;
+    rmutex.unlock();
+    wmutex.unlock();
+  }
+
+  bool empty() const { return frontIdx == backIdx; }
+
+  bool full() const { return ((backIdx + 1) % BufferSize) == frontIdx; }
+
+  size_type size() const {
+    if (frontIdx <= backIdx)
+      return backIdx - frontIdx;
+    else
+      return backIdx + BufferSize - frontIdx;
+  }
+
+  SignalPtr_t signal;
+  /// Index of the next value to be read.
+  size_type frontIdx;
+  /// Index of the slot where to write next value (does not contain valid data).
+  size_type backIdx;
+  buffer_t buffer;
+  boost::mutex wmutex, rmutex;
+  T last;
+  bool init;
+
+  template <typename R>
+  void writer(const R& data);
+  T& reader(T& val, int time);
+};
+}  // namespace internal
+
+/// \brief Publish ROS information in the dynamic-graph.
+class RosQueuedSubscribe : public dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
+  typedef boost::posix_time::ptime ptime;
+
+ public:
+  typedef boost::shared_ptr<internal::BindedSignalBase> bindedSignal_t;
+
+  RosQueuedSubscribe(const std::string& n);
+  virtual ~RosQueuedSubscribe();
+
+  virtual std::string getDocString() const;
+  void display(std::ostream& os) const;
+
+  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);
+  std::size_t queueSize(const std::string& signal) const;
+
+  template <typename T>
+  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_; }
+
+  template <typename R, typename S>
+  void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
+                const R& data);
+
+  template <typename R>
+  void callbackTimestamp(
+      boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
+      const R& data);
+
+  template <typename T>
+  friend class internal::Add;
+
+ private:
+  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_;
+
+  template <typename T>
+  friend class internal::BindedSignal;
+};
+}  // end of namespace dynamicgraph.
+
+#include "ros_queued_subscribe.hxx"
+#endif  //! DYNAMIC_GRAPH_QUEUED_ROS_SUBSCRIBE_HH

src/ros_queued_subscribe.hxx

+//
+// Copyright (c) 2017-2018 CNRS
+// Authors: Joseph Mirabel
+//
+//
+
+#ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
+#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
+#define ENABLE_RT_LOG
+
+#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/real-time-logger.h>
+#include <dynamic-graph/signal-cast-helper.h>
+#include <dynamic-graph/signal-caster.h>
+#include <std_msgs/Float64.h>
+
+#include <boost/bind.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <vector>
+
+#include "dynamic_graph_bridge_msgs/Matrix.h"
+#include "dynamic_graph_bridge_msgs/Vector.h"
+
+namespace dynamicgraph {
+namespace internal {
+static const int BUFFER_SIZE = 1 << 10;
+
+template <typename T>
+struct Add {
+  void operator()(RosQueuedSubscribe& rosSubscribe, const std::string& type,
+                  const std::string& signal, const std::string& topic) {
+    typedef typename SotToRos<T>::sot_t sot_t;
+    typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
+    typedef BindedSignal<sot_t, BUFFER_SIZE> BindedSignal_t;
+    typedef typename BindedSignal_t::Signal_t Signal_t;
+
+    // Initialize the bindedSignal object.
+    BindedSignal_t* bs = new BindedSignal_t(&rosSubscribe);
+    SotToRos<T>::setDefault(bs->last);
+
+    // Initialize the signal.
+    boost::format signalName("RosQueuedSubscribe(%1%)::output(%2%)::%3%");
+    signalName % rosSubscribe.getName() % type % signal;
+
+    bs->signal.reset(new Signal_t(signalName.str()));
+    bs->signal->setFunction(boost::bind(&BindedSignal_t::reader, bs, _1, _2));
+    rosSubscribe.signalRegistration(*bs->signal);
+
+    // Initialize the subscriber.
+    typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
+    callback_t callback =
+        boost::bind(&BindedSignal_t::template writer<ros_const_ptr_t>, bs, _1);
+
+    // Keep 50 messages in queue, but only 20 are sent every 100ms
+    // -> No message should be lost because of a full buffer
+    bs->subscriber = boost::make_shared<ros::Subscriber>(
+        rosSubscribe.nh().subscribe(topic, BUFFER_SIZE, callback));
+
+    RosQueuedSubscribe::bindedSignal_t bindedSignal(bs);
+    rosSubscribe.bindedSignal()[signal] = bindedSignal;
+    rosSubscribe.topics()[signal] = topic;
+  }
+};
+
+// template <typename T, typename R>
+template <typename T, int N>
+template <typename R>
+void BindedSignal<T, N>::writer(const R& data) {
+  // synchronize with method clear
+  boost::mutex::scoped_lock lock(wmutex);
+  if (full()) {
+    rmutex.lock();
+    frontIdx = (frontIdx + 1) % N;
+    rmutex.unlock();
+  }
+  converter(buffer[backIdx], data);
+  // No need to synchronize with reader here because:
+  // - if the buffer was not empty, then it stays not empty,
+  // - if it was empty, then the current value will be used at next time. It
+  //   means the transmission bandwidth is too low.
+  if (!init) {
+    last = buffer[backIdx];
+    init = true;
+  }
+  backIdx = (backIdx + 1) % N;
+}
+
+template <typename T, int N>
+T& BindedSignal<T, N>::reader(T& data, int time) {
+  // synchronize with method clear:
+  // If reading from the list cannot be done, then return last value.
+  boost::mutex::scoped_lock lock(rmutex, boost::try_to_lock);
+  if (!lock.owns_lock() || entity->readQueue_ == -1 ||
+      time < entity->readQueue_) {
+    data = last;
+  } else {
+    if (empty())
+      data = last;
+    else {
+      data = buffer[frontIdx];
+      frontIdx = (frontIdx + 1) % N;
+      last = data;
+    }
+  }
+  return data;
+}
+}  // end of namespace internal.
+
+template <typename T>
+void RosQueuedSubscribe::add(const std::string& type, const std::string& signal,
+                             const std::string& topic) {
+  internal::Add<T>()(*this, type, signal, topic);
+}
+}  // end of namespace dynamicgraph.
+
+#endif  //! DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX

src/ros_subscribe-python-module-py.cc

+#include <dynamic-graph/python/module.hh>
+
+#include "ros_subscribe.hh"
+
+namespace dg = dynamicgraph;
+
+BOOST_PYTHON_MODULE(wrap) {
+  bp::import("dynamic_graph");
+
+  dg::python::exposeEntity<dg::RosSubscribe, bp::bases<dg::Entity>,
+                           dg::python::AddCommands>()
+      .def("clear", &dg::RosSubscribe::clear,
+           "Remove all signals reading data from a ROS topic")
+      .def("rm", &dg::RosSubscribe::rm,
+           "Remove a signal reading data from a ROS topic",
+           bp::args("signal_name"))
+      .def("list", &dg::RosSubscribe::list,
+           "List signals reading data from a ROS topic");
+}

src/ros_subscribe.cpp

-#include <boost/assign.hpp>
-#include <boost/bind.hpp>
-#include <boost/format.hpp>
-#include <boost/function.hpp>
-#include <boost/make_shared.hpp>
+#include "ros_subscribe.hh"
 
+#include <dynamic-graph/factory.h>
 #include <ros/ros.h>
 #include <std_msgs/Float64.h>
 #include <std_msgs/UInt32.h>
 
-#include <dynamic-graph/factory.h>
+#include <boost/assign.hpp>
+#include <boost/bind.hpp>
+#include <boost/format.hpp>
+#include <boost/function.hpp>
+#include <boost/make_shared.hpp>
 
 #include "dynamic_graph_bridge/ros_init.hh"
-#include "ros_subscribe.hh"
 
-namespace dynamicgraph
-{
-  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe");
-
-  namespace command
-  {
-    namespace rosSubscribe
-    {
-      Clear::Clear
-      (RosSubscribe& entity, const std::string& docstring)
-	: Command
-	  (entity,
-	   std::vector<Value::Type> (),
-	   docstring)
-      {}
-
-      Value Clear::doExecute ()
-      {
-	RosSubscribe& entity =
-	  static_cast<RosSubscribe&> (owner ());
-
-	entity.clear ();
-	return Value ();
-      }
-
-      List::List
-      (RosSubscribe& entity, const std::string& docstring)
-	: Command
-	  (entity,
-	   std::vector<Value::Type> (),
-	   docstring)
-      {}
-
-      Value List::doExecute ()
-      {
-	RosSubscribe& entity =
-	  static_cast<RosSubscribe&> (owner ());
-	return Value (entity.list ());
-      }
-
-      Add::Add
-      (RosSubscribe& entity, const std::string& docstring)
-	: Command
-	  (entity,
-	   boost::assign::list_of
-	   (Value::STRING) (Value::STRING) (Value::STRING),
-	   docstring)
-      {}
-
-      Value Add::doExecute ()
-      {
-	RosSubscribe& entity =
-	  static_cast<RosSubscribe&> (owner ());
-	std::vector<Value> values = getParameterValues ();
-
-	const std::string& type = values[0].value ();
-	const std::string& signal = values[1].value ();
-	const std::string& topic = values[2].value ();
-
-	if (type == "double")
-	  entity.add<double> (signal, topic);
-	else if (type == "unsigned")
-	  entity.add<unsigned int> (signal, topic);
-	else if (type == "matrix")
-	  entity.add<ml::Matrix> (signal, topic);
-	else if (type == "vector")
-	  entity.add<ml::Vector> (signal, topic);
-	else if (type == "vector3")
-	  entity.add<specific::Vector3> (signal, topic);
-	else if (type == "vector3Stamped")
-	  entity.add<std::pair<specific::Vector3, ml::Vector> > (signal, topic);
-	else if (type == "matrixHomo")
-	  entity.add<sot::MatrixHomogeneous> (signal, topic);
-	else if (type == "matrixHomoStamped")
-	  entity.add<std::pair<sot::MatrixHomogeneous, ml::Vector> >
-	    (signal, topic);
-	else if (type == "twist")
-	  entity.add<specific::Twist> (signal, topic);
-	else if (type == "twistStamped")
-	  entity.add<std::pair<specific::Twist, ml::Vector> >
-	    (signal, topic);
-	else
-	  throw std::runtime_error("bad type");
-	return Value ();
-      }
-
-      Rm::Rm
-      (RosSubscribe& entity, const std::string& docstring)
-	: Command
-	  (entity,
-	   boost::assign::list_of (Value::STRING),
-	   docstring)
-      {}
-
-      Value Rm::doExecute ()
-      {
-	RosSubscribe& entity =
-	  static_cast<RosSubscribe&> (owner ());
-	std::vector<Value> values = getParameterValues ();
-	const std::string& signal = values[0].value ();
-	entity.rm (signal);
-	return Value ();
-      }
-    } // end of errorEstimator.
-  } // end of namespace command.
-
-  const std::string RosSubscribe::docstring_
-  ("Subscribe to a ROS topics and convert it into a dynamic-graph signals.\n"
-   "\n"
-   "  Use command \"add\" to subscribe to a new signal.\n");
-
-  RosSubscribe::RosSubscribe (const std::string& n)
-    : dynamicgraph::Entity(n),
-      nh_ (rosInit (true)),
-      bindedSignal_ ()
-  {
-    std::string docstring =
+namespace dynamicgraph {
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe");
+
+namespace command {
+namespace rosSubscribe {
+Add::Add(RosSubscribe& entity, const std::string& docstring)
+    : Command(
+          entity,
+          boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
+          docstring) {}
+
+Value Add::doExecute() {
+  RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
+  std::vector<Value> values = getParameterValues();
+
+  const std::string& type = values[0].value();
+  const std::string& signal = values[1].value();
+  const std::string& topic = values[2].value();
+
+  if (type == "double")
+    entity.add<double>(signal, topic);
+  else if (type == "unsigned")
+    entity.add<unsigned int>(signal, topic);
+  else if (type == "matrix")
+    entity.add<dg::Matrix>(signal, topic);
+  else if (type == "vector")
+    entity.add<dg::Vector>(signal, topic);
+  else if (type == "vector3")
+    entity.add<specific::Vector3>(signal, topic);
+  else if (type == "vector3Stamped")
+    entity.add<std::pair<specific::Vector3, dg::Vector> >(signal, topic);
+  else if (type == "matrixHomo")
+    entity.add<sot::MatrixHomogeneous>(signal, topic);
+  else if (type == "matrixHomoStamped")
+    entity.add<std::pair<sot::MatrixHomogeneous, dg::Vector> >(signal, topic);
+  else if (type == "twist")
+    entity.add<specific::Twist>(signal, topic);
+  else if (type == "twistStamped")
+    entity.add<std::pair<specific::Twist, dg::Vector> >(signal, topic);
+  else if (type == "string")
+    entity.add<std::string>(signal, topic);
+  else
+    throw std::runtime_error("bad type");
+  return Value();
+}
+}  // namespace rosSubscribe
+}  // end of namespace command.
+
+const std::string RosSubscribe::docstring_(
+    "Subscribe to a ROS topics and convert it into a dynamic-graph signals.\n"
+    "\n"
+    "  Use command \"add\" to subscribe to a new signal.\n");
+
+RosSubscribe::RosSubscribe(const std::string& n)
+    : dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_() {
+  std::string docstring =
       "\n"
       "  Add a signal reading data from a ROS topic\n"
       "\n"
       "  Input:\n"
       "    - type: string among ['double', 'matrix', 'vector', 'vector3',\n"
-      "                          'vector3Stamped', 'matrixHomo', 'matrixHomoStamped',\n"
+      "                          'vector3Stamped', 'matrixHomo', "
+      "'matrixHomoStamped',\n"
       "                          'twist', 'twistStamped'],\n"
       "    - signal: the signal name in dynamic-graph,\n"
       "    - topic:  the topic name in ROS.\n"
       "\n";
-    addCommand ("add",
-		new command::rosSubscribe::Add
-		(*this, docstring));
-    docstring =
-      "\n"
-      "  Remove a signal reading data from a ROS topic\n"
-      "\n"
-      "  Input:\n"
-      "    - name of the signal to remove (see method list for the list of signals).\n"
-      "\n";
-    addCommand ("rm",
-		new command::rosSubscribe::Rm
-		(*this, docstring));
-    docstring =
-      "\n"
-      "  Remove all signals reading data from a ROS topic\n"
-      "\n"
-      "  No input:\n"
-      "\n";
-    addCommand ("clear",
-		new command::rosSubscribe::Clear
-		(*this, docstring));
-    docstring =
-      "\n"
-      "  List signals reading data from a ROS topic\n"
-      "\n"
-      "  No input:\n"
-      "\n";
-    addCommand ("list",
-		new command::rosSubscribe::List
-		(*this, docstring));
-  }
+  addCommand("add", new command::rosSubscribe::Add(*this, docstring));
+}
 
-  RosSubscribe::~RosSubscribe ()
-  {}
+RosSubscribe::~RosSubscribe() {}
 
-  void RosSubscribe::display (std::ostream& os) const
-  {
-    os << CLASS_NAME << std::endl;
-  }
+void RosSubscribe::display(std::ostream& os) const {
+  os << CLASS_NAME << std::endl;
+}
 
-  void RosSubscribe::rm (const std::string& signal)
-  {
-    bindedSignal_.erase (signal);
-  }
+void RosSubscribe::rm(const std::string& signal) {
+  std::string signalTs = signal + "Timestamp";
 
-  std::string RosSubscribe::list ()
-  {
-    std::string result("[");
-    for (std::map<std::string, bindedSignal_t>::const_iterator it =
-	   bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
-      result += "'" + it->first + "',";
-    }
-    result += "]";
-    return result;
-  }
+  signalDeregistration(signal);
+  bindedSignal_.erase(signal);
 
-  void RosSubscribe::clear ()
-  {
-    bindedSignal_.clear ();
+  if (bindedSignal_.find(signalTs) != bindedSignal_.end()) {
+    signalDeregistration(signalTs);
+    bindedSignal_.erase(signalTs);
   }
-
-  std::string RosSubscribe::getDocString () const
-  {
-    return docstring_;
+}
+
+std::vector<std::string> RosSubscribe::list() {
+  std::vector<std::string> result(bindedSignal_.size());
+  std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
+                 [](const auto& pair) { return pair.first; });
+  return result;
+}
+
+void RosSubscribe::clear() {
+  std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
+  for (; it != bindedSignal_.end();) {
+    rm(it->first);
+    it = bindedSignal_.begin();
   }
-} // end of namespace dynamicgraph.
+}
+
+std::string RosSubscribe::getDocString() const { return docstring_; }
+}  // end of namespace dynamicgraph.

src/ros_subscribe.hh

 #ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
-# define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
-# include <iostream>
-# include <map>
-
-# include <boost/shared_ptr.hpp>
-
-# include <dynamic-graph/entity.h>
-# include <dynamic-graph/signal-time-dependent.h>
-# include <dynamic-graph/signal-ptr.h>
-# include <dynamic-graph/command.h>
-# include <sot/core/matrix-homogeneous.hh>
-
-# include <ros/ros.h>
-
-# include "converter.hh"
-# include "sot_to_ros.hh"
-
-namespace dynamicgraph
-{
-  class RosSubscribe;
-
-  namespace command
-  {
-    namespace rosSubscribe
-    {
-      using ::dynamicgraph::command::Command;
-      using ::dynamicgraph::command::Value;
-
-# define ROS_SUBSCRIBE_MAKE_COMMAND(CMD)			\
-      class CMD : public Command			\
-      {							\
-      public:						\
-	CMD (RosSubscribe& entity,				\
-	     const std::string& docstring);		\
-	virtual Value doExecute ();			\
-      }
-
-      ROS_SUBSCRIBE_MAKE_COMMAND(Add);
-      ROS_SUBSCRIBE_MAKE_COMMAND(Clear);
-      ROS_SUBSCRIBE_MAKE_COMMAND(List);
-      ROS_SUBSCRIBE_MAKE_COMMAND(Rm);
+#define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
+#include <dynamic-graph/command.h>
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <ros/ros.h>
+
+#include <boost/shared_ptr.hpp>
+#include <map>
+#include <sot/core/matrix-geometry.hh>
+
+#include "converter.hh"
+#include "sot_to_ros.hh"
+
+namespace dynamicgraph {
+class RosSubscribe;
+
+namespace command {
+namespace rosSubscribe {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
+
+#define ROS_SUBSCRIBE_MAKE_COMMAND(CMD)                      \
+  class CMD : public Command {                               \
+   public:                                                   \
+    CMD(RosSubscribe& entity, const std::string& docstring); \
+    virtual Value doExecute();                               \
+  }
+
+ROS_SUBSCRIBE_MAKE_COMMAND(Add);
 
 #undef ROS_SUBSCRIBE_MAKE_COMMAND
 
-    } // end of namespace errorEstimator.
-  } // end of namespace command.
+}  // namespace rosSubscribe
+}  // end of namespace command.
 
+namespace internal {
+template <typename T>
+struct Add;
+}  // namespace internal
 
-  namespace internal
-  {
-    template <typename T>
-    struct Add;
-  } // end of internal namespace.
+/// \brief Publish ROS information in the dynamic-graph.
+class RosSubscribe : public dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
+  typedef boost::posix_time::ptime ptime;
 
-  /// \brief Publish ROS information in the dynamic-graph.
-  class RosSubscribe : public dynamicgraph::Entity
-  {
-    DYNAMIC_GRAPH_ENTITY_DECL();
-    typedef boost::posix_time::ptime ptime;
-  public:
-    typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
-		      boost::shared_ptr<ros::Subscriber> >
+ public:
+  typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
+                    boost::shared_ptr<ros::Subscriber> >
       bindedSignal_t;
 
-    RosSubscribe (const std::string& n);
-    virtual ~RosSubscribe ();
-
-    virtual std::string getDocString () const;
-    void display (std::ostream& os) const;
-
-    void add (const std::string& signal, const std::string& topic);
-    void rm (const std::string& signal);
-    std::string list ();
-    void clear ();
-
-    template <typename T>
-    void add (const std::string& signal, const std::string& topic);
-
-    std::map<std::string, bindedSignal_t>&
-    bindedSignal ()
-    {
-      return bindedSignal_;
-    }
-
-    ros::NodeHandle& nh ()
-    {
-      return nh_;
-    }
-
-    template <typename R, typename S>
-    void callback
-    (boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
-     const R& data);
-
-    template <typename R>
-    void callbackTimestamp
-    (boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
-     const R& data);
-
-    template <typename T>
-    friend class internal::Add;
-  private:
-    static const std::string docstring_;
-    ros::NodeHandle& nh_;
-    std::map<std::string, bindedSignal_t> bindedSignal_;
-  };
-} // end of namespace dynamicgraph.
-
-# include "ros_subscribe.hxx"
-#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
+  RosSubscribe(const std::string& n);
+  virtual ~RosSubscribe();
+
+  virtual std::string getDocString() const;
+  void display(std::ostream& os) const;
+
+  void add(const std::string& signal, const std::string& topic);
+  void rm(const std::string& signal);
+  std::vector<std::string> list();
+  void clear();
+
+  template <typename T>
+  void add(const std::string& signal, const std::string& topic);
+
+  std::map<std::string, bindedSignal_t>& bindedSignal() {
+    return bindedSignal_;
+  }
+
+  ros::NodeHandle& nh() { return nh_; }
+
+  template <typename R, typename S>
+  void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
+                const R& data);
+
+  template <typename R>
+  void callbackTimestamp(
+      boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
+      const R& data);
+
+  template <typename T>
+  friend class internal::Add;
+
+ private:
+  static const std::string docstring_;
+  ros::NodeHandle& nh_;
+  std::map<std::string, bindedSignal_t> bindedSignal_;
+};
+}  // end of namespace dynamicgraph.
+
+#include "ros_subscribe.hxx"
+#endif  //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH

src/ros_subscribe.hxx

 #ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
-# define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
-# include <vector>
-# include <boost/bind.hpp>
-# include <boost/date_time/posix_time/posix_time.hpp>
-# include <dynamic-graph/signal-caster.h>
-# include <dynamic-graph/signal-cast-helper.h>
-# include <jrl/mal/boost.hh>
-# include <std_msgs/Float64.h>
-# include "dynamic_graph_bridge/Matrix.h"
-# include "dynamic_graph_bridge/Vector.h"
-# include "ros_time.hh"
-
-namespace ml = ::maal::boost;
-
-namespace dynamicgraph
-{
-  template <typename R, typename S>
-  void
-  RosSubscribe::callback
-  (boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
-   const R& data)
-  {
-    typedef S sot_t;
-    sot_t value;
-    converter (value, data);
-    signal->setConstant (value);
+#define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
+#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/signal-cast-helper.h>
+#include <dynamic-graph/signal-caster.h>
+#include <std_msgs/Float64.h>
+
+#include <boost/bind.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <vector>
+
+#include "dynamic_graph_bridge_msgs/Matrix.h"
+#include "dynamic_graph_bridge_msgs/Vector.h"
+#include "ros_time.hh"
+
+namespace dg = dynamicgraph;
+
+namespace dynamicgraph {
+template <typename R, typename S>
+void RosSubscribe::callback(
+    boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data) {
+  typedef S sot_t;
+  sot_t value;
+  converter(value, data);
+  signal->setConstant(value);
+}
+
+template <typename R>
+void RosSubscribe::callbackTimestamp(
+    boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
+    const R& data) {
+  ptime time = rosTimeToPtime(data->header.stamp);
+  signal->setConstant(time);
+}
+
+namespace internal {
+template <typename T>
+struct Add {
+  void operator()(RosSubscribe& RosSubscribe, const std::string& signal,
+                  const std::string& topic) {
+    typedef typename SotToRos<T>::sot_t sot_t;
+    typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
+    typedef typename SotToRos<T>::signalIn_t signal_t;
+
+    // Initialize the bindedSignal object.
+    RosSubscribe::bindedSignal_t bindedSignal;
+
+    // Initialize the signal.
+    boost::format signalName("RosSubscribe(%1%)::%2%");
+    signalName % RosSubscribe.getName() % signal;
+
+    boost::shared_ptr<signal_t> signal_(new signal_t(0, signalName.str()));
+    SotToRos<T>::setDefault(*signal_);
+    bindedSignal.first = signal_;
+    RosSubscribe.signalRegistration(*bindedSignal.first);
+
+    // Initialize the subscriber.
+    typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
+    callback_t callback =
+        boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>,
+                    &RosSubscribe, signal_, _1);
+
+    bindedSignal.second = boost::make_shared<ros::Subscriber>(
+        RosSubscribe.nh().subscribe(topic, 1, callback));
+
+    RosSubscribe.bindedSignal()[signal] = bindedSignal;
   }
+};
 
-  template <typename R>
-  void
-  RosSubscribe::callbackTimestamp
-  (boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
-   const R& data)
-  {
-    ptime time =
-      rosTimeToPtime (data->header.stamp);
-    signal->setConstant(time);
-  }
+template <typename T>
+struct Add<std::pair<T, dg::Vector> > {
+  void operator()(RosSubscribe& RosSubscribe, const std::string& signal,
+                  const std::string& topic) {
+    typedef std::pair<T, dg::Vector> type_t;
+
+    typedef typename SotToRos<type_t>::sot_t sot_t;
+    typedef typename SotToRos<type_t>::ros_const_ptr_t ros_const_ptr_t;
+    typedef typename SotToRos<type_t>::signalIn_t signal_t;
+
+    // Initialize the bindedSignal object.
+    RosSubscribe::bindedSignal_t bindedSignal;
+
+    // Initialize the signal.
+    boost::format signalName("RosSubscribe(%1%)::%2%");
+    signalName % RosSubscribe.getName() % signal;
+
+    boost::shared_ptr<signal_t> signal_(new signal_t(0, signalName.str()));
+    SotToRos<T>::setDefault(*signal_);
+    bindedSignal.first = signal_;
+    RosSubscribe.signalRegistration(*bindedSignal.first);
+
+    // Initialize the publisher.
+    typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
+    callback_t callback =
+        boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>,
+                    &RosSubscribe, signal_, _1);
+
+    bindedSignal.second = boost::make_shared<ros::Subscriber>(
+        RosSubscribe.nh().subscribe(topic, 1, callback));
+
+    RosSubscribe.bindedSignal()[signal] = bindedSignal;
 
-  namespace internal
-  {
-    template <typename T>
-    struct Add
-    {
-      void operator () (RosSubscribe& RosSubscribe,
-			const std::string& signal,
-			const std::string& topic)
-      {
-	typedef typename SotToRos<T>::sot_t sot_t;
-	typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
-	typedef typename SotToRos<T>::signalIn_t signal_t;
-
-	// Initialize the bindedSignal object.
-	RosSubscribe::bindedSignal_t bindedSignal;
-
-	// Initialize the signal.
-	boost::format signalName ("RosSubscribe(%1%)::%2%");
-	signalName % RosSubscribe.getName () % signal;
-
-	boost::shared_ptr<signal_t> signal_
-	  (new signal_t (0, signalName.str ()));
-	SotToRos<T>::setDefault(*signal_);
-	bindedSignal.first = signal_;
-	RosSubscribe.signalRegistration (*bindedSignal.first);
-
-	// Initialize the subscriber.
-	typedef boost::function<void (const ros_const_ptr_t& data)> callback_t;
-	callback_t callback = boost::bind
-	  (&RosSubscribe::callback<ros_const_ptr_t, sot_t>,
-	   &RosSubscribe, signal_, _1);
-
-	bindedSignal.second =
-	  boost::make_shared<ros::Subscriber>
-	  (RosSubscribe.nh ().subscribe (topic, 1, callback));
-
-	RosSubscribe.bindedSignal ()[signal] = bindedSignal;
-      }
-    };
-
-    template <typename T>
-    struct Add<std::pair<T, ml::Vector> >
-    {
-      void operator () (RosSubscribe& RosSubscribe,
-			const std::string& signal,
-			const std::string& topic)
-      {
-	typedef std::pair<T, ml::Vector> type_t;
-
-	typedef typename SotToRos<type_t>::sot_t sot_t;
-	typedef typename SotToRos<type_t>::ros_const_ptr_t ros_const_ptr_t;
-	typedef typename SotToRos<type_t>::signalIn_t signal_t;
-
-	// Initialize the bindedSignal object.
-	RosSubscribe::bindedSignal_t bindedSignal;
-
-	// Initialize the signal.
-	boost::format signalName ("RosSubscribe(%1%)::%2%");
-	signalName % RosSubscribe.getName () % signal;
-
-	boost::shared_ptr<signal_t> signal_
-	  (new signal_t (0, signalName.str ()));
-	SotToRos<T>::setDefault(*signal_);
-	bindedSignal.first = signal_;
-	RosSubscribe.signalRegistration (*bindedSignal.first);
-
-	// Initialize the publisher.
-	typedef boost::function<void (const ros_const_ptr_t& data)> callback_t;
-	callback_t callback = boost::bind
-	  (&RosSubscribe::callback<ros_const_ptr_t, sot_t>,
-	   &RosSubscribe, signal_, _1);
-
-	bindedSignal.second =
-	  boost::make_shared<ros::Subscriber>
-	  (RosSubscribe.nh ().subscribe (topic, 1, callback));
-
-	RosSubscribe.bindedSignal ()[signal] = bindedSignal;
-
-
-	// Timestamp.
-	typedef dynamicgraph::SignalPtr<RosSubscribe::ptime, int>
-	  signalTimestamp_t;
-	std::string signalTimestamp =
-	  (boost::format ("%1%%2%") % signal % "Timestamp").str ();
-
-	// Initialize the bindedSignal object.
-	RosSubscribe::bindedSignal_t bindedSignalTimestamp;
-
-	// Initialize the signal.
-	boost::format signalNameTimestamp ("RosSubscribe(%1%)::%2%");
-	signalNameTimestamp % RosSubscribe.name % signalTimestamp;
-
-	boost::shared_ptr<signalTimestamp_t> signalTimestamp_
-	  (new signalTimestamp_t (0, signalNameTimestamp.str ()));
-
-	RosSubscribe::ptime zero (rosTimeToPtime (ros::Time (0, 0)));
-	signalTimestamp_->setConstant (zero);
-	bindedSignalTimestamp.first = signalTimestamp_;
-	RosSubscribe.signalRegistration (*bindedSignalTimestamp.first);
-
-	// Initialize the publisher.
-	typedef boost::function<void (const ros_const_ptr_t& data)> callback_t;
-	callback_t callbackTimestamp = boost::bind
-	  (&RosSubscribe::callbackTimestamp<ros_const_ptr_t>,
-	   &RosSubscribe, signalTimestamp_, _1);
-
-	bindedSignalTimestamp.second =
-	  boost::make_shared<ros::Subscriber>
-	  (RosSubscribe.nh ().subscribe (topic, 1, callbackTimestamp));
-
-	RosSubscribe.bindedSignal ()[signalTimestamp] = bindedSignalTimestamp;
-      }
-    };
-  } // end of namespace internal.
-
-  template <typename T>
-  void RosSubscribe::add (const std::string& signal, const std::string& topic)
-  {
-    internal::Add<T> () (*this, signal, topic);
+    // Timestamp.
+    typedef dynamicgraph::SignalPtr<RosSubscribe::ptime, int> signalTimestamp_t;
+    std::string signalTimestamp =
+        (boost::format("%1%%2%") % signal % "Timestamp").str();
+
+    // Initialize the bindedSignal object.
+    RosSubscribe::bindedSignal_t bindedSignalTimestamp;
+
+    // Initialize the signal.
+    boost::format signalNameTimestamp("RosSubscribe(%1%)::%2%");
+    signalNameTimestamp % RosSubscribe.name % signalTimestamp;
+
+    boost::shared_ptr<signalTimestamp_t> signalTimestamp_(
+        new signalTimestamp_t(0, signalNameTimestamp.str()));
+
+    RosSubscribe::ptime zero(rosTimeToPtime(ros::Time(0, 0)));
+    signalTimestamp_->setConstant(zero);
+    bindedSignalTimestamp.first = signalTimestamp_;
+    RosSubscribe.signalRegistration(*bindedSignalTimestamp.first);
+
+    // Initialize the publisher.
+    typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
+    callback_t callbackTimestamp =
+        boost::bind(&RosSubscribe::callbackTimestamp<ros_const_ptr_t>,
+                    &RosSubscribe, signalTimestamp_, _1);
+
+    bindedSignalTimestamp.second = boost::make_shared<ros::Subscriber>(
+        RosSubscribe.nh().subscribe(topic, 1, callbackTimestamp));
+
+    RosSubscribe.bindedSignal()[signalTimestamp] = bindedSignalTimestamp;
   }
-} // end of namespace dynamicgraph.
+};
+}  // end of namespace internal.
+
+template <typename T>
+void RosSubscribe::add(const std::string& signal, const std::string& topic) {
+  internal::Add<T>()(*this, signal, topic);
+}
+}  // end of namespace dynamicgraph.
 
-#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
+#endif  //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX

src/ros_tf_listener-python-module-py.cc

+#include <dynamic-graph/python/module.hh>
+
+#include "ros_tf_listener.hh"
+
+namespace dg = dynamicgraph;
+
+BOOST_PYTHON_MODULE(wrap) {
+  bp::import("dynamic_graph");
+
+  dg::python::exposeEntity<dg::RosTfListener, bp::bases<dg::Entity>,
+                           dg::python::AddCommands>()
+      .def("add", &dg::RosTfListener::add,
+           "Add a signal containing the transform between two frames.",
+           bp::args("to_frame_name", "from_frame_name", "out_signal_name"))
+      .def("setMaximumDelay", &dg::RosTfListener::setMaximumDelay,
+           "Set the maximum time delay of the transform obtained from tf.",
+           bp::args("signal_name", "delay_seconds"));
+}

src/ros_tf_listener.cpp

+#include "ros_tf_listener.hh"
+
+#include <dynamic-graph/factory.h>
+
+#include <pinocchio/fwd.hpp>
+
+#include "dynamic_graph_bridge/ros_init.hh"
+
+namespace dynamicgraph {
+namespace internal {
+sot::MatrixHomogeneous& TransformListenerData::getTransform(
+    sot::MatrixHomogeneous& res, int time) {
+  availableSig.recompute(time);
+
+  bool available = availableSig.accessCopy();
+
+  if (!available) {
+    failbackSig.recompute(time);
+    res = failbackSig.accessCopy();
+    return res;
+  }
+
+  const geometry_msgs::TransformStamped::_transform_type::_rotation_type& quat =
+      transform.transform.rotation;
+  const geometry_msgs::TransformStamped::_transform_type::_translation_type&
+      trans = transform.transform.translation;
+  res.linear() = sot::Quaternion(quat.w, quat.x, quat.y, quat.z).matrix();
+  res.translation() << trans.x, trans.y, trans.z;
+  return res;
+}
+
+bool& TransformListenerData::isAvailable(bool& available, int time) {
+  static const ros::Time origin(0);
+  available = false;
+  ros::Duration elapsed;
+  std::string msg;
+
+  if (buffer.canTransform(toFrame, fromFrame, origin, &msg)) {
+    transform = buffer.lookupTransform(toFrame, fromFrame, origin);
+    if (transform.header.stamp == origin) {
+      // This is likely a TF2 static transform.
+      available = true;
+    } else {
+      elapsed = ros::Time::now() - transform.header.stamp;
+      available = (elapsed <= max_elapsed);
+    }
+
+    if (!available) {
+      std::ostringstream oss;
+      oss << "Use failback " << signal.getName() << " at time " << time
+          << ". Time since last update of the transform: " << elapsed;
+      entity->SEND_INFO_STREAM_MSG(oss.str());
+    }
+  } else {
+    std::ostringstream oss;
+    oss << "Unable to get transform " << signal.getName() << " at time " << time
+        << ": " << msg;
+    entity->SEND_WARNING_STREAM_MSG(oss.str());
+    available = false;
+  }
+  return available;
+}
+
+}  // namespace internal
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTfListener, "RosTfListener");
+}  // namespace dynamicgraph

src/ros_time-python.hh

+#include "ros_time.hh"
+
+typedef boost::mpl::vector<dynamicgraph::RosTime> entities_t;

src/ros_time.cpp

@@ -3,43 +3,38 @@
 ///
 /// Author: Florent Lamiraux
 
+#include "ros_time.hh"
+
 #include <dynamic-graph/factory.h>
-#include <dynamic-graph/signal-caster.h>
 #include <dynamic-graph/signal-cast-helper.h>
+#include <dynamic-graph/signal-caster.h>
 
-#include "ros_time.hh"
 #include "converter.hh"
 
 namespace dynamicgraph {
 
-  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTime, "RosTime");
-
-  using namespace boost::posix_time;
-
-  const std::string RosTime::docstring_
-  ("Export ROS time into dynamic-graph.\n"
-   "\n"
-   "  Signal \"time\" provides time as given by ros::time as\n"
-   "  boost::posix_time::ptime type.\n");
-
-  RosTime::RosTime (const std::string& name) :
-    Entity (name),
-    now_ ("RosTime("+name+")::output(boost::posix_time::ptime)::time")
-  {
-    signalRegistration (now_);
-    now_.setConstant (rosTimeToPtime (ros::Time::now()));
-    now_.setFunction (boost::bind (&RosTime::update, this, _1, _2));
-  }
-
-  ptime&
-  RosTime::update (ptime& time, const int& t)
-  {
-    time = rosTimeToPtime (ros::Time::now ());
-    return time;
-  }
-
-  std::string RosTime::getDocString () const
-  {
-    return docstring_;
-  }
-} // namespace dynamicgraph
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTime, "RosTime");
+
+using namespace boost::posix_time;
+
+const std::string RosTime::docstring_(
+    "Export ROS time into dynamic-graph.\n"
+    "\n"
+    "  Signal \"time\" provides time as given by ros::time as\n"
+    "  boost::posix_time::ptime type.\n");
+
+RosTime::RosTime(const std::string& _name)
+    : Entity(_name),
+      now_("RosTime(" + _name + ")::output(boost::posix_time::ptime)::time") {
+  signalRegistration(now_);
+  now_.setConstant(rosTimeToPtime(ros::Time::now()));
+  now_.setFunction(boost::bind(&RosTime::update, this, _1, _2));
+}
+
+ptime& RosTime::update(ptime& time, const int&) {
+  time = rosTimeToPtime(ros::Time::now());
+  return time;
+}
+
+std::string RosTime::getDocString() const { return docstring_; }
+}  // namespace dynamicgraph