Adding a simple helper function for converting rclrs::Time to a ros message (#359)

* Adding a simple helper function for converting `rclrs::Time` to a ros
message

This commit adds a simple conversion function that would converts Time to
builtin_interfaces like the one in rclpy.

Signed-off-by: Arjo Chakravarty <[email protected]>

* Style

Signed-off-by: Arjo Chakravarty <[email protected]>

* Update tests

Signed-off-by: Arjo Chakravarty <[email protected]>

* Remove whitespace

Signed-off-by: Arjo Chakravarty <[email protected]>

---------

Signed-off-by: Arjo Chakravarty <[email protected]>

Author: arjo129

rclrs/Cargo.toml

@@ -26,6 +26,9 @@ libloading = { version = "0.8", optional = true }
 # Needed for the Message trait, among others
 rosidl_runtime_rs = "0.4"
 
+[dependencies.builtin_interfaces]
+version = "*"
+
 [dev-dependencies]
 # Needed for e.g. writing yaml files in tests
 tempfile = "3.3.0"

rclrs/src/time.rs

@@ -1,4 +1,5 @@
 use crate::rcl_bindings::*;
+use std::num::TryFromIntError;
 use std::ops::{Add, Sub};
 use std::sync::{Mutex, Weak};
 use std::time::Duration;
@@ -23,6 +24,17 @@ impl Time {
             .ptr_eq(&rhs.clock)
             .then(|| f(self.nsec, rhs.nsec))
     }
+
+    /// Convenience function for converting time to ROS message
+    pub fn to_ros_msg(&self) -> Result<builtin_interfaces::msg::Time, TryFromIntError> {
+        let nanosec = self.nsec % 1_000_000_000;
+        let sec = self.nsec / 1_000_000_000;
+
+        Ok(builtin_interfaces::msg::Time {
+            nanosec: nanosec.try_into()?,
+            sec: sec.try_into()?,
+        })
+    }
 }
 
 impl Add<Duration> for Time {
@@ -84,4 +96,17 @@ mod tests {
         let t3 = t2 - Duration::from_secs(1);
         assert_eq!(t3.nsec, t.nsec);
     }
+
+    #[test]
+    fn test_conversion() {
+        let clock = Clock::system();
+        let t1 = clock.now();
+        let time = Time {
+            nsec: 1_000_000_100,
+            clock: t1.clock.clone(),
+        };
+        let msg = time.to_ros_msg().unwrap();
+        assert_eq!(msg.nanosec, 100);
+        assert_eq!(msg.sec, 1);
+    }
 }