From ae9c8d1c9aefc54b4261a90acb8343c24043ffab Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jo=C3=A3o=20Reis?= <joaocgreis@gmail.com>
Date: Sun, 26 Oct 2014 01:43:15 +0100
Subject: [PATCH] Updated time functions

---
 include/roah_rsbb.h     | 29 ++++++++++++++++++++++-------
 include/ros_roah_rsbb.h | 16 ++++++++++++++++
 2 files changed, 38 insertions(+), 7 deletions(-)

diff --git a/include/roah_rsbb.h b/include/roah_rsbb.h
index f98f282..9678039 100644
--- a/include/roah_rsbb.h
+++ b/include/roah_rsbb.h
@@ -30,19 +30,34 @@
 
 namespace roah_rsbb
 {
-  void
-  now (::roah_rsbb_msgs::Time* time_msg)
+  inline void
+  boost_to_proto_time (boost::posix_time::ptime const& boost, ::roah_rsbb_msgs::Time* proto)
   {
-    boost::posix_time::ptime now = boost::posix_time::microsec_clock::universal_time();
-    boost::posix_time::time_duration diff = now - boost::posix_time::from_time_t (0);
-    time_msg->set_sec (diff.total_seconds());
+    boost::posix_time::time_duration diff = boost - boost::posix_time::from_time_t (0);
+    proto->set_sec (diff.total_seconds());
 #if defined(BOOST_DATE_TIME_HAS_NANOSECONDS)
-    time_msg->set_nsec (diff.fractional_seconds());
+    proto->set_nsec (diff.fractional_seconds());
 #else
-    time_msg->set_nsec (diff.fractional_seconds() * 1000);
+    proto->set_nsec (diff.fractional_seconds() * 1000);
 #endif
   }
 
+  inline boost::posix_time::ptime
+  proto_to_boost_time (::roah_rsbb_msgs::Time const& proto)
+  {
+#if defined(BOOST_DATE_TIME_HAS_NANOSECONDS)
+    return boost::posix_time::from_time_t (proto.sec()) + boost::posix_time::nanoseconds (proto.nsec());
+#else
+    return boost::posix_time::from_time_t (proto.sec()) + boost::posix_time::microseconds (proto.nsec() / 1000.0);
+#endif
+  }
+
+  inline void
+  now (::roah_rsbb_msgs::Time* time_msg)
+  {
+    boost_to_proto_time (boost::posix_time::microsec_clock::universal_time(), time_msg);
+  }
+
 
 
   class CerrErrorHandler
diff --git a/include/ros_roah_rsbb.h b/include/ros_roah_rsbb.h
index a1f6cdf..02459f2 100644
--- a/include/ros_roah_rsbb.h
+++ b/include/ros_roah_rsbb.h
@@ -24,6 +24,7 @@
 
 #include <ros/console.h>
 #include <ros/param.h>
+#include <ros/time.h>
 
 #include "roah_rsbb.h"
 
@@ -31,6 +32,21 @@
 
 namespace roah_rsbb
 {
+  inline void
+  ros_to_proto_time (ros::Time const& ros, ::roah_rsbb_msgs::Time* proto)
+  {
+    proto->set_sec (ros.sec);
+    proto->set_nsec (ros.nsec);
+  }
+
+  inline ros::Time
+  proto_to_ros_time (::roah_rsbb_msgs::Time const& proto)
+  {
+    return ros::Time (proto.sec(), proto.nsec());
+  }
+
+
+
   class RosErrorHandler
   {
     public: