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)
-  {
-    std::string signalTs = signal+"Timestamp";
+void RosSubscribe::rm(const std::string& signal) {
+  std::string signalTs = signal + "Timestamp";
 
-    signalDeregistration(signal);
-    bindedSignal_.erase (signal);
+  signalDeregistration(signal);
+  bindedSignal_.erase(signal);
 
-    if(bindedSignal_.find(signalTs) != bindedSignal_.end())
-    {
-       signalDeregistration(signalTs);
-       bindedSignal_.erase(signalTs);
-    }
+  if (bindedSignal_.find(signalTs) != bindedSignal_.end()) {
+    signalDeregistration(signalTs);
+    bindedSignal_.erase(signalTs);
   }
-
-  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;
+}
+
+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();
   }
+}
 
-  void RosSubscribe::clear ()
-  {
-    std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
-    for(; it!= bindedSignal_.end(); )
-    {
-      rm(it->first);
-      it = bindedSignal_.begin();
-    }
-  }
-
-  std::string RosSubscribe::getDocString () const
-  {
-    return docstring_;
-  }
-} // 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_msgs/Matrix.h"
-# include "dynamic_graph_bridge_msgs/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_tf_listener.hh

+#ifndef DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
+#define DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
+
+#include <dynamic-graph/command-bind.h>
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <boost/bind.hpp>
+#include <dynamic_graph_bridge/ros_init.hh>
+#include <sot/core/matrix-geometry.hh>
+
+namespace dynamicgraph {
+class RosTfListener;
+
+namespace internal {
+struct TransformListenerData {
+  typedef SignalTimeDependent<bool, int> AvailableSignal_t;
+  typedef SignalTimeDependent<sot::MatrixHomogeneous, int> MatrixSignal_t;
+  typedef SignalPtr<sot::MatrixHomogeneous, int> DefaultSignal_t;
+
+  RosTfListener* entity;
+  tf2_ros::Buffer& buffer;
+  const std::string toFrame, fromFrame;
+  geometry_msgs::TransformStamped transform;
+  ros::Duration max_elapsed;
+  AvailableSignal_t availableSig;
+  MatrixSignal_t signal;
+  DefaultSignal_t failbackSig;
+
+  TransformListenerData(RosTfListener* e, tf2_ros::Buffer& b,
+                        const std::string& to, const std::string& from,
+                        const std::string& signame)
+      : entity(e),
+        buffer(b),
+        toFrame(to),
+        fromFrame(from),
+        max_elapsed(0.5),
+        availableSig(signame + "_available"),
+        signal(signame),
+        failbackSig(NULL, signame + "_failback") {
+    signal.setFunction(
+        boost::bind(&TransformListenerData::getTransform, this, _1, _2));
+
+    availableSig.setFunction(
+        boost::bind(&TransformListenerData::isAvailable, this, _1, _2));
+    availableSig.setNeedUpdateFromAllChildren(true);
+
+    failbackSig.setConstant(sot::MatrixHomogeneous::Identity());
+    signal.addDependencies(failbackSig << availableSig);
+  }
+
+  sot::MatrixHomogeneous& getTransform(sot::MatrixHomogeneous& res, int time);
+
+  bool& isAvailable(bool& isAvailable, int time);
+};
+}  // namespace internal
+
+class RosTfListener : public Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+  typedef internal::TransformListenerData TransformListenerData;
+
+  RosTfListener(const std::string& _name)
+      : Entity(_name), buffer(), listener(buffer, rosInit(), false) {}
+
+  ~RosTfListener() {
+    for (const auto& pair : listenerDatas) delete pair.second;
+  }
+
+  void add(const std::string& to, const std::string& from,
+           const std::string& signame) {
+    if (listenerDatas.find(signame) != listenerDatas.end())
+      throw std::invalid_argument("A signal " + signame +
+                                  " already exists in RosTfListener " +
+                                  getName());
+
+    boost::format signalName("RosTfListener(%1%)::output(MatrixHomo)::%2%");
+    signalName % getName() % signame;
+
+    TransformListenerData* tld =
+        new TransformListenerData(this, buffer, to, from, signalName.str());
+    signalRegistration(tld->signal << tld->availableSig << tld->failbackSig);
+    listenerDatas[signame] = tld;
+  }
+
+  void setMaximumDelay(const std::string& signame, const double& max_elapsed) {
+    if (listenerDatas.count(signame) == 0)
+      throw std::invalid_argument("No signal " + signame +
+                                  " in RosTfListener " + getName());
+    listenerDatas[signame]->max_elapsed = ros::Duration(max_elapsed);
+  }
+
+ private:
+  typedef std::map<std::string, TransformListenerData*> Map_t;
+  Map_t listenerDatas;
+  tf2_ros::Buffer buffer;
+  tf2_ros::TransformListener listener;
+};
+}  // end of namespace dynamicgraph.
+
+#endif  // DYNAMIC_GRAPH_ROS_TF_LISTENER_HH

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

src/ros_time.hh

@@ -4,29 +4,36 @@
 /// Author: Florent Lamiraux
 
 #ifndef DYNAMIC_GRAPH_ROS_TIME_HH
-# define DYNAMIC_GRAPH_ROS_TIME_HH
+#define DYNAMIC_GRAPH_ROS_TIME_HH
 
-# include <ros/time.h>
-# include <boost/date_time/posix_time/posix_time_types.hpp>
-# include <dynamic-graph/signal.h>
-# include <dynamic-graph/entity.h>
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal.h>
+#include <ros/time.h>
+
+#include <boost/date_time/posix_time/posix_time_types.hpp>
 
 namespace dynamicgraph {
 
-  class RosTime : public dynamicgraph::Entity
-  {
-    DYNAMIC_GRAPH_ENTITY_DECL ();
-  public:
-    Signal <boost::posix_time::ptime, int> now_;
-    RosTime (const std::string& name);
-    virtual std::string getDocString () const;
-  protected:
-    boost::posix_time::ptime&
-    update (boost::posix_time::ptime& time, const int& t);
-  private:
-    static const std::string docstring_;
-  }; // class RosTime
-
-} // namespace dynamicgraph
-
-#endif // DYNAMIC_GRAPH_ROS_TIME_HH
+class RosTime : public dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+  Signal<boost::posix_time::ptime, int> now_;
+  RosTime(const std::string& name);
+  virtual std::string getDocString() const;
+
+ protected:
+  boost::posix_time::ptime& update(boost::posix_time::ptime& time,
+                                   const int& t);
+
+ private:
+  static const std::string docstring_;
+};  // class RosTime
+
+template <>
+struct signal_io<boost::posix_time::ptime>
+    : signal_io_unimplemented<boost::posix_time::ptime> {};
+
+}  // namespace dynamicgraph
+
+#endif  // DYNAMIC_GRAPH_ROS_TIME_HH

src/sot_loader.cpp

@@ -4,271 +4,214 @@
  *
  * CNRS
  *
- * This file is part of sot-core.
- * sot-core is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-core is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Lesser General Public License for more details.  You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-core.  If not, see <http://www.gnu.org/licenses/>.
  */
 /* -------------------------------------------------------------------------- */
 /* --- INCLUDES ------------------------------------------------------------- */
 /* -------------------------------------------------------------------------- */
 
-#include "sot_loader.hh"
+#include <ros/rate.h>
+
+#include <dynamic_graph_bridge/sot_loader.hh>
+
+#include "dynamic_graph_bridge/ros_init.hh"
 
 // POSIX.1-2001
 #include <dlfcn.h>
 
 using namespace std;
-using namespace dynamicgraph::sot; 
+using namespace dynamicgraph::sot;
 namespace po = boost::program_options;
 
-SotLoader::SotLoader():
-  dynamic_graph_stopped_(true),
-  sensorsIn_ (),
-  controlValues_ (),
-  angleEncoder_ (),
-  angleControl_ (),
-  forces_ (),
-  torques_ (),
-  baseAtt_ (),
-  accelerometer_ (3),
-  gyrometer_ (3)
-{
-  readSotVectorStateParam();
-  initPublication();
-}
-
-int SotLoader::initPublication()
-{
-  ros::NodeHandle n;
+struct DataToLog {
+  const std::size_t N;
+  std::size_t idx, iter;
 
+  std::vector<std::size_t> iters;
+  std::vector<double> times;
+  std::vector<double> ittimes;
 
-  // Prepare message to be published
-  joint_pub_ = n.advertise<sensor_msgs::JointState>("joint_states", 1);
+  DataToLog(std::size_t N_)
+      : N(N_), idx(0), iter(0), iters(N, 0), times(N, 0), ittimes(N, 0) {}
 
-  return 0;
-}
+  void record(const double t, const double itt) {
+    iters[idx] = iter;
+    times[idx] = t;
+    ittimes[idx] = itt;
+    ++idx;
+    ++iter;
+    if (idx == N) idx = 0;
+  }
 
-int SotLoader::readSotVectorStateParam()
-{
-  std::map<std::string,int> from_state_name_to_state_vector;
-  std::map<std::string,std::string> from_parallel_name_to_state_vector_name;
-  ros::NodeHandle n;
+  void save(const char *prefix) {
+    std::ostringstream oss;
+    oss << prefix << "-times.log";
 
-  if (!ros::param::has("/sot/state_vector_map"))
-    {
-      std::cerr<< " Read Sot Vector State Param " << std::endl;
-      return 1;
+    std::ofstream aof(oss.str().c_str());
+    if (aof.is_open()) {
+      for (std::size_t k = 0; k < N; ++k) {
+        aof << iters[(idx + k) % N] << ' ' << times[(idx + k) % N] << ' '
+            << ittimes[(idx + k) % N] << '\n';
+      }
     }
+    aof.close();
+  }
+};
+
+void workThreadLoader(SotLoader *aSotLoader) {
+  ros::Rate rate(1000);  // 1 kHz
+  if (ros::param::has("/sot_controller/dt")) {
+    double periodd;
+    ros::param::get("/sot_controller/dt", periodd);
+    rate = ros::Rate(1 / periodd);
+  }
+  DataToLog dataToLog(5000);
 
-  n.getParam("/sot/state_vector_map", stateVectorMap_);
-  ROS_ASSERT(stateVectorMap_.getType() == XmlRpc::XmlRpcValue::TypeArray);
-  nbOfJoints_ = stateVectorMap_.size();
-
-  if (ros::param::has("/sot/joint_state_parallel"))
-    {
-      XmlRpc::XmlRpcValue joint_state_parallel;
-      n.getParam("/sot/joint_state_parallel", joint_state_parallel);
-      ROS_ASSERT(joint_state_parallel.getType() == XmlRpc::XmlRpcValue::TypeStruct);
-      std::cout << "Type of joint_state_parallel:" << joint_state_parallel.getType() << std::endl;
-
-      for(XmlRpc::XmlRpcValue::iterator it = joint_state_parallel.begin(); 
-          it!= joint_state_parallel.end(); it++) 
-        {
-          XmlRpc::XmlRpcValue local_value=it->second;
-          std::string final_expression, map_expression = static_cast<string>(local_value); 
-          double final_coefficient = 1.0;
-          // deal with sign 
-          if (map_expression[0]=='-')
-            {
-              final_expression = map_expression.substr(1,map_expression.size()-1);
-              final_coefficient = -1.0;
-            }
-          else 
-            final_expression = map_expression;
-
-          std::cout <<it->first.c_str() << ": " << final_coefficient << std::endl;
-          from_parallel_name_to_state_vector_name[it->first.c_str()] = final_expression;
-          coefficient_parallel_joints_.push_back(final_coefficient);
-
-        }
-      nbOfParallelJoints_ = from_parallel_name_to_state_vector_name.size();
-    }
+  rate.reset();
+  while (ros::ok() && aSotLoader->isDynamicGraphStopped()) {
+    rate.sleep();
+  }
 
-  // Prepare joint_state according to robot description.
-  joint_state_.name.resize(nbOfJoints_+nbOfParallelJoints_);
-  joint_state_.position.resize(nbOfJoints_+nbOfParallelJoints_);
-
-  // Fill in the name of the joints from the state vector.
-  // and build local map from state name to state vector
-  for (int32_t i = 0; i < stateVectorMap_.size(); ++i) 
-   {
-     joint_state_.name[i]= static_cast<string>(stateVectorMap_[i]);
-
-     from_state_name_to_state_vector[joint_state_.name[i]] = i;
-   }
-
-  // Fill in the name of the joints from the parallel joint vector.
-  // and build map from parallel name to state vector
-  int i=0;
-  parallel_joints_to_state_vector_.resize(nbOfParallelJoints_);
-  for (std::map<std::string,std::string>::iterator  it = from_parallel_name_to_state_vector_name.begin();
-       it != from_parallel_name_to_state_vector_name.end();
-       it++,i++)
-    {
-      joint_state_.name[i+nbOfJoints_]=it->first.c_str();
-      parallel_joints_to_state_vector_[i] = from_state_name_to_state_vector[it->second];
-    }
+  ros::NodeHandle nh("/geometric_simu");
+  bool paused;
+  ros::Time timeOrigin = ros::Time::now();
+  ros::Time time;
+  while (ros::ok() && !aSotLoader->isDynamicGraphStopped()) {
+    nh.param<bool>("paused", paused, false);
 
-  angleEncoder_.resize(nbOfJoints_);
-  
-  return 0;
+    if (!paused) {
+      time = ros::Time::now();
+      aSotLoader->oneIteration();
+
+      ros::Duration d = ros::Time::now() - time;
+      dataToLog.record((time - timeOrigin).toSec(), d.toSec());
+    }
+    rate.sleep();
+  }
+  dataToLog.save("/tmp/geometric_simu");
+  ros::waitForShutdown();
 }
 
+SotLoader::SotLoader()
+    : sensorsIn_(),
+      controlValues_(),
+      angleEncoder_(),
+      angleControl_(),
+      forces_(),
+      torques_(),
+      baseAtt_(),
+      accelerometer_(3),
+      gyrometer_(3),
+      thread_() {
+  readSotVectorStateParam();
+  initPublication();
 
-int SotLoader::parseOptions(int argc, char *argv[])
-{ 
-  po::options_description desc("Allowed options");
-  desc.add_options()
-    ("help", "produce help message")
-    ("input-file", po::value<string>(), "library to load")
-    ;
-    
-  
-  po::store(po::parse_command_line(argc, argv, desc), vm_);
-  po::notify(vm_);    
-  
-  if (vm_.count("help")) {
-    cout << desc << "\n";
-    return -1;
-  }
-  if (!vm_.count("input-file")) {
-    cout << "No filename specified\n";
-    return -1;
+  freeFlyerPose_.header.frame_id = "odom";
+  freeFlyerPose_.child_frame_id = "base_link";
+  if (ros::param::get("/sot/tf_base_link", freeFlyerPose_.child_frame_id)) {
+    ROS_INFO_STREAM("Publishing " << freeFlyerPose_.child_frame_id << " wrt "
+                                  << freeFlyerPose_.header.frame_id);
   }
-  else
-    dynamicLibraryName_= vm_["input-file"].as< string >();
-  
-  Initialization();
-  return 0;
 }
 
-void SotLoader::Initialization()
-{
-  
-  // Load the SotRobotBipedController library.
-  void * SotRobotControllerLibrary = dlopen( dynamicLibraryName_.c_str(),
-                                             RTLD_LAZY | RTLD_GLOBAL );
-  if (!SotRobotControllerLibrary) {
-    std::cerr << "Cannot load library: " << dlerror() << '\n';
-    return ;
-  }
-  
-  // reset errors
-  dlerror();
-  
-  // Load the symbols.
-  createSotExternalInterface_t * createSot =
-    reinterpret_cast<createSotExternalInterface_t *> 
-    (reinterpret_cast<long> 
-     (dlsym(SotRobotControllerLibrary, 
-	    "createSotExternalInterface")));
-  const char* dlsym_error = dlerror();
-  if (dlsym_error) {
-    std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
-    return ;
+SotLoader::~SotLoader() {
+  dynamic_graph_stopped_ = true;
+  thread_.join();
+}
+
+void SotLoader::startControlLoop() {
+  thread_ = boost::thread(workThreadLoader, this);
+}
+
+void SotLoader::initializeRosNode(int argc, char *argv[]) {
+  SotLoaderBasic::initializeRosNode(argc, argv);
+  // Temporary fix. TODO: where should nbOfJoints_ be initialized from?
+  if (ros::param::has("/sot/state_vector_map")) {
+    angleEncoder_.resize(nbOfJoints_);
+    angleControl_.resize(nbOfJoints_);
   }
-  
-  // Create robot-controller
-  sotController_ = createSot();
-  cout <<"Went out from Initialization." << endl;
+
+  startControlLoop();
 }
 
-void 
-SotLoader::fillSensors(map<string,dgs::SensorValues> & sensorsIn)
-{
-  
+void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) {
   // Update joint values.w
+  assert(angleControl_.size() == angleEncoder_.size());
+
   sensorsIn["joints"].setName("angle");
-  for(unsigned int i=0;i<angleControl_.size();i++)
+  for (unsigned int i = 0; i < angleControl_.size(); i++)
     angleEncoder_[i] = angleControl_[i];
   sensorsIn["joints"].setValues(angleEncoder_);
-  
 }
 
-void 
-SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
-{
-
-  
+void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
   // Update joint values.
-  angleControl_ = controlValues["joints"].getValues();
+  angleControl_ = controlValues["control"].getValues();
+
+  // Debug
+  std::map<std::string, dgs::ControlValues>::iterator it =
+      controlValues.begin();
+  sotDEBUG(30) << "ControlValues to be broadcasted:" << std::endl;
+  for (; it != controlValues.end(); it++) {
+    sotDEBUG(30) << it->first << ":";
+    std::vector<double> ctrlValues_ = it->second.getValues();
+    std::vector<double>::iterator it_d = ctrlValues_.begin();
+    for (; it_d != ctrlValues_.end(); it_d++) sotDEBUG(30) << *it_d << " ";
+    sotDEBUG(30) << std::endl;
+  }
+  sotDEBUG(30) << "End ControlValues" << std::endl;
 
   // Check if the size if coherent with the robot description.
-  if (angleControl_.size()!=(unsigned int)nbOfJoints_)
-    {
-      std::cerr << " angleControl_ and nbOfJoints are different !"
-                << std::endl;
-      exit(-1);
-    }
-
+  if (angleControl_.size() != (unsigned int)nbOfJoints_) {
+    std::cerr << " angleControl_" << angleControl_.size() << " and nbOfJoints"
+              << (unsigned int)nbOfJoints_ << " are different !" << std::endl;
+    exit(-1);
+  }
   // Publish the data.
-  joint_state_.header.stamp = ros::Time::now();  
-  for(int i=0;i<nbOfJoints_;i++)
-    {
-      joint_state_.position[i] = angleControl_[i];
-    }
-  for(unsigned int i=0;i<parallel_joints_to_state_vector_.size();i++)
-    {
-      joint_state_.position[i+nbOfJoints_] = 
-        coefficient_parallel_joints_[i]*angleControl_[parallel_joints_to_state_vector_[i]];
-    }
+  joint_state_.header.stamp = ros::Time::now();
+  for (int i = 0; i < nbOfJoints_; i++) {
+    joint_state_.position[i] = angleControl_[i];
+  }
+  for (unsigned int i = 0; i < parallel_joints_to_state_vector_.size(); i++) {
+    joint_state_.position[i + nbOfJoints_] =
+        coefficient_parallel_joints_[i] *
+        angleControl_[parallel_joints_to_state_vector_[i]];
+  }
 
-  joint_pub_.publish(joint_state_);  
+  joint_pub_.publish(joint_state_);
 
-  
-}
+  // Publish robot pose
+  // get the robot pose values
+  std::vector<double> poseValue_ = controlValues["baseff"].getValues();
 
-void SotLoader::setup()
-{
-  fillSensors(sensorsIn_);
-  sotController_->setupSetSensors(sensorsIn_);
-  sotController_->getControl(controlValues_); 
-  readControl(controlValues_); 
+  freeFlyerPose_.transform.translation.x = poseValue_[0];
+  freeFlyerPose_.transform.translation.y = poseValue_[1];
+  freeFlyerPose_.transform.translation.z = poseValue_[2];
+
+  freeFlyerPose_.transform.rotation.w = poseValue_[3];
+  freeFlyerPose_.transform.rotation.x = poseValue_[4];
+  freeFlyerPose_.transform.rotation.y = poseValue_[5];
+  freeFlyerPose_.transform.rotation.z = poseValue_[6];
+
+  freeFlyerPose_.header.stamp = joint_state_.header.stamp;
+  // Publish
+  freeFlyerPublisher_.sendTransform(freeFlyerPose_);
 }
 
-void SotLoader::oneIteration()
-{
+void SotLoader::setup() {
   fillSensors(sensorsIn_);
-  try 
-    {
-      sotController_->nominalSetSensors(sensorsIn_);
-      sotController_->getControl(controlValues_);
-    } 
-  catch(std::exception &e) { throw e;} 
-  
+  sotController_->setupSetSensors(sensorsIn_);
+  sotController_->getControl(controlValues_);
   readControl(controlValues_);
 }
 
+void SotLoader::oneIteration() {
+  fillSensors(sensorsIn_);
+  try {
+    sotController_->nominalSetSensors(sensorsIn_);
+    sotController_->getControl(controlValues_);
+  } catch (std::exception &) {
+    throw;
+  }
 
-bool SotLoader::start_dg(std_srvs::Empty::Request& , 
-                         std_srvs::Empty::Response& )
-{
-  dynamic_graph_stopped_=false;    
-  return true;
-}
-
-bool SotLoader::stop_dg(std_srvs::Empty::Request& , 
-                         std_srvs::Empty::Response& )
-{
-  dynamic_graph_stopped_ = true;
-  return true;
+  readControl(controlValues_);
 }
-

src/sot_loader_basic.cpp

+/*
+ * Copyright 2016,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ */
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+#include <dynamic-graph/pool.h>
+
+#include <dynamic_graph_bridge/sot_loader.hh>
+
+#include "dynamic_graph_bridge/ros_init.hh"
+#include "dynamic_graph_bridge/ros_parameter.hh"
+
+// POSIX.1-2001
+#include <dlfcn.h>
+
+using namespace std;
+using namespace dynamicgraph::sot;
+namespace po = boost::program_options;
+
+SotLoaderBasic::SotLoaderBasic()
+    : dynamic_graph_stopped_(true), sotRobotControllerLibrary_(0) {
+  readSotVectorStateParam();
+  initPublication();
+}
+
+int SotLoaderBasic::initPublication() {
+  ros::NodeHandle& n = dynamicgraph::rosInit(false);
+
+  // Prepare message to be published
+  joint_pub_ = n.advertise<sensor_msgs::JointState>("joint_states", 1);
+
+  return 0;
+}
+
+void SotLoaderBasic::initializeRosNode(int, char*[]) {
+  ROS_INFO("Ready to start dynamic graph.");
+  ros::NodeHandle n;
+  service_start_ = n.advertiseService("start_dynamic_graph",
+                                      &SotLoaderBasic::start_dg, this);
+
+  service_stop_ =
+      n.advertiseService("stop_dynamic_graph", &SotLoaderBasic::stop_dg, this);
+
+  dynamicgraph::parameter_server_read_robot_description();
+}
+
+void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) {
+  dynamicLibraryName_ = afilename;
+}
+
+int SotLoaderBasic::readSotVectorStateParam() {
+  std::map<std::string, int> from_state_name_to_state_vector;
+  std::map<std::string, std::string> from_parallel_name_to_state_vector_name;
+  ros::NodeHandle n;
+
+  if (!ros::param::has("/sot/state_vector_map")) {
+    std::cerr << " Read Sot Vector State Param " << std::endl;
+    return 1;
+  }
+
+  n.getParam("/sot/state_vector_map", stateVectorMap_);
+  ROS_ASSERT(stateVectorMap_.getType() == XmlRpc::XmlRpcValue::TypeArray);
+  nbOfJoints_ = stateVectorMap_.size();
+  nbOfParallelJoints_ = 0;
+
+  if (ros::param::has("/sot/joint_state_parallel")) {
+    XmlRpc::XmlRpcValue joint_state_parallel;
+    n.getParam("/sot/joint_state_parallel", joint_state_parallel);
+    ROS_ASSERT(joint_state_parallel.getType() ==
+               XmlRpc::XmlRpcValue::TypeStruct);
+    std::cout << "Type of joint_state_parallel:"
+              << joint_state_parallel.getType() << std::endl;
+
+    for (XmlRpc::XmlRpcValue::iterator it = joint_state_parallel.begin();
+         it != joint_state_parallel.end(); it++) {
+      XmlRpc::XmlRpcValue local_value = it->second;
+      std::string final_expression,
+          map_expression = static_cast<string>(local_value);
+      double final_coefficient = 1.0;
+      // deal with sign
+      if (map_expression[0] == '-') {
+        final_expression = map_expression.substr(1, map_expression.size() - 1);
+        final_coefficient = -1.0;
+      } else
+        final_expression = map_expression;
+
+      std::cout << it->first.c_str() << ": " << final_coefficient << std::endl;
+      from_parallel_name_to_state_vector_name[it->first.c_str()] =
+          final_expression;
+      coefficient_parallel_joints_.push_back(final_coefficient);
+    }
+    nbOfParallelJoints_ = from_parallel_name_to_state_vector_name.size();
+  }
+
+  // Prepare joint_state according to robot description.
+  joint_state_.name.resize(nbOfJoints_ + nbOfParallelJoints_);
+  joint_state_.position.resize(nbOfJoints_ + nbOfParallelJoints_);
+
+  // Fill in the name of the joints from the state vector.
+  // and build local map from state name to state vector
+  for (int32_t i = 0; i < stateVectorMap_.size(); ++i) {
+    joint_state_.name[i] = static_cast<string>(stateVectorMap_[i]);
+
+    from_state_name_to_state_vector[joint_state_.name[i]] = i;
+  }
+
+  // Fill in the name of the joints from the parallel joint vector.
+  // and build map from parallel name to state vector
+  int i = 0;
+  parallel_joints_to_state_vector_.resize(nbOfParallelJoints_);
+  for (std::map<std::string, std::string>::iterator it =
+           from_parallel_name_to_state_vector_name.begin();
+       it != from_parallel_name_to_state_vector_name.end(); it++, i++) {
+    joint_state_.name[i + nbOfJoints_] = it->first.c_str();
+    parallel_joints_to_state_vector_[i] =
+        from_state_name_to_state_vector[it->second];
+  }
+
+  return 0;
+}
+
+int SotLoaderBasic::parseOptions(int argc, char* argv[]) {
+  po::options_description desc("Allowed options");
+  desc.add_options()("help", "produce help message")(
+      "input-file", po::value<string>(), "library to load");
+
+  po::store(po::parse_command_line(argc, argv, desc), vm_);
+  po::notify(vm_);
+
+  if (vm_.count("help")) {
+    cout << desc << "\n";
+    return -1;
+  }
+  if (!vm_.count("input-file")) {
+    cout << "No filename specified\n";
+    return -1;
+  } else
+    dynamicLibraryName_ = vm_["input-file"].as<string>();
+
+  return 0;
+}
+
+void SotLoaderBasic::Initialization() {
+  // Load the SotRobotBipedController library.
+  sotRobotControllerLibrary_ =
+      dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
+  if (!sotRobotControllerLibrary_) {
+    std::cerr << "Cannot load library: " << dlerror() << '\n';
+    return;
+  }
+
+  // reset errors
+  dlerror();
+
+  // Load the symbols.
+  createSotExternalInterface_t* createSot =
+      reinterpret_cast<createSotExternalInterface_t*>(reinterpret_cast<long>(
+          dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
+  const char* dlsym_error = dlerror();
+  if (dlsym_error) {
+    std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
+    return;
+  }
+
+  // Create robot-controller
+  sotController_ = createSot();
+  cout << "Went out from Initialization." << endl;
+}
+
+void SotLoaderBasic::CleanUp() {
+  dynamicgraph::PoolStorage::destroy();
+  // We do not destroy the FactoryStorage singleton because the module will not
+  // be reloaded at next initialization (because Python C API cannot safely
+  // unload a module...).
+  // SignalCaster singleton could probably be destroyed.
+
+  // Load the symbols.
+  destroySotExternalInterface_t* destroySot =
+      reinterpret_cast<destroySotExternalInterface_t*>(reinterpret_cast<long>(
+          dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
+  const char* dlsym_error = dlerror();
+  if (dlsym_error) {
+    std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
+    return;
+  }
+
+  destroySot(sotController_);
+  sotController_ = NULL;
+
+  /// Uncount the number of access to this library.
+  dlclose(sotRobotControllerLibrary_);
+}
+
+bool SotLoaderBasic::start_dg(std_srvs::Empty::Request&,
+                              std_srvs::Empty::Response&) {
+  dynamic_graph_stopped_ = false;
+  return true;
+}
+
+bool SotLoaderBasic::stop_dg(std_srvs::Empty::Request&,
+                             std_srvs::Empty::Response&) {
+  dynamic_graph_stopped_ = true;
+  return true;
+}

src/sot_to_ros.cpp

 #include "sot_to_ros.hh"
 
-namespace dynamicgraph
-{
+namespace dynamicgraph {
 
-  const char* SotToRos<double>::signalTypeName = "Double";
-  const char* SotToRos<unsigned int>::signalTypeName = "Unsigned";
-  const char* SotToRos<ml::Matrix>::signalTypeName = "Matrix";
-  const char* SotToRos<ml::Vector>::signalTypeName = "Vector";
-  const char* SotToRos<specific::Vector3>::signalTypeName = "Vector3";
-  const char* SotToRos<sot::MatrixHomogeneous>::signalTypeName = "MatrixHomo";
-  const char* SotToRos<specific::Twist>::signalTypeName = "Twist";
-  const char* SotToRos
-  <std::pair<specific::Vector3, ml::Vector> >::signalTypeName
-  = "Vector3Stamped";
-  const char* SotToRos
-  <std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalTypeName
-  = "MatrixHomo";
-  const char* SotToRos
-  <std::pair<specific::Twist, ml::Vector> >::signalTypeName
-  = "Twist";
+const char* SotToRos<bool>::signalTypeName = "bool";
+const char* SotToRos<double>::signalTypeName = "Double";
+const char* SotToRos<int>::signalTypeName = "int";
+const char* SotToRos<std::string>::signalTypeName = "string";
+const char* SotToRos<unsigned int>::signalTypeName = "Unsigned";
+const char* SotToRos<Matrix>::signalTypeName = "Matrix";
+const char* SotToRos<Vector>::signalTypeName = "Vector";
+const char* SotToRos<specific::Vector3>::signalTypeName = "Vector3";
+const char* SotToRos<sot::MatrixHomogeneous>::signalTypeName = "MatrixHomo";
+const char* SotToRos<specific::Twist>::signalTypeName = "Twist";
+const char* SotToRos<std::pair<specific::Vector3, Vector> >::signalTypeName =
+    "Vector3Stamped";
+const char*
+    SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalTypeName =
+        "MatrixHomo";
+const char* SotToRos<std::pair<specific::Twist, Vector> >::signalTypeName =
+    "Twist";
 
-} // end of namespace dynamicgraph.
+}  // end of namespace dynamicgraph.

src/sot_to_ros.hh

 #ifndef DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
-# define DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
-# include <vector>
-# include <utility>
-
-# include <boost/format.hpp>
-
-# include <jrl/mal/boost.hh>
-# include <std_msgs/Float64.h>
-# include <std_msgs/UInt32.h>
-# include "dynamic_graph_bridge_msgs/Matrix.h"
-# include "dynamic_graph_bridge_msgs/Vector.h"
-
-# include "geometry_msgs/Transform.h"
-# include "geometry_msgs/TransformStamped.h"
-# include "geometry_msgs/Twist.h"
-# include "geometry_msgs/TwistStamped.h"
-# include "geometry_msgs/Vector3Stamped.h"
-
-# include <dynamic-graph/signal-time-dependent.h>
-# include <dynamic-graph/signal-ptr.h>
-# include <sot/core/matrix-homogeneous.hh>
-
-# define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)	\
-  makeSignalString (typeid (*this).name (), NAME,			\
-		    IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)
-
-namespace dynamicgraph
-{
-  namespace ml = maal::boost;
-
-  /// \brief Types dedicated to identify pairs of (dg,ros) data.
-  ///
-  /// They do not contain any data but allow to make the difference
-  /// between different types with the same structure.
-  /// For instance a vector of six elements vs a twist coordinate.
-  namespace specific
-  {
-    class Vector3 {};
-    class Twist {};
-  } // end of namespace specific.
-
-  /// \brief SotToRos trait type.
-  ///
-  /// This trait provides types associated to a dynamic-graph type:
-  /// - ROS corresponding type,
-  /// - signal type / input signal type,
-  /// - ROS callback type.
-  template <typename SotType>
-  class SotToRos;
-
-  template <>
-  struct SotToRos<double>
-  {
-    typedef double sot_t;
-    typedef std_msgs::Float64 ros_t;
-    typedef std_msgs::Float64ConstPtr ros_const_ptr_t;
-    typedef dynamicgraph::Signal<sot_t, int> signal_t;
-    typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
-    typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
-    static const char* signalTypeName;
-
-    template <typename S>
-    static void setDefault(S& s)
-    {
-      s.setConstant (0.);
-    }
-  };
-
-  template <>
-  struct SotToRos<unsigned int>
-  {
-    typedef unsigned int sot_t;
-    typedef std_msgs::UInt32 ros_t;
-    typedef std_msgs::UInt32ConstPtr ros_const_ptr_t;
-    typedef dynamicgraph::Signal<sot_t, int> signal_t;
-    typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
-    typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
-    static const char* signalTypeName;
-
-    template <typename S>
-    static void setDefault(S& s)
-    {
-      s.setConstant (0);
-    }
-  };
-
-  template <>
-  struct SotToRos<ml::Matrix>
-  {
-    typedef ml::Matrix sot_t;
-    typedef dynamic_graph_bridge_msgs::Matrix ros_t;
-    typedef dynamic_graph_bridge_msgs::MatrixConstPtr ros_const_ptr_t;
-    typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
-    typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
-    typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
-    static const char* signalTypeName;
-
-    template <typename S>
-    static void setDefault(S& s)
-    {
-      ml::Matrix m;
-      m.resize(0, 0);
-      s.setConstant (m);
-    }
-  };
-
-  template <>
-  struct SotToRos<ml::Vector>
-  {
-    typedef ml::Vector sot_t;
-    typedef dynamic_graph_bridge_msgs::Vector ros_t;
-    typedef dynamic_graph_bridge_msgs::VectorConstPtr ros_const_ptr_t;
-    typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
-    typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
-    typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
-    static const char* signalTypeName;
-
-    template <typename S>
-    static void setDefault(S& s)
-    {
-      ml::Vector v;
-      v.resize (0);
-      s.setConstant (v);
-    }
-  };
-
-  template <>
-  struct SotToRos<specific::Vector3>
-  {
-    typedef ml::Vector sot_t;
-    typedef geometry_msgs::Vector3 ros_t;
-    typedef geometry_msgs::Vector3ConstPtr ros_const_ptr_t;
-    typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
-    typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
-    typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
-    static const char* signalTypeName;
-
-    template <typename S>
-    static void setDefault(S& s)
-    {
-      ml::Vector v;
-      v.resize (0);
-      s.setConstant (v);
-    }
-  };
-
-  template <>
-  struct SotToRos<sot::MatrixHomogeneous>
-  {
-    typedef sot::MatrixHomogeneous sot_t;
-    typedef geometry_msgs::Transform ros_t;
-    typedef geometry_msgs::TransformConstPtr ros_const_ptr_t;
-    typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
-    typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
-    typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
-    static const char* signalTypeName;
-
-    template <typename S>
-    static void setDefault(S& s)
-    {
-      sot::MatrixHomogeneous m;
-      s.setConstant (m);
-    }
-  };
-
-  template <>
-  struct SotToRos<specific::Twist>
-  {
-    typedef ml::Vector sot_t;
-    typedef geometry_msgs::Twist ros_t;
-    typedef geometry_msgs::TwistConstPtr ros_const_ptr_t;
-    typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
-    typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
-    typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
-    static const char* signalTypeName;
-
-    template <typename S>
-    static void setDefault(S& s)
-    {
-      ml::Vector v (6);
-      v.setZero ();
-      s.setConstant (v);
-    }
-  };
-
-  // Stamped vector3
-  template <>
-  struct SotToRos<std::pair<specific::Vector3, ml::Vector> >
-  {
-    typedef ml::Vector sot_t;
-    typedef geometry_msgs::Vector3Stamped ros_t;
-    typedef geometry_msgs::Vector3StampedConstPtr ros_const_ptr_t;
-    typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
-    typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
-    typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
-    static const char* signalTypeName;
-
-    template <typename S>
-    static void setDefault(S& s)
-    {
-      SotToRos<sot_t>::setDefault(s);
-    }
-  };
-
-  // Stamped transformation
-  template <>
-  struct SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >
-  {
-    typedef sot::MatrixHomogeneous sot_t;
-    typedef geometry_msgs::TransformStamped ros_t;
-    typedef geometry_msgs::TransformStampedConstPtr ros_const_ptr_t;
-    typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
-    typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
-    typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
-    static const char* signalTypeName;
-
-    template <typename S>
-    static void setDefault(S& s)
-    {
-      SotToRos<sot_t>::setDefault(s);
-    }
-  };
-
-  // Stamped twist
-  template <>
-  struct SotToRos<std::pair<specific::Twist, ml::Vector> >
-  {
-    typedef ml::Vector sot_t;
-    typedef geometry_msgs::TwistStamped ros_t;
-    typedef geometry_msgs::TwistStampedConstPtr ros_const_ptr_t;
-    typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
-    typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
-    typedef boost::function<sot_t& (sot_t&, int)> callback_t;
-
-    static const char* signalTypeName;
-
-    template <typename S>
-    static void setDefault(S& s)
-    {
-      SotToRos<sot_t>::setDefault(s);
-    }
-  };
-
-  inline std::string
-  makeSignalString (const std::string& className,
-		    const std::string& instanceName,
-		    bool isInputSignal,
-		    const std::string& signalType,
-		    const std::string& signalName)
-  {
-    boost::format fmt ("%s(%s)::%s(%s)::%s");
-    fmt % className
-      % instanceName
-      % (isInputSignal ? "input" : "output")
-      % signalType
-      % signalName;
-    return fmt.str ();
+#define DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
+#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/String.h>
+#include <std_msgs/UInt32.h>
+
+#include <boost/format.hpp>
+#include <sot/core/matrix-geometry.hh>
+#include <utility>
+#include <vector>
+
+#include "dynamic_graph_bridge_msgs/Matrix.h"
+#include "dynamic_graph_bridge_msgs/Vector.h"
+#include "geometry_msgs/Transform.h"
+#include "geometry_msgs/TransformStamped.h"
+#include "geometry_msgs/Twist.h"
+#include "geometry_msgs/TwistStamped.h"
+#include "geometry_msgs/Vector3Stamped.h"
+
+#define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)  \
+  makeSignalString(typeid(*this).name(), NAME, IS_INPUT, OUTPUT_TYPE, \
+                   SIGNAL_NAME)
+
+namespace dynamicgraph {
+
+/// \brief Types dedicated to identify pairs of (dg,ros) data.
+///
+/// They do not contain any data but allow to make the difference
+/// between different types with the same structure.
+/// For instance a vector of six elements vs a twist coordinate.
+namespace specific {
+class Vector3 {};
+class Twist {};
+}  // end of namespace specific.
+
+/// \brief SotToRos trait type.
+///
+/// This trait provides types associated to a dynamic-graph type:
+/// - ROS corresponding type,
+/// - signal type / input signal type,
+/// - ROS callback type.
+template <typename SotType>
+class SotToRos;
+
+template <>
+struct SotToRos<bool> {
+  typedef bool sot_t;
+  typedef std_msgs::Bool ros_t;
+  typedef std_msgs::BoolConstPtr ros_const_ptr_t;
+  typedef dynamicgraph::Signal<sot_t, int> signal_t;
+  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+  typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+  static const char* signalTypeName;
+
+  template <typename S>
+  static void setDefault(S& s) {
+    s.setConstant(false);
   }
-} // end of namespace dynamicgraph.
 
-#endif //! DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
+  static void setDefault(sot_t& s) { s = false; }
+};
+
+template <>
+struct SotToRos<double> {
+  typedef double sot_t;
+  typedef std_msgs::Float64 ros_t;
+  typedef std_msgs::Float64ConstPtr ros_const_ptr_t;
+  typedef dynamicgraph::Signal<sot_t, int> signal_t;
+  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+  typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+  static const char* signalTypeName;
+
+  template <typename S>
+  static void setDefault(S& s) {
+    s.setConstant(0.);
+  }
+
+  static void setDefault(sot_t& s) { s = 0.; }
+};
+
+template <>
+struct SotToRos<unsigned int> {
+  typedef unsigned int sot_t;
+  typedef std_msgs::UInt32 ros_t;
+  typedef std_msgs::UInt32ConstPtr ros_const_ptr_t;
+  typedef dynamicgraph::Signal<sot_t, int> signal_t;
+  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+  typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+  static const char* signalTypeName;
+
+  template <typename S>
+  static void setDefault(S& s) {
+    s.setConstant(0);
+  }
+
+  static void setDefault(sot_t& s) { s = 0; }
+};
+
+template <>
+struct SotToRos<int> {
+  typedef int sot_t;
+  typedef std_msgs::Int32 ros_t;
+  typedef std_msgs::Int32ConstPtr ros_const_ptr_t;
+  typedef dynamicgraph::Signal<sot_t, int> signal_t;
+  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+  typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+  static const char* signalTypeName;
+
+  template <typename S>
+  static void setDefault(S& s) {
+    s.setConstant(0);
+  }
+
+  static void setDefault(sot_t& s) { s = 0; }
+};
+
+template <>
+struct SotToRos<std::string> {
+  typedef std::string sot_t;
+  typedef std_msgs::String ros_t;
+  typedef std_msgs::StringConstPtr ros_const_ptr_t;
+  typedef dynamicgraph::Signal<sot_t, int> signal_t;
+  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+  typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+  static const char* signalTypeName;
+
+  template <typename S>
+  static void setDefault(S& s) {
+    s.setConstant("");
+  }
+
+  static void setDefault(sot_t& s) { s = std::string(); }
+};
+
+template <>
+struct SotToRos<Matrix> {
+  typedef Matrix sot_t;
+  typedef dynamic_graph_bridge_msgs::Matrix ros_t;
+  typedef dynamic_graph_bridge_msgs::MatrixConstPtr ros_const_ptr_t;
+  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+  typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+  static const char* signalTypeName;
+
+  template <typename S>
+  static void setDefault(S& s) {
+    Matrix m;
+    m.resize(0, 0);
+    s.setConstant(m);
+  }
+
+  static void setDefault(sot_t& s) { s.resize(0, 0); }
+};
+
+template <>
+struct SotToRos<Vector> {
+  typedef Vector sot_t;
+  typedef dynamic_graph_bridge_msgs::Vector ros_t;
+  typedef dynamic_graph_bridge_msgs::VectorConstPtr ros_const_ptr_t;
+  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+  typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+  static const char* signalTypeName;
+
+  template <typename S>
+  static void setDefault(S& s) {
+    Vector v;
+    v.resize(0);
+    s.setConstant(v);
+  }
+
+  static void setDefault(sot_t& s) { s.resize(0); }
+};
+
+template <>
+struct SotToRos<specific::Vector3> {
+  typedef Vector sot_t;
+  typedef geometry_msgs::Vector3 ros_t;
+  typedef geometry_msgs::Vector3ConstPtr ros_const_ptr_t;
+  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+  typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+  static const char* signalTypeName;
+
+  template <typename S>
+  static void setDefault(S& s) {
+    Vector v(Vector::Zero(3));
+    s.setConstant(v);
+  }
+
+  static void setDefault(sot_t& s) { s = Vector::Zero(3); }
+};
+
+template <>
+struct SotToRos<sot::MatrixHomogeneous> {
+  typedef sot::MatrixHomogeneous sot_t;
+  typedef geometry_msgs::Transform ros_t;
+  typedef geometry_msgs::TransformConstPtr ros_const_ptr_t;
+  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+  typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+  static const char* signalTypeName;
+
+  template <typename S>
+  static void setDefault(S& s) {
+    sot::MatrixHomogeneous m;
+    s.setConstant(m);
+  }
+
+  static void setDefault(sot_t& s) { s.setIdentity(); }
+};
+
+template <>
+struct SotToRos<specific::Twist> {
+  typedef Vector sot_t;
+  typedef geometry_msgs::Twist ros_t;
+  typedef geometry_msgs::TwistConstPtr ros_const_ptr_t;
+  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+  typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+  static const char* signalTypeName;
+
+  template <typename S>
+  static void setDefault(S& s) {
+    Vector v(6);
+    v.setZero();
+    s.setConstant(v);
+  }
+
+  static void setDefault(sot_t& s) { s = Vector::Zero(6); }
+};
+
+// Stamped vector3
+template <>
+struct SotToRos<std::pair<specific::Vector3, Vector> > {
+  typedef Vector sot_t;
+  typedef geometry_msgs::Vector3Stamped ros_t;
+  typedef geometry_msgs::Vector3StampedConstPtr ros_const_ptr_t;
+  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+  typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+  static const char* signalTypeName;
+
+  template <typename S>
+  static void setDefault(S& s) {
+    SotToRos<sot_t>::setDefault(s);
+  }
+};
+
+// Stamped transformation
+template <>
+struct SotToRos<std::pair<sot::MatrixHomogeneous, Vector> > {
+  typedef sot::MatrixHomogeneous sot_t;
+  typedef geometry_msgs::TransformStamped ros_t;
+  typedef geometry_msgs::TransformStampedConstPtr ros_const_ptr_t;
+  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+  typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+  static const char* signalTypeName;
+
+  template <typename S>
+  static void setDefault(S& s) {
+    SotToRos<sot_t>::setDefault(s);
+  }
+};
+
+// Stamped twist
+template <>
+struct SotToRos<std::pair<specific::Twist, Vector> > {
+  typedef Vector sot_t;
+  typedef geometry_msgs::TwistStamped ros_t;
+  typedef geometry_msgs::TwistStampedConstPtr ros_const_ptr_t;
+  typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+  typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+  typedef boost::function<sot_t&(sot_t&, int)> callback_t;
+
+  static const char* signalTypeName;
+
+  template <typename S>
+  static void setDefault(S& s) {
+    SotToRos<sot_t>::setDefault(s);
+  }
+};
+
+inline std::string makeSignalString(const std::string& className,
+                                    const std::string& instanceName,
+                                    bool isInputSignal,
+                                    const std::string& signalType,
+                                    const std::string& signalName) {
+  boost::format fmt("%s(%s)::%s(%s)::%s");
+  fmt % className % instanceName % (isInputSignal ? "input" : "output") %
+      signalType % signalName;
+  return fmt.str();
+}
+}  // end of namespace dynamicgraph.
+
+#endif  //! DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH

srv/RunPythonFile.srv

-string input
----
-string result

tests/CMakeLists.txt

+if(BUILD_PYTHON_INTERFACE)
+  # TODO: this test requires a ros master ADD_PYTHON_UNIT_TEST("py-import"
+  # "tests/test_import.py")
+endif(BUILD_PYTHON_INTERFACE)

tests/test_import.py

-#!/usr/bin/python
+#!/usr/bin/env python
 
 from dynamic_graph.ros import RosImport
 
-ri = RosImport('rosimport')
+ri = RosImport("rosimport")
 
-ri.add('double', 'doubleS', 'doubleT')
-ri.add('vector', 'vectorS', 'vectorT')
-ri.add('matrix', 'matrixS', 'matrixT')
+ri.add("double", "doubleS", "doubleT")
+ri.add("vector", "vectorS", "vectorT")
+ri.add("matrix", "matrixS", "matrixT")
 
-ri.doubleS.value = 42.
-ri.vectorS.value = (42., 42.,)
-ri.matrixS.value = ((42., 42.,),(42., 42.,),)
+ri.doubleS.value = 42.0
+ri.vectorS.value = (
+    42.0,
+    42.0,
+)
+ri.matrixS.value = (
+    (
+        42.0,
+        42.0,
+    ),
+    (
+        42.0,
+        42.0,
+    ),
+)
 
 ri.trigger.recompute(ri.trigger.time + 1)