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 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>, 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> { + /// 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, - _clock: Arc>, + _clock: RwLock, _clock_source: Arc>>, _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> { - 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 } }