Use boost::posix_time::ptime to convey time between Ros and dynamic-graph.

  Define entity RosTime that exports Ros time in a signal,
  add this entity in class dynamic_graph.ros.ros.Ros.
  Transfer Ros time in signal of type ptime instead of vector.

CMakeLists.txt

@@ -71,7 +71,7 @@ link_directories(${DYNAMIC_GRAPH_LIBRARY_DIRS})
 
 compile_plugin(ros_import)
 compile_plugin(ros_export)
-compile_plugin(ros_export)
+compile_plugin(ros_time)
 
 compile_plugin(ros_joint_state)
 target_link_libraries(ros_joint_state "${DYNAMIC_GRAPH_PLUGINDIR}/dynamic.so")

src/converter.hh

@@ -4,6 +4,8 @@
 # include "sot_to_ros.hh"
 
 # include <boost/static_assert.hpp>
+# include <boost/date_time/date.hpp>
+# include <boost/date_time/posix_time/posix_time.hpp>
 
 # include <ros/time.h>
 # include <std_msgs/Header.h>
@@ -270,6 +272,30 @@ namespace dynamicgraph
     // This will always fail if instantiated.
     BOOST_STATIC_ASSERT (sizeof (U) == 0);
   }
+
+  typedef boost::posix_time::ptime ptime;
+  typedef boost::posix_time::seconds seconds;
+  typedef boost::posix_time::microseconds microseconds;
+  typedef boost::posix_time::time_duration time_duration;
+  typedef boost::gregorian::date date;
+
+  boost::posix_time::ptime rosTimeToPtime (const ros::Time& rosTime)
+  {
+    ptime time (date(1970,1,1),	seconds (rosTime.sec) +
+		microseconds (rosTime.nsec/1000));
+    return time;
+  }
+
+  ros::Time pTimeToRostime (const boost::posix_time::ptime& time)
+  {
+    static ptime timeStart(date(1970,1,1));
+    time_duration diff = time - timeStart;
+
+    uint32_t sec = diff.ticks ()/time_duration::rep_type::res_adjust ();
+    uint32_t nsec = diff.fractional_seconds();
+
+    return ros::Time (sec, nsec);
+  }
 } // end of namespace dynamicgraph.
 
 #endif //! DYNAMIC_GRAPH_ROS_CONVERTER_HH

src/dynamic_graph/ros/ros.py

 from ros_import import RosImport
 from ros_export import RosExport
 from ros_joint_state import RosJointState
+from ros_time import RosTime
 
 from dynamic_graph import plug
 
@@ -16,6 +17,7 @@ class Ros(object):
         self.rosExport = RosExport('rosExport{0}'.format(suffix))
         self.rosJointState = RosJointState('rosJointState{0}'.format(suffix))
         self.rosJointState.retrieveJointNames(self.robot.dynamic.name)
+        self.rosTime = RosTime ('rosTime{0}'.format(suffix))
 
         plug(self.robot.device.state, self.rosJointState.state)
         self.robot.device.after.addSignal('{0}.trigger'.format(self.rosImport.name))

src/ros_export.hh

@@ -57,6 +57,7 @@ namespace dynamicgraph
   class RosExport : 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> >
@@ -93,7 +94,7 @@ namespace dynamicgraph
 
     template <typename R>
     void callbackTimestamp
-    (boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector, int> > signal,
+    (boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
      const R& data);
 
     template <typename T>

src/ros_export.hxx

@@ -2,10 +2,14 @@
 # define DYNAMIC_GRAPH_ROS_EXPORT_HXX
 # include <vector>
 # include <boost/bind.hpp>
+# include <boost/date_time/posix_time/posix_time.hpp>
+# include <dynamic-graph/signal-caster.h>
+# include <dynamic-graph/signal-cast-helper.h>
 # include <jrl/mal/boost.hh>
 # include <std_msgs/Float64.h>
 # include "dynamic_graph_bridge/Matrix.h"
 # include "dynamic_graph_bridge/Vector.h"
+# include "ros_time.hh"
 
 namespace ml = ::maal::boost;
 
@@ -26,13 +30,11 @@ namespace dynamicgraph
   template <typename R>
   void
   RosExport::callbackTimestamp
-  (boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector, int> > signal,
+  (boost::shared_ptr<dynamicgraph::SignalPtr<ptime, 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.;
+    ptime time =
+      rosTimeToPtime (data->header.stamp);
     signal->setConstant(time);
   }
 
@@ -116,7 +118,7 @@ namespace dynamicgraph
 
 
 	// Timestamp.
-	typedef dynamicgraph::SignalPtr<ml::Vector, int>
+	typedef dynamicgraph::SignalPtr<RosExport::ptime, int>
 	  signalTimestamp_t;
 	std::string signalTimestamp =
 	  (boost::format ("%1%%2%") % signal % "Timestamp").str ();
@@ -131,8 +133,7 @@ namespace dynamicgraph
 	boost::shared_ptr<signalTimestamp_t> signalTimestamp_
 	  (new signalTimestamp_t (0, signalNameTimestamp.str ()));
 
-	ml::Vector zero (2);
-	zero.setZero ();
+	RosExport::ptime zero (rosTimeToPtime (ros::Time (0, 0)));
 	signalTimestamp_->setConstant (zero);
 	bindedSignalTimestamp.first = signalTimestamp_;
 	rosExport.signalRegistration (*bindedSignalTimestamp.first);

src/ros_time.cpp

+///
+/// Copyright 2012 CNRS
+///
+/// Author: Florent Lamiraux
+
+#include <dynamic-graph/factory.h>
+#include <dynamic-graph/signal-caster.h>
+#include <dynamic-graph/signal-cast-helper.h>
+
+#include "ros_time.hh"
+#include "converter.hh"
+
+namespace dynamicgraph {
+
+  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTime, "RosTime");
+
+  using namespace boost::posix_time;
+
+  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;
+  }
+
+} // namespace dynamicgraph

src/ros_time.hh

+///
+/// Copyright 2012 CNRS
+///
+/// Author: Florent Lamiraux
+
+#ifndef 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>
+
+namespace dynamicgraph {
+
+  class RosTime : public dynamicgraph::Entity
+  {
+    DYNAMIC_GRAPH_ENTITY_DECL ();
+  public:
+    Signal <boost::posix_time::ptime, int> now_;
+    RosTime (const std::string& name);
+  protected:
+    boost::posix_time::ptime&
+    update (boost::posix_time::ptime& time, const int& t);
+  }; // class RosTime
+
+} // namespace dynamicgraph
+
+#endif // DYNAMIC_GRAPH_ROS_TIME_HH