From 7594f24cc4c93a259bbfc6d1e97f356afd98a20d Mon Sep 17 00:00:00 2001
From: Martin Dahl <martin.dahl@gmail.com>
Date: Mon, 14 Sep 2020 19:13:07 +0200
Subject: [PATCH] RosClock

---
 examples/clock.rs | 11 +++++++++++
 src/lib.rs        | 48 ++++++++++++++++++++++++++++++++++++++++++++++-
 2 files changed, 58 insertions(+), 1 deletion(-)
 create mode 100644 examples/clock.rs

diff --git a/examples/clock.rs b/examples/clock.rs
new file mode 100644
index 0000000..2346dd0
--- /dev/null
+++ b/examples/clock.rs
@@ -0,0 +1,11 @@
+use r2r;
+use failure::Error;
+
+
+fn main() -> Result<(), Error> {
+    let mut clock = r2r::RosClock::create()?;
+    let now = clock.get_now()?;
+    let time = r2r::RosClock::to_builtin_time(&now);
+    println!("time: {:?}", time);
+    Ok(())
+}
diff --git a/src/lib.rs b/src/lib.rs
index f78c1ea..d4346a9 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1128,7 +1128,6 @@ impl Node {
         Ok(res)
     }
 
-
     pub fn create_wall_timer(
         &mut self,
         period: Duration,
@@ -1331,6 +1330,53 @@ impl PublisherUntyped {
     }
 }
 
+pub struct RosClock {
+    clock_handle: Box<rcl_clock_t>,
+}
+
+impl RosClock {
+    pub fn create() -> Result<Self> {
+        let mut clock_handle = MaybeUninit::<rcl_clock_t>::uninit();
+
+        let ret = unsafe {
+            rcl_ros_clock_init(clock_handle.as_mut_ptr(), &mut rcutils_get_default_allocator())
+        };
+        if ret != RCL_RET_OK as i32 {
+            eprintln!("could not create steady clock: {}", ret);
+            return Err(Error::from_rcl_error(ret));
+        }
+
+        let clock_handle = Box::new(unsafe { clock_handle.assume_init() });
+        Ok(RosClock { clock_handle })
+    }
+
+    pub fn get_now(&mut self) -> Result<Duration> {
+        let mut tp: rcutils_time_point_value_t = 0;
+        let ret = unsafe {
+            rcl_clock_get_now(&mut *self.clock_handle,
+                              &mut tp)
+        };
+
+        if ret != RCL_RET_OK as i32 {
+            eprintln!("could not create steady clock: {}", ret);
+            return Err(Error::from_rcl_error(ret));
+        }
+
+        let dur = Duration::from_nanos(tp as u64);
+
+        Ok(dur)
+    }
+
+    pub fn to_builtin_time(d: &Duration) -> builtin_interfaces::msg::Time {
+        let sec = d.as_secs() as i32;
+        let nanosec = d.subsec_nanos();
+        builtin_interfaces::msg::Time {
+            sec,
+            nanosec,
+        }
+    }
+}
+
 #[cfg(test)]
 mod tests {
     use super::*;