Updated time functions

rockin-robot-challenge · Oct 26, 2014 · ae9c8d1 · ae9c8d1
1 parent 08094bd
commit ae9c8d1
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 7 deletions.
diff --git 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
@@ -24,13 +24,29 @@
 
 #include <ros/console.h>
 #include <ros/param.h>
+#include <ros/time.h>
 
 #include "roah_rsbb.h"
 
 
 
 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: