Merge in Timer features

Signed-off-by: Michael X. Grey <[email protected]>

Author: mxgrey

examples/rclrs_timer_demo/Cargo.toml

@@ -0,0 +1,12 @@
+[package]
+name = "rclrs_timer_demo"
+version = "0.1.0"
+edition = "2021"
+
+[[bin]]
+name="rclrs_timer_demo"
+path="src/rclrs_timer_demo.rs"
+
+
+[dependencies]
+rclrs = "*"

examples/rclrs_timer_demo/package.xml

@@ -0,0 +1,13 @@
+<package format="3">
+  <name>rclrs_timer_demo</name>
+  <version>0.1.0</version>
+  <description>Shows how to implement a timer within a Node using rclrs.</description>
+  <maintainer email="[email protected]">user</maintainer>
+  <license>TODO: License declaration.</license>
+
+  <depend>rclrs</depend>
+
+  <export>
+    <build_type>ament_cargo</build_type>
+  </export>
+</package>

examples/rclrs_timer_demo/src/rclrs_timer_demo.rs

@@ -0,0 +1,46 @@
+/// Creates a SimpleTimerNode, initializes a node and the timer with a callback
+/// that prints the timer callback execution iteration. The callback is executed
+/// thanks to the spin, which is in charge of executing the timer's events among
+/// other entities' events.
+use rclrs::{create_node, Context, Node, RclrsError, Timer};
+use std::{
+    env,
+    sync::Arc,
+    time::Duration,
+};
+
+/// Contains both the node and timer.
+struct SimpleTimerNode {
+    node: Arc<Node>,
+    #[allow(unused)]
+    timer: Arc<Timer>,
+}
+
+impl SimpleTimerNode {
+    /// Creates a node and a timer with a callback.
+    ///
+    /// The callback will simply print to stdout:
+    /// "Drinking 🧉 for the xth time every p nanoseconds."
+    /// where x is the iteration callback counter and p is the period of the timer.
+    fn new(context: &Context, timer_period: Duration) -> Result<Self, RclrsError> {
+        let node = create_node(context, "simple_timer_node")?;
+        let mut x = 0;
+        let timer = node.create_timer_repeating(
+            timer_period,
+            move || {
+                x += 1;
+                println!(
+                    "Drinking 🧉 for the {x}th time every {:?}.",
+                    timer_period,
+                );
+            },
+        )?;
+        Ok(Self { node, timer })
+    }
+}
+
+fn main() -> Result<(), RclrsError> {
+    let context = Context::new(env::args()).unwrap();
+    let simple_timer_node = Arc::new(SimpleTimerNode::new(&context, Duration::from_secs(1)).unwrap());
+    rclrs::spin(simple_timer_node.node.clone())
+}

rclrs/src/clock.rs

@@ -66,21 +66,31 @@ impl Clock {
     }
 
     fn make(kind: ClockType) -> Self {
-        let mut rcl_clock;
+        let rcl_clock;
         unsafe {
             // SAFETY: Getting a default value is always safe.
-            rcl_clock = Self::init_generic_clock();
+            let allocator = rcutils_get_default_allocator();
+            rcl_clock = Arc::new(Mutex::new(rcl_clock_t {
+                type_: rcl_clock_type_t::RCL_CLOCK_UNINITIALIZED,
+                jump_callbacks: std::ptr::null_mut(),
+                num_jump_callbacks: 0,
+                get_now: None,
+                data: std::ptr::null_mut::<std::os::raw::c_void>(),
+                allocator,
+            }));
             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(kind.into(), &mut rcl_clock, &mut allocator)
+            rcl_clock_init(kind.into(), &mut *rcl_clock.lock().unwrap(), &mut allocator)
                 .ok()
                 .unwrap();
         }
-        Self {
-            kind,
-            rcl_clock: Arc::new(Mutex::new(rcl_clock)),
-        }
+        Self { kind, rcl_clock }
+    }
+
+    /// Returns the clock's `rcl_clock_t`.
+    pub(crate) fn get_rcl_clock(&self) -> &Arc<Mutex<rcl_clock_t>> {
+        &self.rcl_clock
     }
 
     /// Returns the clock's `ClockType`.
@@ -101,22 +111,6 @@ impl Clock {
             clock: Arc::downgrade(&self.rcl_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();
-        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 ClockSource {

rclrs/src/error.rs

@@ -32,6 +32,10 @@ pub enum RclrsError {
     },
     /// It was attempted to add a waitable to a wait set twice.
     AlreadyAddedToWaitSet,
+    /// A negative duration was obtained from rcl which should have been positive.
+    ///
+    /// The value represents nanoseconds.
+    NegativeDuration(i64),
     /// The guard condition that you tried to trigger is not owned by the
     /// [`GuardCondition`][crate::GuardCondition] instance.
     UnownedGuardCondition,
@@ -84,6 +88,12 @@ impl Display for RclrsError {
                     "Could not add entity to wait set because it was already added to a wait set"
                 )
             }
+            RclrsError::NegativeDuration(duration) => {
+                write!(
+                    f,
+                    "A duration was negative when it should not have been: {duration:?}"
+                )
+            }
             RclrsError::UnownedGuardCondition => {
                 write!(
                     f,
@@ -130,6 +140,7 @@ impl Error for RclrsError {
             // TODO(@mxgrey): We should provide source information for these other types.
             // It should be easy to do this using the thiserror crate.
             RclrsError::AlreadyAddedToWaitSet => None,
+            RclrsError::NegativeDuration(_) => None,
             RclrsError::UnownedGuardCondition => None,
             RclrsError::InvalidPayload { .. } => None,
         }

rclrs/src/lib.rs

@@ -20,6 +20,7 @@ mod service;
 mod subscription;
 mod time;
 mod time_source;
+mod timer;
 mod vendor;
 mod wait_set;
 mod worker;
@@ -48,5 +49,6 @@ pub use service::*;
 pub use subscription::*;
 pub use time::*;
 use time_source::*;
+pub use timer::*;
 pub use wait_set::*;
 pub use worker::*;

rclrs/src/node.rs

@@ -34,7 +34,8 @@ use crate::{
     Publisher, PublisherOptions, PublisherState, RclrsError, Service, IntoAsyncServiceCallback,
     IntoNodeServiceCallback, ServiceOptions, ServiceState, Subscription, IntoAsyncSubscriptionCallback,
     IntoNodeSubscriptionCallback, SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams,
-    ENTITY_LIFECYCLE_MUTEX, IntoWorkerOptions, Worker, WorkerState,
+    ENTITY_LIFECYCLE_MUTEX, IntoWorkerOptions, Worker, WorkerState, IntoTimerOptions, AnyTimerCallback,
+    Timer, IntoNodeTimerRepeatingCallback, IntoNodeTimerOneshotCallback, TimerState,
 };
 
 /// A processing unit that can communicate with other nodes.
@@ -244,7 +245,7 @@ impl NodeState {
         options: impl IntoWorkerOptions<Payload>,
     ) -> Worker<Payload>
     where
-        Payload: 'static + Send,
+        Payload: 'static + Send + Sync,
     {
         let options = options.into_worker_options();
         let commands = self.commands.create_worker_commands(Box::new(options.payload));
@@ -555,6 +556,70 @@ impl NodeState {
         )
     }
 
+    /// Create a [`Timer`] with a repeating callback.
+    ///
+    /// See also:
+    /// * [`Self::create_timer_oneshot`]
+    /// * [`Self::create_timer_inert`]
+    pub fn create_timer_repeating<'a, Args>(
+        &self,
+        options: impl IntoTimerOptions<'a>,
+        callback: impl IntoNodeTimerRepeatingCallback<Args>,
+    ) -> Result<Timer, RclrsError> {
+        self.create_timer(options, callback.into_node_timer_repeating_callback())
+    }
+
+    /// Create a [`Timer`] whose callback will be triggered once after the period
+    /// of the timer has elapsed. After that you will need to use
+    /// [`Timer::set_callback`] or a related method or else nothing will happen
+    /// the following times that the `Timer` elapses.
+    ///
+    /// See also:
+    /// * [`Self::create_timer_repeating`]
+    /// * [`Self::create_time_inert`]
+    pub fn create_timer_oneshot<'a, Args>(
+        &self,
+        options: impl IntoTimerOptions<'a>,
+        callback: impl IntoNodeTimerOneshotCallback<Args>,
+    ) -> Result<Timer, RclrsError> {
+        self.create_timer(options, callback.into_node_timer_oneshot_callback())
+    }
+
+    /// Create a [`Timer`] without a callback. Nothing will happen when this
+    /// `Timer` elapses until you use [`Timer::set_callback`] or a related method.
+    ///
+    /// See also:
+    /// * [`Self::create_timer_repeating`]
+    /// * [`Self::create_timer_oneshot`]
+    pub fn create_timer_inert<'a>(
+        &self,
+        options: impl IntoTimerOptions<'a>,
+    ) -> Result<Timer, RclrsError> {
+        self.create_timer(options, AnyTimerCallback::Inert)
+    }
+
+    /// Used internally to create a [`Timer`].
+    ///
+    /// Downstream users should instead use:
+    /// * [`Self::create_timer_repeating`]
+    /// * [`Self::create_timer_oneshot`]
+    /// * [`Self::create_timer_inert`]
+    fn create_timer<'a>(
+        &self,
+        options: impl IntoTimerOptions<'a>,
+        callback: AnyTimerCallback<Node>,
+    ) -> Result<Timer, RclrsError> {
+        let options = options.into_timer_options();
+        let clock = options.clock.as_clock(self);
+        TimerState::create(
+            options.period,
+            clock,
+            callback,
+            self.commands.async_worker_commands(),
+            &self.handle.context_handle,
+        )
+    }
+
     /// Returns the ROS domain ID that the node is using.
     ///
     /// The domain ID controls which nodes can send messages to each other, see the [ROS 2 concept article][1].
@@ -748,6 +813,11 @@ unsafe impl Send for rcl_node_t {}
 mod tests {
     use crate::{test_helpers::*, *};
 
+    use std::{
+        time::Duration,
+        sync::{Arc, atomic::{AtomicU64, Ordering}},
+    };
+
     #[test]
     fn traits() {
         assert_send::<NodeState>();
@@ -794,6 +864,68 @@ mod tests {
         Ok(())
     }
 
+    #[test]
+    fn test_create_timer() -> Result<(), RclrsError> {
+        dbg!();
+        let mut executor = Context::default().create_basic_executor();
+        let node = executor.create_node("node_with_timer")?;
+
+        let repeat_counter = Arc::new(AtomicU64::new(0));
+        let repeat_counter_check = Arc::clone(&repeat_counter);
+        let _repeating_timer = node.create_timer_repeating(
+            Duration::from_millis(1),
+            move || {
+                dbg!();
+                repeat_counter.fetch_add(1, Ordering::AcqRel);
+            },
+        )?;
+
+        let oneshot_counter = Arc::new(AtomicU64::new(0));
+        let oneshot_counter_check = Arc::clone(&oneshot_counter);
+        let _oneshot_timer = node.create_timer_oneshot(
+            Duration::from_millis(1)
+            .node_time(),
+            move || {
+                dbg!();
+                oneshot_counter.fetch_add(1, Ordering::AcqRel);
+            },
+        )?;
+
+        let oneshot_resetting_counter = Arc::new(AtomicU64::new(0));
+        let oneshot_resetting_counter_check = Arc::clone(&oneshot_resetting_counter);
+        let _oneshot_resetting_timer = node.create_timer_oneshot(
+            Duration::from_millis(1),
+            move |timer: &Timer| {
+                dbg!();
+                recursive_oneshot(timer, oneshot_resetting_counter);
+            },
+        );
+
+        dbg!();
+        executor.spin(SpinOptions::new().timeout(Duration::from_millis(10)));
+
+        // We give a little leeway to the exact count since timers won't always
+        // be triggered perfectly. The important thing is that it was
+        // successfully called repeatedly.
+        assert!(repeat_counter_check.load(Ordering::Acquire) > 5);
+        assert!(oneshot_resetting_counter_check.load(Ordering::Acquire) > 5);
+
+        // This should only have been triggered exactly once
+        assert_eq!(oneshot_counter_check.load(Ordering::Acquire), 1);
+
+        Ok(())
+    }
+
+    fn recursive_oneshot(
+        timer: &Timer,
+        counter: Arc<AtomicU64>,
+    ) {
+        counter.fetch_add(1, Ordering::AcqRel);
+        timer.set_oneshot(move |timer: &Timer| {
+            recursive_oneshot(timer, counter);
+        });
+    }
+
     #[test]
     fn test_logger_name() -> Result<(), RclrsError> {
         // Use helper to create 2 nodes for us

rclrs/src/rcl_bindings.rs

@@ -89,6 +89,10 @@ cfg_if::cfg_if! {
         #[derive(Debug)]
         pub struct rcl_wait_set_t;
 
+        #[repr(C)]
+        #[derive(Debug)]
+        pub struct rcl_timer_t;
+
         #[repr(C)]
         #[derive(Debug)]
         pub struct rcutils_string_array_t;