diff --git a/examples/timer_demo/Cargo.toml b/examples/timer_demo/Cargo.toml new file mode 100644 index 00000000..b4356436 --- /dev/null +++ b/examples/timer_demo/Cargo.toml @@ -0,0 +1,8 @@ +[package] +name = "examples_timer_demo" +version = "0.1.0" +edition = "2021" + +[dependencies] +rclrs = "0.4" +example_interfaces = "*" diff --git a/examples/timer_demo/package.xml b/examples/timer_demo/package.xml new file mode 100644 index 00000000..684cea8f --- /dev/null +++ b/examples/timer_demo/package.xml @@ -0,0 +1,21 @@ + + + + examples_timer_demo + Esteve Fernandez + + Jacob Hassold + 0.4.1 + Package containing an example of how to use a worker in rclrs. + Apache License 2.0 + + rclrs + rosidl_runtime_rs + example_interfaces + + + ament_cargo + + diff --git a/examples/timer_demo/src/main.rs b/examples/timer_demo/src/main.rs new file mode 100644 index 00000000..608a82f1 --- /dev/null +++ b/examples/timer_demo/src/main.rs @@ -0,0 +1,22 @@ +/// 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::*; +use std::time::Duration; + +fn main() -> Result<(), RclrsError> { + let mut executor = Context::default_from_env()?.create_basic_executor(); + let node = executor.create_node("timer_demo")?; + let worker = node.create_worker::(0); + let timer_period = Duration::from_secs(1); + let _timer = worker.create_timer_repeating(timer_period, move |count: &mut usize| { + *count += 1; + println!( + "Drinking 🧉 for the {}th time every {:?}.", + *count, timer_period, + ); + })?; + + executor.spin(SpinOptions::default()).first_error() +} diff --git a/examples/worker_demo/src/main.rs b/examples/worker_demo/src/main.rs index 253a95fc..bb3a9761 100644 --- a/examples/worker_demo/src/main.rs +++ b/examples/worker_demo/src/main.rs @@ -1,5 +1,5 @@ use rclrs::*; -use std::sync::Arc; +use std::time::Duration; fn main() -> Result<(), RclrsError> { let mut executor = Context::default_from_env()?.create_basic_executor(); @@ -15,27 +15,12 @@ fn main() -> Result<(), RclrsError> { }, )?; - // // Use this timer-based implementation when timers are available instead - // // of using std::thread::spawn. - // let _timer = worker.create_timer_repeating( - // Duration::from_secs(1), - // move |data: &mut String| { - // let msg = example_interfaces::msg::String { - // data: data.clone() - // }; - - // publisher.publish(msg).ok(); - // } - // )?; - - std::thread::spawn(move || loop { - std::thread::sleep(std::time::Duration::from_secs(1)); - let publisher = Arc::clone(&publisher); - let _ = worker.run(move |data: &mut String| { + let _timer = + worker.create_timer_repeating(Duration::from_secs(1), move |data: &mut String| { let msg = example_interfaces::msg::String { data: data.clone() }; - publisher.publish(msg).unwrap(); - }); - }); + + publisher.publish(msg).ok(); + })?; println!( "Beginning repeater... \n >> \ diff --git a/rclrs/Cargo.lock b/rclrs/Cargo.lock index 5fc5e7bd..834786e1 100644 --- a/rclrs/Cargo.lock +++ b/rclrs/Cargo.lock @@ -4,10 +4,11 @@ version = 3 [[package]] name = "action_msgs" -version = "1.2.1" +version = "2.0.2" dependencies = [ "builtin_interfaces", "rosidl_runtime_rs", + "service_msgs", "unique_identifier_msgs", ] @@ -45,6 +46,126 @@ dependencies = [ "walkdir", ] +[[package]] +name = "async-channel" +version = "1.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "81953c529336010edd6d8e358f886d9581267795c61b19475b71314bffa46d35" +dependencies = [ + "concurrent-queue", + "event-listener 2.5.3", + "futures-core", +] + +[[package]] +name = "async-channel" +version = "2.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "89b47800b0be77592da0afd425cc03468052844aff33b84e33cc696f64e77b6a" +dependencies = [ + "concurrent-queue", + "event-listener-strategy", + "futures-core", + "pin-project-lite", +] + +[[package]] +name = "async-executor" +version = "1.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bb812ffb58524bdd10860d7d974e2f01cc0950c2438a74ee5ec2e2280c6c4ffa" +dependencies = [ + "async-task", + "concurrent-queue", + "fastrand", + "futures-lite", + "pin-project-lite", + "slab", +] + +[[package]] +name = "async-global-executor" +version = "2.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "05b1b633a2115cd122d73b955eadd9916c18c8f510ec9cd1686404c60ad1c29c" +dependencies = [ + "async-channel 2.3.1", + "async-executor", + "async-io", + "async-lock", + "blocking", + "futures-lite", + "once_cell", +] + +[[package]] +name = "async-io" +version = "2.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1237c0ae75a0f3765f58910ff9cdd0a12eeb39ab2f4c7de23262f337f0aacbb3" +dependencies = [ + "async-lock", + "cfg-if", + "concurrent-queue", + "futures-io", + "futures-lite", + "parking", + "polling", + "rustix", + "slab", + "tracing", + "windows-sys", +] + +[[package]] +name = "async-lock" +version = "3.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff6e472cdea888a4bd64f342f09b3f50e1886d32afe8df3d663c01140b811b18" +dependencies = [ + "event-listener 5.4.0", + "event-listener-strategy", + "pin-project-lite", +] + +[[package]] +name = "async-std" +version = "1.13.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "730294c1c08c2e0f85759590518f6333f0d5a0a766a27d519c1b244c3dfd8a24" +dependencies = [ + "async-channel 1.9.0", + "async-global-executor", + "async-io", + "async-lock", + "crossbeam-utils", + "futures-channel", + "futures-core", + "futures-io", + "futures-lite", + "gloo-timers", + "kv-log-macro", + "log", + "memchr", + "once_cell", + "pin-project-lite", + "pin-utils", + "slab", + "wasm-bindgen-futures", +] + +[[package]] +name = "async-task" +version = "4.7.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b75356056920673b02621b35afd0f7dda9306d03c79a30f5c56c44cf256e3de" + +[[package]] +name = "atomic-waker" +version = "1.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1505bd5d3d116872e7271a6d4e16d81d0c8570876c8de68093a09ac269d8aac0" + [[package]] name = "autocfg" version = "1.4.0" @@ -92,13 +213,32 @@ version = "2.9.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1b8e56985ec62d17e9c1001dc89c88ecd7dc08e47eba5ec7c29c7b5eeecde967" +[[package]] +name = "blocking" +version = "1.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "703f41c54fc768e63e091340b424302bb1c29ef4aa0c7f10fe849dfb114d29ea" +dependencies = [ + "async-channel 2.3.1", + "async-task", + "futures-io", + "futures-lite", + "piper", +] + [[package]] name = "builtin_interfaces" -version = "1.2.1" +version = "2.0.2" dependencies = [ "rosidl_runtime_rs", ] +[[package]] +name = "bumpalo" +version = "3.19.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "46c5e41b57b8bba42a04676d81cb89e9ee8e859a1a66f80a5a72e1cb76b34d43" + [[package]] name = "cexpr" version = "0.6.0" @@ -125,6 +265,21 @@ dependencies = [ "libloading", ] +[[package]] +name = "concurrent-queue" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4ca0197aee26d1ae37445ee532fefce43251d24cc7c166799f4d46817f1d3973" +dependencies = [ + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-utils" +version = "0.8.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d0a5c400df2834b80a4c3327b3aad3a4c4cd4de0629063962b03235697506a28" + [[package]] name = "either" version = "1.15.0" @@ -141,6 +296,44 @@ dependencies = [ "windows-sys", ] +[[package]] +name = "event-listener" +version = "2.5.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0206175f82b8d6bf6652ff7d71a1e27fd2e4efde587fd368662814d6ec1d9ce0" + +[[package]] +name = "event-listener" +version = "5.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3492acde4c3fc54c845eaab3eed8bd00c7a7d881f78bfc801e43a93dec1331ae" +dependencies = [ + "concurrent-queue", + "parking", + "pin-project-lite", +] + +[[package]] +name = "event-listener-strategy" +version = "0.5.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8be9f3dfaaffdae2972880079a491a1a8bb7cbed0b8dd7a347f668b4150a3b93" +dependencies = [ + "event-listener 5.4.0", + "pin-project-lite", +] + +[[package]] +name = "example_interfaces" +version = "0.12.0" +dependencies = [ + "action_msgs", + "builtin_interfaces", + "rosidl_runtime_rs", + "service_msgs", + "unique_identifier_msgs", +] + [[package]] name = "fastrand" version = "2.3.0" @@ -195,6 +388,19 @@ version = "0.3.31" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9e5c1b78ca4aae1ac06c48a526a655760685149f0d465d21f37abfe57ce075c6" +[[package]] +name = "futures-lite" +version = "2.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f5edaec856126859abb19ed65f39e90fea3a9574b9707f13539acf4abf7eb532" +dependencies = [ + "fastrand", + "futures-core", + "futures-io", + "parking", + "pin-project-lite", +] + [[package]] name = "futures-macro" version = "0.3.31" @@ -260,6 +466,24 @@ version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a8d1add55171497b4705a648c6b583acafb01d58050a51727785f0b2c8e0a2b2" +[[package]] +name = "gloo-timers" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bbb143cf96099802033e0d4f4963b19fd2e0b728bcf076cd9cf7f6634f092994" +dependencies = [ + "futures-channel", + "futures-core", + "js-sys", + "wasm-bindgen", +] + +[[package]] +name = "hermit-abi" +version = "0.5.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fc0fef456e4baa96da950455cd02c081ca953b141298e41db3fc7e36b1da849c" + [[package]] name = "itertools" version = "0.8.2" @@ -278,6 +502,25 @@ dependencies = [ "either", ] +[[package]] +name = "js-sys" +version = "0.3.77" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1cfaf33c695fc6e08064efbc1f72ec937429614f25eef83af942d0e227c3a28f" +dependencies = [ + "once_cell", + "wasm-bindgen", +] + +[[package]] +name = "kv-log-macro" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0de8b303297635ad57c9f5059fd9cee7a47f8e8daa09df0fcd07dd39fb22977f" +dependencies = [ + "log", +] + [[package]] name = "libc" version = "0.2.172" @@ -305,6 +548,9 @@ name = "log" version = "0.4.27" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94" +dependencies = [ + "value-bag", +] [[package]] name = "memchr" @@ -352,6 +598,12 @@ version = "1.21.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d" +[[package]] +name = "parking" +version = "2.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f38d5652c16fde515bb1ecef450ab0f6a219d619a7274976324d5e377f7dceba" + [[package]] name = "pin-project-lite" version = "0.2.16" @@ -364,6 +616,32 @@ version = "0.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" +[[package]] +name = "piper" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96c8c490f422ef9a4efd2cb5b42b76c8613d7e7dfc1caf667b8a3350a5acc066" +dependencies = [ + "atomic-waker", + "fastrand", + "futures-io", +] + +[[package]] +name = "polling" +version = "3.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b53a684391ad002dd6a596ceb6c74fd004fdce75f4be2e3f615068abbea5fd50" +dependencies = [ + "cfg-if", + "concurrent-queue", + "hermit-abi", + "pin-project-lite", + "rustix", + "tracing", + "windows-sys", +] + [[package]] name = "prettyplease" version = "0.2.32" @@ -403,8 +681,10 @@ name = "rclrs" version = "0.4.1" dependencies = [ "ament_rs", + "async-std", "bindgen", "cfg-if", + "example_interfaces", "futures", "libloading", "rosidl_runtime_rs", @@ -477,6 +757,12 @@ dependencies = [ "windows-sys", ] +[[package]] +name = "rustversion" +version = "1.0.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8a0d197bd2c9dc6e53b84da9556a69ba4cdfab8619eb41a8bd1cc2027a0f6b1d" + [[package]] name = "same-file" version = "1.0.6" @@ -515,6 +801,14 @@ dependencies = [ "syn", ] +[[package]] +name = "service_msgs" +version = "2.0.2" +dependencies = [ + "builtin_interfaces", + "rosidl_runtime_rs", +] + [[package]] name = "shlex" version = "1.3.0" @@ -556,11 +850,12 @@ dependencies = [ [[package]] name = "test_msgs" -version = "1.2.1" +version = "2.0.2" dependencies = [ "action_msgs", "builtin_interfaces", "rosidl_runtime_rs", + "service_msgs", "unique_identifier_msgs", ] @@ -586,6 +881,22 @@ dependencies = [ "syn", ] +[[package]] +name = "tracing" +version = "0.1.41" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "784e0ac535deb450455cbfa28a6f0df145ea1bb7ae51b821cf5e7927fdcfbdd0" +dependencies = [ + "pin-project-lite", + "tracing-core", +] + +[[package]] +name = "tracing-core" +version = "0.1.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9d12581f227e93f094d3af2ae690a574abb8a2b9b7a96e7cfe9647b2b617678" + [[package]] name = "unicode-ident" version = "1.0.18" @@ -594,11 +905,17 @@ checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512" [[package]] name = "unique_identifier_msgs" -version = "2.2.1" +version = "2.5.0" dependencies = [ "rosidl_runtime_rs", ] +[[package]] +name = "value-bag" +version = "1.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "943ce29a8a743eb10d6082545d861b24f9d1b160b7d741e0f2cdf726bec909c5" + [[package]] name = "walkdir" version = "2.5.0" @@ -618,6 +935,87 @@ dependencies = [ "wit-bindgen-rt", ] +[[package]] +name = "wasm-bindgen" +version = "0.2.100" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1edc8929d7499fc4e8f0be2262a241556cfc54a0bea223790e71446f2aab1ef5" +dependencies = [ + "cfg-if", + "once_cell", + "rustversion", + "wasm-bindgen-macro", +] + +[[package]] +name = "wasm-bindgen-backend" +version = "0.2.100" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2f0a0651a5c2bc21487bde11ee802ccaf4c51935d0d3d42a6101f98161700bc6" +dependencies = [ + "bumpalo", + "log", + "proc-macro2", + "quote", + "syn", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-futures" +version = "0.4.50" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "555d470ec0bc3bb57890405e5d4322cc9ea83cebb085523ced7be4144dac1e61" +dependencies = [ + "cfg-if", + "js-sys", + "once_cell", + "wasm-bindgen", + "web-sys", +] + +[[package]] +name = "wasm-bindgen-macro" +version = "0.2.100" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7fe63fc6d09ed3792bd0897b314f53de8e16568c2b3f7982f468c0bf9bd0b407" +dependencies = [ + "quote", + "wasm-bindgen-macro-support", +] + +[[package]] +name = "wasm-bindgen-macro-support" +version = "0.2.100" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ae87ea40c9f689fc23f209965b6fb8a99ad69aeeb0231408be24920604395de" +dependencies = [ + "proc-macro2", + "quote", + "syn", + "wasm-bindgen-backend", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-shared" +version = "0.2.100" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1a05d73b933a847d6cccdda8f838a22ff101ad9bf93e33684f39c1f5f0eece3d" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "web-sys" +version = "0.3.77" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "33b6dd2ef9186f1f2072e409e99cd22a975331a6b3591b12c764e0e55c60d5d2" +dependencies = [ + "js-sys", + "wasm-bindgen", +] + [[package]] name = "winapi-util" version = "0.1.9" @@ -775,8 +1173,20 @@ dependencies = [ [[patch.unused]] name = "rcl_interfaces" -version = "1.2.1" +version = "2.0.2" + +[[patch.unused]] +name = "rclrs" +version = "0.4.1" + +[[patch.unused]] +name = "rclrs_example_msgs" +version = "0.4.1" [[patch.unused]] name = "rosgraph_msgs" -version = "1.2.1" +version = "2.0.2" + +[[patch.unused]] +name = "std_msgs" +version = "5.3.6" diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index 992cd4b4..8dd54345 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -88,6 +88,11 @@ impl Clock { Self { kind, rcl_clock } } + /// Returns the clock's `rcl_clock_t`. + pub(crate) fn get_rcl_clock(&self) -> &Arc> { + &self.rcl_clock + } + /// Returns the clock's `ClockType`. pub fn clock_type(&self) -> ClockType { self.kind diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 366e499b..f5584fa2 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -189,6 +189,7 @@ mod service; mod subscription; mod time; mod time_source; +mod timer; mod vendor; mod wait_set; mod worker; @@ -217,5 +218,6 @@ pub use service::*; pub use subscription::*; pub use time::*; use time_source::*; +pub use timer::*; pub use wait_set::*; pub use worker::*; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index dd01d060..777e3898 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -29,12 +29,14 @@ use async_std::future::timeout; use rosidl_runtime_rs::Message; use crate::{ - rcl_bindings::*, Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands, - IntoAsyncServiceCallback, IntoAsyncSubscriptionCallback, IntoNodeServiceCallback, - IntoNodeSubscriptionCallback, LogParams, Logger, ParameterBuilder, ParameterInterface, - ParameterVariant, Parameters, Promise, Publisher, PublisherOptions, PublisherState, RclrsError, - Service, ServiceOptions, ServiceState, Subscription, SubscriptionOptions, SubscriptionState, - TimeSource, ToLogParams, Worker, WorkerOptions, WorkerState, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, AnyTimerCallback, Client, ClientOptions, ClientState, Clock, ContextHandle, + ExecutorCommands, IntoAsyncServiceCallback, IntoAsyncSubscriptionCallback, + IntoNodeServiceCallback, IntoNodeSubscriptionCallback, IntoNodeTimerOneshotCallback, + IntoNodeTimerRepeatingCallback, IntoTimerOptions, LogParams, Logger, ParameterBuilder, + ParameterInterface, ParameterVariant, Parameters, Promise, Publisher, PublisherOptions, + PublisherState, RclrsError, Service, ServiceOptions, ServiceState, Subscription, + SubscriptionOptions, SubscriptionState, TimeSource, Timer, TimerState, ToLogParams, Worker, + WorkerOptions, WorkerState, ENTITY_LIFECYCLE_MUTEX, }; /// A processing unit that can communicate with other nodes. See the API of @@ -893,6 +895,214 @@ impl NodeState { ) } + /// Create a [`Timer`] with a repeating callback. + /// + /// See also: + /// * [`Self::create_timer_oneshot`] + /// * [`Self::create_timer_inert`] + /// + /// # Behavior + /// + /// While the callback of this timer is running, no other callbacks associated + /// with this node will be able to run. This is in contrast to callbacks given + /// to [`Self::create_subscription`] which can run multiple times in parallel. + /// + /// Since the callback of this timer may block other callbacks from being able + /// to run, it is strongly recommended to ensure that the callback returns + /// quickly. If the callback needs to trigger long-running behavior then you + /// can consider using [`std::thread::spawn`], or for async behaviors you can + /// capture an [`ExecutorCommands`] in your callback and use [`ExecutorCommands::run`] + /// to issue a task for the executor to run in its async task pool. + /// + /// Since these callbacks are blocking, you may use [`FnMut`] here instead of + /// being limited to [`Fn`]. + /// + /// # Timer Options + /// + /// You can choose both + /// 1. a timer period (duration) which determines how often the callback is triggered + /// 2. a clock to measure the passage of time + /// + /// Both of these choices are expressed by [`TimerOptions`][1]. + /// + /// By default the steady clock time will be used, but you could choose + /// node time instead if you want the timer to automatically use simulated + /// time when running as part of a simulation: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// use std::time::Duration; + /// + /// let timer = node.create_timer_repeating( + /// TimerOptions::new(Duration::from_secs(1)) + /// .node_time(), + /// || { + /// println!("Triggering once each simulated second"); + /// }, + /// )?; + /// # Ok::<(), RclrsError>(()) + /// ``` + /// + /// If there is a specific manually-driven clock you want to use, you can + /// also select that: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// use std::time::Duration; + /// + /// let (my_clock, my_source) = Clock::with_source(); + /// + /// let timer = node.create_timer_repeating( + /// TimerOptions::new(Duration::from_secs(1)) + /// .clock(&my_clock), + /// || { + /// println!("Triggering once each simulated second"); + /// }, + /// )?; + /// + /// my_source.set_ros_time_override(1_500_000_000); + /// # Ok::<(), RclrsError>(()) + /// ``` + /// + /// If you are okay with the default choice of clock (steady clock) then you + /// can choose to simply pass a duration in as the options: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// use std::time::Duration; + /// + /// let timer = node.create_timer_repeating( + /// Duration::from_secs(1), + /// || { + /// println!("Triggering per steady clock second"); + /// }, + /// )?; + /// # Ok::<(), RclrsError>(()) + /// ``` + /// + /// # Node Timer Repeating Callbacks + /// + /// Node Timer repeating callbacks support three signatures: + /// - [FnMut] () + /// - [FnMut] ([Time][2]) + /// - [FnMut] (&[Timer]) + /// + /// You can choose to receive the current time when the callback is being + /// triggered. + /// + /// Or instead of the current time, you can get a borrow of the [`Timer`] + /// itself, that way if you need to access it from inside the callback, you + /// do not need to worry about capturing a [`Weak`][3] and then locking it. + /// This is useful if you need to change the callback of the timer from inside + /// the callback of the timer. + /// + /// For an [`FnOnce`] instead of [`FnMut`], use [`Self::create_timer_oneshot`]. + /// + /// [1]: crate::TimerOptions + /// [2]: crate::Time + /// [3]: std::sync::Weak + pub fn create_timer_repeating<'a, Args>( + &self, + options: impl IntoTimerOptions<'a>, + callback: impl IntoNodeTimerRepeatingCallback, + ) -> Result { + 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 + /// [`TimerState::set_repeating`] or [`TimerState::set_oneshot`] or else + /// nothing will happen the following times that the `Timer` elapses. + /// + /// See also: + /// * [`Self::create_timer_repeating`] + /// * [`Self::create_timer_inert`] + /// + /// # Behavior + /// + /// While the callback of this timer is running, no other callbacks associated + /// with this node will be able to run. This is in contrast to callbacks given + /// to [`Self::create_subscription`] which can run multiple times in parallel. + /// + /// Since the callback of this timer may block other callbacks from being able + /// to run, it is strongly recommended to ensure that the callback returns + /// quickly. If the callback needs to trigger long-running behavior then you + /// can consider using [`std::thread::spawn`], or for async behaviors you can + /// capture an [`ExecutorCommands`] in your callback and use [`ExecutorCommands::run`] + /// to issue a task for the executor to run in its async task pool. + /// + /// Since these callbacks will only be triggered once, you may use [`FnOnce`] here. + /// + /// # Timer Options + /// + /// See [`NodeSate::create_timer_repeating`][3] for examples of setting the + /// timer options. + /// + /// # Node Timer Oneshot Callbacks + /// + /// Node Timer repeating callbacks support three signatures: + /// - [FnMut] () + /// - [FnMut] ([Time][2]) + /// - [FnMut] (&[Timer]) + /// + /// You can choose to receive the current time when the callback is being + /// triggered. + /// + /// Or instead of the current time, you can get a borrow of the [`Timer`] + /// itself, that way if you need to access it from inside the callback, you + /// do not need to worry about capturing a [`Weak`][3] and then locking it. + /// This is useful if you need to change the callback of the timer from inside + /// the callback of the timer. + /// + /// [2]: crate::Time + /// [3]: std::sync::Weak + pub fn create_timer_oneshot<'a, Args>( + &self, + options: impl IntoTimerOptions<'a>, + callback: impl IntoNodeTimerOneshotCallback, + ) -> Result { + 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 [`TimerState::set_repeating`] or + /// [`TimerState::set_oneshot`]. + /// + /// See also: + /// * [`Self::create_timer_repeating`] + /// * [`Self::create_timer_oneshot`] + pub fn create_timer_inert<'a>( + &self, + options: impl IntoTimerOptions<'a>, + ) -> Result { + 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, + ) -> Result { + 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]. diff --git a/rclrs/src/subscription/readonly_loaned_message.rs b/rclrs/src/subscription/readonly_loaned_message.rs index 67ac3fd6..0ea73c1f 100644 --- a/rclrs/src/subscription/readonly_loaned_message.rs +++ b/rclrs/src/subscription/readonly_loaned_message.rs @@ -12,8 +12,6 @@ use crate::{rcl_bindings::*, subscription::SubscriptionHandle, ToResult}; /// /// This type may be used in subscription callbacks to receive a message. The /// loan is returned by dropping the `ReadOnlyLoanedMessage`. -/// -/// [1]: crate::SubscriptionState::take_loaned pub struct ReadOnlyLoanedMessage where T: Message, diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs new file mode 100644 index 00000000..abaac3fb --- /dev/null +++ b/rclrs/src/timer.rs @@ -0,0 +1,812 @@ +use crate::{ + clock::Clock, context::ContextHandle, error::RclrsError, log_error, rcl_bindings::*, Node, + RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, ToLogParams, ToResult, Waitable, + WaitableLifecycle, WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, +}; +// TODO: fix me when the callback type is properly defined. +// use std::fmt::Debug; +use std::{ + any::Any, + sync::{Arc, Mutex, Weak}, + time::Duration, +}; + +mod any_timer_callback; +pub use any_timer_callback::*; + +mod timer_options; +pub use timer_options::*; + +mod into_node_timer_callback; +pub use into_node_timer_callback::*; + +mod into_worker_timer_callback; +pub use into_worker_timer_callback::*; + +/// Struct for executing periodic events. +/// +/// The executor needs to be [spinning][1] for a timer's callback to be triggered. +/// +/// Timers can be created by a [`Node`] using one of these methods: +/// - [`NodeState::create_timer_repeating`][2] +/// - [`NodeState::create_timer_oneshot`][3] +/// - [`NodeState::create_timer_inert`][4] +/// +/// Timers can also be created by a [`Worker`], in which case they can access the worker's payload: +/// - [`WorkerState::create_timer_repeating`][5] +/// - [`WorkerState::create_timer_oneshot`][6] +/// - [`WorkerState::create_timer_inert`][7] +/// +/// The API of timers is given by [`TimerState`]. +/// +/// [1]: crate::Executor::spin +/// [2]: crate::NodeState::create_timer_repeating +/// [3]: crate::NodeState::create_timer_oneshot +/// [4]: crate::NodeState::create_timer_inert +/// [5]: crate::WorkerState::create_timer_repeating +/// [6]: crate::WorkerState::create_timer_oneshot +/// [7]: crate::WorkerState::create_timer_inert +pub type Timer = Arc>; + +/// A [`Timer`] that runs on a [`Worker`]. +/// +/// Create a worker timer using [`create_timer_repeating`][1], +/// [`create_timer_oneshot`][2], or [`create_timer_inert`][3]. +/// +/// [1]: crate::WorkerState::create_timer_repeating +/// [2]: crate::WorkerState::create_timer_oneshot +/// [3]: crate::WorkerState::create_timer_inert +pub type WorkerTimer = Arc>>; + +/// The inner state of a [`Timer`]. +/// +/// This is public so that you can choose to create a [`Weak`] reference to it +/// if you want to be able to refer to a [`Timer`] in a non-owning way. It is +/// generally recommended to manage the `TimerState` inside of an [`Arc`], and +/// [`Timer`] is provided as a convenience alias for that. +/// +/// The public API of [`Timer`] is implemented via `TimerState`. +/// +/// Timers that run inside of a [`Worker`] are represented by [`WorkerTimer`]. +pub struct TimerState { + pub(crate) handle: Arc, + /// The callback function that runs when the timer is due. + callback: Mutex>>, + /// What was the last time lapse between calls to this timer + last_elapse: Mutex, + /// We use Mutex> here because we need to construct the TimerState object + /// before we can get the lifecycle handle. + #[allow(unused)] + lifecycle: Mutex>, + _ignore: std::marker::PhantomData, +} + +impl TimerState { + /// Gets the period of the timer + pub fn get_timer_period(&self) -> Result { + let mut timer_period_ns = 0; + unsafe { + let rcl_timer = self.handle.rcl_timer.lock().unwrap(); + rcl_timer_get_period(&*rcl_timer, &mut timer_period_ns) + } + .ok()?; + + rcl_duration(timer_period_ns) + } + + /// Cancels the timer, stopping the execution of the callback + pub fn cancel(&self) -> Result<(), RclrsError> { + let mut rcl_timer = self.handle.rcl_timer.lock().unwrap(); + let cancel_result = unsafe { rcl_timer_cancel(&mut *rcl_timer) }.ok()?; + Ok(cancel_result) + } + + /// Checks whether the timer is canceled or not + pub fn is_canceled(&self) -> Result { + let mut is_canceled = false; + unsafe { + let rcl_timer = self.handle.rcl_timer.lock().unwrap(); + rcl_timer_is_canceled(&*rcl_timer, &mut is_canceled) + } + .ok()?; + Ok(is_canceled) + } + + /// Get the last time lapse between calls to the timer. + /// + /// This is different from [`Self::time_since_last_call`] because it remains + /// constant between calls to the Timer. + /// + /// It keeps track of the what the value of [`Self::time_since_last_call`] + /// was immediately before the most recent call to the callback. This will + /// be [`Duration::ZERO`] if the `Timer` has never been triggered. + pub fn last_elapse(&self) -> Duration { + *self.last_elapse.lock().unwrap() + } + + /// Retrieves the time since the last call to the callback + pub fn time_since_last_call(&self) -> Result { + let mut time_value_ns: i64 = 0; + unsafe { + let rcl_timer = self.handle.rcl_timer.lock().unwrap(); + rcl_timer_get_time_since_last_call(&*rcl_timer, &mut time_value_ns) + } + .ok()?; + + rcl_duration(time_value_ns) + } + + /// Retrieves the time until the next call of the callback + pub fn time_until_next_call(&self) -> Result { + let mut time_value_ns: i64 = 0; + unsafe { + let rcl_timer = self.handle.rcl_timer.lock().unwrap(); + rcl_timer_get_time_until_next_call(&*rcl_timer, &mut time_value_ns) + } + .ok()?; + + rcl_duration(time_value_ns) + } + + /// Resets the timer. + pub fn reset(&self) -> Result<(), RclrsError> { + let mut rcl_timer = self.handle.rcl_timer.lock().unwrap(); + unsafe { rcl_timer_reset(&mut *rcl_timer) }.ok() + } + + /// Checks if the timer is ready (not canceled) + pub fn is_ready(&self) -> Result { + let is_ready = unsafe { + let mut is_ready: bool = false; + let rcl_timer = self.handle.rcl_timer.lock().unwrap(); + rcl_timer_is_ready(&*rcl_timer, &mut is_ready).ok()?; + is_ready + }; + + Ok(is_ready) + } + + /// Get the clock that this timer runs on. + pub fn clock(&self) -> &Clock { + &self.handle.clock + } + + /// Set a new callback for the timer. This will return whatever callback + /// was already present unless you are calling the function from inside of + /// the timer's callback, in which case you will receive [`None`]. + /// + /// See also: + /// * [`Self::set_repeating`] + /// * [`Self::set_oneshot`] + /// * [`Self::set_inert`]. + pub fn set_callback( + &self, + callback: AnyTimerCallback, + ) -> Option> { + self.callback.lock().unwrap().replace(callback) + } + + /// Remove the callback from the timer. + /// + /// This does not cancel the timer; it will continue to wake up and be + /// triggered at its regular period. However, nothing will happen when the + /// timer is triggered until you give a new callback to the timer. + /// + /// You can give the timer a new callback at any time by calling: + /// * [`Self::set_repeating`] + /// * [`Self::set_oneshot`] + pub fn set_inert(&self) -> Option> { + self.set_callback(AnyTimerCallback::Inert) + } + + /// Creates a new timer. Users should call one of [`Node::create_timer`], + /// [`Node::create_timer_repeating`], [`Node::create_timer_oneshot`], or + /// [`Node::create_timer_inert`]. + pub(crate) fn create<'a>( + period: Duration, + clock: Clock, + callback: AnyTimerCallback, + commands: &Arc, + context: &ContextHandle, + ) -> Result, RclrsError> { + let period = period.as_nanos() as i64; + + // Callbacks will be handled at the rclrs layer. + let rcl_timer_callback: rcl_timer_callback_t = None; + + let rcl_timer = Arc::new(Mutex::new( + // SAFETY: Zero-initializing a timer is always safe + unsafe { rcl_get_zero_initialized_timer() }, + )); + + unsafe { + let mut rcl_clock = clock.get_rcl_clock().lock().unwrap(); + let mut rcl_context = context.rcl_context.lock().unwrap(); + + // SAFETY: Getting a default value is always safe. + let allocator = rcutils_get_default_allocator(); + + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + + // The API for initializing timers changed with the kilted releaase. + #[cfg(any(ros_distro = "humble", ros_distro = "jazzy"))] + { + // SAFETY: We lock the lifecycle mutex since rcl_timer_init is not + // thread-safe. + rcl_timer_init( + &mut *rcl_timer.lock().unwrap(), + &mut *rcl_clock, + &mut *rcl_context, + period, + rcl_timer_callback, + allocator, + ) + } + + // The API for initializing timers changed with the kilted releaase. + // This new API allows you to opt out of automatically starting the + // timer as soon as it is created. We could consider exposing this + // capability to the user, but for now we are just telling it to + // immediately start the timer. + #[cfg(not(any(ros_distro = "humble", ros_distro = "jazzy")))] + { + // SAFETY: We lock the lifecycle mutex since rcl_timer_init is not + // thread-safe. + rcl_timer_init2( + &mut *rcl_timer.lock().unwrap(), + &mut *rcl_clock, + &mut *rcl_context, + period, + rcl_timer_callback, + allocator, + true, + ) + } + } + .ok()?; + + let timer = Arc::new(TimerState { + handle: Arc::new(TimerHandle { rcl_timer, clock }), + callback: Mutex::new(Some(callback)), + last_elapse: Mutex::new(Duration::ZERO), + lifecycle: Mutex::default(), + _ignore: Default::default(), + }); + + let (waitable, lifecycle) = Waitable::new( + Box::new(TimerExecutable:: { + timer: Arc::downgrade(&timer), + handle: Arc::clone(&timer.handle), + }), + Some(Arc::clone(commands.get_guard_condition())), + ); + + *timer.lifecycle.lock().unwrap() = Some(lifecycle); + + commands.add_to_wait_set(waitable); + + Ok(timer) + } + + /// Force the timer to be called, even if it is not ready to be triggered yet. + /// We could consider making this public, but the behavior may confuse users. + fn call(self: &Arc, any_payload: &mut dyn Any) -> Result<(), RclrsError> { + // Keep track of the time elapsed since the last call. We need to run + // this before we trigger rcl_call. + let last_elapse = self.time_since_last_call().unwrap_or(Duration::ZERO); + *self.last_elapse.lock().unwrap() = last_elapse; + + if let Err(err) = self.rcl_call() { + log_error!("timer", "Unable to call timer: {err:?}",); + } + + let Some(callback) = self.callback.lock().unwrap().take() else { + log_error!( + "timer".once(), + "Timer is missing its callback information. This should not \ + be possible, please report it to the maintainers of rclrs.", + ); + return Ok(()); + }; + + let Some(payload) = any_payload.downcast_mut::() else { + return Err(RclrsError::InvalidPayload { + expected: std::any::TypeId::of::(), + received: (*any_payload).type_id(), + }); + }; + + match callback { + AnyTimerCallback::Repeating(mut callback) => { + callback(payload, self); + self.restore_callback(AnyTimerCallback::Repeating(callback).into()); + } + AnyTimerCallback::OneShot(callback) => { + callback(payload, self); + self.restore_callback(AnyTimerCallback::Inert); + } + AnyTimerCallback::Inert => { + self.restore_callback(AnyTimerCallback::Inert); + } + } + + Ok(()) + } + + /// Updates the state of the rcl_timer to know that it has been called. This + /// should only be called by [`Self::call`]. + /// + /// The callback held by the rcl_timer is null because we store the callback + /// in the [`Timer`] struct. This means there are no side-effects to this + /// except to keep track of when the timer has been called. + fn rcl_call(&self) -> Result<(), RclrsError> { + let mut rcl_timer = self.handle.rcl_timer.lock().unwrap(); + unsafe { rcl_timer_call(&mut *rcl_timer) }.ok() + } + + /// Used by [`Timer::execute`] to restore the state of the callback if and + /// only if the user has not already set a new callback. + fn restore_callback(&self, callback: AnyTimerCallback) { + let mut self_callback = self.callback.lock().unwrap(); + if self_callback.is_none() { + *self_callback = Some(callback); + } + } +} + +impl TimerState { + /// Set a repeating callback for this timer. + /// + /// See also: + /// * [`Self::set_oneshot`] + /// * [`Self::set_inert`] + pub fn set_repeating( + &self, + f: impl IntoNodeTimerRepeatingCallback, + ) -> Option> { + self.set_callback(f.into_node_timer_repeating_callback()) + } + + /// Set a one-shot callback for the timer. + /// + /// The next time the timer is triggered, the callback will be set to + /// [`AnyTimerCallback::Inert`] after this callback is triggered. To keep the + /// timer useful, you can reset the Timer callback at any time, including + /// inside the one-shot callback itself. + /// + /// See also: + /// * [`Self::set_repeating`] + /// * [`Self::set_inert`] + pub fn set_oneshot( + &self, + f: impl IntoNodeTimerOneshotCallback, + ) -> Option> { + self.set_callback(f.into_node_timer_oneshot_callback()) + } +} + +impl TimerState> { + /// Set a repeating callback for this worker timer. + /// + /// See also: + /// * [`Self::set_worker_oneshot`] + /// * [`Self::set_inert`] + pub fn set_worker_repeating( + &self, + f: impl IntoWorkerTimerRepeatingCallback, Args>, + ) -> Option>> { + self.set_callback(f.into_worker_timer_repeating_callback()) + } + + /// Set a one-shot callback for the worker timer. + /// + /// The next time the timer is triggered, the callback will be set to + /// [`AnyTimerCallback::Inert`] after this callback is triggered. To keep the + /// timer useful, you can reset the Timer callback at any time, including + /// inside the one-shot callback itself. + /// + /// See also: + /// * [`Self::set_worker_repeating`] + /// * [`Self::set_inert`] + pub fn set_worker_oneshot( + &self, + f: impl IntoWorkerTimerOneshotCallback, Args>, + ) -> Option>> { + self.set_callback(f.into_worker_timer_oneshot_callback()) + } +} + +struct TimerExecutable { + timer: Weak>, + handle: Arc, +} + +impl RclPrimitive for TimerExecutable { + unsafe fn execute(&mut self, payload: &mut dyn Any) -> Result<(), RclrsError> { + if let Some(timer) = self.timer.upgrade() { + if timer.is_ready()? { + timer.call(payload)?; + } + } + + Ok(()) + } + + fn kind(&self) -> RclPrimitiveKind { + RclPrimitiveKind::Timer + } + + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::Timer(self.handle.rcl_timer.lock().unwrap()) + } +} + +impl PartialEq for TimerState { + fn eq(&self, other: &Self) -> bool { + Arc::ptr_eq(&self.handle.rcl_timer, &other.handle.rcl_timer) + } +} + +fn rcl_duration(duration_value_ns: i64) -> Result { + if duration_value_ns < 0 { + Err(RclrsError::NegativeDuration(duration_value_ns)) + } else { + Ok(Duration::from_nanos(duration_value_ns as u64)) + } +} + +/// Manage the lifecycle of an `rcl_timer_t`, including managing its dependency +/// on `rcl_clock_t` by ensuring that this dependency are [dropped after][1] +/// the `rcl_timer_t`. +/// +/// [1]: +pub(crate) struct TimerHandle { + pub(crate) rcl_timer: Arc>, + clock: Clock, +} + +/// 'Drop' trait implementation to be able to release the resources +impl Drop for TimerHandle { + fn drop(&mut self) { + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The lifecycle mutex is locked and the clock for the timer + // must still be valid because TimerHandle keeps it alive. + unsafe { rcl_timer_fini(&mut *self.rcl_timer.lock().unwrap()) }; + } +} + +// 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_timer_t {} + +#[cfg(test)] +mod tests { + use super::TimerExecutable; + use crate::*; + use std::{ + sync::{ + atomic::{AtomicBool, Ordering}, + Arc, + }, + thread, + time::Duration, + }; + + #[test] + fn traits() { + use crate::test_helpers::*; + + assert_send::>(); + assert_sync::>(); + } + + #[test] + fn test_new_with_system_clock() { + let executor = Context::default().create_basic_executor(); + let result = TimerState::::create( + Duration::from_millis(1), + Clock::system(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + assert!(result.is_ok()); + } + + #[test] + fn test_new_with_steady_clock() { + let executor = Context::default().create_basic_executor(); + let result = TimerState::::create( + Duration::from_millis(1), + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + assert!(result.is_ok()); + } + + #[test] + fn test_new_with_source_clock() { + let (clock, source) = Clock::with_source(); + // No manual time set, it should default to 0 + assert_eq!(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); + + let executor = Context::default().create_basic_executor(); + let result = TimerState::::create( + Duration::from_millis(1), + clock, + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + assert!(result.is_ok()); + } + + #[test] + fn test_get_period() { + let period = Duration::from_millis(1); + + let executor = Context::default().create_basic_executor(); + + let result = TimerState::::create( + period, + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + + let timer = result.unwrap(); + let timer_period = timer.get_timer_period().unwrap(); + assert_eq!(timer_period, period); + } + + #[test] + fn test_cancel() { + let executor = Context::default().create_basic_executor(); + + let result = TimerState::::create( + Duration::from_millis(1), + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + + let timer = result.unwrap(); + assert!(!timer.is_canceled().unwrap()); + timer.cancel().unwrap(); + assert!(timer.is_canceled().unwrap()); + } + + #[test] + fn test_time_since_last_call_before_first_event() { + let executor = Context::default().create_basic_executor(); + + let result = TimerState::::create( + Duration::from_millis(2), + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + let timer = result.unwrap(); + + let sleep_period = Duration::from_millis(1); + thread::sleep(sleep_period); + + let time_since_last_call = timer.time_since_last_call().unwrap(); + assert!( + time_since_last_call >= sleep_period, + "time_since_last_call: {:?} vs sleep period: {:?}", + time_since_last_call, + sleep_period, + ); + } + + #[test] + fn test_time_until_next_call_before_first_event() { + let executor = Context::default().create_basic_executor(); + let period = Duration::from_millis(2); + + let result = TimerState::::create( + period, + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + let timer = result.unwrap(); + + let time_until_next_call = timer.time_until_next_call().unwrap(); + assert!( + time_until_next_call <= period, + "time_until_next_call: {:?} vs period: {:?}", + time_until_next_call, + period, + ); + } + + #[test] + fn test_reset() { + let executor = Context::default().create_basic_executor(); + let period = Duration::from_millis(2); + let timer = TimerState::::create( + period, + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ) + .unwrap(); + + // The unwrap will panic if the remaining time is negative + timer.time_until_next_call().unwrap(); + + // Sleep until we're past the timer period + thread::sleep(Duration::from_millis(3)); + + // Now the time until next call should give an error + assert!(matches!( + timer.time_until_next_call(), + Err(RclrsError::NegativeDuration(_)) + )); + + // Reset the timer so its interval begins again + assert!(timer.reset().is_ok()); + + // The unwrap will panic if the remaining time is negative + timer.time_until_next_call().unwrap(); + } + + #[test] + fn test_call() { + let executor = Context::default().create_basic_executor(); + let timer = TimerState::::create( + Duration::from_millis(1), + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ) + .unwrap(); + + // The unwrap will panic if the remaining time is negative + timer.time_until_next_call().unwrap(); + + // Sleep until we're past the timer period + thread::sleep(Duration::from_micros(1500)); + + // Now the time until the next call should give an error + assert!(matches!( + timer.time_until_next_call(), + Err(RclrsError::NegativeDuration(_)) + )); + + // The unwrap will panic if anything went wrong with the call + timer.call(&mut ()).unwrap(); + + // The unwrap will panic if the remaining time is negative + timer.time_until_next_call().unwrap(); + } + + #[test] + fn test_is_ready() { + let executor = Context::default().create_basic_executor(); + let timer = TimerState::::create( + Duration::from_millis(1), + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ) + .unwrap(); + + assert!(!timer.is_ready().unwrap()); + + // Sleep until the period has elapsed + thread::sleep(Duration::from_micros(1100)); + + assert!(timer.is_ready().unwrap()); + } + + #[test] + fn test_callback() { + let clock = Clock::steady(); + let initial_time = clock.now(); + + let executor = Context::default().create_basic_executor(); + let executed = Arc::new(AtomicBool::new(false)); + + let timer = TimerState::::create( + Duration::from_millis(1), + clock, + create_timer_callback_for_testing(initial_time, Arc::clone(&executed)), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ) + .unwrap(); + + timer.call(&mut ()).unwrap(); + assert!(executed.load(Ordering::Acquire)); + } + + #[test] + fn test_execute_when_is_not_ready() { + let clock = Clock::steady(); + let initial_time = clock.now(); + + let executor = Context::default().create_basic_executor(); + let executed = Arc::new(AtomicBool::new(false)); + + let timer = TimerState::::create( + Duration::from_millis(1), + clock, + create_timer_callback_for_testing(initial_time, Arc::clone(&executed)), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ) + .unwrap(); + + let mut executable = TimerExecutable { + timer: Arc::downgrade(&timer), + handle: Arc::clone(&timer.handle), + }; + + // SAFETY: Node timers expect a payload of () + unsafe { + executable.execute(&mut ()).unwrap(); + } + assert!(!executed.load(Ordering::Acquire)); + } + + #[test] + fn test_execute_when_is_ready() { + let clock = Clock::steady(); + let initial_time = clock.now(); + + let executor = Context::default().create_basic_executor(); + let executed = Arc::new(AtomicBool::new(false)); + + let timer = TimerState::::create( + Duration::from_millis(1), + clock, + create_timer_callback_for_testing(initial_time, Arc::clone(&executed)), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ) + .unwrap(); + + let mut executable = TimerExecutable { + timer: Arc::downgrade(&timer), + handle: Arc::clone(&timer.handle), + }; + + thread::sleep(Duration::from_millis(2)); + + // SAFETY: Node timers expect a payload of () + unsafe { + executable.execute(&mut ()).unwrap(); + } + assert!(executed.load(Ordering::Acquire)); + } + + fn create_timer_callback_for_testing( + initial_time: Time, + executed: Arc, + ) -> AnyTimerCallback { + (move |t: Time| { + assert!(t + .compare_with(&initial_time, |t, initial| t >= initial) + .unwrap()); + executed.store(true, Ordering::Release); + }) + .into_node_timer_oneshot_callback() + } +} diff --git a/rclrs/src/timer/any_timer_callback.rs b/rclrs/src/timer/any_timer_callback.rs new file mode 100644 index 00000000..3c4493d7 --- /dev/null +++ b/rclrs/src/timer/any_timer_callback.rs @@ -0,0 +1,15 @@ +use crate::{TimerState, WorkScope}; +use std::sync::Arc; + +/// A callback that can be triggered when a timer elapses. +pub enum AnyTimerCallback { + /// This callback will be triggered repeatedly, each time the period of the + /// timer elapses. + Repeating(Box>) + Send>), + /// This callback will be triggered exactly once, the first time the period + /// of the timer elapses. + OneShot(Box>) + Send>), + /// Do nothing when the timer elapses. This can be replaced later so that + /// the timer does something. + Inert, +} diff --git a/rclrs/src/timer/into_node_timer_callback.rs b/rclrs/src/timer/into_node_timer_callback.rs new file mode 100644 index 00000000..5ef3772f --- /dev/null +++ b/rclrs/src/timer/into_node_timer_callback.rs @@ -0,0 +1,67 @@ +use crate::{AnyTimerCallback, Node, Time, Timer}; + +/// This trait is used to create timer callbacks for repeating timers in a Node. +pub trait IntoNodeTimerRepeatingCallback: 'static + Send { + /// Convert a suitable object into a repeating timer callback for the node scope + fn into_node_timer_repeating_callback(self) -> AnyTimerCallback; +} + +impl IntoNodeTimerRepeatingCallback<()> for Func +where + Func: FnMut() + 'static + Send, +{ + fn into_node_timer_repeating_callback(mut self) -> AnyTimerCallback { + AnyTimerCallback::Repeating(Box::new(move |_, _| self())).into() + } +} + +impl IntoNodeTimerRepeatingCallback for Func +where + Func: FnMut(&Timer) + 'static + Send, +{ + fn into_node_timer_repeating_callback(mut self) -> AnyTimerCallback { + AnyTimerCallback::Repeating(Box::new(move |_, t| self(t))).into() + } +} + +impl IntoNodeTimerRepeatingCallback