From 17145d2a869d335853aef6a23e08939366e61b16 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 31 Jul 2023 16:16:10 +0800
Subject: [PATCH 01/37] WIP Add minimal TimeSource implementation

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/Cargo.toml          |   3 +
 rclrs/src/lib.rs          |  15 +-
 rclrs/src/node.rs         |  21 ++-
 rclrs/src/node/builder.rs |  23 ++-
 rclrs/src/qos.rs          |  15 ++
 rclrs/src/time.rs         | 307 ++++++++++++++++++++++++++++++++++++++
 6 files changed, 373 insertions(+), 11 deletions(-)
 create mode 100644 rclrs/src/time.rs

diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml
index c7451db0..6e225712 100644
--- a/rclrs/Cargo.toml
+++ b/rclrs/Cargo.toml
@@ -31,5 +31,8 @@ tempfile = "3.3.0"
 # Needed for FFI
 bindgen = "0.66.1"
 
+[dependencies.rosgraph_msgs]
+version = "*"
+
 [features]
 dyn_msg = ["ament_rs", "libloading"]
diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs
index cda4a382..345ccc4c 100644
--- a/rclrs/src/lib.rs
+++ b/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;
 
@@ -23,11 +26,12 @@ mod rcl_bindings;
 #[cfg(feature = "dyn_msg")]
 pub mod dynamic_message;
 
-use std::sync::Arc;
+use std::sync::{Arc, Mutex};
 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,12 @@ 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()?))
+    println!("Creating node");
+    let mut node = Arc::new(Node::builder(context, node_name).build()?);
+    *node._time_source.lock().unwrap() =
+        Some(TimeSourceBuilder::new(node.clone(), node.get_clock()).build());
+    Ok(node)
+    //Ok(Arc::new(Node::builder(context, node_name).build()?))
 }
 
 /// Creates a [`NodeBuilder`][1].
diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index 74eb5ac8..81abb79f 100644
--- a/rclrs/src/node.rs
+++ b/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, TimeSourceBuilder, ToResult,
 };
 
 impl Drop for rcl_node_t {
@@ -71,6 +71,9 @@ 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: Arc<Mutex<Clock>>,
+    // TODO(luca) set to private
+    pub _time_source: Arc<Mutex<Option<TimeSource>>>,
     _parameter_map: ParameterOverrideMap,
 }
 
@@ -96,7 +99,11 @@ impl Node {
     /// See [`NodeBuilder::new()`] for documentation.
     #[allow(clippy::new_ret_no_self)]
     pub fn new(context: &Context, node_name: &str) -> Result<Node, RclrsError> {
-        Self::builder(context, node_name).build()
+        Self::builder(context, node_name).build().into()
+    }
+
+    pub fn get_clock(&self) -> Arc<Mutex<Clock>> {
+        self._clock.clone()
     }
 
     /// Returns the name of the node.
@@ -355,6 +362,12 @@ impl Node {
         domain_id
     }
 
+    // TODO(luca) There should really be parameter callbacks, this is only for testing
+    // temporarily
+    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].
diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index 60e3afe2..4d96ad77 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -3,8 +3,8 @@ use std::sync::{Arc, Mutex};
 
 use crate::rcl_bindings::*;
 use crate::{
-    node::call_string_getter_with_handle, resolve_parameter_overrides, Context, Node, RclrsError,
-    ToResult,
+    node::call_string_getter_with_handle, resolve_parameter_overrides, Clock, ClockType, Context,
+    Node, RclrsError, TimeSource, TimeSourceBuilder, ToResult,
 };
 
 /// A builder for creating a [`Node`][1].
@@ -17,6 +17,7 @@ use crate::{
 /// - `use_global_arguments: true`
 /// - `arguments: []`
 /// - `enable_rosout: true`
+/// - `clock_type: ClockType::RosTime`
 ///
 /// # Example
 /// ```
@@ -46,6 +47,7 @@ pub struct NodeBuilder {
     use_global_arguments: bool,
     arguments: Vec<String>,
     enable_rosout: bool,
+    clock_type: ClockType,
 }
 
 impl NodeBuilder {
@@ -92,6 +94,7 @@ impl NodeBuilder {
             use_global_arguments: true,
             arguments: vec![],
             enable_rosout: true,
+            clock_type: ClockType::RosTime,
         }
     }
 
@@ -262,6 +265,8 @@ impl NodeBuilder {
             .ok()?;
         };
 
+        let clock = Clock::new(self.clock_type)?;
+
         let _parameter_map = unsafe {
             let fqn = call_string_getter_with_handle(&rcl_node, rcl_node_get_fully_qualified_name);
             resolve_parameter_overrides(
@@ -271,16 +276,24 @@ impl NodeBuilder {
             )?
         };
         let rcl_node_mtx = Arc::new(Mutex::new(rcl_node));
-
-        Ok(Node {
+        let mut node = Node {
             rcl_node_mtx,
             rcl_context_mtx: self.context.clone(),
             clients_mtx: Mutex::new(vec![]),
             guard_conditions_mtx: Mutex::new(vec![]),
             services_mtx: Mutex::new(vec![]),
             subscriptions_mtx: Mutex::new(vec![]),
+            _clock: Arc::new(Mutex::new(clock)),
+            _time_source: Arc::new(Mutex::new(None)),
             _parameter_map,
-        })
+        };
+        /*
+        let node_mtx = Arc::new(node);
+        node._time_source = Some(Arc::new(Mutex::new(TimeSourceBuilder::new(node_mtx).build())));
+        */
+        //node._time_source = Some(Arc::new(Mutex::new(TimeSourceBuilder::new(node_mtx).build())));
+
+        Ok(node)
     }
 
     /// Creates a rcl_node_options_t struct from this builder.
diff --git a/rclrs/src/qos.rs b/rclrs/src/qos.rs
index b4904262..0de35dc1 100644
--- a/rclrs/src/qos.rs
+++ b/rclrs/src/qos.rs
@@ -296,6 +296,21 @@ pub const QOS_PROFILE_SENSOR_DATA: QoSProfile = QoSProfile {
     avoid_ros_namespace_conventions: false,
 };
 
+/// Equivalent to `ClockQos` from the [`rclcpp` package][1].
+/// Same as sensor data but with a history depth of 1
+///
+/// [1]: https://github.com/ros2/rclcpp/blob/rolling/rclcpp/include/rclcpp/qos.hpp
+pub const QOS_PROFILE_CLOCK: QoSProfile = QoSProfile {
+    history: QoSHistoryPolicy::KeepLast { depth: 1 },
+    reliability: QoSReliabilityPolicy::BestEffort,
+    durability: QoSDurabilityPolicy::Volatile,
+    deadline: QoSDuration::SystemDefault,
+    lifespan: QoSDuration::SystemDefault,
+    liveliness: QoSLivelinessPolicy::SystemDefault,
+    liveliness_lease_duration: QoSDuration::SystemDefault,
+    avoid_ros_namespace_conventions: false,
+};
+
 /// Equivalent to `rmw_qos_profile_parameters` from the [`rmw` package][1].
 ///
 /// [1]: https://github.com/ros2/rmw/blob/master/rmw/include/rmw/qos_profiles.h
diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs
new file mode 100644
index 00000000..9bac0a87
--- /dev/null
+++ b/rclrs/src/time.rs
@@ -0,0 +1,307 @@
+use crate::rcl_bindings::*;
+use crate::{error::ToResult, to_rclrs_result, RclrsError};
+use crate::{Node, ParameterValue, QoSProfile, Subscription, QOS_PROFILE_CLOCK};
+use std::fmt;
+use std::sync::{Arc, Mutex};
+
+use rosgraph_msgs::msg::Clock as ClockMsg;
+
+/// 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 {
+    RosTime = 1,
+    SystemTime = 2,
+    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,
+        }
+    }
+}
+
+#[derive(Debug)]
+pub struct Time {
+    nsec: i64,
+    clock_type: ClockType,
+}
+
+impl From<Time> for rcl_time_point_t {
+    fn from(time: Time) -> Self {
+        Self {
+            nanoseconds: time.nsec,
+            clock_type: time.clock_type.into(),
+        }
+    }
+}
+
+pub struct Clock {
+    _type: ClockType,
+    _rcl_clock: Arc<Mutex<rcl_clock_t>>,
+    // TODO(luca) Implement jump callbacks
+}
+
+impl Clock {
+    // TODO(luca) proper error handling
+    pub fn new(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)),
+        })
+    }
+
+    pub fn rcl_clock(&self) -> Arc<Mutex<rcl_clock_t>> {
+        self._rcl_clock.clone()
+    }
+
+    pub fn clock_type(&self) -> ClockType {
+        self._type
+    }
+
+    pub fn set_ros_time(&mut 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 {
+                rcl_enable_ros_time_override(&mut *clock);
+            }
+        } else {
+            // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
+            // by the mutex
+            unsafe {
+                rcl_disable_ros_time_override(&mut *clock);
+            }
+        }
+    }
+
+    pub fn now(&self) -> Result<Time, RclrsError> {
+        let mut clock = self._rcl_clock.lock().unwrap();
+        let mut time_point: i64 = 0;
+        unsafe {
+            // SAFETY: No preconditions for this function
+            rcl_clock_get_now(&mut *clock, &mut time_point).ok()?;
+        }
+        Ok(Time {
+            nsec: time_point,
+            clock_type: self._type,
+        })
+    }
+
+    /// 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 actuall 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 Drop for Clock {
+    fn drop(&mut self) {
+        // SAFETY: No preconditions for this function
+        let rc = unsafe { rcl_clock_fini(&mut *self._rcl_clock.lock().unwrap()) };
+        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 {}
+
+/// Time source for a node that subscribes to the /clock node, if the use_sim_time parameter is set
+pub struct TimeSource {
+    _node: Arc<Node>,
+    _clocks: Arc<Mutex<Vec<Arc<Mutex<Clock>>>>>,
+    _clock_qos: QoSProfile,
+    // TODO(luca) implement clock threads, for now will run in main thread
+    _use_clock_thread: bool,
+    // TODO(luca) Update this with parameter callbacks for use_sim_time
+    _ros_time_active: Arc<Mutex<bool>>,
+    _clock_subscription: Option<Arc<Subscription<ClockMsg>>>,
+    // TODO(luca) add last time message for newly attached clocks initialization
+}
+
+pub struct TimeSourceBuilder {
+    node: Arc<Node>,
+    clock_qos: QoSProfile,
+    use_clock_thread: bool,
+    clock: Arc<Mutex<Clock>>,
+}
+
+impl TimeSourceBuilder {
+    pub fn new(node: Arc<Node>, clock: Arc<Mutex<Clock>>) -> Self {
+        Self {
+            node,
+            clock_qos: QOS_PROFILE_CLOCK,
+            use_clock_thread: true,
+            clock,
+        }
+    }
+
+    pub fn clock_qos(mut self, clock_qos: QoSProfile) -> Self {
+        self.clock_qos = clock_qos;
+        self
+    }
+
+    pub fn use_clock_thread(mut self, use_clock_thread: bool) -> Self {
+        self.use_clock_thread = use_clock_thread;
+        self
+    }
+
+    pub fn build(self) -> TimeSource {
+        let mut source = TimeSource {
+            _node: self.node.clone(),
+            _clocks: Arc::new(Mutex::new(vec![])),
+            _clock_qos: self.clock_qos,
+            _use_clock_thread: self.use_clock_thread,
+            _ros_time_active: Arc::new(Mutex::new(false)),
+            _clock_subscription: None,
+        };
+        source.attach_clock(self.clock);
+        source.attach_node(self.node);
+        source
+    }
+}
+
+pub struct ClockMismatchError(ClockType);
+
+impl fmt::Display for ClockMismatchError {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(
+            f,
+            "Unexpected clock type {:?}, only RosTime can be used as a time source when use_sim_time is set",
+            self.0
+        )
+    }
+}
+
+impl TimeSource {
+    pub fn attach_clock(&mut self, clock: Arc<Mutex<Clock>>) -> Result<(), ClockMismatchError> {
+        {
+            let clock = clock.lock().unwrap();
+            let clock_type = clock.clock_type();
+            if !matches!(clock_type, ClockType::RosTime) && *self._ros_time_active.lock().unwrap() {
+                return Err(ClockMismatchError(clock_type));
+            }
+        }
+        // TODO(luca) this would allow duplicates to be stored in the vector but it seems other
+        // client libraries do the same, should we check and no-op if the value exists already?
+        self._clocks.lock().unwrap().push(clock);
+        Ok(())
+    }
+
+    // TODO(luca) should we return a result to denote whether the clock was removed?
+    pub fn detach_clock(&mut self, clock: Arc<Mutex<Clock>>) {
+        self._clocks
+            .lock()
+            .unwrap()
+            .retain(|c| !Arc::ptr_eq(c, &clock));
+    }
+
+    pub fn attach_node(&mut self, node: Arc<Node>) -> Result<(), RclrsError> {
+        self._node = node;
+        println!("Checking for use sim time");
+        // TODO*luca) REMOVE THIS
+        self._clock_subscription = Some(self.create_clock_sub()?);
+        self.set_ros_time(true);
+        return Ok(());
+
+        if let Some(sim_param) = node.get_parameter("use_sim_time") {
+            match sim_param {
+                ParameterValue::Bool(val) => {
+                    if val {
+                        println!("use sim time set");
+                        self._clock_subscription = Some(self.create_clock_sub()?);
+                        self.set_ros_time(true);
+                    } else {
+                        // TODO(luca) cleanup subscription, clear set_ros_time
+                    }
+                }
+                // TODO(luca) more graceful error handling
+                _ => panic!("use_sim_time parameter must be boolean"),
+            }
+        }
+        Ok(())
+    }
+
+    fn set_ros_time(&self, enable: bool) {
+        *self._ros_time_active.lock().unwrap() = enable;
+        for clock in self._clocks.lock().unwrap().iter() {
+            clock.lock().unwrap().set_ros_time(enable);
+        }
+    }
+
+    fn update_all_clocks(clocks: &Arc<Mutex<Vec<Arc<Mutex<Clock>>>>>, nanoseconds: i64) {
+        let clocks = clocks.lock().unwrap();
+        for clock in clocks.iter() {
+            let clock = clock.lock().unwrap().rcl_clock();
+            let mut clock = clock.lock().unwrap();
+            // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
+            // by the mutex
+            unsafe {
+                rcl_set_ros_time_override(&mut *clock, nanoseconds);
+            }
+        }
+    }
+
+    fn create_clock_sub(&self) -> Result<Arc<Subscription<ClockMsg>>, RclrsError> {
+        let ros_time_active = self._ros_time_active.clone();
+        let clocks = self._clocks.clone();
+        self._node.create_subscription::<ClockMsg, _>(
+            "/clock",
+            self._clock_qos,
+            move |msg: ClockMsg| {
+                if *ros_time_active.lock().unwrap() {
+                    let nanoseconds: i64 =
+                        (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64;
+                    Self::update_all_clocks(&clocks, nanoseconds);
+                }
+            },
+        )
+    }
+}
+
+/*
+impl Default for Time {
+    fn default() -> Self {
+        Self {
+            nsec: 0,
+            clock_type: ClockType::SystemTime,
+        }
+    }
+}
+
+impl From<crate::vendor::builtin_interfaces::msg::Time> for Time {
+    fn from(time_msg: crate::vendor::builtin_interfaces::msg::Time) -> Self {
+        Self {
+            nsec: (time_msg.sec as i64 * 1_000_000_000) + time_msg.nanosec as i64,
+            clock_type: ClockType::RosTime,
+        }
+    }
+}
+*/

From 5de661a608c340fd73bd11c108ba8792b5cb9ec0 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 31 Jul 2023 17:08:12 +0800
Subject: [PATCH 02/37] Minor cleanup and code reorganization

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs        | 115 ++++++++++++++++
 rclrs/src/lib.rs          |   6 +-
 rclrs/src/node.rs         |   2 +-
 rclrs/src/node/builder.rs |   7 +-
 rclrs/src/time.rs         | 276 +-------------------------------------
 rclrs/src/time_source.rs  | 162 ++++++++++++++++++++++
 6 files changed, 284 insertions(+), 284 deletions(-)
 create mode 100644 rclrs/src/clock.rs
 create mode 100644 rclrs/src/time_source.rs

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
new file mode 100644
index 00000000..01f50c66
--- /dev/null
+++ b/rclrs/src/clock.rs
@@ -0,0 +1,115 @@
+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 {
+    RosTime = 1,
+    SystemTime = 2,
+    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,
+        }
+    }
+}
+
+pub struct Clock {
+    _type: ClockType,
+    _rcl_clock: Arc<Mutex<rcl_clock_t>>,
+    // TODO(luca) Implement jump callbacks
+}
+
+impl Clock {
+    // TODO(luca) proper error handling
+    pub fn new(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)),
+        })
+    }
+
+    pub fn rcl_clock(&self) -> Arc<Mutex<rcl_clock_t>> {
+        self._rcl_clock.clone()
+    }
+
+    pub fn clock_type(&self) -> ClockType {
+        self._type
+    }
+
+    pub fn set_ros_time(&mut 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 {
+                rcl_enable_ros_time_override(&mut *clock);
+            }
+        } else {
+            // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
+            // by the mutex
+            unsafe {
+                rcl_disable_ros_time_override(&mut *clock);
+            }
+        }
+    }
+
+    pub fn now(&self) -> Result<Time, RclrsError> {
+        let mut clock = self._rcl_clock.lock().unwrap();
+        let mut time_point: i64 = 0;
+        unsafe {
+            // SAFETY: No preconditions for this function
+            rcl_clock_get_now(&mut *clock, &mut time_point).ok()?;
+        }
+        Ok(Time {
+            nsec: time_point,
+            clock_type: self._type,
+        })
+    }
+
+    /// 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 actuall 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 Drop for Clock {
+    fn drop(&mut self) {
+        // SAFETY: No preconditions for this function
+        let rc = unsafe { rcl_clock_fini(&mut *self._rcl_clock.lock().unwrap()) };
+        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 {}
diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs
index 345ccc4c..4673d6ea 100644
--- a/rclrs/src/lib.rs
+++ b/rclrs/src/lib.rs
@@ -26,7 +26,7 @@ mod rcl_bindings;
 #[cfg(feature = "dyn_msg")]
 pub mod dynamic_message;
 
-use std::sync::{Arc, Mutex};
+use std::sync::Arc;
 use std::time::Duration;
 
 pub use arguments::*;
@@ -115,12 +115,10 @@ pub fn spin(node: Arc<Node>) -> Result<(), RclrsError> {
 /// # Ok::<(), RclrsError>(())
 /// ```
 pub fn create_node(context: &Context, node_name: &str) -> Result<Arc<Node>, RclrsError> {
-    println!("Creating node");
-    let mut node = Arc::new(Node::builder(context, node_name).build()?);
+    let node = Arc::new(Node::builder(context, node_name).build()?);
     *node._time_source.lock().unwrap() =
         Some(TimeSourceBuilder::new(node.clone(), node.get_clock()).build());
     Ok(node)
-    //Ok(Arc::new(Node::builder(context, node_name).build()?))
 }
 
 /// Creates a [`NodeBuilder`][1].
diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index 81abb79f..fc39e2b5 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -99,7 +99,7 @@ impl Node {
     /// See [`NodeBuilder::new()`] for documentation.
     #[allow(clippy::new_ret_no_self)]
     pub fn new(context: &Context, node_name: &str) -> Result<Node, RclrsError> {
-        Self::builder(context, node_name).build().into()
+        Self::builder(context, node_name).build()
     }
 
     pub fn get_clock(&self) -> Arc<Mutex<Clock>> {
diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index 4d96ad77..288c8be0 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -276,7 +276,7 @@ impl NodeBuilder {
             )?
         };
         let rcl_node_mtx = Arc::new(Mutex::new(rcl_node));
-        let mut node = Node {
+        let node = Node {
             rcl_node_mtx,
             rcl_context_mtx: self.context.clone(),
             clients_mtx: Mutex::new(vec![]),
@@ -287,11 +287,6 @@ impl NodeBuilder {
             _time_source: Arc::new(Mutex::new(None)),
             _parameter_map,
         };
-        /*
-        let node_mtx = Arc::new(node);
-        node._time_source = Some(Arc::new(Mutex::new(TimeSourceBuilder::new(node_mtx).build())));
-        */
-        //node._time_source = Some(Arc::new(Mutex::new(TimeSourceBuilder::new(node_mtx).build())));
 
         Ok(node)
     }
diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs
index 9bac0a87..ceebb812 100644
--- a/rclrs/src/time.rs
+++ b/rclrs/src/time.rs
@@ -1,34 +1,10 @@
+use crate::clock::ClockType;
 use crate::rcl_bindings::*;
-use crate::{error::ToResult, to_rclrs_result, RclrsError};
-use crate::{Node, ParameterValue, QoSProfile, Subscription, QOS_PROFILE_CLOCK};
-use std::fmt;
-use std::sync::{Arc, Mutex};
-
-use rosgraph_msgs::msg::Clock as ClockMsg;
-
-/// 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 {
-    RosTime = 1,
-    SystemTime = 2,
-    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,
-        }
-    }
-}
 
 #[derive(Debug)]
 pub struct Time {
-    nsec: i64,
-    clock_type: ClockType,
+    pub nsec: i64,
+    pub clock_type: ClockType,
 }
 
 impl From<Time> for rcl_time_point_t {
@@ -40,252 +16,6 @@ impl From<Time> for rcl_time_point_t {
     }
 }
 
-pub struct Clock {
-    _type: ClockType,
-    _rcl_clock: Arc<Mutex<rcl_clock_t>>,
-    // TODO(luca) Implement jump callbacks
-}
-
-impl Clock {
-    // TODO(luca) proper error handling
-    pub fn new(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)),
-        })
-    }
-
-    pub fn rcl_clock(&self) -> Arc<Mutex<rcl_clock_t>> {
-        self._rcl_clock.clone()
-    }
-
-    pub fn clock_type(&self) -> ClockType {
-        self._type
-    }
-
-    pub fn set_ros_time(&mut 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 {
-                rcl_enable_ros_time_override(&mut *clock);
-            }
-        } else {
-            // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
-            // by the mutex
-            unsafe {
-                rcl_disable_ros_time_override(&mut *clock);
-            }
-        }
-    }
-
-    pub fn now(&self) -> Result<Time, RclrsError> {
-        let mut clock = self._rcl_clock.lock().unwrap();
-        let mut time_point: i64 = 0;
-        unsafe {
-            // SAFETY: No preconditions for this function
-            rcl_clock_get_now(&mut *clock, &mut time_point).ok()?;
-        }
-        Ok(Time {
-            nsec: time_point,
-            clock_type: self._type,
-        })
-    }
-
-    /// 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 actuall 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 Drop for Clock {
-    fn drop(&mut self) {
-        // SAFETY: No preconditions for this function
-        let rc = unsafe { rcl_clock_fini(&mut *self._rcl_clock.lock().unwrap()) };
-        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 {}
-
-/// Time source for a node that subscribes to the /clock node, if the use_sim_time parameter is set
-pub struct TimeSource {
-    _node: Arc<Node>,
-    _clocks: Arc<Mutex<Vec<Arc<Mutex<Clock>>>>>,
-    _clock_qos: QoSProfile,
-    // TODO(luca) implement clock threads, for now will run in main thread
-    _use_clock_thread: bool,
-    // TODO(luca) Update this with parameter callbacks for use_sim_time
-    _ros_time_active: Arc<Mutex<bool>>,
-    _clock_subscription: Option<Arc<Subscription<ClockMsg>>>,
-    // TODO(luca) add last time message for newly attached clocks initialization
-}
-
-pub struct TimeSourceBuilder {
-    node: Arc<Node>,
-    clock_qos: QoSProfile,
-    use_clock_thread: bool,
-    clock: Arc<Mutex<Clock>>,
-}
-
-impl TimeSourceBuilder {
-    pub fn new(node: Arc<Node>, clock: Arc<Mutex<Clock>>) -> Self {
-        Self {
-            node,
-            clock_qos: QOS_PROFILE_CLOCK,
-            use_clock_thread: true,
-            clock,
-        }
-    }
-
-    pub fn clock_qos(mut self, clock_qos: QoSProfile) -> Self {
-        self.clock_qos = clock_qos;
-        self
-    }
-
-    pub fn use_clock_thread(mut self, use_clock_thread: bool) -> Self {
-        self.use_clock_thread = use_clock_thread;
-        self
-    }
-
-    pub fn build(self) -> TimeSource {
-        let mut source = TimeSource {
-            _node: self.node.clone(),
-            _clocks: Arc::new(Mutex::new(vec![])),
-            _clock_qos: self.clock_qos,
-            _use_clock_thread: self.use_clock_thread,
-            _ros_time_active: Arc::new(Mutex::new(false)),
-            _clock_subscription: None,
-        };
-        source.attach_clock(self.clock);
-        source.attach_node(self.node);
-        source
-    }
-}
-
-pub struct ClockMismatchError(ClockType);
-
-impl fmt::Display for ClockMismatchError {
-    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
-        write!(
-            f,
-            "Unexpected clock type {:?}, only RosTime can be used as a time source when use_sim_time is set",
-            self.0
-        )
-    }
-}
-
-impl TimeSource {
-    pub fn attach_clock(&mut self, clock: Arc<Mutex<Clock>>) -> Result<(), ClockMismatchError> {
-        {
-            let clock = clock.lock().unwrap();
-            let clock_type = clock.clock_type();
-            if !matches!(clock_type, ClockType::RosTime) && *self._ros_time_active.lock().unwrap() {
-                return Err(ClockMismatchError(clock_type));
-            }
-        }
-        // TODO(luca) this would allow duplicates to be stored in the vector but it seems other
-        // client libraries do the same, should we check and no-op if the value exists already?
-        self._clocks.lock().unwrap().push(clock);
-        Ok(())
-    }
-
-    // TODO(luca) should we return a result to denote whether the clock was removed?
-    pub fn detach_clock(&mut self, clock: Arc<Mutex<Clock>>) {
-        self._clocks
-            .lock()
-            .unwrap()
-            .retain(|c| !Arc::ptr_eq(c, &clock));
-    }
-
-    pub fn attach_node(&mut self, node: Arc<Node>) -> Result<(), RclrsError> {
-        self._node = node;
-        println!("Checking for use sim time");
-        // TODO*luca) REMOVE THIS
-        self._clock_subscription = Some(self.create_clock_sub()?);
-        self.set_ros_time(true);
-        return Ok(());
-
-        if let Some(sim_param) = node.get_parameter("use_sim_time") {
-            match sim_param {
-                ParameterValue::Bool(val) => {
-                    if val {
-                        println!("use sim time set");
-                        self._clock_subscription = Some(self.create_clock_sub()?);
-                        self.set_ros_time(true);
-                    } else {
-                        // TODO(luca) cleanup subscription, clear set_ros_time
-                    }
-                }
-                // TODO(luca) more graceful error handling
-                _ => panic!("use_sim_time parameter must be boolean"),
-            }
-        }
-        Ok(())
-    }
-
-    fn set_ros_time(&self, enable: bool) {
-        *self._ros_time_active.lock().unwrap() = enable;
-        for clock in self._clocks.lock().unwrap().iter() {
-            clock.lock().unwrap().set_ros_time(enable);
-        }
-    }
-
-    fn update_all_clocks(clocks: &Arc<Mutex<Vec<Arc<Mutex<Clock>>>>>, nanoseconds: i64) {
-        let clocks = clocks.lock().unwrap();
-        for clock in clocks.iter() {
-            let clock = clock.lock().unwrap().rcl_clock();
-            let mut clock = clock.lock().unwrap();
-            // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
-            // by the mutex
-            unsafe {
-                rcl_set_ros_time_override(&mut *clock, nanoseconds);
-            }
-        }
-    }
-
-    fn create_clock_sub(&self) -> Result<Arc<Subscription<ClockMsg>>, RclrsError> {
-        let ros_time_active = self._ros_time_active.clone();
-        let clocks = self._clocks.clone();
-        self._node.create_subscription::<ClockMsg, _>(
-            "/clock",
-            self._clock_qos,
-            move |msg: ClockMsg| {
-                if *ros_time_active.lock().unwrap() {
-                    let nanoseconds: i64 =
-                        (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64;
-                    Self::update_all_clocks(&clocks, nanoseconds);
-                }
-            },
-        )
-    }
-}
-
 /*
 impl Default for Time {
     fn default() -> Self {
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
new file mode 100644
index 00000000..b8a35218
--- /dev/null
+++ b/rclrs/src/time_source.rs
@@ -0,0 +1,162 @@
+use crate::rcl_bindings::*;
+use crate::{
+    clock::{Clock, ClockType},
+    RclrsError,
+};
+use crate::{Node, ParameterValue, QoSProfile, Subscription, QOS_PROFILE_CLOCK};
+use rosgraph_msgs::msg::Clock as ClockMsg;
+use std::fmt;
+use std::sync::{Arc, Mutex};
+
+/// Time source for a node that subscribes to the /clock node, if the use_sim_time parameter is set
+pub struct TimeSource {
+    _node: Arc<Node>,
+    _clocks: Arc<Mutex<Vec<Arc<Mutex<Clock>>>>>,
+    _clock_qos: QoSProfile,
+    // TODO(luca) implement clock threads, for now will run in main thread
+    _use_clock_thread: bool,
+    // TODO(luca) Update this with parameter callbacks for use_sim_time
+    _ros_time_active: Arc<Mutex<bool>>,
+    _clock_subscription: Option<Arc<Subscription<ClockMsg>>>,
+    // TODO(luca) add last time message for newly attached clocks initialization
+}
+
+pub struct TimeSourceBuilder {
+    node: Arc<Node>,
+    clock_qos: QoSProfile,
+    use_clock_thread: bool,
+    clock: Arc<Mutex<Clock>>,
+}
+
+impl TimeSourceBuilder {
+    pub fn new(node: Arc<Node>, clock: Arc<Mutex<Clock>>) -> Self {
+        Self {
+            node,
+            clock_qos: QOS_PROFILE_CLOCK,
+            use_clock_thread: true,
+            clock,
+        }
+    }
+
+    pub fn clock_qos(mut self, clock_qos: QoSProfile) -> Self {
+        self.clock_qos = clock_qos;
+        self
+    }
+
+    pub fn use_clock_thread(mut self, use_clock_thread: bool) -> Self {
+        self.use_clock_thread = use_clock_thread;
+        self
+    }
+
+    pub fn build(self) -> TimeSource {
+        let mut source = TimeSource {
+            _node: self.node.clone(),
+            _clocks: Arc::new(Mutex::new(vec![])),
+            _clock_qos: self.clock_qos,
+            _use_clock_thread: self.use_clock_thread,
+            _ros_time_active: Arc::new(Mutex::new(false)),
+            _clock_subscription: None,
+        };
+        source.attach_clock(self.clock);
+        source.attach_node(self.node);
+        source
+    }
+}
+
+pub struct ClockMismatchError(ClockType);
+
+impl fmt::Display for ClockMismatchError {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(
+            f,
+            "Unexpected clock type {:?}, only RosTime can be used as a time source when use_sim_time is set",
+            self.0
+        )
+    }
+}
+
+impl TimeSource {
+    pub fn attach_clock(&mut self, clock: Arc<Mutex<Clock>>) -> Result<(), ClockMismatchError> {
+        {
+            let clock = clock.lock().unwrap();
+            let clock_type = clock.clock_type();
+            if !matches!(clock_type, ClockType::RosTime) && *self._ros_time_active.lock().unwrap() {
+                return Err(ClockMismatchError(clock_type));
+            }
+        }
+        // TODO(luca) this would allow duplicates to be stored in the vector but it seems other
+        // client libraries do the same, should we check and no-op if the value exists already?
+        self._clocks.lock().unwrap().push(clock);
+        Ok(())
+    }
+
+    // TODO(luca) should we return a result to denote whether the clock was removed?
+    pub fn detach_clock(&mut self, clock: Arc<Mutex<Clock>>) {
+        self._clocks
+            .lock()
+            .unwrap()
+            .retain(|c| !Arc::ptr_eq(c, &clock));
+    }
+
+    pub fn attach_node(&mut self, node: Arc<Node>) -> Result<(), RclrsError> {
+        self._node = node;
+        println!("Checking for use sim time");
+        // TODO*luca) REMOVE THIS
+        self._clock_subscription = Some(self.create_clock_sub()?);
+        self.set_ros_time(true);
+        return Ok(());
+
+        if let Some(sim_param) = node.get_parameter("use_sim_time") {
+            match sim_param {
+                ParameterValue::Bool(val) => {
+                    if val {
+                        println!("use sim time set");
+                        self._clock_subscription = Some(self.create_clock_sub()?);
+                        self.set_ros_time(true);
+                    } else {
+                        // TODO(luca) cleanup subscription, clear set_ros_time
+                    }
+                }
+                // TODO(luca) more graceful error handling
+                _ => panic!("use_sim_time parameter must be boolean"),
+            }
+        }
+        Ok(())
+    }
+
+    fn set_ros_time(&self, enable: bool) {
+        *self._ros_time_active.lock().unwrap() = enable;
+        for clock in self._clocks.lock().unwrap().iter() {
+            clock.lock().unwrap().set_ros_time(enable);
+        }
+    }
+
+    fn update_all_clocks(clocks: &Arc<Mutex<Vec<Arc<Mutex<Clock>>>>>, nanoseconds: i64) {
+        let clocks = clocks.lock().unwrap();
+        for clock in clocks.iter() {
+            let clock = clock.lock().unwrap().rcl_clock();
+            let mut clock = clock.lock().unwrap();
+            // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
+            // by the mutex
+            unsafe {
+                rcl_set_ros_time_override(&mut *clock, nanoseconds);
+            }
+        }
+    }
+
+    fn create_clock_sub(&self) -> Result<Arc<Subscription<ClockMsg>>, RclrsError> {
+        let ros_time_active = self._ros_time_active.clone();
+        let clocks = self._clocks.clone();
+        self._node.create_subscription::<ClockMsg, _>(
+            "/clock",
+            self._clock_qos,
+            move |msg: ClockMsg| {
+                if *ros_time_active.lock().unwrap() {
+                    let nanoseconds: i64 =
+                        (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64;
+                    Self::update_all_clocks(&clocks, nanoseconds);
+                }
+            },
+        )
+    }
+}

From 7676a412d3e9717bfe03c6b5b0dd125af03d80b3 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 31 Jul 2023 17:42:03 +0800
Subject: [PATCH 03/37] Minor cleanups, add debug code

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 .../minimal_pub_sub/src/minimal_subscriber.rs |  3 ++
 rclrs/src/time.rs                             | 21 +--------
 rclrs/src/time_source.rs                      | 44 ++++++++++++-------
 3 files changed, 31 insertions(+), 37 deletions(-)

diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs
index ebc5fc19..eb91616a 100644
--- a/examples/minimal_pub_sub/src/minimal_subscriber.rs
+++ b/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().lock().unwrap().now();
+            dbg!(now);
         },
     )?;
 
diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs
index ceebb812..33673bba 100644
--- a/rclrs/src/time.rs
+++ b/rclrs/src/time.rs
@@ -1,6 +1,7 @@
 use crate::clock::ClockType;
 use crate::rcl_bindings::*;
 
+// TODO(luca) this is currently unused, maybe remove?
 #[derive(Debug)]
 pub struct Time {
     pub nsec: i64,
@@ -15,23 +16,3 @@ impl From<Time> for rcl_time_point_t {
         }
     }
 }
-
-/*
-impl Default for Time {
-    fn default() -> Self {
-        Self {
-            nsec: 0,
-            clock_type: ClockType::SystemTime,
-        }
-    }
-}
-
-impl From<crate::vendor::builtin_interfaces::msg::Time> for Time {
-    fn from(time_msg: crate::vendor::builtin_interfaces::msg::Time) -> Self {
-        Self {
-            nsec: (time_msg.sec as i64 * 1_000_000_000) + time_msg.nanosec as i64,
-            clock_type: ClockType::RosTime,
-        }
-    }
-}
-*/
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index b8a35218..c5a2b9a5 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -18,7 +18,7 @@ pub struct TimeSource {
     // TODO(luca) Update this with parameter callbacks for use_sim_time
     _ros_time_active: Arc<Mutex<bool>>,
     _clock_subscription: Option<Arc<Subscription<ClockMsg>>>,
-    // TODO(luca) add last time message for newly attached clocks initialization
+    _last_time_msg: Arc<Mutex<Option<ClockMsg>>>,
 }
 
 pub struct TimeSourceBuilder {
@@ -56,6 +56,7 @@ impl TimeSourceBuilder {
             _use_clock_thread: self.use_clock_thread,
             _ros_time_active: Arc::new(Mutex::new(false)),
             _clock_subscription: None,
+            _last_time_msg: Arc::new(Mutex::new(None)),
         };
         source.attach_clock(self.clock);
         source.attach_node(self.node);
@@ -76,13 +77,15 @@ impl fmt::Display for ClockMismatchError {
 }
 
 impl TimeSource {
-    pub fn attach_clock(&mut self, clock: Arc<Mutex<Clock>>) -> Result<(), ClockMismatchError> {
-        {
-            let clock = clock.lock().unwrap();
-            let clock_type = clock.clock_type();
-            if !matches!(clock_type, ClockType::RosTime) && *self._ros_time_active.lock().unwrap() {
-                return Err(ClockMismatchError(clock_type));
-            }
+    pub fn attach_clock(&self, clock: Arc<Mutex<Clock>>) -> Result<(), ClockMismatchError> {
+        let clock_type = clock.clone().lock().unwrap().clock_type();
+        if !matches!(clock_type, ClockType::RosTime) && *self._ros_time_active.lock().unwrap() {
+            return Err(ClockMismatchError(clock_type));
+        }
+        if let Some(last_msg) = self._last_time_msg.lock().unwrap().clone() {
+            let nanoseconds: i64 =
+                (last_msg.clock.sec as i64 * 1_000_000_000) + last_msg.clock.nanosec as i64;
+            Self::update_clock(&clock, nanoseconds);
         }
         // TODO(luca) this would allow duplicates to be stored in the vector but it seems other
         // client libraries do the same, should we check and no-op if the value exists already?
@@ -91,7 +94,7 @@ impl TimeSource {
     }
 
     // TODO(luca) should we return a result to denote whether the clock was removed?
-    pub fn detach_clock(&mut self, clock: Arc<Mutex<Clock>>) {
+    pub fn detach_clock(&self, clock: Arc<Mutex<Clock>>) {
         self._clocks
             .lock()
             .unwrap()
@@ -114,7 +117,8 @@ impl TimeSource {
                         self._clock_subscription = Some(self.create_clock_sub()?);
                         self.set_ros_time(true);
                     } else {
-                        // TODO(luca) cleanup subscription, clear set_ros_time
+                        self._clock_subscription = None;
+                        self.set_ros_time(false);
                     }
                 }
                 // TODO(luca) more graceful error handling
@@ -131,22 +135,27 @@ impl TimeSource {
         }
     }
 
+    fn update_clock(clock: &Arc<Mutex<Clock>>, nanoseconds: i64) {
+        let clock = clock.lock().unwrap().rcl_clock();
+        let mut clock = clock.lock().unwrap();
+        // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
+        // by the mutex
+        unsafe {
+            rcl_set_ros_time_override(&mut *clock, nanoseconds);
+        }
+    }
+
     fn update_all_clocks(clocks: &Arc<Mutex<Vec<Arc<Mutex<Clock>>>>>, nanoseconds: i64) {
         let clocks = clocks.lock().unwrap();
         for clock in clocks.iter() {
-            let clock = clock.lock().unwrap().rcl_clock();
-            let mut clock = clock.lock().unwrap();
-            // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
-            // by the mutex
-            unsafe {
-                rcl_set_ros_time_override(&mut *clock, nanoseconds);
-            }
+            Self::update_clock(clock, nanoseconds);
         }
     }
 
     fn create_clock_sub(&self) -> Result<Arc<Subscription<ClockMsg>>, RclrsError> {
         let ros_time_active = self._ros_time_active.clone();
         let clocks = self._clocks.clone();
+        let last_time_msg = self._last_time_msg.clone();
         self._node.create_subscription::<ClockMsg, _>(
             "/clock",
             self._clock_qos,
@@ -154,6 +163,7 @@ impl TimeSource {
                 if *ros_time_active.lock().unwrap() {
                     let nanoseconds: i64 =
                         (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64;
+                    *last_time_msg.lock().unwrap() = Some(msg);
                     Self::update_all_clocks(&clocks, nanoseconds);
                 }
             },

From 8f2e1b809f364e4e8c492ee387c8bb980aa52e38 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 31 Jul 2023 18:46:02 +0800
Subject: [PATCH 04/37] Minor cleanup

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs       | 23 ++++++++++++++---------
 rclrs/src/time_source.rs |  9 ++-------
 2 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index 01f50c66..b0d18503 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -43,10 +43,6 @@ impl Clock {
         })
     }
 
-    pub fn rcl_clock(&self) -> Arc<Mutex<rcl_clock_t>> {
-        self._rcl_clock.clone()
-    }
-
     pub fn clock_type(&self) -> ClockType {
         self._type
     }
@@ -68,17 +64,26 @@ impl Clock {
         }
     }
 
-    pub fn now(&self) -> Result<Time, RclrsError> {
+    pub fn now(&self) -> Time {
         let mut clock = self._rcl_clock.lock().unwrap();
         let mut time_point: i64 = 0;
         unsafe {
-            // SAFETY: No preconditions for this function
-            rcl_clock_get_now(&mut *clock, &mut time_point).ok()?;
+            // SAFETY: The function will only fail if the clock is not initialized
+            rcl_clock_get_now(&mut *clock, &mut time_point);
         }
-        Ok(Time {
+        Time {
             nsec: time_point,
             clock_type: self._type,
-        })
+        }
+    }
+
+    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 {
+            rcl_set_ros_time_override(&mut *clock, nanoseconds);
+        }
     }
 
     /// Helper function to initialize a default clock, same behavior as `rcl_init_generic_clock`.
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index c5a2b9a5..6d4ddd56 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -136,13 +136,8 @@ impl TimeSource {
     }
 
     fn update_clock(clock: &Arc<Mutex<Clock>>, nanoseconds: i64) {
-        let clock = clock.lock().unwrap().rcl_clock();
-        let mut clock = clock.lock().unwrap();
-        // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
-        // by the mutex
-        unsafe {
-            rcl_set_ros_time_override(&mut *clock, nanoseconds);
-        }
+        let clock = clock.lock().unwrap();
+        clock.set_ros_time_override(nanoseconds);
     }
 
     fn update_all_clocks(clocks: &Arc<Mutex<Vec<Arc<Mutex<Clock>>>>>, nanoseconds: i64) {

From 978fd2eeef990978a08f9f47b8231ffb66f96f19 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 1 Aug 2023 09:55:21 +0800
Subject: [PATCH 05/37] Change safety note to reflect get_now safety

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index b0d18503..94b7dd37 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -68,7 +68,7 @@ impl Clock {
         let mut clock = self._rcl_clock.lock().unwrap();
         let mut time_point: i64 = 0;
         unsafe {
-            // SAFETY: The function will only fail if the clock is not initialized
+            // SAFETY: No preconditions for his function
             rcl_clock_get_now(&mut *clock, &mut time_point);
         }
         Time {

From eda7103123275832ebd5c609387679b736b4fb59 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 1 Aug 2023 10:12:43 +0800
Subject: [PATCH 06/37] Fix comment spelling

Co-authored-by: jhdcs <48914066+jhdcs@users.noreply.github.com>
---
 rclrs/src/clock.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index 94b7dd37..e53ba3ea 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -88,7 +88,7 @@ impl 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 actuall return one, so we need a function to generate one. Doing this
+    /// 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.

From 213978f471cbfe906324220bc97467a3c87d90d3 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 1 Aug 2023 15:03:44 +0800
Subject: [PATCH 07/37] Cleanup and add some docs / tests

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs        | 52 +++++++++++++++++++++++-
 rclrs/src/node.rs         |  3 +-
 rclrs/src/node/builder.rs |  2 +-
 rclrs/src/time.rs         |  4 +-
 rclrs/src/time_source.rs  | 85 +++++++++++++++++++++++++++++++--------
 5 files changed, 125 insertions(+), 21 deletions(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index e53ba3ea..5b1c6079 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -6,8 +6,12 @@ use std::sync::{Arc, Mutex};
 /// 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,
 }
 
@@ -21,6 +25,7 @@ impl From<ClockType> for rcl_clock_type_t {
     }
 }
 
+/// Struct that implements a Clock and wraps `rcl_clock_t`.
 pub struct Clock {
     _type: ClockType,
     _rcl_clock: Arc<Mutex<rcl_clock_t>>,
@@ -28,6 +33,7 @@ pub struct Clock {
 }
 
 impl Clock {
+    /// Creates a new clock of the given `ClockType`.
     // TODO(luca) proper error handling
     pub fn new(type_: ClockType) -> Result<Self, RclrsError> {
         let mut rcl_clock;
@@ -43,11 +49,14 @@ impl Clock {
         })
     }
 
+    /// Returns the clock's `ClockType`.
     pub fn clock_type(&self) -> ClockType {
         self._type
     }
 
-    pub fn set_ros_time(&mut self, enable: bool) {
+    /// 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(&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
@@ -64,6 +73,7 @@ impl Clock {
         }
     }
 
+    /// 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;
@@ -77,6 +87,7 @@ impl Clock {
         }
     }
 
+    /// 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
@@ -118,3 +129,42 @@ impl Drop for Clock {
 // 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::new(ClockType::SystemTime).unwrap();
+        assert!(clock.now().nsec > 0);
+    }
+
+    #[test]
+    fn clock_ros_time_with_override() {
+        let clock = Clock::new(ClockType::RosTime).unwrap();
+        let start = clock.now();
+        // Ros time is not set, should return wall time
+        assert!(start.nsec > 0);
+        clock.set_ros_time(true);
+        // No manual time set, it should default to 0
+        assert!(clock.now().nsec == 0);
+        let set_time = 1234i64;
+        clock.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
+        clock.set_ros_time(false);
+        assert!(clock.now().nsec != set_time);
+        assert!(clock.now().nsec > start.nsec);
+    }
+}
diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index fc39e2b5..9a6ae84c 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -15,7 +15,7 @@ use crate::rcl_bindings::*;
 use crate::{
     Client, ClientBase, Clock, Context, GuardCondition, ParameterOverrideMap, ParameterValue,
     Publisher, QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase,
-    SubscriptionCallback, TimeSource, TimeSourceBuilder, ToResult,
+    SubscriptionCallback, TimeSource, ToResult,
 };
 
 impl Drop for rcl_node_t {
@@ -102,6 +102,7 @@ impl Node {
         Self::builder(context, node_name).build()
     }
 
+    /// Gets the clock associated with this node.
     pub fn get_clock(&self) -> Arc<Mutex<Clock>> {
         self._clock.clone()
     }
diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index 288c8be0..5196e081 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -4,7 +4,7 @@ use std::sync::{Arc, Mutex};
 use crate::rcl_bindings::*;
 use crate::{
     node::call_string_getter_with_handle, resolve_parameter_overrides, Clock, ClockType, Context,
-    Node, RclrsError, TimeSource, TimeSourceBuilder, ToResult,
+    Node, RclrsError, ToResult,
 };
 
 /// A builder for creating a [`Node`][1].
diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs
index 33673bba..d5427470 100644
--- a/rclrs/src/time.rs
+++ b/rclrs/src/time.rs
@@ -1,10 +1,12 @@
 use crate::clock::ClockType;
 use crate::rcl_bindings::*;
 
-// TODO(luca) this is currently unused, maybe remove?
+/// Struct that represents time.
 #[derive(Debug)]
 pub struct Time {
+    /// Timestamp in nanoseconds.
     pub nsec: i64,
+    /// Clock type.
     pub clock_type: ClockType,
 }
 
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index 6d4ddd56..26fa6b0d 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -1,4 +1,3 @@
-use crate::rcl_bindings::*;
 use crate::{
     clock::{Clock, ClockType},
     RclrsError,
@@ -8,7 +7,9 @@ use rosgraph_msgs::msg::Clock as ClockMsg;
 use std::fmt;
 use std::sync::{Arc, Mutex};
 
-/// Time source for a node that subscribes to the /clock node, if the use_sim_time parameter is set
+/// Time source for a node that drives the attached clocks.
+/// If the node's `use_sim_time` parameter is set to `true`, the `TimeSource` will subscribe
+/// to the `/clock` topic and drive the attached clocks
 pub struct TimeSource {
     _node: Arc<Node>,
     _clocks: Arc<Mutex<Vec<Arc<Mutex<Clock>>>>>,
@@ -21,6 +22,19 @@ pub struct TimeSource {
     _last_time_msg: Arc<Mutex<Option<ClockMsg>>>,
 }
 
+/// A builder for creating a [`TimeSource`][1].
+///
+/// The builder pattern allows selectively setting some fields, and leaving all others at their default values.
+/// This struct instance can be created via [`TimeSource::builder()`][2].
+///
+/// The default values for optional fields are:
+/// - `clock_qos: QOS_PROFILE_CLOCK`[3]
+/// - `use_clock_thread: true`
+///
+///
+/// [1]: crate::TimeSource
+/// [2]: crate::TimeSource::builder
+/// [3]: crate::QOS_PROFILE_CLOCK
 pub struct TimeSourceBuilder {
     node: Arc<Node>,
     clock_qos: QoSProfile,
@@ -29,6 +43,9 @@ pub struct TimeSourceBuilder {
 }
 
 impl TimeSourceBuilder {
+    /// Creates a builder for a time source that drives the given clock for the given node.
+    /// The clock's ClockType must be set to `RosTime` to allow updates based on the `/clock`
+    /// topic.
     pub fn new(node: Arc<Node>, clock: Arc<Mutex<Clock>>) -> Self {
         Self {
             node,
@@ -38,16 +55,22 @@ impl TimeSourceBuilder {
         }
     }
 
+    /// Sets the QoS for the `/clock` topic.
     pub fn clock_qos(mut self, clock_qos: QoSProfile) -> Self {
         self.clock_qos = clock_qos;
         self
     }
 
+    /// Sets use_clock_thread.
+    ///
+    /// If set to `true`, the clock callbacks will run in a separate thread.
+    /// NOTE: Currently unimplemented
     pub fn use_clock_thread(mut self, use_clock_thread: bool) -> Self {
         self.use_clock_thread = use_clock_thread;
         self
     }
 
+    /// Builds the `TimeSource` and attaches the provided `Node` and `Clock`.
     pub fn build(self) -> TimeSource {
         let mut source = TimeSource {
             _node: self.node.clone(),
@@ -64,6 +87,7 @@ impl TimeSourceBuilder {
     }
 }
 
+#[derive(Debug)]
 pub struct ClockMismatchError(ClockType);
 
 impl fmt::Display for ClockMismatchError {
@@ -77,6 +101,12 @@ impl fmt::Display for ClockMismatchError {
 }
 
 impl TimeSource {
+    /// Creates a new time source with default parameters.
+    pub fn new(node: Arc<Node>, clock: Arc<Mutex<Clock>>) -> Self {
+        TimeSourceBuilder::new(node, clock).build()
+    }
+
+    /// Attaches the given clock to the `TimeSource`, enabling the `TimeSource` to control it.
     pub fn attach_clock(&self, clock: Arc<Mutex<Clock>>) -> Result<(), ClockMismatchError> {
         let clock_type = clock.clone().lock().unwrap().clock_type();
         if !matches!(clock_type, ClockType::RosTime) && *self._ros_time_active.lock().unwrap() {
@@ -93,6 +123,7 @@ impl TimeSource {
         Ok(())
     }
 
+    /// Detaches the given clock from the `TimeSource`.
     // TODO(luca) should we return a result to denote whether the clock was removed?
     pub fn detach_clock(&self, clock: Arc<Mutex<Clock>>) {
         self._clocks
@@ -101,25 +132,19 @@ impl TimeSource {
             .retain(|c| !Arc::ptr_eq(c, &clock));
     }
 
+    /// Attaches the given node to to the `TimeSource`, using its interface to read the
+    /// `use_sim_time` parameter and create the clock subscription.
     pub fn attach_node(&mut self, node: Arc<Node>) -> Result<(), RclrsError> {
         self._node = node;
-        println!("Checking for use sim time");
         // TODO*luca) REMOVE THIS
-        self._clock_subscription = Some(self.create_clock_sub()?);
-        self.set_ros_time(true);
+        self.set_ros_time(true)?;
         return Ok(());
 
+        // TODO(luca) register a parameter callback
         if let Some(sim_param) = node.get_parameter("use_sim_time") {
             match sim_param {
                 ParameterValue::Bool(val) => {
-                    if val {
-                        println!("use sim time set");
-                        self._clock_subscription = Some(self.create_clock_sub()?);
-                        self.set_ros_time(true);
-                    } else {
-                        self._clock_subscription = None;
-                        self.set_ros_time(false);
-                    }
+                    self.set_ros_time(val)?;
                 }
                 // TODO(luca) more graceful error handling
                 _ => panic!("use_sim_time parameter must be boolean"),
@@ -128,11 +153,20 @@ impl TimeSource {
         Ok(())
     }
 
-    fn set_ros_time(&self, enable: bool) {
-        *self._ros_time_active.lock().unwrap() = enable;
-        for clock in self._clocks.lock().unwrap().iter() {
-            clock.lock().unwrap().set_ros_time(enable);
+    fn set_ros_time(&mut self, enable: bool) -> Result<(), RclrsError> {
+        let mut ros_time_active = self._ros_time_active.lock().unwrap();
+        if enable != *ros_time_active {
+            *ros_time_active = enable;
+            for clock in self._clocks.lock().unwrap().iter() {
+                clock.lock().unwrap().set_ros_time(enable);
+            }
+            if enable {
+                self._clock_subscription = Some(self.create_clock_sub()?);
+            } else {
+                self._clock_subscription = None;
+            }
         }
+        Ok(())
     }
 
     fn update_clock(clock: &Arc<Mutex<Clock>>, nanoseconds: i64) {
@@ -165,3 +199,20 @@ impl TimeSource {
         )
     }
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::create_node;
+    use crate::Context;
+
+    #[test]
+    fn time_source_attach_clock() {
+        let clock = Arc::new(Mutex::new(Clock::new(ClockType::RosTime).unwrap()));
+        let node = create_node(&Context::new([]).unwrap(), "test_node").unwrap();
+        let time_source = TimeSource::new(node, clock);
+        let clock = Arc::new(Mutex::new(Clock::new(ClockType::RosTime).unwrap()));
+        // Attaching additional clocks should be OK
+        time_source.attach_clock(clock).unwrap();
+    }
+}

From 1f5ea5cae786017d8b05b99a1741010a4253e9c8 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 1 Aug 2023 15:10:48 +0800
Subject: [PATCH 08/37] Avoid duplicated clocks in TimeSource

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/time_source.rs | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index 26fa6b0d..4e7d434a 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -117,9 +117,10 @@ impl TimeSource {
                 (last_msg.clock.sec as i64 * 1_000_000_000) + last_msg.clock.nanosec as i64;
             Self::update_clock(&clock, nanoseconds);
         }
-        // TODO(luca) this would allow duplicates to be stored in the vector but it seems other
-        // client libraries do the same, should we check and no-op if the value exists already?
-        self._clocks.lock().unwrap().push(clock);
+        let mut clocks = self._clocks.lock().unwrap();
+        if !clocks.iter().any(|c| Arc::ptr_eq(c, &clock)) {
+            clocks.push(clock);
+        }
         Ok(())
     }
 

From 19a73c3eb0a33e562d55f43024f3120964f91fbd Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 2 Aug 2023 10:59:50 +0800
Subject: [PATCH 09/37] Change _rcl_clock to just Mutex

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index 5b1c6079..bbda7628 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -28,7 +28,7 @@ impl From<ClockType> for rcl_clock_type_t {
 /// Struct that implements a Clock and wraps `rcl_clock_t`.
 pub struct Clock {
     _type: ClockType,
-    _rcl_clock: Arc<Mutex<rcl_clock_t>>,
+    _rcl_clock: Mutex<rcl_clock_t>,
     // TODO(luca) Implement jump callbacks
 }
 
@@ -45,7 +45,7 @@ impl Clock {
         }
         Ok(Self {
             _type: type_,
-            _rcl_clock: Arc::new(Mutex::new(rcl_clock)),
+            _rcl_clock: Mutex::new(rcl_clock),
         })
     }
 

From fde0f35a85d2315b87323cec127d2b1c2c9c41de Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 2 Aug 2023 11:35:00 +0800
Subject: [PATCH 10/37] Remove Mutex from node clock

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 .../minimal_pub_sub/src/minimal_subscriber.rs |  2 +-
 rclrs/src/node.rs                             |  4 ++--
 rclrs/src/node/builder.rs                     |  2 +-
 rclrs/src/time_source.rs                      | 21 +++++++++----------
 4 files changed, 14 insertions(+), 15 deletions(-)

diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs
index eb91616a..b4212d4c 100644
--- a/examples/minimal_pub_sub/src/minimal_subscriber.rs
+++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs
@@ -17,7 +17,7 @@ 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().lock().unwrap().now();
+            let now = inner_node.get_clock().now();
             dbg!(now);
         },
     )?;
diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index 9a6ae84c..77f5276a 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -71,7 +71,7 @@ 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: Arc<Mutex<Clock>>,
+    _clock: Arc<Clock>,
     // TODO(luca) set to private
     pub _time_source: Arc<Mutex<Option<TimeSource>>>,
     _parameter_map: ParameterOverrideMap,
@@ -103,7 +103,7 @@ impl Node {
     }
 
     /// Gets the clock associated with this node.
-    pub fn get_clock(&self) -> Arc<Mutex<Clock>> {
+    pub fn get_clock(&self) -> Arc<Clock> {
         self._clock.clone()
     }
 
diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index 5196e081..d3e978e0 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -283,7 +283,7 @@ impl NodeBuilder {
             guard_conditions_mtx: Mutex::new(vec![]),
             services_mtx: Mutex::new(vec![]),
             subscriptions_mtx: Mutex::new(vec![]),
-            _clock: Arc::new(Mutex::new(clock)),
+            _clock: Arc::new(clock),
             _time_source: Arc::new(Mutex::new(None)),
             _parameter_map,
         };
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index 4e7d434a..36c2548c 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -12,7 +12,7 @@ use std::sync::{Arc, Mutex};
 /// to the `/clock` topic and drive the attached clocks
 pub struct TimeSource {
     _node: Arc<Node>,
-    _clocks: Arc<Mutex<Vec<Arc<Mutex<Clock>>>>>,
+    _clocks: Arc<Mutex<Vec<Arc<Clock>>>>,
     _clock_qos: QoSProfile,
     // TODO(luca) implement clock threads, for now will run in main thread
     _use_clock_thread: bool,
@@ -39,14 +39,14 @@ pub struct TimeSourceBuilder {
     node: Arc<Node>,
     clock_qos: QoSProfile,
     use_clock_thread: bool,
-    clock: Arc<Mutex<Clock>>,
+    clock: Arc<Clock>,
 }
 
 impl TimeSourceBuilder {
     /// Creates a builder for a time source that drives the given clock for the given node.
     /// The clock's ClockType must be set to `RosTime` to allow updates based on the `/clock`
     /// topic.
-    pub fn new(node: Arc<Node>, clock: Arc<Mutex<Clock>>) -> Self {
+    pub fn new(node: Arc<Node>, clock: Arc<Clock>) -> Self {
         Self {
             node,
             clock_qos: QOS_PROFILE_CLOCK,
@@ -102,13 +102,13 @@ impl fmt::Display for ClockMismatchError {
 
 impl TimeSource {
     /// Creates a new time source with default parameters.
-    pub fn new(node: Arc<Node>, clock: Arc<Mutex<Clock>>) -> Self {
+    pub fn new(node: Arc<Node>, clock: Arc<Clock>) -> Self {
         TimeSourceBuilder::new(node, clock).build()
     }
 
     /// Attaches the given clock to the `TimeSource`, enabling the `TimeSource` to control it.
-    pub fn attach_clock(&self, clock: Arc<Mutex<Clock>>) -> Result<(), ClockMismatchError> {
-        let clock_type = clock.clone().lock().unwrap().clock_type();
+    pub fn attach_clock(&self, clock: Arc<Clock>) -> Result<(), ClockMismatchError> {
+        let clock_type = clock.clock_type();
         if !matches!(clock_type, ClockType::RosTime) && *self._ros_time_active.lock().unwrap() {
             return Err(ClockMismatchError(clock_type));
         }
@@ -126,7 +126,7 @@ impl TimeSource {
 
     /// Detaches the given clock from the `TimeSource`.
     // TODO(luca) should we return a result to denote whether the clock was removed?
-    pub fn detach_clock(&self, clock: Arc<Mutex<Clock>>) {
+    pub fn detach_clock(&self, clock: Arc<Clock>) {
         self._clocks
             .lock()
             .unwrap()
@@ -159,7 +159,7 @@ impl TimeSource {
         if enable != *ros_time_active {
             *ros_time_active = enable;
             for clock in self._clocks.lock().unwrap().iter() {
-                clock.lock().unwrap().set_ros_time(enable);
+                clock.set_ros_time(enable);
             }
             if enable {
                 self._clock_subscription = Some(self.create_clock_sub()?);
@@ -170,12 +170,11 @@ impl TimeSource {
         Ok(())
     }
 
-    fn update_clock(clock: &Arc<Mutex<Clock>>, nanoseconds: i64) {
-        let clock = clock.lock().unwrap();
+    fn update_clock(clock: &Arc<Clock>, nanoseconds: i64) {
         clock.set_ros_time_override(nanoseconds);
     }
 
-    fn update_all_clocks(clocks: &Arc<Mutex<Vec<Arc<Mutex<Clock>>>>>, nanoseconds: i64) {
+    fn update_all_clocks(clocks: &Arc<Mutex<Vec<Arc<Clock>>>>, nanoseconds: i64) {
         let clocks = clocks.lock().unwrap();
         for clock in clocks.iter() {
             Self::update_clock(clock, nanoseconds);

From b808cc4bdd06ef6a76308a3aa07e996bbfd71847 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 2 Aug 2023 11:55:15 +0800
Subject: [PATCH 11/37] Change Mutex<bool> to AtomicBool

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/time_source.rs | 25 ++++++++++++++++++-------
 1 file changed, 18 insertions(+), 7 deletions(-)

diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index 36c2548c..b9e667b7 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -5,6 +5,7 @@ use crate::{
 use crate::{Node, ParameterValue, QoSProfile, Subscription, QOS_PROFILE_CLOCK};
 use rosgraph_msgs::msg::Clock as ClockMsg;
 use std::fmt;
+use std::sync::atomic::{AtomicBool, Ordering};
 use std::sync::{Arc, Mutex};
 
 /// Time source for a node that drives the attached clocks.
@@ -17,7 +18,7 @@ pub struct TimeSource {
     // TODO(luca) implement clock threads, for now will run in main thread
     _use_clock_thread: bool,
     // TODO(luca) Update this with parameter callbacks for use_sim_time
-    _ros_time_active: Arc<Mutex<bool>>,
+    _ros_time_active: Arc<AtomicBool>,
     _clock_subscription: Option<Arc<Subscription<ClockMsg>>>,
     _last_time_msg: Arc<Mutex<Option<ClockMsg>>>,
 }
@@ -77,7 +78,7 @@ impl TimeSourceBuilder {
             _clocks: Arc::new(Mutex::new(vec![])),
             _clock_qos: self.clock_qos,
             _use_clock_thread: self.use_clock_thread,
-            _ros_time_active: Arc::new(Mutex::new(false)),
+            _ros_time_active: Arc::new(AtomicBool::new(false)),
             _clock_subscription: None,
             _last_time_msg: Arc::new(Mutex::new(None)),
         };
@@ -109,7 +110,9 @@ impl TimeSource {
     /// Attaches the given clock to the `TimeSource`, enabling the `TimeSource` to control it.
     pub fn attach_clock(&self, clock: Arc<Clock>) -> Result<(), ClockMismatchError> {
         let clock_type = clock.clock_type();
-        if !matches!(clock_type, ClockType::RosTime) && *self._ros_time_active.lock().unwrap() {
+        if !matches!(clock_type, ClockType::RosTime)
+            && self._ros_time_active.load(Ordering::Relaxed)
+        {
             return Err(ClockMismatchError(clock_type));
         }
         if let Some(last_msg) = self._last_time_msg.lock().unwrap().clone() {
@@ -155,9 +158,17 @@ impl TimeSource {
     }
 
     fn set_ros_time(&mut self, enable: bool) -> Result<(), RclrsError> {
-        let mut ros_time_active = self._ros_time_active.lock().unwrap();
-        if enable != *ros_time_active {
-            *ros_time_active = enable;
+        let updated = self
+            ._ros_time_active
+            .fetch_update(Ordering::Relaxed, Ordering::Relaxed, |prev| {
+                if prev != enable {
+                    Some(enable)
+                } else {
+                    None
+                }
+            })
+            .is_ok();
+        if updated {
             for clock in self._clocks.lock().unwrap().iter() {
                 clock.set_ros_time(enable);
             }
@@ -189,7 +200,7 @@ impl TimeSource {
             "/clock",
             self._clock_qos,
             move |msg: ClockMsg| {
-                if *ros_time_active.lock().unwrap() {
+                if ros_time_active.load(Ordering::Relaxed) {
                     let nanoseconds: i64 =
                         (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64;
                     *last_time_msg.lock().unwrap() = Some(msg);

From f11b4b005ea0eba26e564206f3d309f3737913c5 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 2 Aug 2023 12:05:35 +0800
Subject: [PATCH 12/37] Avoid cloning message

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/time_source.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index b9e667b7..ad294265 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -115,7 +115,7 @@ impl TimeSource {
         {
             return Err(ClockMismatchError(clock_type));
         }
-        if let Some(last_msg) = self._last_time_msg.lock().unwrap().clone() {
+        if let Some(last_msg) = self._last_time_msg.lock().unwrap().as_ref() {
             let nanoseconds: i64 =
                 (last_msg.clock.sec as i64 * 1_000_000_000) + last_msg.clock.nanosec as i64;
             Self::update_clock(&clock, nanoseconds);

From 90b09382d358c0cb9095226748c5c42b88280762 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 2 Aug 2023 14:34:17 +0800
Subject: [PATCH 13/37] Minor cleanup, add dependency

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/package.xml        | 1 +
 rclrs/src/time_source.rs | 8 ++++----
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/rclrs/package.xml b/rclrs/package.xml
index 70b7d349..efe2541c 100644
--- a/rclrs/package.xml
+++ b/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>
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index ad294265..28eb69c6 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -172,11 +172,11 @@ impl TimeSource {
             for clock in self._clocks.lock().unwrap().iter() {
                 clock.set_ros_time(enable);
             }
-            if enable {
-                self._clock_subscription = Some(self.create_clock_sub()?);
+            self._clock_subscription = if enable {
+                Some(self.create_clock_sub()?)
             } else {
-                self._clock_subscription = None;
-            }
+                None
+            };
         }
         Ok(())
     }

From debaac9c43749c0375d0a1bae8630302b0402f65 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 2 Aug 2023 15:18:17 +0800
Subject: [PATCH 14/37] Remove hardcoded use_sim_time

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/time_source.rs | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index 28eb69c6..c1cdbadf 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -140,17 +140,14 @@ impl TimeSource {
     /// `use_sim_time` parameter and create the clock subscription.
     pub fn attach_node(&mut self, node: Arc<Node>) -> Result<(), RclrsError> {
         self._node = node;
-        // TODO*luca) REMOVE THIS
-        self.set_ros_time(true)?;
-        return Ok(());
 
         // TODO(luca) register a parameter callback
-        if let Some(sim_param) = node.get_parameter("use_sim_time") {
+        if let Some(sim_param) = self._node.get_parameter("use_sim_time") {
             match sim_param {
                 ParameterValue::Bool(val) => {
                     self.set_ros_time(val)?;
                 }
-                // TODO(luca) more graceful error handling
+                // TODO(luca) more graceful error handling?
                 _ => panic!("use_sim_time parameter must be boolean"),
             }
         }
@@ -225,5 +222,7 @@ mod tests {
         let clock = Arc::new(Mutex::new(Clock::new(ClockType::RosTime).unwrap()));
         // Attaching additional clocks should be OK
         time_source.attach_clock(clock).unwrap();
+        // Default clock should be above 0 (use_sim_time is default false)
+        assert!(node.get_clock().now() > 0);
     }
 }

From cc59f0a290196a09c3d348c801baa2becc462359 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 2 Aug 2023 16:51:04 +0800
Subject: [PATCH 15/37] Cleanup remaining warnings / clippy

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs       |  2 +-
 rclrs/src/lib.rs         |  7 +++++--
 rclrs/src/node.rs        |  6 +++---
 rclrs/src/time_source.rs | 43 +++++++++++++++++-----------------------
 4 files changed, 27 insertions(+), 31 deletions(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index bbda7628..210e6130 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -1,6 +1,6 @@
 use crate::rcl_bindings::*;
 use crate::{error::ToResult, time::Time, to_rclrs_result, RclrsError};
-use std::sync::{Arc, Mutex};
+use std::sync::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.
diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs
index 4673d6ea..c565fcd3 100644
--- a/rclrs/src/lib.rs
+++ b/rclrs/src/lib.rs
@@ -116,8 +116,11 @@ pub fn spin(node: Arc<Node>) -> Result<(), RclrsError> {
 /// ```
 pub fn create_node(context: &Context, node_name: &str) -> Result<Arc<Node>, RclrsError> {
     let node = Arc::new(Node::builder(context, node_name).build()?);
-    *node._time_source.lock().unwrap() =
-        Some(TimeSourceBuilder::new(node.clone(), node.get_clock()).build());
+    *node._time_source.lock().unwrap() = Some(
+        TimeSourceBuilder::new(node.clone(), node.get_clock())
+            .build()
+            .unwrap(),
+    );
     Ok(node)
 }
 
diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index 77f5276a..acf274cb 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -73,7 +73,7 @@ pub struct Node {
     pub(crate) subscriptions_mtx: Mutex<Vec<Weak<dyn SubscriptionBase>>>,
     _clock: Arc<Clock>,
     // TODO(luca) set to private
-    pub _time_source: Arc<Mutex<Option<TimeSource>>>,
+    pub(crate) _time_source: Arc<Mutex<Option<TimeSource>>>,
     _parameter_map: ParameterOverrideMap,
 }
 
@@ -363,8 +363,8 @@ impl Node {
         domain_id
     }
 
-    // TODO(luca) There should really be parameter callbacks, this is only for testing
-    // temporarily
+    /// 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()
     }
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index c1cdbadf..a1cb48b8 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -1,7 +1,4 @@
-use crate::{
-    clock::{Clock, ClockType},
-    RclrsError,
-};
+use crate::clock::{Clock, ClockType};
 use crate::{Node, ParameterValue, QoSProfile, Subscription, QOS_PROFILE_CLOCK};
 use rosgraph_msgs::msg::Clock as ClockMsg;
 use std::fmt;
@@ -72,7 +69,7 @@ impl TimeSourceBuilder {
     }
 
     /// Builds the `TimeSource` and attaches the provided `Node` and `Clock`.
-    pub fn build(self) -> TimeSource {
+    pub fn build(self) -> Result<TimeSource, ClockMismatchError> {
         let mut source = TimeSource {
             _node: self.node.clone(),
             _clocks: Arc::new(Mutex::new(vec![])),
@@ -82,12 +79,15 @@ impl TimeSourceBuilder {
             _clock_subscription: None,
             _last_time_msg: Arc::new(Mutex::new(None)),
         };
-        source.attach_clock(self.clock);
+        source.attach_clock(self.clock)?;
         source.attach_node(self.node);
-        source
+        Ok(source)
     }
 }
 
+/// Error created when a non RosTime clock is attached but the `use_sim_time` parameter is set.
+/// This case could behave in unexpected ways since the /clock topic would be ignored even though
+/// the user explicitly requested to use it through a command line argument.
 #[derive(Debug)]
 pub struct ClockMismatchError(ClockType);
 
@@ -104,7 +104,7 @@ impl fmt::Display for ClockMismatchError {
 impl TimeSource {
     /// Creates a new time source with default parameters.
     pub fn new(node: Arc<Node>, clock: Arc<Clock>) -> Self {
-        TimeSourceBuilder::new(node, clock).build()
+        TimeSourceBuilder::new(node, clock).build().unwrap()
     }
 
     /// Attaches the given clock to the `TimeSource`, enabling the `TimeSource` to control it.
@@ -138,23 +138,22 @@ impl TimeSource {
 
     /// Attaches the given node to to the `TimeSource`, using its interface to read the
     /// `use_sim_time` parameter and create the clock subscription.
-    pub fn attach_node(&mut self, node: Arc<Node>) -> Result<(), RclrsError> {
+    pub fn attach_node(&mut self, node: Arc<Node>) {
         self._node = node;
 
         // TODO(luca) register a parameter callback
         if let Some(sim_param) = self._node.get_parameter("use_sim_time") {
             match sim_param {
                 ParameterValue::Bool(val) => {
-                    self.set_ros_time(val)?;
+                    self.set_ros_time(val);
                 }
                 // TODO(luca) more graceful error handling?
                 _ => panic!("use_sim_time parameter must be boolean"),
             }
         }
-        Ok(())
     }
 
-    fn set_ros_time(&mut self, enable: bool) -> Result<(), RclrsError> {
+    fn set_ros_time(&mut self, enable: bool) {
         let updated = self
             ._ros_time_active
             .fetch_update(Ordering::Relaxed, Ordering::Relaxed, |prev| {
@@ -169,13 +168,8 @@ impl TimeSource {
             for clock in self._clocks.lock().unwrap().iter() {
                 clock.set_ros_time(enable);
             }
-            self._clock_subscription = if enable {
-                Some(self.create_clock_sub()?)
-            } else {
-                None
-            };
+            self._clock_subscription = enable.then(|| self.create_clock_sub());
         }
-        Ok(())
     }
 
     fn update_clock(clock: &Arc<Clock>, nanoseconds: i64) {
@@ -189,22 +183,21 @@ impl TimeSource {
         }
     }
 
-    fn create_clock_sub(&self) -> Result<Arc<Subscription<ClockMsg>>, RclrsError> {
+    fn create_clock_sub(&self) -> Arc<Subscription<ClockMsg>> {
         let ros_time_active = self._ros_time_active.clone();
         let clocks = self._clocks.clone();
         let last_time_msg = self._last_time_msg.clone();
-        self._node.create_subscription::<ClockMsg, _>(
-            "/clock",
-            self._clock_qos,
-            move |msg: ClockMsg| {
+        // Safe to unwrap since the function will only fail if invalid arguments are provided
+        self._node
+            .create_subscription::<ClockMsg, _>("/clock", self._clock_qos, move |msg: ClockMsg| {
                 if ros_time_active.load(Ordering::Relaxed) {
                     let nanoseconds: i64 =
                         (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64;
                     *last_time_msg.lock().unwrap() = Some(msg);
                     Self::update_all_clocks(&clocks, nanoseconds);
                 }
-            },
-        )
+            })
+            .unwrap()
     }
 }
 

From d7bb9f2d1eb3cd95fa2b0dfb8f14ab9fab0b0aee Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 2 Aug 2023 17:09:21 +0800
Subject: [PATCH 16/37] Fix tests

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/time_source.rs | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index a1cb48b8..5a247ddc 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -209,13 +209,13 @@ mod tests {
 
     #[test]
     fn time_source_attach_clock() {
-        let clock = Arc::new(Mutex::new(Clock::new(ClockType::RosTime).unwrap()));
+        let clock = Arc::new(Clock::new(ClockType::RosTime).unwrap());
         let node = create_node(&Context::new([]).unwrap(), "test_node").unwrap();
-        let time_source = TimeSource::new(node, clock);
-        let clock = Arc::new(Mutex::new(Clock::new(ClockType::RosTime).unwrap()));
+        let time_source = TimeSource::new(node.clone(), clock);
+        let clock = Arc::new(Clock::new(ClockType::RosTime).unwrap());
         // Attaching additional clocks should be OK
         time_source.attach_clock(clock).unwrap();
         // Default clock should be above 0 (use_sim_time is default false)
-        assert!(node.get_clock().now() > 0);
+        assert!(node.get_clock().now().nsec > 0);
     }
 }

From 5df379fd4c4c162664e2cd8a6ff09e0056f1f1b4 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 3 Aug 2023 14:54:14 +0800
Subject: [PATCH 17/37] Refactor API to use ClockSource

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs        | 129 +++++++++++++++++++++++++-------------
 rclrs/src/lib.rs          |   8 +--
 rclrs/src/node.rs         |   8 +--
 rclrs/src/node/builder.rs |  20 ++++--
 rclrs/src/time_source.rs  |  54 +++++++---------
 5 files changed, 128 insertions(+), 91 deletions(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index 210e6130..cd4fb4c9 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -1,6 +1,6 @@
 use crate::rcl_bindings::*;
 use crate::{error::ToResult, time::Time, to_rclrs_result, RclrsError};
-use std::sync::Mutex;
+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.
@@ -28,14 +28,53 @@ impl From<ClockType> for rcl_clock_type_t {
 /// Struct that implements a Clock and wraps `rcl_clock_t`.
 pub struct Clock {
     _type: ClockType,
-    _rcl_clock: Mutex<rcl_clock_t>,
+    _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`.
     // TODO(luca) proper error handling
-    pub fn new(type_: ClockType) -> Result<Self, RclrsError> {
+    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))
+    }
+
+    // TODO(luca) proper error handling
+    fn make(type_: ClockType) -> Result<Self, RclrsError> {
         let mut rcl_clock;
         unsafe {
             // SAFETY: Getting a default value is always safe.
@@ -45,34 +84,10 @@ impl Clock {
         }
         Ok(Self {
             _type: type_,
-            _rcl_clock: Mutex::new(rcl_clock),
+            _rcl_clock: Arc::new(Mutex::new(rcl_clock)),
         })
     }
 
-    /// Returns the clock's `ClockType`.
-    pub fn clock_type(&self) -> ClockType {
-        self._type
-    }
-
-    /// 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(&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 {
-                rcl_enable_ros_time_override(&mut *clock);
-            }
-        } else {
-            // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
-            // by the mutex
-            unsafe {
-                rcl_disable_ros_time_override(&mut *clock);
-            }
-        }
-    }
-
     /// Returns the current clock's timestamp.
     pub fn now(&self) -> Time {
         let mut clock = self._rcl_clock.lock().unwrap();
@@ -87,16 +102,6 @@ impl Clock {
         }
     }
 
-    /// 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 {
-            rcl_set_ros_time_override(&mut *clock, nanoseconds);
-        }
-    }
-
     /// 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
@@ -116,6 +121,43 @@ impl Clock {
     }
 }
 
+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 {
+                rcl_enable_ros_time_override(&mut *clock);
+            }
+        } else {
+            // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
+            // by the mutex
+            unsafe {
+                rcl_disable_ros_time_override(&mut *clock);
+            }
+        }
+    }
+
+    /// 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 {
+            rcl_set_ros_time_override(&mut *clock, nanoseconds);
+        }
+    }
+}
+
 impl Drop for Clock {
     fn drop(&mut self) {
         // SAFETY: No preconditions for this function
@@ -129,6 +171,7 @@ impl Drop for Clock {
 // 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 {}
+unsafe impl Sync for rcl_clock_t {}
 
 #[cfg(test)]
 mod tests {
@@ -145,25 +188,25 @@ mod tests {
 
     #[test]
     fn clock_system_time_now() {
-        let clock = Clock::new(ClockType::SystemTime).unwrap();
+        let clock = Clock::system().unwrap();
         assert!(clock.now().nsec > 0);
     }
 
     #[test]
     fn clock_ros_time_with_override() {
-        let clock = Clock::new(ClockType::RosTime).unwrap();
+        let (clock, source) = Clock::with_source().unwrap();
         let start = clock.now();
         // Ros time is not set, should return wall time
         assert!(start.nsec > 0);
-        clock.set_ros_time(true);
+        source.set_ros_time_enable(true);
         // No manual time set, it should default to 0
         assert!(clock.now().nsec == 0);
         let set_time = 1234i64;
-        clock.set_ros_time_override(set_time);
+        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
-        clock.set_ros_time(false);
+        source.set_ros_time_enable(false);
         assert!(clock.now().nsec != set_time);
         assert!(clock.now().nsec > start.nsec);
     }
diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs
index c565fcd3..de3988a9 100644
--- a/rclrs/src/lib.rs
+++ b/rclrs/src/lib.rs
@@ -115,13 +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> {
-    let node = Arc::new(Node::builder(context, node_name).build()?);
-    *node._time_source.lock().unwrap() = Some(
-        TimeSourceBuilder::new(node.clone(), node.get_clock())
-            .build()
-            .unwrap(),
-    );
-    Ok(node)
+    Node::builder(context, node_name).build()
 }
 
 /// Creates a [`NodeBuilder`][1].
diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index acf274cb..0c137d08 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -71,7 +71,7 @@ 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: Arc<Clock>,
+    _clock: Clock,
     // TODO(luca) set to private
     pub(crate) _time_source: Arc<Mutex<Option<TimeSource>>>,
     _parameter_map: ParameterOverrideMap,
@@ -98,13 +98,13 @@ 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) -> Arc<Clock> {
-        self._clock.clone()
+    pub fn get_clock(&self) -> &Clock {
+        &self._clock
     }
 
     /// Returns the name of the node.
diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index d3e978e0..4036b089 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -4,7 +4,7 @@ use std::sync::{Arc, Mutex};
 use crate::rcl_bindings::*;
 use crate::{
     node::call_string_getter_with_handle, resolve_parameter_overrides, Clock, ClockType, Context,
-    Node, RclrsError, ToResult,
+    Node, RclrsError, TimeSourceBuilder, ToResult,
 };
 
 /// A builder for creating a [`Node`][1].
@@ -234,7 +234,7 @@ impl NodeBuilder {
     /// For example usage, see the [`NodeBuilder`][1] docs.
     ///
     /// [1]: crate::NodeBuilder
-    pub fn build(&self) -> Result<Node, RclrsError> {
+    pub fn build(&self) -> Result<Arc<Node>, RclrsError> {
         let node_name =
             CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul {
                 err,
@@ -265,7 +265,7 @@ impl NodeBuilder {
             .ok()?;
         };
 
-        let clock = Clock::new(self.clock_type)?;
+        let (clock, clock_source) = Clock::new(self.clock_type)?;
 
         let _parameter_map = unsafe {
             let fqn = call_string_getter_with_handle(&rcl_node, rcl_node_get_fully_qualified_name);
@@ -276,17 +276,25 @@ impl NodeBuilder {
             )?
         };
         let rcl_node_mtx = Arc::new(Mutex::new(rcl_node));
-        let node = Node {
+        let node = Arc::new(Node {
             rcl_node_mtx,
             rcl_context_mtx: self.context.clone(),
             clients_mtx: Mutex::new(vec![]),
             guard_conditions_mtx: Mutex::new(vec![]),
             services_mtx: Mutex::new(vec![]),
             subscriptions_mtx: Mutex::new(vec![]),
-            _clock: Arc::new(clock),
+            _clock: clock,
             _time_source: Arc::new(Mutex::new(None)),
             _parameter_map,
-        };
+        });
+
+        if let Some(clock_source) = clock_source {
+            *node._time_source.lock().unwrap() = Some(
+                TimeSourceBuilder::new(node.clone(), clock_source)
+                    .build()
+                    .unwrap(),
+            );
+        }
 
         Ok(node)
     }
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index 5a247ddc..1a21b361 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -1,4 +1,4 @@
-use crate::clock::{Clock, ClockType};
+use crate::clock::{ClockSource, ClockType};
 use crate::{Node, ParameterValue, QoSProfile, Subscription, QOS_PROFILE_CLOCK};
 use rosgraph_msgs::msg::Clock as ClockMsg;
 use std::fmt;
@@ -10,7 +10,7 @@ use std::sync::{Arc, Mutex};
 /// to the `/clock` topic and drive the attached clocks
 pub struct TimeSource {
     _node: Arc<Node>,
-    _clocks: Arc<Mutex<Vec<Arc<Clock>>>>,
+    _clocks: Arc<Mutex<Vec<ClockSource>>>,
     _clock_qos: QoSProfile,
     // TODO(luca) implement clock threads, for now will run in main thread
     _use_clock_thread: bool,
@@ -37,19 +37,19 @@ pub struct TimeSourceBuilder {
     node: Arc<Node>,
     clock_qos: QoSProfile,
     use_clock_thread: bool,
-    clock: Arc<Clock>,
+    clock_source: ClockSource,
 }
 
 impl TimeSourceBuilder {
     /// Creates a builder for a time source that drives the given clock for the given node.
     /// The clock's ClockType must be set to `RosTime` to allow updates based on the `/clock`
     /// topic.
-    pub fn new(node: Arc<Node>, clock: Arc<Clock>) -> Self {
+    pub fn new(node: Arc<Node>, clock_source: ClockSource) -> Self {
         Self {
             node,
             clock_qos: QOS_PROFILE_CLOCK,
             use_clock_thread: true,
-            clock,
+            clock_source,
         }
     }
 
@@ -79,7 +79,7 @@ impl TimeSourceBuilder {
             _clock_subscription: None,
             _last_time_msg: Arc::new(Mutex::new(None)),
         };
-        source.attach_clock(self.clock)?;
+        source.attach_clock(self.clock_source);
         source.attach_node(self.node);
         Ok(source)
     }
@@ -103,37 +103,27 @@ impl fmt::Display for ClockMismatchError {
 
 impl TimeSource {
     /// Creates a new time source with default parameters.
-    pub fn new(node: Arc<Node>, clock: Arc<Clock>) -> Self {
+    pub fn new(node: Arc<Node>, clock: ClockSource) -> Self {
         TimeSourceBuilder::new(node, clock).build().unwrap()
     }
 
     /// Attaches the given clock to the `TimeSource`, enabling the `TimeSource` to control it.
-    pub fn attach_clock(&self, clock: Arc<Clock>) -> Result<(), ClockMismatchError> {
-        let clock_type = clock.clock_type();
-        if !matches!(clock_type, ClockType::RosTime)
-            && self._ros_time_active.load(Ordering::Relaxed)
-        {
-            return Err(ClockMismatchError(clock_type));
-        }
+    pub fn attach_clock(&self, clock: ClockSource) {
         if let Some(last_msg) = self._last_time_msg.lock().unwrap().as_ref() {
             let nanoseconds: i64 =
                 (last_msg.clock.sec as i64 * 1_000_000_000) + last_msg.clock.nanosec as i64;
             Self::update_clock(&clock, nanoseconds);
         }
         let mut clocks = self._clocks.lock().unwrap();
-        if !clocks.iter().any(|c| Arc::ptr_eq(c, &clock)) {
+        if !clocks.iter().any(|c| c != &clock) {
             clocks.push(clock);
         }
-        Ok(())
     }
 
     /// Detaches the given clock from the `TimeSource`.
     // TODO(luca) should we return a result to denote whether the clock was removed?
-    pub fn detach_clock(&self, clock: Arc<Clock>) {
-        self._clocks
-            .lock()
-            .unwrap()
-            .retain(|c| !Arc::ptr_eq(c, &clock));
+    pub fn detach_clock(&self, clock: &ClockSource) {
+        self._clocks.lock().unwrap().retain(|c| c != clock);
     }
 
     /// Attaches the given node to to the `TimeSource`, using its interface to read the
@@ -145,7 +135,8 @@ impl TimeSource {
         if let Some(sim_param) = self._node.get_parameter("use_sim_time") {
             match sim_param {
                 ParameterValue::Bool(val) => {
-                    self.set_ros_time(val);
+                    println!("Setting ros time");
+                    self.set_ros_time_enable(val);
                 }
                 // TODO(luca) more graceful error handling?
                 _ => panic!("use_sim_time parameter must be boolean"),
@@ -153,7 +144,7 @@ impl TimeSource {
         }
     }
 
-    fn set_ros_time(&mut self, enable: bool) {
+    fn set_ros_time_enable(&mut self, enable: bool) {
         let updated = self
             ._ros_time_active
             .fetch_update(Ordering::Relaxed, Ordering::Relaxed, |prev| {
@@ -166,17 +157,17 @@ impl TimeSource {
             .is_ok();
         if updated {
             for clock in self._clocks.lock().unwrap().iter() {
-                clock.set_ros_time(enable);
+                clock.set_ros_time_enable(enable);
             }
             self._clock_subscription = enable.then(|| self.create_clock_sub());
         }
     }
 
-    fn update_clock(clock: &Arc<Clock>, nanoseconds: i64) {
+    fn update_clock(clock: &ClockSource, nanoseconds: i64) {
         clock.set_ros_time_override(nanoseconds);
     }
 
-    fn update_all_clocks(clocks: &Arc<Mutex<Vec<Arc<Clock>>>>, nanoseconds: i64) {
+    fn update_all_clocks(clocks: &Arc<Mutex<Vec<ClockSource>>>, nanoseconds: i64) {
         let clocks = clocks.lock().unwrap();
         for clock in clocks.iter() {
             Self::update_clock(clock, nanoseconds);
@@ -205,17 +196,18 @@ impl TimeSource {
 mod tests {
     use super::*;
     use crate::create_node;
-    use crate::Context;
+    use crate::{Clock, Context};
 
     #[test]
     fn time_source_attach_clock() {
-        let clock = Arc::new(Clock::new(ClockType::RosTime).unwrap());
         let node = create_node(&Context::new([]).unwrap(), "test_node").unwrap();
-        let time_source = TimeSource::new(node.clone(), clock);
-        let clock = Arc::new(Clock::new(ClockType::RosTime).unwrap());
+        let (_, source) = Clock::with_source().unwrap();
+        let time_source = TimeSource::new(node.clone(), source);
+        let (_, source) = Clock::with_source().unwrap();
         // Attaching additional clocks should be OK
-        time_source.attach_clock(clock).unwrap();
+        time_source.attach_clock(source);
         // Default clock should be above 0 (use_sim_time is default false)
         assert!(node.get_clock().now().nsec > 0);
+        // TODO(luca) an integration test by creating a node and setting its use_sim_time
     }
 }

From 0fcd2968dde0ccd101fb8080cb791025730530ab Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 3 Aug 2023 16:18:35 +0800
Subject: [PATCH 18/37] Fix Drop implementation, finish builder and add
 comments

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs        | 20 +++++++++++++-------
 rclrs/src/node/builder.rs |  6 ++++++
 2 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index cd4fb4c9..768f793b 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -60,7 +60,6 @@ impl Clock {
     }
 
     /// Creates a new clock of the given `ClockType`.
-    // TODO(luca) proper error handling
     pub fn new(type_: ClockType) -> Result<(Self, Option<ClockSource>), RclrsError> {
         let clock = Self::make(type_)?;
         let clock_source = if matches!(type_, ClockType::RosTime) {
@@ -73,7 +72,6 @@ impl Clock {
         Ok((clock, clock_source))
     }
 
-    // TODO(luca) proper error handling
     fn make(type_: ClockType) -> Result<Self, RclrsError> {
         let mut rcl_clock;
         unsafe {
@@ -136,13 +134,17 @@ impl ClockSource {
             // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
             // by the mutex
             unsafe {
-                rcl_enable_ros_time_override(&mut *clock);
+                // 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 {
-                rcl_disable_ros_time_override(&mut *clock);
+                // Function will only fail if timer was uninitialized or not RosTime, which should
+                // not happen
+                rcl_disable_ros_time_override(&mut *clock).ok().unwrap();
             }
         }
     }
@@ -153,15 +155,19 @@ impl ClockSource {
         // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
         // by the mutex
         unsafe {
-            rcl_set_ros_time_override(&mut *clock, nanoseconds);
+            // 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 Clock {
+impl Drop for rcl_clock_t {
     fn drop(&mut self) {
         // SAFETY: No preconditions for this function
-        let rc = unsafe { rcl_clock_fini(&mut *self._rcl_clock.lock().unwrap()) };
+        let rc = unsafe { rcl_clock_fini(&mut *self) };
         if let Err(e) = to_rclrs_result(rc) {
             panic!("Unable to release Clock. {:?}", e)
         }
diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index 4036b089..6eb8397a 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -227,6 +227,12 @@ impl NodeBuilder {
         self
     }
 
+    /// Sets the node's clock type.
+    pub fn clock_type(mut self, clock_type: ClockType) -> Self {
+        self.clock_type = clock_type;
+        self
+    }
+
     /// Builds the node instance.
     ///
     /// Node name and namespace validation is performed in this method.

From 524475d5867586f0b8d7c22f677665c0c30cbbb7 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 3 Aug 2023 16:31:55 +0800
Subject: [PATCH 19/37] Minor cleanups

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs       |  5 +++++
 rclrs/src/lib.rs         |  2 +-
 rclrs/src/time_source.rs | 13 ++++---------
 3 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index 768f793b..de66ba7c 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -86,6 +86,11 @@ impl 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();
diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs
index de3988a9..6fbfcb6a 100644
--- a/rclrs/src/lib.rs
+++ b/rclrs/src/lib.rs
@@ -115,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> {
-    Node::builder(context, node_name).build()
+    Node::new(context, node_name)
 }
 
 /// Creates a [`NodeBuilder`][1].
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index 1a21b361..5f3e8356 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -135,10 +135,8 @@ impl TimeSource {
         if let Some(sim_param) = self._node.get_parameter("use_sim_time") {
             match sim_param {
                 ParameterValue::Bool(val) => {
-                    println!("Setting ros time");
                     self.set_ros_time_enable(val);
                 }
-                // TODO(luca) more graceful error handling?
                 _ => panic!("use_sim_time parameter must be boolean"),
             }
         }
@@ -175,18 +173,15 @@ impl TimeSource {
     }
 
     fn create_clock_sub(&self) -> Arc<Subscription<ClockMsg>> {
-        let ros_time_active = self._ros_time_active.clone();
         let clocks = self._clocks.clone();
         let last_time_msg = self._last_time_msg.clone();
         // Safe to unwrap since the function will only fail if invalid arguments are provided
         self._node
             .create_subscription::<ClockMsg, _>("/clock", self._clock_qos, move |msg: ClockMsg| {
-                if ros_time_active.load(Ordering::Relaxed) {
-                    let nanoseconds: i64 =
-                        (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64;
-                    *last_time_msg.lock().unwrap() = Some(msg);
-                    Self::update_all_clocks(&clocks, nanoseconds);
-                }
+                let nanoseconds: i64 =
+                    (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64;
+                *last_time_msg.lock().unwrap() = Some(msg);
+                Self::update_all_clocks(&clocks, nanoseconds);
             })
             .unwrap()
     }

From 013cce52791772375b872aca6a7ada830f67aa64 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 3 Aug 2023 16:55:25 +0800
Subject: [PATCH 20/37] Fix graph_tests

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs_tests/src/graph_tests.rs | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/rclrs_tests/src/graph_tests.rs b/rclrs_tests/src/graph_tests.rs
index b32447ca..251cbc2e 100644
--- a/rclrs_tests/src/graph_tests.rs
+++ b/rclrs_tests/src/graph_tests.rs
@@ -3,10 +3,11 @@ use rclrs::{
     QOS_PROFILE_SYSTEM_DEFAULT,
 };
 use test_msgs::{msg, srv};
+use std::sync::Arc;
 
 struct TestGraph {
-    node1: Node,
-    node2: Node,
+    node1: Arc<Node>,
+    node2: Arc<Node>,
 }
 
 fn construct_test_graph(namespace: &str) -> Result<TestGraph, RclrsError> {

From a5bdb07904e8fe82a4e1316df498c6f716e4b719 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 3 Aug 2023 17:24:30 +0800
Subject: [PATCH 21/37] Remove Arc from time source

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/node.rs         | 3 +--
 rclrs/src/node/builder.rs | 2 +-
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index 0c137d08..92d5bcfa 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -72,8 +72,7 @@ pub struct Node {
     pub(crate) services_mtx: Mutex<Vec<Weak<dyn ServiceBase>>>,
     pub(crate) subscriptions_mtx: Mutex<Vec<Weak<dyn SubscriptionBase>>>,
     _clock: Clock,
-    // TODO(luca) set to private
-    pub(crate) _time_source: Arc<Mutex<Option<TimeSource>>>,
+    _time_source: Mutex<Option<TimeSource>>,
     _parameter_map: ParameterOverrideMap,
 }
 
diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index 6eb8397a..b31da5d2 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -290,7 +290,7 @@ impl NodeBuilder {
             services_mtx: Mutex::new(vec![]),
             subscriptions_mtx: Mutex::new(vec![]),
             _clock: clock,
-            _time_source: Arc::new(Mutex::new(None)),
+            _time_source: Mutex::new(None),
             _parameter_map,
         });
 

From d02ccc50a31aecf4bdcf16aa775d36ff26f43276 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 3 Aug 2023 17:34:00 +0800
Subject: [PATCH 22/37] Remove Sync trait, format

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs             | 1 -
 rclrs_tests/src/graph_tests.rs | 2 +-
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index de66ba7c..ea4ac4e7 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -182,7 +182,6 @@ impl Drop for rcl_clock_t {
 // 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 {}
-unsafe impl Sync for rcl_clock_t {}
 
 #[cfg(test)]
 mod tests {
diff --git a/rclrs_tests/src/graph_tests.rs b/rclrs_tests/src/graph_tests.rs
index 251cbc2e..11072e09 100644
--- a/rclrs_tests/src/graph_tests.rs
+++ b/rclrs_tests/src/graph_tests.rs
@@ -2,8 +2,8 @@ use rclrs::{
     Context, Node, NodeBuilder, RclrsError, TopicEndpointInfo, TopicNamesAndTypes,
     QOS_PROFILE_SYSTEM_DEFAULT,
 };
-use test_msgs::{msg, srv};
 use std::sync::Arc;
+use test_msgs::{msg, srv};
 
 struct TestGraph {
     node1: Arc<Node>,

From 9c3d987f071f02494c33ff5973632446d523ec5f Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 4 Aug 2023 11:26:52 +0800
Subject: [PATCH 23/37] Add comparison function for times, use reference to
 clock

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs |  2 +-
 rclrs/src/time.rs  | 48 +++++++++++++++++++++++++++++++++++++---------
 2 files changed, 40 insertions(+), 10 deletions(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index ea4ac4e7..3668516d 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -101,7 +101,7 @@ impl Clock {
         }
         Time {
             nsec: time_point,
-            clock_type: self._type,
+            clock: Arc::downgrade(&self._rcl_clock),
         }
     }
 
diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs
index d5427470..14e1704d 100644
--- a/rclrs/src/time.rs
+++ b/rclrs/src/time.rs
@@ -1,20 +1,50 @@
-use crate::clock::ClockType;
 use crate::rcl_bindings::*;
+use std::sync::{Mutex, Weak};
 
 /// Struct that represents time.
 #[derive(Debug)]
 pub struct Time {
     /// Timestamp in nanoseconds.
     pub nsec: i64,
-    /// Clock type.
-    pub clock_type: ClockType,
+    /// Weak reference to the clock that generated this time
+    pub clock: Weak<Mutex<rcl_clock_t>>,
 }
 
-impl From<Time> for rcl_time_point_t {
-    fn from(time: Time) -> Self {
-        Self {
-            nanoseconds: time.nsec,
-            clock_type: time.clock_type.into(),
-        }
+impl Time {
+    /// Compares self to rhs, if they can be compared (originated from the same clock) calls f with
+    /// the values of the timestamps.
+    pub fn compare_with<U, F>(&self, rhs: &Time, f: F) -> Option<U>
+    where
+        F: FnOnce(i64, i64) -> U,
+    {
+        self.clock
+            .ptr_eq(&rhs.clock)
+            .then(|| f(self.nsec, rhs.nsec))
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::clock::Clock;
+
+    #[test]
+    fn compare_times_from_same_clock() {
+        let clock = Clock::system().unwrap();
+        let t1 = clock.now();
+        std::thread::sleep(std::time::Duration::from_micros(1));
+        let t2 = clock.now();
+        assert_eq!(t1.compare_with(&t2, |t1, t2| t1 > t2), Some(false));
+        assert_eq!(t1.compare_with(&t2, |t1, t2| t2 > t1), Some(true));
+    }
+
+    #[test]
+    fn compare_times_from_different_clocks() {
+        // Times from different clocks, even if of the same type, can't be compared
+        let c1 = Clock::system().unwrap();
+        let c2 = Clock::system().unwrap();
+        let t1 = c1.now();
+        let t2 = c2.now();
+        assert!(t2.compare_with(&t1, |_, _| ()).is_none());
+        assert!(t1.compare_with(&t2, |_, _| ()).is_none());
     }
 }

From 99401aa97824c953571a21042b60d12c4d54f113 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 4 Aug 2023 15:00:22 +0800
Subject: [PATCH 24/37] Implement Add<Duration> and Sub<Duration> for Time

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/time.rs        | 41 ++++++++++++++++++++++++++++++++++++++--
 rclrs/src/time_source.rs |  7 ++++++-
 2 files changed, 45 insertions(+), 3 deletions(-)

diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs
index 14e1704d..4227a0d2 100644
--- a/rclrs/src/time.rs
+++ b/rclrs/src/time.rs
@@ -1,8 +1,10 @@
 use crate::rcl_bindings::*;
+use std::ops::{Add, Sub};
 use std::sync::{Mutex, Weak};
+use std::time::Duration;
 
 /// Struct that represents time.
-#[derive(Debug)]
+#[derive(Clone, Debug)]
 pub struct Time {
     /// Timestamp in nanoseconds.
     pub nsec: i64,
@@ -23,9 +25,34 @@ impl Time {
     }
 }
 
+impl Add<Duration> for Time {
+    type Output = Self;
+
+    fn add(self, other: Duration) -> Self {
+        let dur_ns = i64::try_from(other.as_nanos()).unwrap();
+        Time {
+            nsec: self.nsec.checked_add(dur_ns).unwrap(),
+            clock: self.clock.clone(),
+        }
+    }
+}
+
+impl Sub<Duration> for Time {
+    type Output = Self;
+
+    fn sub(self, other: Duration) -> Self {
+        let dur_ns = i64::try_from(other.as_nanos()).unwrap();
+        Time {
+            nsec: self.nsec.checked_sub(dur_ns).unwrap(),
+            clock: self.clock.clone(),
+        }
+    }
+}
+
 #[cfg(test)]
 mod tests {
-    use crate::clock::Clock;
+    use super::*;
+    use crate::Clock;
 
     #[test]
     fn compare_times_from_same_clock() {
@@ -47,4 +74,14 @@ mod tests {
         assert!(t2.compare_with(&t1, |_, _| ()).is_none());
         assert!(t1.compare_with(&t2, |_, _| ()).is_none());
     }
+
+    #[test]
+    fn add_duration_to_time() {
+        let (clock, _) = Clock::with_source().unwrap();
+        let t = clock.now();
+        let t2 = t.clone() + Duration::from_secs(1);
+        assert_eq!(t2.nsec - t.nsec, 1_000_000_000i64);
+        let t3 = t2 - Duration::from_secs(1);
+        assert_eq!(t3.nsec, t.nsec);
+    }
 }
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index 5f3e8356..4ea33b7d 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -102,11 +102,16 @@ impl fmt::Display for ClockMismatchError {
 }
 
 impl TimeSource {
-    /// Creates a new time source with default parameters.
+    /// Creates a new `TimeSource` with default parameters.
     pub fn new(node: Arc<Node>, clock: ClockSource) -> Self {
         TimeSourceBuilder::new(node, clock).build().unwrap()
     }
 
+    /// Creates a new `TimeSourceBuilder` with default parameters.
+    pub fn builder(node: Arc<Node>, clock: ClockSource) -> TimeSourceBuilder {
+        TimeSourceBuilder::new(node, clock)
+    }
+
     /// Attaches the given clock to the `TimeSource`, enabling the `TimeSource` to control it.
     pub fn attach_clock(&self, clock: ClockSource) {
         if let Some(last_msg) = self._last_time_msg.lock().unwrap().as_ref() {

From 90fe308bdc6acaebc016c79a7a12d559d84c6d2b Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 7 Aug 2023 12:27:59 +0800
Subject: [PATCH 25/37] Make node pointer Weak to avoid memory leaks

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/Cargo.toml         |  5 ++---
 rclrs/src/time_source.rs | 20 +++++++++++++-------
 2 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml
index 6e225712..a0ba674a 100644
--- a/rclrs/Cargo.toml
+++ b/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"
 
@@ -31,8 +33,5 @@ tempfile = "3.3.0"
 # Needed for FFI
 bindgen = "0.66.1"
 
-[dependencies.rosgraph_msgs]
-version = "*"
-
 [features]
 dyn_msg = ["ament_rs", "libloading"]
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index 4ea33b7d..031406f6 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -3,13 +3,13 @@ use crate::{Node, ParameterValue, QoSProfile, Subscription, QOS_PROFILE_CLOCK};
 use rosgraph_msgs::msg::Clock as ClockMsg;
 use std::fmt;
 use std::sync::atomic::{AtomicBool, Ordering};
-use std::sync::{Arc, Mutex};
+use std::sync::{Arc, Mutex, Weak};
 
 /// Time source for a node that drives the attached clocks.
 /// If the node's `use_sim_time` parameter is set to `true`, the `TimeSource` will subscribe
 /// to the `/clock` topic and drive the attached clocks
 pub struct TimeSource {
-    _node: Arc<Node>,
+    _node: Weak<Node>,
     _clocks: Arc<Mutex<Vec<ClockSource>>>,
     _clock_qos: QoSProfile,
     // TODO(luca) implement clock threads, for now will run in main thread
@@ -34,7 +34,7 @@ pub struct TimeSource {
 /// [2]: crate::TimeSource::builder
 /// [3]: crate::QOS_PROFILE_CLOCK
 pub struct TimeSourceBuilder {
-    node: Arc<Node>,
+    node: Weak<Node>,
     clock_qos: QoSProfile,
     use_clock_thread: bool,
     clock_source: ClockSource,
@@ -46,7 +46,7 @@ impl TimeSourceBuilder {
     /// topic.
     pub fn new(node: Arc<Node>, clock_source: ClockSource) -> Self {
         Self {
-            node,
+            node: Arc::downgrade(&node),
             clock_qos: QOS_PROFILE_CLOCK,
             use_clock_thread: true,
             clock_source,
@@ -71,7 +71,7 @@ impl TimeSourceBuilder {
     /// Builds the `TimeSource` and attaches the provided `Node` and `Clock`.
     pub fn build(self) -> Result<TimeSource, ClockMismatchError> {
         let mut source = TimeSource {
-            _node: self.node.clone(),
+            _node: Weak::new(),
             _clocks: Arc::new(Mutex::new(vec![])),
             _clock_qos: self.clock_qos,
             _use_clock_thread: self.use_clock_thread,
@@ -133,11 +133,15 @@ impl TimeSource {
 
     /// Attaches the given node to to the `TimeSource`, using its interface to read the
     /// `use_sim_time` parameter and create the clock subscription.
-    pub fn attach_node(&mut self, node: Arc<Node>) {
+    pub fn attach_node(&mut self, node: Weak<Node>) {
         self._node = node;
 
         // TODO(luca) register a parameter callback
-        if let Some(sim_param) = self._node.get_parameter("use_sim_time") {
+        if let Some(sim_param) = self
+            ._node
+            .upgrade()
+            .and_then(|n| n.get_parameter("use_sim_time"))
+        {
             match sim_param {
                 ParameterValue::Bool(val) => {
                     self.set_ros_time_enable(val);
@@ -182,6 +186,8 @@ impl TimeSource {
         let last_time_msg = self._last_time_msg.clone();
         // Safe to unwrap since the function will only fail if invalid arguments are provided
         self._node
+            .upgrade()
+            .unwrap()
             .create_subscription::<ClockMsg, _>("/clock", self._clock_qos, move |msg: ClockMsg| {
                 let nanoseconds: i64 =
                     (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64;

From 0ee8954d9aa49171113bb295f6431ee0eb8d26ef Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 12 Sep 2023 16:09:32 +0800
Subject: [PATCH 26/37] Cleanups

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/node/builder.rs |  7 ++-----
 rclrs/src/time_source.rs  | 29 +++++------------------------
 2 files changed, 7 insertions(+), 29 deletions(-)

diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index b31da5d2..1fda6f9b 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -295,11 +295,8 @@ impl NodeBuilder {
         });
 
         if let Some(clock_source) = clock_source {
-            *node._time_source.lock().unwrap() = Some(
-                TimeSourceBuilder::new(node.clone(), clock_source)
-                    .build()
-                    .unwrap(),
-            );
+            *node._time_source.lock().unwrap() =
+                Some(TimeSourceBuilder::new(node.clone(), clock_source).build());
         }
 
         Ok(node)
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index 031406f6..fea49608 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -1,7 +1,6 @@
-use crate::clock::{ClockSource, ClockType};
+use crate::clock::ClockSource;
 use crate::{Node, ParameterValue, QoSProfile, Subscription, QOS_PROFILE_CLOCK};
 use rosgraph_msgs::msg::Clock as ClockMsg;
-use std::fmt;
 use std::sync::atomic::{AtomicBool, Ordering};
 use std::sync::{Arc, Mutex, Weak};
 
@@ -42,8 +41,6 @@ pub struct TimeSourceBuilder {
 
 impl TimeSourceBuilder {
     /// Creates a builder for a time source that drives the given clock for the given node.
-    /// The clock's ClockType must be set to `RosTime` to allow updates based on the `/clock`
-    /// topic.
     pub fn new(node: Arc<Node>, clock_source: ClockSource) -> Self {
         Self {
             node: Arc::downgrade(&node),
@@ -69,7 +66,7 @@ impl TimeSourceBuilder {
     }
 
     /// Builds the `TimeSource` and attaches the provided `Node` and `Clock`.
-    pub fn build(self) -> Result<TimeSource, ClockMismatchError> {
+    pub fn build(self) -> TimeSource {
         let mut source = TimeSource {
             _node: Weak::new(),
             _clocks: Arc::new(Mutex::new(vec![])),
@@ -81,30 +78,14 @@ impl TimeSourceBuilder {
         };
         source.attach_clock(self.clock_source);
         source.attach_node(self.node);
-        Ok(source)
-    }
-}
-
-/// Error created when a non RosTime clock is attached but the `use_sim_time` parameter is set.
-/// This case could behave in unexpected ways since the /clock topic would be ignored even though
-/// the user explicitly requested to use it through a command line argument.
-#[derive(Debug)]
-pub struct ClockMismatchError(ClockType);
-
-impl fmt::Display for ClockMismatchError {
-    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
-        write!(
-            f,
-            "Unexpected clock type {:?}, only RosTime can be used as a time source when use_sim_time is set",
-            self.0
-        )
+        source
     }
 }
 
 impl TimeSource {
     /// Creates a new `TimeSource` with default parameters.
     pub fn new(node: Arc<Node>, clock: ClockSource) -> Self {
-        TimeSourceBuilder::new(node, clock).build().unwrap()
+        TimeSourceBuilder::new(node, clock).build()
     }
 
     /// Creates a new `TimeSourceBuilder` with default parameters.
@@ -120,7 +101,7 @@ impl TimeSource {
             Self::update_clock(&clock, nanoseconds);
         }
         let mut clocks = self._clocks.lock().unwrap();
-        if !clocks.iter().any(|c| c != &clock) {
+        if clocks.iter().all(|c| c != &clock) {
             clocks.push(clock);
         }
     }

From 4645935757fb87c82e6254c7dc146ddb74b86da1 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 12 Sep 2023 22:24:07 +0800
Subject: [PATCH 27/37] WIP change clock type when use_sim_time parameter is
 changed

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 .../minimal_pub_sub/src/minimal_subscriber.rs |   2 +-
 rclrs/src/clock.rs                            |  63 ++++-----
 rclrs/src/node.rs                             |  10 +-
 rclrs/src/node/builder.rs                     |  13 +-
 rclrs/src/time_source.rs                      | 128 ++++++++----------
 5 files changed, 96 insertions(+), 120 deletions(-)

diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs
index b4212d4c..eb91616a 100644
--- a/examples/minimal_pub_sub/src/minimal_subscriber.rs
+++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs
@@ -17,7 +17,7 @@ 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();
+            let now = inner_node.get_clock().lock().unwrap().now();
             dbg!(now);
         },
     )?;
diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index 3668516d..d2c61138 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -53,22 +53,15 @@ impl Clock {
     /// 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(),
-        };
+        let clock_source = ClockSource::new(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
-        };
+        let clock_source =
+            matches!(type_, ClockType::RosTime).then(|| ClockSource::new(clock._rcl_clock.clone()));
         Ok((clock, clock_source))
     }
 
@@ -124,6 +117,12 @@ impl Clock {
     }
 }
 
+impl Drop for ClockSource {
+    fn drop(&mut self) {
+        self.set_ros_time_enable(false);
+    }
+}
+
 impl PartialEq for ClockSource {
     fn eq(&self, other: &Self) -> bool {
         Arc::ptr_eq(&self._rcl_clock, &other._rcl_clock)
@@ -131,9 +130,29 @@ impl PartialEq for ClockSource {
 }
 
 impl ClockSource {
+    /// 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();
+        }
+    }
+
+    fn new(clock: Arc<Mutex<rcl_clock_t>>) -> Self {
+        let source = Self { _rcl_clock: clock };
+        source.set_ros_time_enable(true);
+        source
+    }
+
     /// 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) {
+    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
@@ -153,20 +172,6 @@ impl ClockSource {
             }
         }
     }
-
-    /// 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 {
@@ -205,19 +210,11 @@ mod tests {
     #[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);
     }
 }
diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index 92d5bcfa..85d63b9d 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -71,7 +71,6 @@ 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,
 }
@@ -102,8 +101,13 @@ impl Node {
     }
 
     /// Gets the clock associated with this node.
-    pub fn get_clock(&self) -> &Clock {
-        &self._clock
+    pub fn get_clock(&self) -> Arc<Mutex<Clock>> {
+        self._time_source
+            .lock()
+            .unwrap()
+            .as_ref()
+            .unwrap()
+            .get_clock()
     }
 
     /// Returns the name of the node.
diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index 1fda6f9b..30374c45 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -3,8 +3,8 @@ use std::sync::{Arc, Mutex};
 
 use crate::rcl_bindings::*;
 use crate::{
-    node::call_string_getter_with_handle, resolve_parameter_overrides, Clock, ClockType, Context,
-    Node, RclrsError, TimeSourceBuilder, ToResult,
+    node::call_string_getter_with_handle, resolve_parameter_overrides, ClockType, Context, Node,
+    RclrsError, TimeSource, ToResult,
 };
 
 /// A builder for creating a [`Node`][1].
@@ -271,8 +271,6 @@ impl NodeBuilder {
             .ok()?;
         };
 
-        let (clock, clock_source) = Clock::new(self.clock_type)?;
-
         let _parameter_map = unsafe {
             let fqn = call_string_getter_with_handle(&rcl_node, rcl_node_get_fully_qualified_name);
             resolve_parameter_overrides(
@@ -289,15 +287,12 @@ impl NodeBuilder {
             guard_conditions_mtx: Mutex::new(vec![]),
             services_mtx: Mutex::new(vec![]),
             subscriptions_mtx: Mutex::new(vec![]),
-            _clock: clock,
             _time_source: Mutex::new(None),
             _parameter_map,
         });
 
-        if let Some(clock_source) = clock_source {
-            *node._time_source.lock().unwrap() =
-                Some(TimeSourceBuilder::new(node.clone(), clock_source).build());
-        }
+        *node._time_source.lock().unwrap() =
+            Some(TimeSource::builder(Arc::downgrade(&node), self.clock_type).build()?);
 
         Ok(node)
     }
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index fea49608..37bd9e21 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -1,22 +1,21 @@
-use crate::clock::ClockSource;
-use crate::{Node, ParameterValue, QoSProfile, Subscription, QOS_PROFILE_CLOCK};
+use crate::clock::{Clock, ClockSource, ClockType};
+use crate::{Node, ParameterValue, QoSProfile, RclrsError, Subscription, QOS_PROFILE_CLOCK};
 use rosgraph_msgs::msg::Clock as ClockMsg;
-use std::sync::atomic::{AtomicBool, Ordering};
 use std::sync::{Arc, Mutex, Weak};
 
-/// Time source for a node that drives the attached clocks.
+/// Time source for a node that drives the attached clock.
 /// If the node's `use_sim_time` parameter is set to `true`, the `TimeSource` will subscribe
-/// to the `/clock` topic and drive the attached clocks
+/// to the `/clock` topic and drive the attached clock
 pub struct TimeSource {
     _node: Weak<Node>,
-    _clocks: Arc<Mutex<Vec<ClockSource>>>,
+    _clock: Arc<Mutex<Clock>>,
+    _clock_source: Arc<Mutex<Option<ClockSource>>>,
+    _requested_clock_type: ClockType,
     _clock_qos: QoSProfile,
     // TODO(luca) implement clock threads, for now will run in main thread
     _use_clock_thread: bool,
-    // TODO(luca) Update this with parameter callbacks for use_sim_time
-    _ros_time_active: Arc<AtomicBool>,
     _clock_subscription: Option<Arc<Subscription<ClockMsg>>>,
-    _last_time_msg: Arc<Mutex<Option<ClockMsg>>>,
+    _last_received_time: Arc<Mutex<Option<i64>>>,
 }
 
 /// A builder for creating a [`TimeSource`][1].
@@ -36,17 +35,17 @@ pub struct TimeSourceBuilder {
     node: Weak<Node>,
     clock_qos: QoSProfile,
     use_clock_thread: bool,
-    clock_source: ClockSource,
+    clock_type: ClockType,
 }
 
 impl TimeSourceBuilder {
     /// Creates a builder for a time source that drives the given clock for the given node.
-    pub fn new(node: Arc<Node>, clock_source: ClockSource) -> Self {
+    pub fn new(node: Weak<Node>, clock_type: ClockType) -> Self {
         Self {
-            node: Arc::downgrade(&node),
+            node,
             clock_qos: QOS_PROFILE_CLOCK,
             use_clock_thread: true,
-            clock_source,
+            clock_type,
         }
     }
 
@@ -66,50 +65,40 @@ impl TimeSourceBuilder {
     }
 
     /// Builds the `TimeSource` and attaches the provided `Node` and `Clock`.
-    pub fn build(self) -> TimeSource {
+    pub fn build(self) -> Result<TimeSource, RclrsError> {
+        let clock = match self.clock_type {
+            ClockType::RosTime | ClockType::SystemTime => Clock::system(),
+            ClockType::SteadyTime => Clock::steady(),
+        }?;
         let mut source = TimeSource {
             _node: Weak::new(),
-            _clocks: Arc::new(Mutex::new(vec![])),
+            _clock: Arc::new(Mutex::new(clock)),
+            _clock_source: Arc::new(Mutex::new(None)),
+            _requested_clock_type: self.clock_type,
             _clock_qos: self.clock_qos,
             _use_clock_thread: self.use_clock_thread,
-            _ros_time_active: Arc::new(AtomicBool::new(false)),
             _clock_subscription: None,
-            _last_time_msg: Arc::new(Mutex::new(None)),
+            _last_received_time: Arc::new(Mutex::new(None)),
         };
-        source.attach_clock(self.clock_source);
         source.attach_node(self.node);
-        source
+        Ok(source)
     }
 }
 
 impl TimeSource {
     /// Creates a new `TimeSource` with default parameters.
-    pub fn new(node: Arc<Node>, clock: ClockSource) -> Self {
-        TimeSourceBuilder::new(node, clock).build()
+    pub fn new(node: Weak<Node>, clock_type: ClockType) -> Self {
+        TimeSourceBuilder::new(node, clock_type).build().unwrap()
     }
 
     /// Creates a new `TimeSourceBuilder` with default parameters.
-    pub fn builder(node: Arc<Node>, clock: ClockSource) -> TimeSourceBuilder {
-        TimeSourceBuilder::new(node, clock)
+    pub fn builder(node: Weak<Node>, clock_type: ClockType) -> TimeSourceBuilder {
+        TimeSourceBuilder::new(node, clock_type)
     }
 
-    /// Attaches the given clock to the `TimeSource`, enabling the `TimeSource` to control it.
-    pub fn attach_clock(&self, clock: ClockSource) {
-        if let Some(last_msg) = self._last_time_msg.lock().unwrap().as_ref() {
-            let nanoseconds: i64 =
-                (last_msg.clock.sec as i64 * 1_000_000_000) + last_msg.clock.nanosec as i64;
-            Self::update_clock(&clock, nanoseconds);
-        }
-        let mut clocks = self._clocks.lock().unwrap();
-        if clocks.iter().all(|c| c != &clock) {
-            clocks.push(clock);
-        }
-    }
-
-    /// Detaches the given clock from the `TimeSource`.
-    // TODO(luca) should we return a result to denote whether the clock was removed?
-    pub fn detach_clock(&self, clock: &ClockSource) {
-        self._clocks.lock().unwrap().retain(|c| c != clock);
+    /// Returns the clock that this TimeSource is controlling.
+    pub fn get_clock(&self) -> Arc<Mutex<Clock>> {
+        self._clock.clone()
     }
 
     /// Attaches the given node to to the `TimeSource`, using its interface to read the
@@ -133,38 +122,34 @@ impl TimeSource {
     }
 
     fn set_ros_time_enable(&mut self, enable: bool) {
-        let updated = self
-            ._ros_time_active
-            .fetch_update(Ordering::Relaxed, Ordering::Relaxed, |prev| {
-                if prev != enable {
-                    Some(enable)
-                } else {
-                    None
+        if matches!(self._requested_clock_type, ClockType::RosTime) {
+            let mut clock = self._clock.lock().unwrap();
+            if enable && matches!(clock.clock_type(), ClockType::SystemTime) {
+                // TODO(luca) remove unwrap here?
+                let (new_clock, mut clock_source) = Clock::with_source().unwrap();
+                if let Some(last_received_time) = *self._last_received_time.lock().unwrap() {
+                    Self::update_clock(&mut clock_source, last_received_time);
                 }
-            })
-            .is_ok();
-        if updated {
-            for clock in self._clocks.lock().unwrap().iter() {
-                clock.set_ros_time_enable(enable);
+                *clock = new_clock;
+                *self._clock_source.lock().unwrap() = Some(clock_source);
+                self._clock_subscription = Some(self.create_clock_sub());
+            }
+            if !enable && matches!(clock.clock_type(), ClockType::RosTime) {
+                // TODO(luca) remove unwrap here?
+                *clock = Clock::system().unwrap();
+                *self._clock_source.lock().unwrap() = None;
+                self._clock_subscription = None;
             }
-            self._clock_subscription = enable.then(|| self.create_clock_sub());
         }
     }
 
-    fn update_clock(clock: &ClockSource, nanoseconds: i64) {
+    fn update_clock(clock: &mut ClockSource, nanoseconds: i64) {
         clock.set_ros_time_override(nanoseconds);
     }
 
-    fn update_all_clocks(clocks: &Arc<Mutex<Vec<ClockSource>>>, nanoseconds: i64) {
-        let clocks = clocks.lock().unwrap();
-        for clock in clocks.iter() {
-            Self::update_clock(clock, nanoseconds);
-        }
-    }
-
     fn create_clock_sub(&self) -> Arc<Subscription<ClockMsg>> {
-        let clocks = self._clocks.clone();
-        let last_time_msg = self._last_time_msg.clone();
+        let clock = self._clock_source.clone();
+        let last_received_time = self._last_received_time.clone();
         // Safe to unwrap since the function will only fail if invalid arguments are provided
         self._node
             .upgrade()
@@ -172,8 +157,10 @@ impl TimeSource {
             .create_subscription::<ClockMsg, _>("/clock", self._clock_qos, move |msg: ClockMsg| {
                 let nanoseconds: i64 =
                     (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64;
-                *last_time_msg.lock().unwrap() = Some(msg);
-                Self::update_all_clocks(&clocks, nanoseconds);
+                *last_received_time.lock().unwrap() = Some(nanoseconds);
+                if let Some(clock) = clock.lock().unwrap().as_mut() {
+                    Self::update_clock(clock, nanoseconds);
+                }
             })
             .unwrap()
     }
@@ -181,20 +168,13 @@ impl TimeSource {
 
 #[cfg(test)]
 mod tests {
-    use super::*;
-    use crate::create_node;
-    use crate::{Clock, Context};
+    use crate::{create_node, Context};
 
     #[test]
     fn time_source_attach_clock() {
         let node = create_node(&Context::new([]).unwrap(), "test_node").unwrap();
-        let (_, source) = Clock::with_source().unwrap();
-        let time_source = TimeSource::new(node.clone(), source);
-        let (_, source) = Clock::with_source().unwrap();
-        // Attaching additional clocks should be OK
-        time_source.attach_clock(source);
         // Default clock should be above 0 (use_sim_time is default false)
-        assert!(node.get_clock().now().nsec > 0);
+        assert!(node.get_clock().lock().unwrap().now().nsec > 0);
         // TODO(luca) an integration test by creating a node and setting its use_sim_time
     }
 }

From edce20e5a813011ffd8a794bfd171972d67e7700 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 13 Sep 2023 13:07:05 +0800
Subject: [PATCH 28/37] Remove need for mutex in node TimeSource

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/node.rs         | 9 ++-------
 rclrs/src/node/builder.rs | 7 ++-----
 2 files changed, 4 insertions(+), 12 deletions(-)

diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index 85d63b9d..9391df66 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -71,7 +71,7 @@ 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>>>,
-    _time_source: Mutex<Option<TimeSource>>,
+    _time_source: TimeSource,
     _parameter_map: ParameterOverrideMap,
 }
 
@@ -102,12 +102,7 @@ impl Node {
 
     /// Gets the clock associated with this node.
     pub fn get_clock(&self) -> Arc<Mutex<Clock>> {
-        self._time_source
-            .lock()
-            .unwrap()
-            .as_ref()
-            .unwrap()
-            .get_clock()
+        self._time_source.get_clock()
     }
 
     /// Returns the name of the node.
diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index 30374c45..c3357ee9 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -280,20 +280,17 @@ impl NodeBuilder {
             )?
         };
         let rcl_node_mtx = Arc::new(Mutex::new(rcl_node));
-        let node = Arc::new(Node {
+        let node = Arc::new_cyclic(|weak| Node {
             rcl_node_mtx,
             rcl_context_mtx: self.context.clone(),
             clients_mtx: Mutex::new(vec![]),
             guard_conditions_mtx: Mutex::new(vec![]),
             services_mtx: Mutex::new(vec![]),
             subscriptions_mtx: Mutex::new(vec![]),
-            _time_source: Mutex::new(None),
+            _time_source: TimeSource::new(weak.clone(), self.clock_type),
             _parameter_map,
         });
 
-        *node._time_source.lock().unwrap() =
-            Some(TimeSource::builder(Arc::downgrade(&node), self.clock_type).build()?);
-
         Ok(node)
     }
 

From 632e082f272fd27a853409599ca134058e82df2d Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 13 Sep 2023 14:06:36 +0800
Subject: [PATCH 29/37] Change get_clock() to return cloned Clock object

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 examples/minimal_pub_sub/src/minimal_subscriber.rs |  2 +-
 rclrs/src/clock.rs                                 |  1 +
 rclrs/src/node.rs                                  |  4 ++--
 rclrs/src/time_source.rs                           | 14 +++++++-------
 4 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs
index eb91616a..b4212d4c 100644
--- a/examples/minimal_pub_sub/src/minimal_subscriber.rs
+++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs
@@ -17,7 +17,7 @@ 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().lock().unwrap().now();
+            let now = inner_node.get_clock().now();
             dbg!(now);
         },
     )?;
diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index d2c61138..ea75db90 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -26,6 +26,7 @@ impl From<ClockType> for rcl_clock_type_t {
 }
 
 /// Struct that implements a Clock and wraps `rcl_clock_t`.
+#[derive(Clone, Debug)]
 pub struct Clock {
     _type: ClockType,
     _rcl_clock: Arc<Mutex<rcl_clock_t>>,
diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index 9391df66..a7fa2580 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -100,8 +100,8 @@ impl Node {
         Self::builder(context, node_name).build()
     }
 
-    /// Gets the clock associated with this node.
-    pub fn get_clock(&self) -> Arc<Mutex<Clock>> {
+    /// Returns the clock associated with this node.
+    pub fn get_clock(&self) -> Clock {
         self._time_source.get_clock()
     }
 
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index 37bd9e21..ec8afebf 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -1,14 +1,14 @@
 use crate::clock::{Clock, ClockSource, ClockType};
 use crate::{Node, ParameterValue, QoSProfile, RclrsError, Subscription, QOS_PROFILE_CLOCK};
 use rosgraph_msgs::msg::Clock as ClockMsg;
-use std::sync::{Arc, Mutex, Weak};
+use std::sync::{Arc, Mutex, RwLock, Weak};
 
 /// Time source for a node that drives the attached clock.
 /// If the node's `use_sim_time` parameter is set to `true`, the `TimeSource` will subscribe
 /// to the `/clock` topic and drive the attached clock
 pub struct TimeSource {
     _node: Weak<Node>,
-    _clock: Arc<Mutex<Clock>>,
+    _clock: RwLock<Clock>,
     _clock_source: Arc<Mutex<Option<ClockSource>>>,
     _requested_clock_type: ClockType,
     _clock_qos: QoSProfile,
@@ -72,7 +72,7 @@ impl TimeSourceBuilder {
         }?;
         let mut source = TimeSource {
             _node: Weak::new(),
-            _clock: Arc::new(Mutex::new(clock)),
+            _clock: RwLock::new(clock),
             _clock_source: Arc::new(Mutex::new(None)),
             _requested_clock_type: self.clock_type,
             _clock_qos: self.clock_qos,
@@ -97,8 +97,8 @@ impl TimeSource {
     }
 
     /// Returns the clock that this TimeSource is controlling.
-    pub fn get_clock(&self) -> Arc<Mutex<Clock>> {
-        self._clock.clone()
+    pub fn get_clock(&self) -> Clock {
+        self._clock.read().unwrap().clone()
     }
 
     /// Attaches the given node to to the `TimeSource`, using its interface to read the
@@ -123,7 +123,7 @@ impl TimeSource {
 
     fn set_ros_time_enable(&mut self, enable: bool) {
         if matches!(self._requested_clock_type, ClockType::RosTime) {
-            let mut clock = self._clock.lock().unwrap();
+            let mut clock = self._clock.write().unwrap();
             if enable && matches!(clock.clock_type(), ClockType::SystemTime) {
                 // TODO(luca) remove unwrap here?
                 let (new_clock, mut clock_source) = Clock::with_source().unwrap();
@@ -174,7 +174,7 @@ mod tests {
     fn time_source_attach_clock() {
         let node = create_node(&Context::new([]).unwrap(), "test_node").unwrap();
         // Default clock should be above 0 (use_sim_time is default false)
-        assert!(node.get_clock().lock().unwrap().now().nsec > 0);
+        assert!(node.get_clock().now().nsec > 0);
         // TODO(luca) an integration test by creating a node and setting its use_sim_time
     }
 }

From 7e536e072b05e0de1379d544d580d385aab10ead Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 13 Sep 2023 15:56:34 +0800
Subject: [PATCH 30/37] Minor cleanup

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/node/builder.rs | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index c3357ee9..80eb2a64 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -280,7 +280,7 @@ impl NodeBuilder {
             )?
         };
         let rcl_node_mtx = Arc::new(Mutex::new(rcl_node));
-        let node = Arc::new_cyclic(|weak| Node {
+        Ok(Arc::new_cyclic(|weak| Node {
             rcl_node_mtx,
             rcl_context_mtx: self.context.clone(),
             clients_mtx: Mutex::new(vec![]),
@@ -289,9 +289,7 @@ impl NodeBuilder {
             subscriptions_mtx: Mutex::new(vec![]),
             _time_source: TimeSource::new(weak.clone(), self.clock_type),
             _parameter_map,
-        });
-
-        Ok(node)
+        }))
     }
 
     /// Creates a rcl_node_options_t struct from this builder.

From 7286ce10d367706ac139c435053bfe6bc89f68e1 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 15 Sep 2023 14:51:12 +0800
Subject: [PATCH 31/37] Make get_parameter pub(crate)

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/node.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs
index a7fa2580..09ff147a 100644
--- a/rclrs/src/node.rs
+++ b/rclrs/src/node.rs
@@ -363,7 +363,7 @@ impl Node {
 
     /// 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> {
+    pub(crate) fn get_parameter(&self, name: &str) -> Option<ParameterValue> {
         self._parameter_map.get(name).cloned()
     }
 

From ed583f67d44e382015766a325041b8b5389dfd38 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 18 Sep 2023 15:50:27 +0800
Subject: [PATCH 32/37] Address review feedback

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 examples/minimal_pub_sub/src/minimal_subscriber.rs |  3 ---
 rclrs/src/clock.rs                                 | 10 ++++------
 2 files changed, 4 insertions(+), 9 deletions(-)

diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs
index b4212d4c..ebc5fc19 100644
--- a/examples/minimal_pub_sub/src/minimal_subscriber.rs
+++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs
@@ -6,7 +6,6 @@ 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;
 
@@ -17,8 +16,6 @@ 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);
         },
     )?;
 
diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index ea75db90..9b74d59f 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -90,7 +90,7 @@ impl Clock {
         let mut clock = self._rcl_clock.lock().unwrap();
         let mut time_point: i64 = 0;
         unsafe {
-            // SAFETY: No preconditions for his function
+            // SAFETY: No preconditions for this function
             rcl_clock_get_now(&mut *clock, &mut time_point);
         }
         Time {
@@ -99,11 +99,9 @@ impl 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
+    /// Helper function to privately initialize a default clock, with the same behavior as
+    /// `rcl_init_generic_clock`. By defining a private function instead of implementing
+    /// `Default`,  we avoid 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();

From 382f780edf87a5190d1fdc3b3ea14d9bd3a25a22 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 18 Sep 2023 16:35:47 +0800
Subject: [PATCH 33/37] Make time_source pub(crate), remove unused APIs

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/lib.rs          |  2 +-
 rclrs/src/node/builder.rs | 16 ++++++++++++++--
 rclrs/src/time_source.rs  | 36 ++++++++----------------------------
 3 files changed, 23 insertions(+), 31 deletions(-)

diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs
index 6fbfcb6a..c5ac5718 100644
--- a/rclrs/src/lib.rs
+++ b/rclrs/src/lib.rs
@@ -43,7 +43,7 @@ pub use rcl_bindings::rmw_request_id_t;
 pub use service::*;
 pub use subscription::*;
 pub use time::*;
-pub use time_source::*;
+use time_source::*;
 pub use wait::*;
 
 /// Polls the node for new messages and executes the corresponding callbacks.
diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index 80eb2a64..ed5c1ed2 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -4,7 +4,7 @@ use std::sync::{Arc, Mutex};
 use crate::rcl_bindings::*;
 use crate::{
     node::call_string_getter_with_handle, resolve_parameter_overrides, ClockType, Context, Node,
-    RclrsError, TimeSource, ToResult,
+    QoSProfile, RclrsError, TimeSource, ToResult, QOS_PROFILE_CLOCK,
 };
 
 /// A builder for creating a [`Node`][1].
@@ -18,6 +18,7 @@ use crate::{
 /// - `arguments: []`
 /// - `enable_rosout: true`
 /// - `clock_type: ClockType::RosTime`
+/// - `clock_qos: QOS_PROFILE_CLOCK`
 ///
 /// # Example
 /// ```
@@ -48,6 +49,7 @@ pub struct NodeBuilder {
     arguments: Vec<String>,
     enable_rosout: bool,
     clock_type: ClockType,
+    clock_qos: QoSProfile,
 }
 
 impl NodeBuilder {
@@ -95,6 +97,7 @@ impl NodeBuilder {
             arguments: vec![],
             enable_rosout: true,
             clock_type: ClockType::RosTime,
+            clock_qos: QOS_PROFILE_CLOCK,
         }
     }
 
@@ -233,6 +236,12 @@ impl NodeBuilder {
         self
     }
 
+    /// Sets the QoSProfile for the clock subscription.
+    pub fn clock_qos(mut self, clock_qos: QoSProfile) -> Self {
+        self.clock_qos = clock_qos;
+        self
+    }
+
     /// Builds the node instance.
     ///
     /// Node name and namespace validation is performed in this method.
@@ -287,7 +296,10 @@ impl NodeBuilder {
             guard_conditions_mtx: Mutex::new(vec![]),
             services_mtx: Mutex::new(vec![]),
             subscriptions_mtx: Mutex::new(vec![]),
-            _time_source: TimeSource::new(weak.clone(), self.clock_type),
+            _time_source: TimeSource::builder(weak.clone(), self.clock_type)
+                .clock_qos(self.clock_qos)
+                .build()
+                .unwrap(),
             _parameter_map,
         }))
     }
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index ec8afebf..81add00b 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -6,14 +6,12 @@ use std::sync::{Arc, Mutex, RwLock, Weak};
 /// Time source for a node that drives the attached clock.
 /// If the node's `use_sim_time` parameter is set to `true`, the `TimeSource` will subscribe
 /// to the `/clock` topic and drive the attached clock
-pub struct TimeSource {
+pub(crate) struct TimeSource {
     _node: Weak<Node>,
     _clock: RwLock<Clock>,
     _clock_source: Arc<Mutex<Option<ClockSource>>>,
     _requested_clock_type: ClockType,
     _clock_qos: QoSProfile,
-    // TODO(luca) implement clock threads, for now will run in main thread
-    _use_clock_thread: bool,
     _clock_subscription: Option<Arc<Subscription<ClockMsg>>>,
     _last_received_time: Arc<Mutex<Option<i64>>>,
 }
@@ -25,47 +23,35 @@ pub struct TimeSource {
 ///
 /// The default values for optional fields are:
 /// - `clock_qos: QOS_PROFILE_CLOCK`[3]
-/// - `use_clock_thread: true`
 ///
 ///
 /// [1]: crate::TimeSource
 /// [2]: crate::TimeSource::builder
 /// [3]: crate::QOS_PROFILE_CLOCK
-pub struct TimeSourceBuilder {
+pub(crate) struct TimeSourceBuilder {
     node: Weak<Node>,
     clock_qos: QoSProfile,
-    use_clock_thread: bool,
     clock_type: ClockType,
 }
 
 impl TimeSourceBuilder {
     /// Creates a builder for a time source that drives the given clock for the given node.
-    pub fn new(node: Weak<Node>, clock_type: ClockType) -> Self {
+    pub(crate) fn new(node: Weak<Node>, clock_type: ClockType) -> Self {
         Self {
             node,
             clock_qos: QOS_PROFILE_CLOCK,
-            use_clock_thread: true,
             clock_type,
         }
     }
 
     /// Sets the QoS for the `/clock` topic.
-    pub fn clock_qos(mut self, clock_qos: QoSProfile) -> Self {
+    pub(crate) fn clock_qos(mut self, clock_qos: QoSProfile) -> Self {
         self.clock_qos = clock_qos;
         self
     }
 
-    /// Sets use_clock_thread.
-    ///
-    /// If set to `true`, the clock callbacks will run in a separate thread.
-    /// NOTE: Currently unimplemented
-    pub fn use_clock_thread(mut self, use_clock_thread: bool) -> Self {
-        self.use_clock_thread = use_clock_thread;
-        self
-    }
-
     /// Builds the `TimeSource` and attaches the provided `Node` and `Clock`.
-    pub fn build(self) -> Result<TimeSource, RclrsError> {
+    pub(crate) fn build(self) -> Result<TimeSource, RclrsError> {
         let clock = match self.clock_type {
             ClockType::RosTime | ClockType::SystemTime => Clock::system(),
             ClockType::SteadyTime => Clock::steady(),
@@ -76,7 +62,6 @@ impl TimeSourceBuilder {
             _clock_source: Arc::new(Mutex::new(None)),
             _requested_clock_type: self.clock_type,
             _clock_qos: self.clock_qos,
-            _use_clock_thread: self.use_clock_thread,
             _clock_subscription: None,
             _last_received_time: Arc::new(Mutex::new(None)),
         };
@@ -86,24 +71,19 @@ impl TimeSourceBuilder {
 }
 
 impl TimeSource {
-    /// Creates a new `TimeSource` with default parameters.
-    pub fn new(node: Weak<Node>, clock_type: ClockType) -> Self {
-        TimeSourceBuilder::new(node, clock_type).build().unwrap()
-    }
-
     /// Creates a new `TimeSourceBuilder` with default parameters.
-    pub fn builder(node: Weak<Node>, clock_type: ClockType) -> TimeSourceBuilder {
+    pub(crate) fn builder(node: Weak<Node>, clock_type: ClockType) -> TimeSourceBuilder {
         TimeSourceBuilder::new(node, clock_type)
     }
 
     /// Returns the clock that this TimeSource is controlling.
-    pub fn get_clock(&self) -> Clock {
+    pub(crate) fn get_clock(&self) -> Clock {
         self._clock.read().unwrap().clone()
     }
 
     /// Attaches the given node to to the `TimeSource`, using its interface to read the
     /// `use_sim_time` parameter and create the clock subscription.
-    pub fn attach_node(&mut self, node: Weak<Node>) {
+    fn attach_node(&mut self, node: Weak<Node>) {
         self._node = node;
 
         // TODO(luca) register a parameter callback

From e66ff75c5cc76c4eb31333a476b7bd2e759092bc Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 30 Oct 2023 13:41:24 +0800
Subject: [PATCH 34/37] Use MandatoryParameter for use_sim_time

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/time_source.rs | 16 ++++++----------
 1 file changed, 6 insertions(+), 10 deletions(-)

diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index 7600c2f8..eddab322 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -14,8 +14,7 @@ pub(crate) struct TimeSource {
     _clock_qos: QoSProfile,
     _clock_subscription: Mutex<Option<Arc<Subscription<ClockMsg>>>>,
     _last_received_time: Arc<Mutex<Option<i64>>>,
-    // TODO(luca) MandatoryParameter here
-    _use_sim_time: bool,
+    _use_sim_time: Mutex<Option<MandatoryParameter<bool>>>,
 }
 
 /// A builder for creating a [`TimeSource`][1].
@@ -64,7 +63,7 @@ impl TimeSourceBuilder {
             _clock_qos: self.clock_qos,
             _clock_subscription: Mutex::new(None),
             _last_received_time: Arc::new(Mutex::new(None)),
-            _use_sim_time: false,
+            _use_sim_time: Mutex::new(None),
         };
         Ok(source)
     }
@@ -84,20 +83,17 @@ impl TimeSource {
     /// Attaches the given node to to the `TimeSource`, using its interface to read the
     /// `use_sim_time` parameter and create the clock subscription.
     pub(crate) fn attach_node(&self, node: Weak<Node>) {
-        *self._node.lock().unwrap() = node;
-
         // TODO(luca) register a parameter callback
-        let val = self
-            ._node
-            .lock()
-            .unwrap()
+        let param = node
             .upgrade()
             .unwrap()
             .declare_parameter("use_sim_time")
             .default(false)
             .mandatory()
             .unwrap();
-        self.set_ros_time_enable(val.get());
+        self.set_ros_time_enable(param.get());
+        *self._node.lock().unwrap() = node;
+        *self._use_sim_time.lock().unwrap() = Some(param);
     }
 
     fn set_ros_time_enable(&self, enable: bool) {

From 7e7c6d9e3d879ae3b5c790e102ecf3b733783e8f Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 30 Oct 2023 13:48:13 +0800
Subject: [PATCH 35/37] Remove error variant from clock builder

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs        | 28 +++++++++++++++-------------
 rclrs/src/node/builder.rs |  3 +--
 rclrs/src/time_source.rs  | 17 ++++++++---------
 3 files changed, 24 insertions(+), 24 deletions(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index 9b74d59f..fe8df420 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -1,5 +1,5 @@
 use crate::rcl_bindings::*;
-use crate::{error::ToResult, time::Time, to_rclrs_result, RclrsError};
+use crate::{error::ToResult, time::Time, to_rclrs_result};
 use std::sync::{Arc, Mutex};
 
 /// Enum to describe clock type. Redefined for readability and to eliminate the uninitialized case
@@ -41,43 +41,45 @@ pub struct ClockSource {
 
 impl Clock {
     /// Creates a new Clock with `ClockType::SystemTime`
-    pub fn system() -> Result<Self, RclrsError> {
+    pub fn system() -> Self {
         Self::make(ClockType::SystemTime)
     }
 
     /// Creates a new Clock with `ClockType::SteadyTime`
-    pub fn steady() -> Result<Self, RclrsError> {
+    pub fn steady() -> Self {
         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)?;
+    pub fn with_source() -> (Self, ClockSource) {
+        let clock = Self::make(ClockType::RosTime);
         let clock_source = ClockSource::new(clock._rcl_clock.clone());
-        Ok((clock, clock_source))
+        (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_)?;
+    pub fn new(type_: ClockType) -> (Self, Option<ClockSource>) {
+        let clock = Self::make(type_);
         let clock_source =
             matches!(type_, ClockType::RosTime).then(|| ClockSource::new(clock._rcl_clock.clone()));
-        Ok((clock, clock_source))
+        (clock, clock_source)
     }
 
-    fn make(type_: ClockType) -> Result<Self, RclrsError> {
+    fn make(type_: ClockType) -> Self {
         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()?;
+            // Function will return Err(_) only if there isn't enough memory to allocate a clock
+            // object.
+            rcl_clock_init(type_.into(), &mut rcl_clock, &mut allocator).ok().unwrap();
         }
-        Ok(Self {
+        Self {
             _type: type_,
             _rcl_clock: Arc::new(Mutex::new(rcl_clock)),
-        })
+        }
     }
 
     /// Returns the clock's `ClockType`.
diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index 70b1ebcf..ae5cf834 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -295,8 +295,7 @@ impl NodeBuilder {
             subscriptions_mtx: Mutex::new(vec![]),
             _time_source: TimeSource::builder(self.clock_type)
                 .clock_qos(self.clock_qos)
-                .build()
-                .unwrap(),
+                .build(),
             _parameter,
         });
         node._time_source.attach_node(Arc::downgrade(&node));
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index eddab322..b2d940d8 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -1,5 +1,5 @@
 use crate::clock::{Clock, ClockSource, ClockType};
-use crate::{MandatoryParameter, Node, QoSProfile, RclrsError, Subscription, QOS_PROFILE_CLOCK};
+use crate::{MandatoryParameter, Node, QoSProfile, Subscription, QOS_PROFILE_CLOCK};
 use rosgraph_msgs::msg::Clock as ClockMsg;
 use std::sync::{Arc, Mutex, RwLock, Weak};
 
@@ -50,11 +50,11 @@ impl TimeSourceBuilder {
     }
 
     /// Builds the `TimeSource` and attaches the provided `Node` and `Clock`.
-    pub(crate) fn build(self) -> Result<TimeSource, RclrsError> {
+    pub(crate) fn build(self) -> TimeSource {
         let clock = match self.clock_type {
             ClockType::RosTime | ClockType::SystemTime => Clock::system(),
             ClockType::SteadyTime => Clock::steady(),
-        }?;
+        };
         let source = TimeSource {
             _node: Mutex::new(Weak::new()),
             _clock: RwLock::new(clock),
@@ -65,7 +65,7 @@ impl TimeSourceBuilder {
             _last_received_time: Arc::new(Mutex::new(None)),
             _use_sim_time: Mutex::new(None),
         };
-        Ok(source)
+        source
     }
 }
 
@@ -83,7 +83,8 @@ impl TimeSource {
     /// Attaches the given node to to the `TimeSource`, using its interface to read the
     /// `use_sim_time` parameter and create the clock subscription.
     pub(crate) fn attach_node(&self, node: Weak<Node>) {
-        // TODO(luca) register a parameter callback
+        // TODO(luca) register a parameter callback that calls set_ros_time(bool) once parameter
+        // callbacks are implemented.
         let param = node
             .upgrade()
             .unwrap()
@@ -100,8 +101,7 @@ impl TimeSource {
         if matches!(self._requested_clock_type, ClockType::RosTime) {
             let mut clock = self._clock.write().unwrap();
             if enable && matches!(clock.clock_type(), ClockType::SystemTime) {
-                // TODO(luca) remove unwrap here?
-                let (new_clock, mut clock_source) = Clock::with_source().unwrap();
+                let (new_clock, mut clock_source) = Clock::with_source();
                 if let Some(last_received_time) = *self._last_received_time.lock().unwrap() {
                     Self::update_clock(&mut clock_source, last_received_time);
                 }
@@ -110,8 +110,7 @@ impl TimeSource {
                 *self._clock_subscription.lock().unwrap() = Some(self.create_clock_sub());
             }
             if !enable && matches!(clock.clock_type(), ClockType::RosTime) {
-                // TODO(luca) remove unwrap here?
-                *clock = Clock::system().unwrap();
+                *clock = Clock::system();
                 *self._clock_source.lock().unwrap() = None;
                 *self._clock_subscription.lock().unwrap() = None;
             }

From 26d5dd2d0e2989610d65a8babe2bcccb54eb750f Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 30 Oct 2023 14:13:27 +0800
Subject: [PATCH 36/37] Add integration test for use_sim_time = true

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs       |  8 +++++---
 rclrs/src/time.rs        |  8 ++++----
 rclrs/src/time_source.rs | 23 +++++++++++++++++------
 3 files changed, 26 insertions(+), 13 deletions(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index fe8df420..c89705d9 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -74,7 +74,9 @@ impl Clock {
             let mut allocator = rcutils_get_default_allocator();
             // Function will return Err(_) only if there isn't enough memory to allocate a clock
             // object.
-            rcl_clock_init(type_.into(), &mut rcl_clock, &mut allocator).ok().unwrap();
+            rcl_clock_init(type_.into(), &mut rcl_clock, &mut allocator)
+                .ok()
+                .unwrap();
         }
         Self {
             _type: type_,
@@ -204,13 +206,13 @@ mod tests {
 
     #[test]
     fn clock_system_time_now() {
-        let clock = Clock::system().unwrap();
+        let clock = Clock::system();
         assert!(clock.now().nsec > 0);
     }
 
     #[test]
     fn clock_ros_time_with_override() {
-        let (clock, source) = Clock::with_source().unwrap();
+        let (clock, source) = Clock::with_source();
         // No manual time set, it should default to 0
         assert!(clock.now().nsec == 0);
         let set_time = 1234i64;
diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs
index 4227a0d2..848981aa 100644
--- a/rclrs/src/time.rs
+++ b/rclrs/src/time.rs
@@ -56,7 +56,7 @@ mod tests {
 
     #[test]
     fn compare_times_from_same_clock() {
-        let clock = Clock::system().unwrap();
+        let clock = Clock::system();
         let t1 = clock.now();
         std::thread::sleep(std::time::Duration::from_micros(1));
         let t2 = clock.now();
@@ -67,8 +67,8 @@ mod tests {
     #[test]
     fn compare_times_from_different_clocks() {
         // Times from different clocks, even if of the same type, can't be compared
-        let c1 = Clock::system().unwrap();
-        let c2 = Clock::system().unwrap();
+        let c1 = Clock::system();
+        let c2 = Clock::system();
         let t1 = c1.now();
         let t2 = c2.now();
         assert!(t2.compare_with(&t1, |_, _| ()).is_none());
@@ -77,7 +77,7 @@ mod tests {
 
     #[test]
     fn add_duration_to_time() {
-        let (clock, _) = Clock::with_source().unwrap();
+        let (clock, _) = Clock::with_source();
         let t = clock.now();
         let t2 = t.clone() + Duration::from_secs(1);
         assert_eq!(t2.nsec - t.nsec, 1_000_000_000i64);
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index b2d940d8..906155ed 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -55,7 +55,7 @@ impl TimeSourceBuilder {
             ClockType::RosTime | ClockType::SystemTime => Clock::system(),
             ClockType::SteadyTime => Clock::steady(),
         };
-        let source = TimeSource {
+        TimeSource {
             _node: Mutex::new(Weak::new()),
             _clock: RwLock::new(clock),
             _clock_source: Arc::new(Mutex::new(None)),
@@ -64,8 +64,7 @@ impl TimeSourceBuilder {
             _clock_subscription: Mutex::new(None),
             _last_received_time: Arc::new(Mutex::new(None)),
             _use_sim_time: Mutex::new(None),
-        };
-        source
+        }
     }
 }
 
@@ -92,8 +91,8 @@ impl TimeSource {
             .default(false)
             .mandatory()
             .unwrap();
-        self.set_ros_time_enable(param.get());
         *self._node.lock().unwrap() = node;
+        self.set_ros_time_enable(param.get());
         *self._use_sim_time.lock().unwrap() = Some(param);
     }
 
@@ -147,10 +146,22 @@ mod tests {
     use crate::{create_node, Context};
 
     #[test]
-    fn time_source_attach_clock() {
+    fn time_source_default_clock() {
         let node = create_node(&Context::new([]).unwrap(), "test_node").unwrap();
         // Default clock should be above 0 (use_sim_time is default false)
         assert!(node.get_clock().now().nsec > 0);
-        // TODO(luca) an integration test by creating a node and setting its use_sim_time
+    }
+
+    #[test]
+    fn time_source_sim_time() {
+        let ctx = Context::new([
+            String::from("--ros-args"),
+            String::from("-p"),
+            String::from("use_sim_time:=true"),
+        ])
+        .unwrap();
+        let node = create_node(&ctx, "test_node").unwrap();
+        // Default sim time value should be 0 (no message received)
+        assert_eq!(node.get_clock().now().nsec, 0);
     }
 }

From 3a9b9334737b0f4522de001082b12cb724ed1114 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 30 Oct 2023 14:24:54 +0800
Subject: [PATCH 37/37] Minor cleanup for attach_node

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/node/builder.rs | 2 +-
 rclrs/src/time_source.rs  | 6 ++----
 2 files changed, 3 insertions(+), 5 deletions(-)

diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs
index ae5cf834..543bd50d 100644
--- a/rclrs/src/node/builder.rs
+++ b/rclrs/src/node/builder.rs
@@ -298,7 +298,7 @@ impl NodeBuilder {
                 .build(),
             _parameter,
         });
-        node._time_source.attach_node(Arc::downgrade(&node));
+        node._time_source.attach_node(&node);
         Ok(node)
     }
 
diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs
index 906155ed..d2aac431 100644
--- a/rclrs/src/time_source.rs
+++ b/rclrs/src/time_source.rs
@@ -81,17 +81,15 @@ impl TimeSource {
 
     /// Attaches the given node to to the `TimeSource`, using its interface to read the
     /// `use_sim_time` parameter and create the clock subscription.
-    pub(crate) fn attach_node(&self, node: Weak<Node>) {
+    pub(crate) fn attach_node(&self, node: &Arc<Node>) {
         // TODO(luca) register a parameter callback that calls set_ros_time(bool) once parameter
         // callbacks are implemented.
         let param = node
-            .upgrade()
-            .unwrap()
             .declare_parameter("use_sim_time")
             .default(false)
             .mandatory()
             .unwrap();
-        *self._node.lock().unwrap() = node;
+        *self._node.lock().unwrap() = Arc::downgrade(node);
         self.set_ros_time_enable(param.get());
         *self._use_sim_time.lock().unwrap() = Some(param);
     }