Handle timestamped types (TransformStamped).

src/converter.hh

@@ -7,6 +7,13 @@
 
 namespace dynamicgraph
 {
+  /// \brief Handle ROS <-> dynamic-graph conversion.
+  ///
+  /// Implements all ROS/dynamic-graph conversions required by the
+  /// bridge.
+  ///
+  ///Relies on the SotToRos type trait to make sure valid pair of
+  /// types are used.
   template <typename D, typename S>
   void converter (D& dst, const S& src);
 
@@ -55,6 +62,16 @@ namespace dynamicgraph
     dst.rotation.w = quaternion.w ();
   }
 
+  template <>
+  inline void converter
+  (SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >::ros_t& dst,
+   const SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >::sot_t& src)
+  {
+    converter
+      <SotToRos<sot::MatrixHomogeneous>::ros_t,
+      SotToRos<sot::MatrixHomogeneous>::sot_t> (dst.transform, src);
+  }
+
   template <>
   inline void converter (SotToRos<ml::Vector>::ros_t& dst,
 			 const SotToRos<ml::Vector>::sot_t& src)
@@ -104,6 +121,16 @@ namespace dynamicgraph
     dst(2, 3) = src.translation.z;
   }
 
+  template <>
+  inline void converter
+  (SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >::sot_t& dst,
+   const SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >::ros_t& src)
+  {
+    converter
+      <SotToRos<sot::MatrixHomogeneous>::sot_t,
+      SotToRos<sot::MatrixHomogeneous>::ros_t> (dst, src.transform);
+  }
+
   template <>
   inline void converter
   (SotToRos<ml::Vector>::sot_t& dst,
@@ -134,6 +161,17 @@ namespace dynamicgraph
       SotToRos<sot::MatrixHomogeneous>::ros_t> (dst, *src);
   }
 
+  template <>
+  inline void converter
+   (SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >::sot_t& dst,
+    const boost::shared_ptr
+    <SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >::ros_t const>& src)
+  {
+    converter
+      <SotToRos<sot::MatrixHomogeneous>::sot_t,
+      SotToRos<sot::MatrixHomogeneous>::ros_t> (dst, src->transform);
+  }
+
 
 } // end of namespace dynamicgraph.
 

src/ros_export.cpp

@@ -79,6 +79,9 @@ namespace dynamicgraph
 	  entity.add<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
 	  throw std::runtime_error("bad type");
 	return Value ();

src/ros_export.hh

@@ -47,6 +47,13 @@ namespace dynamicgraph
   } // end of namespace command.
 
 
+  namespace internal
+  {
+    template <typename T>
+    struct Add;
+  } // end of internal namespace.
+
+  /// \brief Publish ROS information in the dynamic-graph.
   class RosExport : public dynamicgraph::Entity
   {
     DYNAMIC_GRAPH_ENTITY_DECL();
@@ -68,12 +75,30 @@ namespace dynamicgraph
     template <typename T>
     void add (const std::string& signal, const std::string& topic);
 
-  private:
+    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<ml::Vector, int> > signal,
+     const R& data);
+
+    template <typename T>
+    friend class internal::Add;
+  private:
     ros::NodeHandle nh_;
     std::map<std::string, bindedSignal_t> bindedSignal_;
     ros::AsyncSpinner spinner_;

src/ros_export.hxx

@@ -7,8 +7,6 @@
 # include "dynamic_graph_bridge/Matrix.h"
 # include "dynamic_graph_bridge/Vector.h"
 
-#include <iostream>//FIXME:
-
 namespace ml = ::maal::boost;
 
 namespace dynamicgraph
@@ -25,36 +23,139 @@ namespace dynamicgraph
     signal->setConstant (value);
   }
 
+  template <typename R>
+  void
+  RosExport::callbackTimestamp
+  (boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector, int> > signal,
+   const R& data)
+  {
+    ml::Vector time (2);
+    time (0) = data->header.stamp.sec;
+    // Convert nanoseconds into microseconds (i.e. timeval structure).
+    time (1) = data->header.stamp.nsec / 1000.;
+    signal->setConstant(time);
+  }
 
-  template <typename T>
-  void RosExport::add (const std::string& signal, const std::string& topic)
+  namespace internal
   {
-    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;
+    template <typename T>
+    struct Add
+    {
+      void operator () (RosExport& rosExport,
+			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.
+	RosExport::bindedSignal_t bindedSignal;
+
+	// Initialize the signal.
+	boost::format signalName ("RosExport(%1%)::%2%");
+	signalName % rosExport.getName () % signal;
+
+	boost::shared_ptr<signal_t> signal_
+	  (new signal_t (0, signalName.str ()));
+	signal_->setConstant (sot_t ());
+	bindedSignal.first = signal_;
+	rosExport.signalRegistration (*bindedSignal.first);
+
+	// Initialize the publisher.
+	typedef boost::function<void (const ros_const_ptr_t& data)> callback_t;
+	callback_t callback = boost::bind
+	  (&RosExport::callback<ros_const_ptr_t, sot_t>,
+	   &rosExport, signal_, _1);
+
+	bindedSignal.second =
+	  boost::make_shared<ros::Subscriber>
+	  (rosExport.nh ().subscribe (topic, 1, callback));
+
+	rosExport.bindedSignal ()[signal] = bindedSignal;
+      }
+    };
+
+    template <typename T>
+    struct Add<std::pair<T, ml::Vector> >
+    {
+      void operator () (RosExport& rosExport,
+			const std::string& signal,
+			const std::string& topic)
+      {
+	typedef std::pair<T, ml::Vector> type_t;
 
-    // Initialize the bindedSignal object.
-    bindedSignal_t bindedSignal;
+	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 signal.
-    boost::format signalName ("RosExport(%1%)::%2%");
-    signalName % name % signal;
+	// Initialize the bindedSignal object.
+	RosExport::bindedSignal_t bindedSignal;
 
-    boost::shared_ptr<signal_t> signal_ (new signal_t (0, signalName.str ()));
-    signal_->setConstant (sot_t ());
-    bindedSignal.first = signal_;
-    signalRegistration (*bindedSignal.first);
+	// Initialize the signal.
+	boost::format signalName ("RosExport(%1%)::%2%");
+	signalName % rosExport.getName () % signal;
 
-    // Initialize the publisher.
-    typedef boost::function<void (const ros_const_ptr_t& data)> callback_t;
-    callback_t callback = boost::bind
-      (&RosExport::callback<ros_const_ptr_t, sot_t>,
-       this, signal_, _1);
+	boost::shared_ptr<signal_t> signal_
+	  (new signal_t (0, signalName.str ()));
+	signal_->setConstant (sot_t ());
+	bindedSignal.first = signal_;
+	rosExport.signalRegistration (*bindedSignal.first);
 
-    bindedSignal.second =
-      boost::make_shared<ros::Subscriber> (nh_.subscribe (topic, 1, callback));
+	// Initialize the publisher.
+	typedef boost::function<void (const ros_const_ptr_t& data)> callback_t;
+	callback_t callback = boost::bind
+	  (&RosExport::callback<ros_const_ptr_t, sot_t>,
+	   &rosExport, signal_, _1);
 
-    bindedSignal_[signal] = bindedSignal;
+	bindedSignal.second =
+	  boost::make_shared<ros::Subscriber>
+	  (rosExport.nh ().subscribe (topic, 1, callback));
+
+	rosExport.bindedSignal ()[signal] = bindedSignal;
+
+
+	// Timestamp.
+	typedef dynamicgraph::SignalPtr<ml::Vector, int>
+	  signalTimestamp_t;
+	std::string signalTimestamp =
+	  (boost::format ("%1%%2%") % signal % "Timestamp").str ();
+
+	// Initialize the bindedSignal object.
+	RosExport::bindedSignal_t bindedSignalTimestamp;
+
+	// Initialize the signal.
+	boost::format signalNameTimestamp ("RosExport(%1%)::%2%");
+	signalNameTimestamp % rosExport.name % signalTimestamp;
+
+	boost::shared_ptr<signalTimestamp_t> signalTimestamp_
+	  (new signalTimestamp_t (0, signalNameTimestamp.str ()));
+
+	ml::Vector zero (2);
+	zero.setZero ();
+	signalTimestamp_->setConstant (zero);
+	bindedSignalTimestamp.first = signalTimestamp_;
+	rosExport.signalRegistration (*bindedSignalTimestamp.first);
+
+	// Initialize the publisher.
+	typedef boost::function<void (const ros_const_ptr_t& data)> callback_t;
+	callback_t callbackTimestamp = boost::bind
+	  (&RosExport::callbackTimestamp<ros_const_ptr_t>,
+	   &rosExport, signalTimestamp_, _1);
+
+	bindedSignalTimestamp.second =
+	  boost::make_shared<ros::Subscriber>
+	  (rosExport.nh ().subscribe (topic, 1, callbackTimestamp));
+
+	rosExport.bindedSignal ()[signalTimestamp] = bindedSignalTimestamp;
+      }
+    };
+  } // end of namespace internal.
+
+  template <typename T>
+  void RosExport::add (const std::string& signal, const std::string& topic)
+  {
+    internal::Add<T> () (*this, signal, topic);
   }
 } // end of namespace dynamicgraph.
 

src/ros_import.hh

@@ -45,6 +45,7 @@ namespace dynamicgraph
   } // end of namespace command.
 
 
+  /// \brief Publish dynamic-graph information into ROS.
   class RosImport : public dynamicgraph::Entity
   {
     DYNAMIC_GRAPH_ENTITY_DECL();

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 <jrl/mal/boost.hh>
 # include <std_msgs/Float64.h>
 # include "dynamic_graph_bridge/Matrix.h"
 # include "dynamic_graph_bridge/Vector.h"
 
 # include "geometry_msgs/Transform.h"
+# include "geometry_msgs/TransformStamped.h"
 
 # include <dynamic-graph/signal-time-dependent.h>
 # include <dynamic-graph/signal-ptr.h>
@@ -16,6 +19,12 @@ namespace dynamicgraph
 {
   namespace ml = maal::boost;
 
+  /// \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;
 
@@ -63,6 +72,18 @@ namespace dynamicgraph
     typedef boost::function<sot_t& (sot_t&, int)> callback_t;
   };
 
+  // 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;
+  };
+
 } // end of namespace dynamicgraph.
 
 #endif //! DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH