Add time source and clock API to nodes

examples/minimal_pub_sub/src/minimal_subscriber.rs

@@ -6,6 +6,7 @@ fn main() -> Result<(), Error> {
     let context = rclrs::Context::new(env::args())?;
 
     let node = rclrs::create_node(&context, "minimal_subscriber")?;
+    let inner_node = node.clone();
 
     let mut num_messages: usize = 0;
 
@@ -16,6 +17,8 @@ fn main() -> Result<(), Error> {
             num_messages += 1;
             println!("I heard: '{}'", msg.data);
             println!("(Got {} messages so far)", num_messages);
+            let now = inner_node.get_clock().now();
+            dbg!(now);
         },
     )?;
 

rclrs/Cargo.toml

@@ -20,6 +20,8 @@ ament_rs = { version = "0.2", optional = true }
 futures = "0.3"
 # Needed for dynamic messages
 libloading = { version = "0.8", optional = true }
+# Needed for /clock topic subscription when using simulation time
+rosgraph_msgs = "*"
 # Needed for the Message trait, among others
 rosidl_runtime_rs = "0.3"
 

rclrs/package.xml

@@ -18,6 +18,7 @@
   <build_depend>rcl</build_depend>
   <depend>builtin_interfaces</depend>
   <depend>rcl_interfaces</depend>
+  <depend>rosgraph_msgs</depend>
 
   <export>
     <build_type>ament_cargo</build_type>

rclrs/src/clock.rs

@@ -0,0 +1,223 @@
+use crate::rcl_bindings::*;
+use crate::{error::ToResult, time::Time, to_rclrs_result, RclrsError};
+use std::sync::{Arc, Mutex};
+
+/// Enum to describe clock type. Redefined for readability and to eliminate the uninitialized case
+/// from the `rcl_clock_type_t` enum in the binding.
+#[derive(Clone, Debug, Copy)]
+pub enum ClockType {
+    /// Time with behavior dependent on the `set_ros_time(bool)` function. If called with `true`
+    /// it will be driven by a manual value override, otherwise it will be System Time
+    RosTime = 1,
+    /// Wall time depending on the current system
+    SystemTime = 2,
+    /// Steady time, monotonically increasing but not necessarily equal to wall time.
+    SteadyTime = 3,
+}
+
+impl From<ClockType> for rcl_clock_type_t {
+    fn from(clock_type: ClockType) -> Self {
+        match clock_type {
+            ClockType::RosTime => rcl_clock_type_t::RCL_ROS_TIME,
+            ClockType::SystemTime => rcl_clock_type_t::RCL_SYSTEM_TIME,
+            ClockType::SteadyTime => rcl_clock_type_t::RCL_STEADY_TIME,
+        }
+    }
+}
+
+/// Struct that implements a Clock and wraps `rcl_clock_t`.
+pub struct Clock {
+    _type: ClockType,
+    _rcl_clock: Arc<Mutex<rcl_clock_t>>,
+    // TODO(luca) Implement jump callbacks
+}
+
+/// A clock source that can be used to drive the contained clock. Created when a clock of type
+/// `ClockType::RosTime` is constructed
+pub struct ClockSource {
+    _rcl_clock: Arc<Mutex<rcl_clock_t>>,
+}
+
+impl Clock {
+    /// Creates a new Clock with `ClockType::SystemTime`
+    pub fn system() -> Result<Self, RclrsError> {
+        Self::make(ClockType::SystemTime)
+    }
+
+    /// Creates a new Clock with `ClockType::SteadyTime`
+    pub fn steady() -> Result<Self, RclrsError> {
+        Self::make(ClockType::SteadyTime)
+    }
+
+    /// Creates a new Clock with `ClockType::RosTime` and a matching `ClockSource` that can be used
+    /// to update it
+    pub fn with_source() -> Result<(Self, ClockSource), RclrsError> {
+        let clock = Self::make(ClockType::RosTime)?;
+        let clock_source = ClockSource {
+            _rcl_clock: clock._rcl_clock.clone(),
+        };
+        Ok((clock, clock_source))
+    }
+
+    /// Creates a new clock of the given `ClockType`.
+    pub fn new(type_: ClockType) -> Result<(Self, Option<ClockSource>), RclrsError> {
+        let clock = Self::make(type_)?;
+        let clock_source = if matches!(type_, ClockType::RosTime) {
+            Some(ClockSource {
+                _rcl_clock: clock._rcl_clock.clone(),
+            })
+        } else {
+            None
+        };
+        Ok((clock, clock_source))
+    }
+
+    fn make(type_: ClockType) -> Result<Self, RclrsError> {
+        let mut rcl_clock;
+        unsafe {
+            // SAFETY: Getting a default value is always safe.
+            rcl_clock = Self::init_generic_clock();
+            let mut allocator = rcutils_get_default_allocator();
+            rcl_clock_init(type_.into(), &mut rcl_clock, &mut allocator).ok()?;
+        }
+        Ok(Self {
+            _type: type_,
+            _rcl_clock: Arc::new(Mutex::new(rcl_clock)),
+        })
+    }
+
+    /// Returns the clock's `ClockType`.
+    pub fn clock_type(&self) -> ClockType {
+        self._type
+    }
+
+    /// Returns the current clock's timestamp.
+    pub fn now(&self) -> Time {
+        let mut clock = self._rcl_clock.lock().unwrap();
+        let mut time_point: i64 = 0;
+        unsafe {
+            // SAFETY: No preconditions for his function
+            rcl_clock_get_now(&mut *clock, &mut time_point);
+        }
+        Time {
+            nsec: time_point,
+            clock: Arc::downgrade(&self._rcl_clock),
+        }
+    }
+
+    /// Helper function to initialize a default clock, same behavior as `rcl_init_generic_clock`.
+    /// Needed because functions that initialize a clock take as an input a mutable reference
+    /// to a clock and don't actually return one, so we need a function to generate one. Doing this
+    /// instead of a `Default` implementation allows the function to be private and avoids
+    /// exposing a public API to create an invalid clock
+    // SAFETY: Getting a default value is always safe.
+    unsafe fn init_generic_clock() -> rcl_clock_t {
+        let allocator = rcutils_get_default_allocator();
+        rcl_clock_t {
+            type_: rcl_clock_type_t::RCL_CLOCK_UNINITIALIZED,
+            jump_callbacks: std::ptr::null_mut::<rcl_jump_callback_info_t>(),
+            num_jump_callbacks: 0,
+            get_now: None,
+            data: std::ptr::null_mut::<std::os::raw::c_void>(),
+            allocator,
+        }
+    }
+}
+
+impl PartialEq for ClockSource {
+    fn eq(&self, other: &Self) -> bool {
+        Arc::ptr_eq(&self._rcl_clock, &other._rcl_clock)
+    }
+}
+
+impl ClockSource {
+    /// Sets the clock to use ROS Time, if enabled the clock will report the last value set through
+    /// `Clock::set_ros_time_override(nanoseconds: i64)`.
+    pub fn set_ros_time_enable(&self, enable: bool) {
+        let mut clock = self._rcl_clock.lock().unwrap();
+        if enable {
+            // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
+            // by the mutex
+            unsafe {
+                // Function will only fail if timer was uninitialized or not RosTime, which should
+                // not happen
+                rcl_enable_ros_time_override(&mut *clock).ok().unwrap();
+            }
+        } else {
+            // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
+            // by the mutex
+            unsafe {
+                // Function will only fail if timer was uninitialized or not RosTime, which should
+                // not happen
+                rcl_disable_ros_time_override(&mut *clock).ok().unwrap();
+            }
+        }
+    }
+
+    /// Sets the value of the current ROS time.
+    pub fn set_ros_time_override(&self, nanoseconds: i64) {
+        let mut clock = self._rcl_clock.lock().unwrap();
+        // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
+        // by the mutex
+        unsafe {
+            // Function will only fail if timer was uninitialized or not RosTime, which should
+            // not happen
+            rcl_set_ros_time_override(&mut *clock, nanoseconds)
+                .ok()
+                .unwrap();
+        }
+    }
+}
+
+impl Drop for rcl_clock_t {
+    fn drop(&mut self) {
+        // SAFETY: No preconditions for this function
+        let rc = unsafe { rcl_clock_fini(&mut *self) };
+        if let Err(e) = to_rclrs_result(rc) {
+            panic!("Unable to release Clock. {:?}", e)
+        }
+    }
+}
+
+// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
+// they are running in. Therefore, this type can be safely sent to another thread.
+unsafe impl Send for rcl_clock_t {}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn assert_send<T: Send>() {}
+    fn assert_sync<T: Sync>() {}
+
+    #[test]
+    fn clock_is_send_and_sync() {
+        assert_send::<Clock>();
+        assert_sync::<Clock>();
+    }
+
+    #[test]
+    fn clock_system_time_now() {
+        let clock = Clock::system().unwrap();
+        assert!(clock.now().nsec > 0);
+    }
+
+    #[test]
+    fn clock_ros_time_with_override() {
+        let (clock, source) = Clock::with_source().unwrap();
+        let start = clock.now();
+        // Ros time is not set, should return wall time
+        assert!(start.nsec > 0);
+        source.set_ros_time_enable(true);
+        // No manual time set, it should default to 0
+        assert!(clock.now().nsec == 0);
+        let set_time = 1234i64;
+        source.set_ros_time_override(set_time);
+        // Ros time is set, should return the value that was set
+        assert_eq!(clock.now().nsec, set_time);
+        // Back to normal time, should be greater than before
+        source.set_ros_time_enable(false);
+        assert!(clock.now().nsec != set_time);
+        assert!(clock.now().nsec > start.nsec);
+    }
+}

rclrs/src/lib.rs

@@ -7,6 +7,7 @@
 
 mod arguments;
 mod client;
+mod clock;
 mod context;
 mod error;
 mod node;
@@ -15,6 +16,8 @@ mod publisher;
 mod qos;
 mod service;
 mod subscription;
+mod time;
+mod time_source;
 mod vendor;
 mod wait;
 
@@ -28,6 +31,7 @@ use std::time::Duration;
 
 pub use arguments::*;
 pub use client::*;
+pub use clock::*;
 pub use context::*;
 pub use error::*;
 pub use node::*;
@@ -38,6 +42,8 @@ use rcl_bindings::rcl_context_is_valid;
 pub use rcl_bindings::rmw_request_id_t;
 pub use service::*;
 pub use subscription::*;
+pub use time::*;
+pub use time_source::*;
 pub use wait::*;
 
 /// Polls the node for new messages and executes the corresponding callbacks.
@@ -109,7 +115,7 @@ pub fn spin(node: Arc<Node>) -> Result<(), RclrsError> {
 /// # Ok::<(), RclrsError>(())
 /// ```
 pub fn create_node(context: &Context, node_name: &str) -> Result<Arc<Node>, RclrsError> {
-    Ok(Arc::new(Node::builder(context, node_name).build()?))
+    Node::new(context, node_name)
 }
 
 /// Creates a [`NodeBuilder`][1].

rclrs/src/node.rs

@@ -13,9 +13,9 @@ pub use self::builder::*;
 pub use self::graph::*;
 use crate::rcl_bindings::*;
 use crate::{
-    Client, ClientBase, Context, GuardCondition, ParameterOverrideMap, Publisher, QoSProfile,
-    RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback,
-    ToResult,
+    Client, ClientBase, Clock, Context, GuardCondition, ParameterOverrideMap, ParameterValue,
+    Publisher, QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase,
+    SubscriptionCallback, TimeSource, ToResult,
 };
 
 impl Drop for rcl_node_t {
@@ -71,6 +71,8 @@ pub struct Node {
     pub(crate) guard_conditions_mtx: Mutex<Vec<Weak<GuardCondition>>>,
     pub(crate) services_mtx: Mutex<Vec<Weak<dyn ServiceBase>>>,
     pub(crate) subscriptions_mtx: Mutex<Vec<Weak<dyn SubscriptionBase>>>,
+    _clock: Clock,
+    _time_source: Mutex<Option<TimeSource>>,
     _parameter_map: ParameterOverrideMap,
 }
 
@@ -95,10 +97,15 @@ impl Node {
     ///
     /// See [`NodeBuilder::new()`] for documentation.
     #[allow(clippy::new_ret_no_self)]
-    pub fn new(context: &Context, node_name: &str) -> Result<Node, RclrsError> {
+    pub fn new(context: &Context, node_name: &str) -> Result<Arc<Node>, RclrsError> {
         Self::builder(context, node_name).build()
     }
 
+    /// Gets the clock associated with this node.
+    pub fn get_clock(&self) -> &Clock {
+        &self._clock
+    }
+
     /// Returns the name of the node.
     ///
     /// This returns the name after remapping, so it is not necessarily the same as the name that
@@ -355,6 +362,12 @@ impl Node {
         domain_id
     }
 
+    /// Gets a parameter given the name.
+    /// Returns None if no parameter with the requested name was found.
+    pub fn get_parameter(&self, name: &str) -> Option<ParameterValue> {
+        self._parameter_map.get(name).cloned()
+    }
+
     /// Creates a [`NodeBuilder`][1] with the given name.
     ///
     /// Convenience function equivalent to [`NodeBuilder::new()`][2].