diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index 430f3c3af..1c5f7a7cd 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -73,8 +73,10 @@ rustflags = "0.1" ament_rs = "0.3" [features] -default = [] +default = ["tokio-executor"] serde = ["dep:serde", "dep:serde-big-array", "rosidl_runtime_rs/serde", "ros-env/serde"] +# Enables the event-driven, Tokio-based multi-threaded executor +tokio-executor = ["tokio/rt-multi-thread", "tokio/macros", "tokio/time"] # This feature is solely for the purpose of being able to generate documetation without a ROS installation # The only intended usage of this feature is for docs.rs builders to work, and is not intended to be used by end users use_ros_shim = ["paste", "ros-env/use_ros_shim", "rosidl_runtime_rs/use_ros_shim"] diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs index b123e0440..ae4939fc6 100644 --- a/rclrs/src/action.rs +++ b/rclrs/src/action.rs @@ -404,6 +404,122 @@ mod tests { executor.spin(SpinOptions::default().until_promise_resolved(promise)); } + /// A full goal round-trip (feedback streaming + result) driven by the + /// event-driven Tokio executor, exercising the action push-callback path + /// (`rcl_action_{server,client}_set_*_callback`). The completion flag makes a + /// timeout fail the test rather than pass silently. + #[cfg(feature = "tokio-executor")] + #[test] + fn test_action_success_streaming_tokio() { + use std::sync::{ + atomic::{AtomicBool, Ordering}, + Arc, + }; + + let mut executor = Context::default().create_tokio_executor(); + + let node = executor + .create_node(&format!("test_action_success_tokio_{}", line!())) + .unwrap(); + let action_name = format!("test_action_success_tokio_{}_action", line!()); + let _action_server = node + .create_action_server(&action_name, |handle| { + fibonacci_action(handle, TestActionSettings::default()) + }) + .unwrap(); + + let client = node + .create_action_client::(&action_name) + .unwrap(); + + let order_10_sequence = [1, 1, 2, 3, 5, 8, 13, 21, 34, 55]; + let request = client.request_goal(Fibonacci_Goal { order: 10 }); + + let done = Arc::new(AtomicBool::new(false)); + let done_cb = Arc::clone(&done); + let promise = executor.commands().run(async move { + let mut goal_client_stream = request.await.unwrap().stream(); + let mut expected_feedback_len = 0; + while let Some(event) = goal_client_stream.next().await { + match event { + GoalEvent::Feedback(feedback) => { + expected_feedback_len += 1; + assert_eq!(feedback.sequence.len(), expected_feedback_len); + } + GoalEvent::Status(_) => {} + GoalEvent::Result((status, result)) => { + assert_eq!(status, GoalStatusCode::Succeeded); + assert_eq!(result.sequence, order_10_sequence); + done_cb.store(true, Ordering::Relaxed); + return; + } + } + } + }); + + executor.spin( + SpinOptions::default() + .until_promise_resolved(promise) + .timeout(Duration::from_secs(15)), + ); + + assert!( + done.load(Ordering::Relaxed), + "action goal round-trip did not complete on the Tokio executor", + ); + } + + /// A goal cancellation driven by the Tokio executor, exercising the action + /// client's cancel-client and the server's cancel-service push callbacks. + #[cfg(feature = "tokio-executor")] + #[test] + fn test_action_cancel_tokio() { + use std::sync::{ + atomic::{AtomicBool, Ordering}, + Arc, + }; + + let mut executor = Context::default().create_tokio_executor(); + + let node = executor + .create_node(&format!("test_action_cancel_tokio_{}", line!())) + .unwrap(); + let action_name = format!("test_action_cancel_tokio_{}_action", line!()); + let _action_server = node + .create_action_server(&action_name, |handle| { + fibonacci_action(handle, TestActionSettings::slow()) + }) + .unwrap(); + + let client = node + .create_action_client::(&action_name) + .unwrap(); + + let request = client.request_goal(Fibonacci_Goal { order: 10 }); + + let done = Arc::new(AtomicBool::new(false)); + let done_cb = Arc::clone(&done); + let promise = executor.commands().run(async move { + let goal_client = request.await.unwrap(); + let cancellation = goal_client.cancellation.cancel().await; + assert!(cancellation.is_accepted()); + let (status, _) = goal_client.result.await; + assert_eq!(status, GoalStatusCode::Cancelled); + done_cb.store(true, Ordering::Relaxed); + }); + + executor.spin( + SpinOptions::default() + .until_promise_resolved(promise) + .timeout(Duration::from_secs(15)), + ); + + assert!( + done.load(Ordering::Relaxed), + "action cancellation did not complete on the Tokio executor", + ); + } + #[test] fn test_action_cancel_rejection() { let mut executor = Context::default().create_basic_executor(); diff --git a/rclrs/src/action/action_client.rs b/rclrs/src/action/action_client.rs index 0d2967dab..39d64c69b 100644 --- a/rclrs/src/action/action_client.rs +++ b/rclrs/src/action/action_client.rs @@ -1,9 +1,9 @@ use super::empty_goal_status_array; use crate::{ - log_warn, rcl_bindings::*, CancelResponse, CancelResponseCode, DropGuard, GoalStatus, - GoalStatusCode, GoalUuid, MultiCancelResponse, Node, NodeHandle, QoSProfile, RclPrimitive, - RclPrimitiveHandle, RclPrimitiveKind, RclrsError, ReadyKind, TakeFailedAsNone, ToResult, - Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, + log_warn, rcl_bindings::*, ActionClientReady, CancelResponse, CancelResponseCode, DropGuard, + GoalStatus, GoalStatusCode, GoalUuid, MultiCancelResponse, Node, NodeHandle, QoSProfile, + RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, ReadyKind, TakeFailedAsNone, + ToResult, Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, }; use ros_env::{action_msgs::srv::CancelGoal_Response, builtin_interfaces::msg::Time}; use rosidl_runtime_rs::{Action, Message, RmwFeedbackMessage, RmwGoalResponse, RmwResultResponse}; @@ -872,6 +872,115 @@ impl RclPrimitive for ActionClientExecutable { fn handle(&self) -> crate::RclPrimitiveHandle<'_> { RclPrimitiveHandle::ActionClient(self.board.handle.lock()) } + + fn register_on_ready( + &self, + on_ready: Box, + ) -> Result>, RclrsError> { + use crate::executor::event_callback::{CompositeOnReady, OnReadyRegistration}; + + // An action client bundles two subscriptions (feedback/status) and three + // service clients (goal/cancel/result). Register a push callback on each, + // tagging events with the matching readiness flag so the executor runs the + // right handler. + let on_ready = Arc::new(on_ready); + let handle = &self.board.handle; + let mk = |bits: ActionClientReady| -> Box { + let on_ready = Arc::clone(&on_ready); + Box::new(move |n| on_ready(ReadyKind::ActionClient(bits), n)) + }; + + let regs: Vec> = vec![ + Box::new(OnReadyRegistration::new( + Arc::clone(handle), + set_action_client_feedback_callback, + mk(ActionClientReady { + feedback: true, + ..Default::default() + }), + )?), + Box::new(OnReadyRegistration::new( + Arc::clone(handle), + set_action_client_status_callback, + mk(ActionClientReady { + status: true, + ..Default::default() + }), + )?), + Box::new(OnReadyRegistration::new( + Arc::clone(handle), + set_action_client_goal_callback, + mk(ActionClientReady { + goal_response: true, + ..Default::default() + }), + )?), + Box::new(OnReadyRegistration::new( + Arc::clone(handle), + set_action_client_cancel_callback, + mk(ActionClientReady { + cancel_response: true, + ..Default::default() + }), + )?), + Box::new(OnReadyRegistration::new( + Arc::clone(handle), + set_action_client_result_callback, + mk(ActionClientReady { + result_response: true, + ..Default::default() + }), + )?), + ]; + + Ok(Some(Box::new(CompositeOnReady(regs)))) + } +} + +/// Install (or, with a null callback/user_data, clear) the "on new feedback" +/// push callback on an action client. Encapsulates the handle lock and rcl call. +unsafe fn set_action_client_feedback_callback( + handle: &ActionClientHandle, + callback: rcl_event_callback_t, + user_data: *const std::os::raw::c_void, +) -> rcl_ret_t { + rcl_action_client_set_feedback_subscription_callback(&*handle.lock(), callback, user_data) +} + +/// As [`set_action_client_feedback_callback`], for the status subscription. +unsafe fn set_action_client_status_callback( + handle: &ActionClientHandle, + callback: rcl_event_callback_t, + user_data: *const std::os::raw::c_void, +) -> rcl_ret_t { + rcl_action_client_set_status_subscription_callback(&*handle.lock(), callback, user_data) +} + +/// As [`set_action_client_feedback_callback`], for the goal service client. +unsafe fn set_action_client_goal_callback( + handle: &ActionClientHandle, + callback: rcl_event_callback_t, + user_data: *const std::os::raw::c_void, +) -> rcl_ret_t { + rcl_action_client_set_goal_client_callback(&*handle.lock(), callback, user_data) +} + +/// As [`set_action_client_feedback_callback`], for the cancel service client. +unsafe fn set_action_client_cancel_callback( + handle: &ActionClientHandle, + callback: rcl_event_callback_t, + user_data: *const std::os::raw::c_void, +) -> rcl_ret_t { + rcl_action_client_set_cancel_client_callback(&*handle.lock(), callback, user_data) +} + +/// As [`set_action_client_feedback_callback`], for the result service client. +unsafe fn set_action_client_result_callback( + handle: &ActionClientHandle, + callback: rcl_event_callback_t, + user_data: *const std::os::raw::c_void, +) -> rcl_ret_t { + rcl_action_client_set_result_client_callback(&*handle.lock(), callback, user_data) } /// Manage the lifecycle of an `rcl_action_client_t`, including managing its dependencies diff --git a/rclrs/src/action/action_server.rs b/rclrs/src/action/action_server.rs index 468eff101..5251f4667 100644 --- a/rclrs/src/action/action_server.rs +++ b/rclrs/src/action/action_server.rs @@ -1,9 +1,9 @@ use super::empty_goal_status_array; use crate::{ - action::GoalUuid, error::ToResult, rcl_bindings::*, ActionGoalReceiver, CancelResponseCode, - DropGuard, GoalStatusCode, Node, NodeHandle, QoSProfile, RclPrimitive, RclPrimitiveHandle, - RclPrimitiveKind, RclrsError, ReadyKind, TakeFailedAsNone, Waitable, WaitableLifecycle, - ENTITY_LIFECYCLE_MUTEX, + action::GoalUuid, error::ToResult, rcl_bindings::*, ActionGoalReceiver, ActionServerReady, + CancelResponseCode, DropGuard, GoalStatusCode, Node, NodeHandle, QoSProfile, RclPrimitive, + RclPrimitiveHandle, RclPrimitiveKind, RclrsError, ReadyKind, TakeFailedAsNone, Waitable, + WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, }; use futures::future::BoxFuture; use ros_env::action_msgs::srv::CancelGoal_Response; @@ -665,6 +665,81 @@ impl RclPrimitive for ActionServerExecutable { fn handle(&self) -> RclPrimitiveHandle<'_> { RclPrimitiveHandle::ActionServer(self.board.handle.lock()) } + + fn register_on_ready( + &self, + on_ready: Box, + ) -> Result>, RclrsError> { + use crate::executor::event_callback::{CompositeOnReady, OnReadyRegistration}; + + // An action server bundles three services (goal/cancel/result). Register + // a push callback on each, tagging events with the matching readiness + // flag so the executor runs the right handler. Goal expiration has no rcl + // push callback and is driven separately (a periodic poll in the executor). + let on_ready = Arc::new(on_ready); + let handle = &self.board.handle; + let mk = |bits: ActionServerReady| -> Box { + let on_ready = Arc::clone(&on_ready); + Box::new(move |n| on_ready(ReadyKind::ActionServer(bits), n)) + }; + + let regs: Vec> = vec![ + Box::new(OnReadyRegistration::new( + Arc::clone(handle), + set_action_server_goal_callback::, + mk(ActionServerReady { + goal_request: true, + ..Default::default() + }), + )?), + Box::new(OnReadyRegistration::new( + Arc::clone(handle), + set_action_server_cancel_callback::, + mk(ActionServerReady { + cancel_request: true, + ..Default::default() + }), + )?), + Box::new(OnReadyRegistration::new( + Arc::clone(handle), + set_action_server_result_callback::, + mk(ActionServerReady { + result_request: true, + ..Default::default() + }), + )?), + ]; + + Ok(Some(Box::new(CompositeOnReady(regs)))) + } +} + +/// Install (or, with a null callback/user_data, clear) the "on new goal request" +/// push callback on an action server. Encapsulates the handle lock and rcl call. +unsafe fn set_action_server_goal_callback( + handle: &ActionServerHandle, + callback: rcl_event_callback_t, + user_data: *const std::os::raw::c_void, +) -> rcl_ret_t { + rcl_action_server_set_goal_service_callback(&*handle.lock(), callback, user_data) +} + +/// As [`set_action_server_goal_callback`], for the cancel service. +unsafe fn set_action_server_cancel_callback( + handle: &ActionServerHandle, + callback: rcl_event_callback_t, + user_data: *const std::os::raw::c_void, +) -> rcl_ret_t { + rcl_action_server_set_cancel_service_callback(&*handle.lock(), callback, user_data) +} + +/// As [`set_action_server_goal_callback`], for the result service. +unsafe fn set_action_server_result_callback( + handle: &ActionServerHandle, + callback: rcl_event_callback_t, + user_data: *const std::os::raw::c_void, +) -> rcl_ret_t { + rcl_action_server_set_result_service_callback(&*handle.lock(), callback, user_data) } /// Manage the lifecycle of an `rcl_action_server_t`, including managing its dependencies diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index a229bac13..1535dc7ab 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -468,6 +468,31 @@ where fn kind(&self) -> RclPrimitiveKind { RclPrimitiveKind::Client } + + fn register_on_ready( + &self, + on_ready: Box, + ) -> Result>, RclrsError> { + // A client has a single readiness path; report it as `Basic`. + let on_ready = move |n| on_ready(ReadyKind::Basic, n); + let registration = crate::executor::event_callback::OnReadyRegistration::new( + Arc::clone(&self.handle), + set_client_on_new_response, + Box::new(on_ready), + )?; + Ok(Some(Box::new(registration))) + } +} + +/// Install (or, with a null callback/user_data, clear) the "on new response" +/// push callback used by the event-driven executor. Encapsulates the client +/// lock and the rcl call within this module. +unsafe fn set_client_on_new_response( + handle: &ClientHandle, + callback: rcl_event_callback_t, + user_data: *const std::os::raw::c_void, +) -> rcl_ret_t { + rcl_client_set_on_new_response_callback(&*handle.lock(), callback, user_data) } type SequenceNumber = i64; diff --git a/rclrs/src/dynamic_message/dynamic_subscription.rs b/rclrs/src/dynamic_message/dynamic_subscription.rs index ddc3de290..c2555ad60 100644 --- a/rclrs/src/dynamic_message/dynamic_subscription.rs +++ b/rclrs/src/dynamic_message/dynamic_subscription.rs @@ -236,6 +236,23 @@ impl RclPrimitive for DynamicSubscriptionExecutable { fn handle(&self) -> RclPrimitiveHandle { RclPrimitiveHandle::Subscription(self.handle.lock()) } + + fn register_on_ready( + &self, + on_ready: Box, + ) -> Result>, RclrsError> { + // A dynamic subscription shares the rcl subscription readiness path with a + // regular subscription, so report it as `Basic` and reuse the same setter. + // Without this, an event-driven executor never learns the subscription has + // a message and its callback never fires. + let on_ready = move |n| on_ready(ReadyKind::Basic, n); + let registration = crate::executor::event_callback::OnReadyRegistration::new( + Arc::clone(&self.handle), + crate::subscription::set_subscription_on_new_message, + Box::new(on_ready), + )?; + Ok(Some(Box::new(registration))) + } } /// Struct for receiving messages whose type is only known at runtime. diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 0d97f985f..a0b9ea563 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,6 +1,13 @@ mod basic_executor; pub use self::basic_executor::*; +pub(crate) mod event_callback; + +#[cfg(feature = "tokio-executor")] +mod tokio_executor; +#[cfg(feature = "tokio-executor")] +pub use self::tokio_executor::*; + use crate::{ Context, ContextHandle, GuardCondition, IntoNodeOptions, Node, RclrsError, Waitable, WeakActivityListener, diff --git a/rclrs/src/executor/event_callback.rs b/rclrs/src/executor/event_callback.rs new file mode 100644 index 000000000..f23b2c840 --- /dev/null +++ b/rclrs/src/executor/event_callback.rs @@ -0,0 +1,252 @@ +//! Safe RAII wrapper around rcl's "on new ___" push-callback APIs +//! (`rcl_subscription_set_on_new_message_callback` and the service/client/event +//! equivalents). +//! +//! These let an event-driven executor learn that an entity has become ready +//! *without* polling `rcl_wait`: the middleware invokes a C callback (possibly +//! from its own thread) when data arrives. We forward that to a Rust closure, +//! which an executor uses to enqueue work. +//! +//! [`OnReadyRegistration`] is generic over the entity handle type `H`. Each +//! entity module provides a [`SetOnReadyFn`] that locks its handle and calls the +//! appropriate `rcl_*_set_on_new_*_callback`; the registration owns the boxed +//! callback context and the handle, and deregisters on drop. +//! +//! # Safety model +//! +//! rcl stores the `user_data` pointer we hand it and passes it back to the C +//! callback on every event. That pointer must stay valid for as long as the +//! callback is registered. We therefore: +//! +//! - box the [`EventCallbackCtx`] so it has a stable heap address, and +//! - in `Drop`, **unregister the callback first** (so the middleware can no +//! longer invoke the trampoline) and only then free the context. +//! +//! Getting that ordering wrong is a use-after-free, since the middleware may be +//! calling the trampoline from another thread at the moment of teardown. + +use std::{os::raw::c_void, sync::Arc}; + +use crate::{rcl_bindings::*, OnReadyHandle, RclrsError, ToResult}; + +/// The context carried through rcl as `user_data`. Boxed so its address is +/// stable for the lifetime of the registration. +struct EventCallbackCtx { + on_ready: Box, +} + +/// The C trampoline that rcl/rmw invokes when an entity becomes ready. It may be +/// called from a middleware thread, so it does nothing but forward to the Rust +/// closure. It must not run user code or take locks that could deadlock the +/// middleware. +unsafe extern "C" fn on_ready_trampoline(user_data: *const c_void, number_of_events: usize) { + // SAFETY: `user_data` is the pointer to the `EventCallbackCtx` we passed to + // the rcl setter. It stays valid until the owning registration's `Drop` + // clears the callback, which always happens before the box is freed. + let ctx = unsafe { &*(user_data as *const EventCallbackCtx) }; + (ctx.on_ready)(number_of_events); +} + +/// A function that registers (or, with a null callback/user_data, clears) the +/// "on ready" push callback on an entity handle of type `H`. Implemented per +/// entity module so that `H`'s (private) lock and its specific +/// `rcl_*_set_on_new_*_callback` stay encapsulated there. +pub(crate) type SetOnReadyFn = unsafe fn(&H, rcl_event_callback_t, *const c_void) -> rcl_ret_t; + +/// RAII registration of a push "on ready" callback on an rcl entity. +/// +/// While alive, `on_ready(number_of_events)` is invoked by the middleware +/// whenever the entity becomes ready. Dropping it unregisters the callback +/// before releasing the context, so the middleware can never call into freed +/// memory. +pub(crate) struct OnReadyRegistration { + set_callback: SetOnReadyFn, + // Field order is important for teardown safety: `handle` is declared + // (and therefore dropped) before `ctx`. Dropping the last `Arc` + // finalizes the rcl entity (destroying the middleware reader), so by the + // time `ctx` is freed no middleware thread can still invoke the trampoline + // against it. This mirrors rclcpp, which frees its callback storage only + // after `rcl_*_fini`. See `Drop` below. + handle: Arc, + + // Never read directly, held only so its `Drop` frees the callback context. + #[allow(dead_code)] + ctx: CtxBox, +} + +impl OnReadyHandle for OnReadyRegistration {} + +/// Bundles several [`OnReadyHandle`]s into one, for composite primitives (action +/// servers/clients) that register a push callback per internal source. Dropping +/// it drops every contained registration, deregistering each callback. +// The Vec is never read — dropping it drops (and thus deregisters) every +// contained registration, which is the entire purpose of holding them. +pub(crate) struct CompositeOnReady(#[allow(dead_code)] pub(crate) Vec>); + +impl OnReadyHandle for CompositeOnReady {} + +impl OnReadyRegistration { + /// Register `on_ready` to be called by the middleware whenever the entity + /// becomes ready. `set_callback` locks `handle` and installs the trampoline. + pub(crate) fn new( + handle: Arc, + set_callback: SetOnReadyFn, + on_ready: Box, + ) -> Result { + let ctx = Box::into_raw(Box::new(EventCallbackCtx { on_ready })); + + // SAFETY: `ctx` points to a live, heap-stable context that outlives the + // registration (only freed once, when the `CtxBox` field is dropped). + let result = + unsafe { set_callback(&handle, Some(on_ready_trampoline), ctx as *const c_void).ok() }; + + if let Err(err) = result { + // Registration failed, so nothing else references `ctx`. Reclaim it. + // SAFETY: `ctx` came from `Box::into_raw` above and was never + // successfully registered. + unsafe { + drop(Box::from_raw(ctx)); + } + return Err(err); + } + + Ok(Self { + set_callback, + handle, + ctx: CtxBox(ctx), + }) + } +} + +impl Drop for OnReadyRegistration { + fn drop(&mut self) { + // Detach the callback so the middleware stops invoking the trampoline. + // The context is NOT freed here, the `ctx: CtxBox` field is dropped + // *after* `handle`, so the rcl entity is finalized before + // the context is freed, avoiding a use-after-free if a callback is still + // in flight at teardown. + // + // SAFETY: handle is valid and locked by the setter. A null callback + + // null user_data clears the registration. + unsafe { + let _ = (self.set_callback)(&self.handle, None, std::ptr::null()); + } + } +} + +/// Owns the heap-allocated [`EventCallbackCtx`] and frees it on drop. Kept as a +/// separate field of [`OnReadyRegistration`] so its drop runs *after* the +/// `handle` Arc. +struct CtxBox(*mut EventCallbackCtx); + +// SAFETY: the pointer is only dereferenced by the middleware via the trampoline +// (forwarding to a `Send + Sync` closure). It carries no thread-unsafe state. +unsafe impl Send for CtxBox {} +unsafe impl Sync for CtxBox {} + +impl Drop for CtxBox { + fn drop(&mut self) { + // SAFETY: by the time this runs, `OnReadyRegistration::drop` has cleared + // the callback and the `handle` field has been dropped (finalizing the + // rcl entity if it was the last reference), so no middleware thread can + // still be dereferencing this context. Reclaim it exactly once. + unsafe { + drop(Box::from_raw(self.0)); + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::{subscription::set_subscription_on_new_message, *}; + use ros_env::test_msgs::msg; + use std::{ + sync::atomic::{AtomicUsize, Ordering}, + time::{Duration, Instant}, + }; + + /// The push callback fires when messages arrive, without ever spinning the + /// executor (i.e. without `rcl_wait`). + #[test] + fn push_callback_fires_without_spinning() -> Result<(), RclrsError> { + let executor = Context::default().create_basic_executor(); + let node = executor.create_node(&format!("test_push_callback_{}", line!()))?; + let qos = QoSProfile::default().reliable().keep_last(10); + + let publisher = node.create_publisher::("test_push_topic".qos(qos))?; + let subscription = node + .create_subscription::("test_push_topic".qos(qos), |_: msg::Empty| {})?; + + let count = Arc::new(AtomicUsize::new(0)); + let count_cb = Arc::clone(&count); + let _registration = OnReadyRegistration::new( + Arc::clone(subscription.handle()), + set_subscription_on_new_message, + Box::new(move |n| { + count_cb.fetch_add(n, Ordering::Relaxed); + }), + )?; + + // Publish repeatedly (to ride out discovery) and wait for the push + // callback to fire. We deliberately never spin the executor. + let deadline = Instant::now() + Duration::from_secs(10); + while count.load(Ordering::Relaxed) == 0 && Instant::now() < deadline { + publisher.publish(msg::Empty::default())?; + std::thread::sleep(Duration::from_millis(20)); + } + + assert!( + count.load(Ordering::Relaxed) > 0, + "push callback never fired" + ); + Ok(()) + } + + /// Rapidly create and drop registrations while messages are flowing. If the + /// drop ordering is wrong (freeing the context before unregistering), the + /// middleware thread can call into freed memory; this stresses that path. + #[test] + fn rapid_register_unregister_is_sound() -> Result<(), RclrsError> { + let executor = Context::default().create_basic_executor(); + let node = executor.create_node(&format!("test_push_raii_{}", line!()))?; + let qos = QoSProfile::default().reliable().keep_last(10); + + let publisher = node.create_publisher::("test_push_raii_topic".qos(qos))?; + let subscription = node.create_subscription::( + "test_push_raii_topic".qos(qos), + |_: msg::Empty| {}, + )?; + + // A background thread floods the topic the whole time. + let stop = Arc::new(std::sync::atomic::AtomicBool::new(false)); + let stop_pub = Arc::clone(&stop); + let flood = std::thread::spawn(move || { + while !stop_pub.load(Ordering::Acquire) { + let _ = publisher.publish(msg::Empty::default()); + std::thread::sleep(Duration::from_micros(50)); + } + }); + + // Register/unregister many times against the live subscription. + for _ in 0..2000 { + let count = Arc::new(AtomicUsize::new(0)); + let count_cb = Arc::clone(&count); + let registration = OnReadyRegistration::new( + Arc::clone(subscription.handle()), + set_subscription_on_new_message, + Box::new(move |n| { + count_cb.fetch_add(n, Ordering::Relaxed); + }), + )?; + // Hold briefly so the middleware can fire into this context, then drop + // (which must unregister before freeing). + std::thread::sleep(Duration::from_micros(100)); + drop(registration); + } + + stop.store(true, Ordering::Release); + flood.join().unwrap(); + Ok(()) + } +} diff --git a/rclrs/src/executor/tokio_executor.rs b/rclrs/src/executor/tokio_executor.rs new file mode 100644 index 000000000..95c44a5f0 --- /dev/null +++ b/rclrs/src/executor/tokio_executor.rs @@ -0,0 +1,1969 @@ +//! Event-driven, Tokio-backed executor for rclrs. +//! +//! Readiness is push-based (rcl `set_on_new_*_callback`), not polled. Each +//! **Worker** (the node's default group is its main worker) gets its own +//! `tokio::mpsc` mailbox and one spawned task that drains it. Because a Tokio +//! task is never polled by two threads at once, that single task gives +//! per-worker mutual exclusion *and* FIFO ordering for free; Tokio's scheduler +//! provides the thread pool, work-stealing, and M:N multiplexing. So different +//! workers run in parallel automatically, with no per-event spawn and nothing +//! for the user to configure. +//! +//! Worker tasks are **gated by spinning**: they only execute ROS entity +//! callbacks (subscriptions, services, clients, timers, actions) while `spin()` +//! is active, preserving rclrs's contract that those callbacks do not run until +//! you spin and that none are still running once `spin()` returns (quiescence is +//! enforced by waiting for in-flight callbacks before returning). +//! +//! This gating applies to entity callbacks, not to free-standing async tasks +//! spawned through the executor commands (e.g. `commands().run(..)`): those are +//! ordinary Tokio tasks and run on the runtime independently of `spin()`. +//! Code that needs work confined to spinning should put it in an entity callback +//! rather than a spawned task. + +mod timer_scheduler; + +use std::{ + any::Any, + collections::HashMap, + panic::AssertUnwindSafe, + sync::{ + atomic::{AtomicBool, AtomicU64, AtomicUsize, Ordering}, + Arc, Mutex, + }, + time::{Duration, Instant}, +}; + +use futures::future::BoxFuture; +use tokio::sync::{ + mpsc::{UnboundedReceiver, UnboundedSender}, + watch, Notify, +}; + +use crate::{ + log_error, ActionClientReady, ActionServerReady, Context, ExecutorChannel, ExecutorRuntime, + ExecutorWorkerOptions, OnReadyHandle, PayloadTask, RclPrimitiveKind, RclReturnCode, RclrsError, + ReadyKind, SpinConditions, TimerSchedulerHandles, Waitable, WeakActivityListener, + WorkerChannel, +}; + +use timer_scheduler::{TimerRegistration, TimerScheduler}; + +pub(crate) use timer_scheduler::TimerSchedulerNotify; + +use super::Executor; + +/// Identifies an entity within a worker. +type EntityId = u64; + +/// A multi-threaded async executor backed by a Tokio runtime, driven by rcl push +/// callbacks, with one task per worker (see the module docs). +pub struct TokioExecutorRuntime { + host: RuntimeHost, + shared: Arc, +} + +impl TokioExecutorRuntime { + /// Create an executor that owns a fresh multi-threaded Tokio runtime. + /// + /// Users should call [`CreateTokioExecutor::create_tokio_executor`] instead. + pub(crate) fn new() -> Self { + let runtime = tokio::runtime::Builder::new_multi_thread() + .enable_all() + .build() + .expect("Failed to create Tokio runtime for rclrs executor"); + Self::with_runtime(runtime) + } + + /// Create an executor that owns a caller-provided Tokio runtime. + /// + /// Users should call + /// [`CreateTokioExecutor::create_tokio_executor_with_runtime`] instead. + pub(crate) fn with_runtime(runtime: tokio::runtime::Runtime) -> Self { + Self::from_host(RuntimeHost::Owned(runtime)) + } + + /// Create an executor that runs on a runtime adopted from the caller, rather + /// than owning one. + /// + /// Users should call + /// [`CreateTokioExecutor::create_tokio_executor_on_current_runtime`] or + /// [`CreateTokioExecutor::create_tokio_executor_with_handle`] instead. + pub(crate) fn with_handle(handle: tokio::runtime::Handle) -> Self { + Self::from_host(RuntimeHost::Adopted(handle)) + } + + fn from_host(host: RuntimeHost) -> Self { + let (spin, _) = watch::channel(false); + let timer_scheduler = Arc::new(TimerScheduler::new()); + Self { + host, + shared: Arc::new(ExecutorShared { + spin, + halt: Arc::new(Notify::new()), + active: Arc::new(AtomicUsize::new(0)), + outstanding: Arc::new(AtomicUsize::new(0)), + errors: Arc::new(Mutex::new(Vec::new())), + next_entity_id: Arc::new(AtomicU64::new(0)), + timer_scheduler, + }), + } + } + + fn take_errors(&self) -> Vec { + std::mem::take(&mut *self.shared.errors.lock().unwrap()) + } + + /// If the spin is bounded by an until-promise, spawn a task that flips the + /// halt flag and wakes the spin once the promise resolves, and return its + /// handle. The caller aborts that handle when the spin ends, so a spin that + /// stops for another reason (timeout, shutdown) does not leave the task + /// parked on `promise.await` forever, accumulating one detached task per such + /// spin. + fn arm_until_promise( + &self, + conditions: &mut SpinConditions, + ) -> Option> { + let promise = conditions.options.until_promise_resolved.take()?; + let halt_flag = Arc::clone(&conditions.halt_spinning); + let halt_notify = Arc::clone(&self.shared.halt); + Some(self.host.handle().spawn(async move { + let _ = promise.await; + halt_flag.store(true, Ordering::Release); + halt_notify.notify_waiters(); + })) + } + + /// The async core of spinning: open the gate so worker tasks run, wait until + /// the spin should stop, close the gate, then wait for in-flight callbacks to + /// finish (quiescence) before returning. Shared by the blocking `spin()` and + /// the awaitable `spin_async()`. It never calls `block_on`, so it can run on + /// any runtime, including the caller's in the adopted case. + async fn run_spin(&self, mut conditions: SpinConditions) -> Vec { + let promise_task = self.arm_until_promise(&mut conditions); + + let stop_time = conditions.options.timeout.map(|t| Instant::now() + t); + let only_once = conditions.options.only_next_available_work; + + // Open the gate so the worker tasks process, then wait until the spin + // should stop. + let _ = self.shared.spin.send(true); + let timed_out = block_until_stop( + Arc::clone(&conditions.halt_spinning), + conditions.context.clone(), + Arc::clone(&self.shared.active), + Arc::clone(&self.shared.outstanding), + Arc::clone(&self.shared.halt), + stop_time, + only_once, + ) + .await; + + // Close the gate: worker tasks park after finishing any in-flight message. + let _ = self.shared.spin.send(false); + + // Cancel the until-promise watcher so it doesn't outlive this spin. + if let Some(task) = promise_task { + task.abort(); + } + + // Wait for any in-flight callbacks to finish, so no ROS callback is + // running once the spin returns (quiescence). + await_quiescence(Arc::clone(&self.shared.active)).await; + + // Match the basic executor's contract: a timeout is reported as a + // `Timeout` error rather than a silent return. + let mut errors = self.take_errors(); + if timed_out { + errors.push(RclrsError::RclError { + code: RclReturnCode::Timeout, + msg: None, + }); + } + errors + } +} + +impl ExecutorRuntime for TokioExecutorRuntime { + fn channel(&self) -> Arc { + Arc::new(TokioExecutorChannel { + handle: self.host.handle(), + shared: Arc::clone(&self.shared), + }) + } + + fn spin(&mut self, conditions: SpinConditions) -> Vec { + // `spin()` blocks the calling thread by driving the spin to completion. It + // must not be called from within a Tokio runtime: it would block a runtime + // worker thread (an immediate deadlock on a current-thread runtime), and + // `block_on` from inside a runtime panics anyway. Give a clear message + // instead, and steer callers in an async context to `spin_async().await`. + assert!( + tokio::runtime::Handle::try_current().is_err(), + "Executor::spin() blocks the calling thread and must not be called from \ + within a Tokio runtime. Call Executor::spin_async().await instead.", + ); + + match &self.host { + RuntimeHost::Owned(runtime) => runtime.block_on(self.run_spin(conditions)), + // `Handle::block_on` is valid here because the assert above established + // we are not inside a runtime. `clone()` avoids borrowing `self.host` + // across the `self.run_spin(..)` borrow. + RuntimeHost::Adopted(handle) => handle.clone().block_on(self.run_spin(conditions)), + } + } + + fn spin_async( + self: Box, + conditions: SpinConditions, + ) -> BoxFuture<'static, (Box, Vec)> { + // Drive the spin on whatever runtime awaits this future (the caller's, in + // the adopted case). The worker tasks run on the host runtime's threads + // regardless, so no `block_on` and no helper thread are needed. + Box::pin(async move { + let errors = self.run_spin(conditions).await; + (self as Box, errors) + }) + } +} + +/// Where the executor's tasks run: a runtime it owns, or one adopted from the +/// caller (e.g. the `#[tokio::main]` runtime). +enum RuntimeHost { + /// A runtime this executor created. Used to `block_on` in the blocking + /// `spin()`, and dropped (shutting down its worker threads) with the executor. + Owned(tokio::runtime::Runtime), + /// A runtime adopted from the caller. We only ever spawn on it; we never + /// `block_on` it or shut it down. + Adopted(tokio::runtime::Handle), +} + +impl RuntimeHost { + /// A handle for spawning, regardless of which kind of host this is. + fn handle(&self) -> tokio::runtime::Handle { + match self { + RuntimeHost::Owned(runtime) => runtime.handle().clone(), + RuntimeHost::Adopted(handle) => handle.clone(), + } + } +} + +/// Block until the current spin should stop, returning whether it stopped because +/// the timeout elapsed (`true`) rather than because of a halt, a context +/// shutdown, or the available work draining (`false`). +/// +/// With `only_once` (the spin_once pattern) it waits for work to arrive, drains +/// it, then returns; while work is in flight it never times out mid-batch. Without +/// it, it runs until `stop_time` (if any), a halt, or context shutdown. +async fn block_until_stop( + halt: Arc, + context: Context, + active: Arc, + outstanding: Arc, + halt_notify: Arc, + stop_time: Option, + only_once: bool, +) -> bool { + // For `only_once`, poll tightly so we detect the available work draining + // without adding latency; otherwise a coarse poll is enough (we only re-check + // halt/timeout/context). + let poll = if only_once { + Duration::from_micros(200) + } else { + Duration::from_millis(100) + }; + + // Whether any work has been seen this spin, so `only_once` waits for work to + // arrive (up to the timeout) before declaring the batch drained. + let mut saw_work = false; + + loop { + if halt.load(Ordering::Acquire) { + return false; + } + + // Stop spinning once the ROS context is no longer valid (shutdown). + if !context.ok() { + return false; + } + + let busy = outstanding.load(Ordering::Acquire) > 0 || active.load(Ordering::Acquire) > 0; + if busy { + saw_work = true; + } + + if only_once { + // Process the currently-available work, then stop. While work is in + // flight we keep draining (never time out mid-batch); once it has + // drained we're done. If no work is in flight we wait for some to + // arrive, up to the timeout. + if saw_work && !busy { + return false; + } + + if !busy && stop_time.is_some_and(|st| Instant::now() >= st) { + return true; + } + } else if stop_time.is_some_and(|st| Instant::now() >= st) { + // Ran for the requested duration. + return true; + } + + let wait = stop_time + .map(|st| st.saturating_duration_since(Instant::now())) + .unwrap_or(poll) + .min(poll); + + tokio::select! { + _ = halt_notify.notified() => {} + _ = tokio::time::sleep(wait) => {} + } + } +} + +/// Wait until no callbacks are in flight across any worker. Used by `spin()` to +/// uphold quiescence before returning. +async fn await_quiescence(active: Arc) { + while active.load(Ordering::Acquire) > 0 { + tokio::time::sleep(Duration::from_micros(100)).await; + } +} + +/// This trait allows [`Context`] to create a Tokio-based executor. +/// +/// There are two families of constructors. The `create_tokio_executor*` methods +/// give the executor its **own** runtime. The `*_on_current_runtime` / +/// `*_with_handle` methods **adopt** a runtime the caller already has (for +/// example the one set up by `#[tokio::main]`), so ROS callbacks and the caller's +/// other Tokio work share a single runtime and thread pool. +/// +/// Inside `#[tokio::main]`, prefer the adopting constructors. An owned-runtime +/// executor is meant for a non-async context: its blocking `spin` cannot run +/// inside a runtime, and dropping the owned runtime from within a runtime panics +/// (a Tokio rule). The adopting constructors avoid both, since they own no +/// runtime. +/// +/// When adopting a runtime, note that: +/// +/// - The blocking [`Executor::spin`] must not be called from within a runtime; +/// use [`Executor::spin_async`]`.await` from async code. (This is true even +/// for an owned runtime: `spin` blocks the calling thread.) +/// - The adopted runtime must have Tokio's time driver enabled (`enable_time` or +/// `enable_all`; `#[tokio::main]` does by default), since timers and the +/// per-worker reap interval rely on it. +/// - A current-thread runtime is fine for async or short non-blocking callbacks, +/// but a blocking or CPU-bound sync callback occupies the single thread, and +/// `tokio::task::block_in_place` (the usual escape hatch) panics on a +/// current-thread runtime. Use a multi-threaded runtime if callbacks can block. +pub trait CreateTokioExecutor { + /// Create an event-driven Tokio-based executor associated with this + /// [`Context`], with its own default multi-threaded Tokio runtime. + fn create_tokio_executor(&self) -> Executor; + + /// Create an event-driven Tokio-based executor with a caller-provided Tokio + /// runtime (e.g. to control worker-thread count or names). The executor owns + /// the runtime. + fn create_tokio_executor_with_runtime(&self, runtime: tokio::runtime::Runtime) -> Executor; + + /// Create an event-driven Tokio-based executor that runs on the **current** + /// Tokio runtime instead of creating its own. Must be called from within a + /// runtime (panics otherwise, like [`tokio::runtime::Handle::current`]). See + /// the trait docs for the time-driver and current-thread caveats. + fn create_tokio_executor_on_current_runtime(&self) -> Executor; + + /// Create an event-driven Tokio-based executor that runs on the runtime + /// behind `handle`, instead of creating its own. Like + /// [`create_tokio_executor_on_current_runtime`][Self::create_tokio_executor_on_current_runtime] + /// but for callers that hold a [`Handle`][tokio::runtime::Handle] without + /// being on a runtime thread at construction time. + fn create_tokio_executor_with_handle(&self, handle: tokio::runtime::Handle) -> Executor; +} + +impl CreateTokioExecutor for Context { + fn create_tokio_executor(&self) -> Executor { + self.create_executor(TokioExecutorRuntime::new()) + } + + fn create_tokio_executor_with_runtime(&self, runtime: tokio::runtime::Runtime) -> Executor { + self.create_executor(TokioExecutorRuntime::with_runtime(runtime)) + } + + fn create_tokio_executor_on_current_runtime(&self) -> Executor { + self.create_executor(TokioExecutorRuntime::with_handle( + tokio::runtime::Handle::current(), + )) + } + + fn create_tokio_executor_with_handle(&self, handle: tokio::runtime::Handle) -> Executor { + self.create_executor(TokioExecutorRuntime::with_handle(handle)) + } +} + +struct TokioExecutorChannel { + handle: tokio::runtime::Handle, + shared: Arc, +} + +impl ExecutorChannel for TokioExecutorChannel { + fn create_worker(&self, options: ExecutorWorkerOptions) -> Arc { + let (mailbox_tx, mailbox_rx) = tokio::sync::mpsc::unbounded_channel(); + let entities = Arc::new(Mutex::new(HashMap::new())); + let listeners = Arc::new(Mutex::new(Vec::new())); + + // One task per worker. Tokio schedules it. Different workers therefore run + // concurrently, while this worker's callbacks stay serialized and ordered. + // The reap `Interval` is created inside the spawned task (i.e. within the + // Tokio runtime), which is where a Tokio timer must be constructed. + let worker_entities = Arc::clone(&entities); + let worker_listeners = Arc::clone(&listeners); + let spinning = self.shared.spin.subscribe(); + let error_sink = Arc::clone(&self.shared.errors); + let active = Arc::clone(&self.shared.active); + let outstanding = Arc::clone(&self.shared.outstanding); + let payload = options.payload; + self.handle.spawn(async move { + WorkerLoop { + mailbox: mailbox_rx, + entities: worker_entities, + payload, + listeners: worker_listeners, + spinning, + error_sink, + active, + outstanding, + reap: tokio::time::interval(Duration::from_secs(1)), + } + .run() + .await + }); + + Arc::new(TokioWorkerChannel { + handle: self.handle.clone(), + mailbox: mailbox_tx, + entities, + listeners, + errors: Arc::clone(&self.shared.errors), + next_entity_id: Arc::clone(&self.shared.next_entity_id), + outstanding: Arc::clone(&self.shared.outstanding), + timer_scheduler: Arc::clone(&self.shared.timer_scheduler), + }) + } + + fn wake_all_wait_sets(&self) { + // Wake any in-progress spin so it re-checks halt_spinning promptly. + self.shared.halt.notify_waiters(); + } +} + +struct TokioWorkerChannel { + handle: tokio::runtime::Handle, + mailbox: UnboundedSender, + entities: Arc>>>, + listeners: Arc>>, + errors: Arc>>, + next_entity_id: Arc, + outstanding: Arc, + timer_scheduler: Arc, +} + +impl TokioWorkerChannel { + /// Build the push-callback closure for an entity. It forwards each + /// middleware notification (with its event count) to the entity's + /// [`EntityDispatch`], which combines repeated notifications so the mailbox + /// holds at most one pending `Ready` per entity even while spinning is paused. + fn make_on_ready( + &self, + dispatch: Arc, + ready: Option>>, + ) -> Box { + Box::new(move |kind, count| { + // Composite primitives merge their per-sub-entity readiness; simple + // ones have no accumulator and stay lock-free. + if let Some(acc) = &ready { + merge_ready(&mut acc.lock().unwrap(), kind); + } + dispatch.notify(count.max(1)); + }) + } +} + +impl WorkerChannel for TokioWorkerChannel { + fn add_async_task(&self, f: BoxFuture<'static, ()>) { + self.handle.spawn(f); + } + + /// Register a worker entity with the executor: insert it into the registry, + /// then install its push callback (for message-driven primitives) or register + /// it with the shared timer scheduler (for timers), so the entity's readiness + /// reaches this worker's mailbox. + /// + /// Guard conditions have no rcl push-callback API, so `register_on_ready` + /// returns `None` and they sit inert here. That is correct for the per-worker + /// wakeup guard conditions: there is no `rcl_wait` to interrupt, since new + /// entities register their callback immediately, payload tasks go straight to + /// the mailbox, and removals are reaped. The one guard condition that does + /// carry a callback is the node graph guard condition (see `node_options.rs`); + /// rmw exposes no "on trigger" callback for guard conditions, so on this + /// executor graph changes are not event-driven. This is not a correctness + /// regression: graph-change listeners (`Node::notify_on_graph_change`) + /// re-check their condition on a period regardless of notifications, so they + /// still resolve, just within that period rather than immediately. + /// Lower-latency graph-change handling is left as future work. + fn add_to_wait_set(&self, new_entity: Waitable) { + let id = self.next_entity_id.fetch_add(1, Ordering::Relaxed); + let kind = new_entity.kind(); + + // Composite primitives (action servers/clients) report different readiness + // per sub-entity through the same entity; accumulate the merged readiness + // here. Simple primitives are always `Basic` and skip this (lock-free). + let ready: Option>> = match kind { + RclPrimitiveKind::ActionServer => Some(Arc::new(Mutex::new(ReadyKind::ActionServer( + ActionServerReady::default(), + )))), + RclPrimitiveKind::ActionClient => Some(Arc::new(Mutex::new(ReadyKind::ActionClient( + ActionClientReady::default(), + )))), + _ => None, + }; + + // Shared readiness dispatch: combines repeated notifications into at most + // one pending `Ready` per entity, with the event count accumulated (and, + // for composite primitives, readiness flags merged in `ready`). + let dispatch = Arc::new(EntityDispatch::new( + id, + self.mailbox.clone(), + Arc::clone(&self.outstanding), + )); + let on_ready = self.make_on_ready(Arc::clone(&dispatch), ready.clone()); + + // Grab the timer-scheduler inputs before `new_entity` is moved into the + // registry below. + let timer_handles = new_entity.timer_scheduler_handles(); + let in_use = new_entity.in_use_handle(); + + // Insert into the registry BEFORE registering the push callback (or the + // timer), so the entity is always resolvable by the time any readiness + // can enqueue a `Ready` for it. + // + // Registering first would race: an early middleware callback could fire, + // enqueue a `Ready`, and have the worker drop it (entity not found yet), + // leaving the dispatch flag stuck set so no further `Ready` is ever sent. + // The `_on_ready` handle is filled in just below, once the callback is live. + let entry = Arc::new(WorkerEntity { + waitable: Mutex::new(new_entity), + dispatch: Arc::clone(&dispatch), + ready: ready.clone(), + _on_ready: Mutex::new(None), + }); + self.entities.lock().unwrap().insert(id, Arc::clone(&entry)); + + // Now register the push callback against the (already-inserted) entity. + // Holding the waitable lock here is safe: the callback only touches the + // entity's `dispatch` (atomics + mailbox), never the waitable. + let registration = match entry.waitable.lock().unwrap().register_on_ready(on_ready) { + Ok(registration) => registration, + Err(err) => { + // Surface the failure both in the log and via spin()'s error + // return, rather than silently leaving an inert entity. + log_error!( + "rclrs.executor.tokio_executor", + "Failed to register an on-ready callback: {err}", + ); + self.errors.lock().unwrap().push(err); + None + } + }; + *entry._on_ready.lock().unwrap() = registration; + + // Timers have no rcl push callback. Register them with the shared timer + // scheduler, which paces all timers from one thread and delivers each fire + // through the same `dispatch` as push callbacks, so a tick is one bounded + // take like any other event. + if let Some(TimerSchedulerHandles { + rcl_timer, + notify_slot, + }) = timer_handles + { + let notify = self.timer_scheduler.register(TimerRegistration { + rcl_timer, + dispatch: Arc::clone(&dispatch), + in_use: Arc::clone(&in_use), + }); + *notify_slot.lock().unwrap() = Some(notify); + } + + // Action-server goal expiration has no rcl push callback (rcl uses an + // internal timer); poll it periodically so completed goals are cleaned up. + if kind == RclPrimitiveKind::ActionServer { + if let Some(acc) = ready { + self.handle + .spawn(action_expire_driver(in_use, Arc::clone(&dispatch), acc)); + } + } + } + + fn send_payload_task(&self, f: PayloadTask) { + // Counts as outstanding work until a worker handles it (so + // `only_next_available_work` waits for payload tasks too). + self.outstanding.fetch_add(1, Ordering::AcqRel); + let _ = self.mailbox.send(WorkerMsg::Payload(f)); + } + + fn add_activity_listener(&self, listener: WeakActivityListener) { + self.listeners.lock().unwrap().push(listener); + } +} + +/// State shared between the runtime and all workers. +struct ExecutorShared { + /// Gate the worker tasks observe: they execute only while this is `true`. + spin: watch::Sender, + + /// Promptly wakes `spin()` when a halt is requested. + halt: Arc, + + /// Number of callbacks currently executing across all workers. `spin()` + /// waits for this to reach zero before returning, so no ROS callback is + /// running once `spin()` has returned (quiescence). + active: Arc, + + /// Number of mailbox messages enqueued across all workers but not yet + /// handled (queued *or* in flight). `spin()` with `only_next_available_work` + /// uses this to detect when the currently-available work has drained. + outstanding: Arc, + + /// Errors produced by callbacks; drained and returned by `spin()`. + errors: Arc>>, + + /// Allocates entity ids across all workers. + next_entity_id: Arc, + + /// Shared timer scheduler (one heap, one waiter) for all workers' timers. + timer_scheduler: Arc, +} + +/// The per-worker event loop. Spawned once per worker (see +/// [`TokioExecutorChannel::create_worker`]). It owns the worker's payload and +/// drains its mailbox for the executor's lifetime, running each message against +/// the payload (and the worker's activity listeners) while the executor is +/// spinning. +struct WorkerLoop { + mailbox: UnboundedReceiver, + entities: Arc>>>, + payload: Box, + listeners: Arc>>, + spinning: watch::Receiver, + error_sink: Arc>>, + + /// Callbacks currently in flight across all workers; `spin()` waits for this + /// to reach zero before returning (quiescence). + active: Arc, + + /// Mailbox messages enqueued but not yet handled; `spin()` uses this to know + /// when the currently-available work has drained. + outstanding: Arc, + + /// Fires periodically so the loop can drop entities whose owning handle has + /// been released (see [`next_message`][Self::next_message]). Must be created + /// inside the Tokio runtime, so the loop is constructed in the spawned task. + reap: tokio::time::Interval, +} + +impl WorkerLoop { + /// Drain the mailbox for the executor's lifetime. Each message is gated on + /// the executor spinning, then handled. Returns when the worker's mailbox is + /// dropped or the executor itself is dropped. + async fn run(mut self) { + loop { + let Some(msg) = self.next_message().await else { + return; // worker dropped + }; + + if !self.wait_until_spinning().await { + return; // executor dropped + } + + self.handle(msg); + + // The message is fully handled: it is no longer in flight, and no + // longer counts as outstanding work for `spin()`. + self.active.fetch_sub(1, Ordering::AcqRel); + self.outstanding.fetch_sub(1, Ordering::AcqRel); + } + } + + /// Wait for the next mailbox message, reaping dropped entities on the side. + /// + /// On a periodic tick we drop entities whose owning handle has been released, + /// so we stop holding their rcl handle and push-callback registration. + /// Entities on active topics are also reaped on-event in + /// [`run_ready_entity`][Self::run_ready_entity]; this catches idle ones. + /// Returns `None` once the worker (its mailbox sender) has been dropped. + async fn next_message(&mut self) -> Option { + // Split the borrow so `select!` can poll the reap timer and the mailbox + // (two separate fields) at the same time. + let Self { + reap, + mailbox, + entities, + .. + } = self; + loop { + tokio::select! { + _ = reap.tick() => { + entities + .lock() + .unwrap() + .retain(|_, e| e.waitable.lock().unwrap().in_use()); + } + msg = mailbox.recv() => return msg, + } + } + } + + /// Count the pending message as in-flight and block until the executor is + /// spinning. + /// + /// The in-flight count is incremented *before* the gate is checked, so a + /// concurrent `spin()` closing the gate either observes this unit (and waits + /// for it) or has already closed the gate before we run anything. Either way + /// no callback runs after `spin()` returns. While parked the count is released + /// so it does not hold up quiescence. + /// + /// Returns `true` once spinning, with the in-flight count left incremented; + /// the caller decrements it once the message is handled. Returns `false` if + /// the executor was dropped, with the in-flight count already released. + async fn wait_until_spinning(&mut self) -> bool { + self.active.fetch_add(1, Ordering::AcqRel); + + loop { + if *self.spinning.borrow_and_update() { + return true; + } + + self.active.fetch_sub(1, Ordering::AcqRel); + + if self.spinning.changed().await.is_err() { + return false; // executor dropped + } + + self.active.fetch_add(1, Ordering::AcqRel); + } + } + + /// Run a single mailbox message against the worker's payload. + fn handle(&mut self, msg: WorkerMsg) { + match msg { + WorkerMsg::Ready { entity } => { + let errors = self.run_ready_entity(entity); + if !errors.is_empty() { + self.error_sink.lock().unwrap().extend(errors); + } + } + WorkerMsg::Payload(task) => { + // Contain a panic so a bad task cannot kill the worker. + if std::panic::catch_unwind(AssertUnwindSafe(|| task(&mut *self.payload))).is_err() + { + log_error!( + "rclrs.executor.tokio_executor", + "A payload task panicked; the executor contained the panic \ + and continues.", + ); + } + } + } + } + + /// Handle a `Ready` notification for `entity`: re-arm notification combining, + /// then either + /// run its callback(s) or, if its owning handle has been dropped, deregister + /// it. Returns any errors the callbacks produced. + fn run_ready_entity(&mut self, entity: EntityId) -> Vec { + // Clone the entry out under a brief lock so a callback may create new + // entities on this worker without deadlocking. + let Some(entry) = self.entities.lock().unwrap().get(&entity).cloned() else { + return Vec::new(); + }; + + // Take all pending events and clear the queued flag; a notification that + // races this re-arms and queues a fresh `Ready`, so no wakeup is lost + // (see [`EntityDispatch::take_pending`]). + let count = entry.dispatch.take_pending(); + let ready = match &entry.ready { + None => ReadyKind::Basic, + Some(acc) => { + let mut acc = acc.lock().unwrap(); + let taken = *acc; + *acc = neutral_ready(&taken); + taken + } + }; + + let mut waitable = entry.waitable.lock().unwrap(); + if !waitable.in_use() { + // The owning handle was dropped: deregister and never run a callback + // for a dropped entity. + drop(waitable); + self.entities.lock().unwrap().remove(&entity); + return Vec::new(); + } + + let (ran, errors) = Self::execute_ready(&mut waitable, ready, count, &mut *self.payload); + drop(waitable); + + if ran { + self.run_listeners_contained(); + } + errors + } + + /// Take up to `count` items from `waitable` and run its callback for each, + /// stopping early once a take turns up empty. + /// + /// The work runs inside `catch_unwind` so a panicking callback cannot kill + /// the worker task or leak the `active`/`outstanding` counters that `spin()` + /// waits on (which would wedge quiescence forever). The mutex guard is held + /// by the caller *outside* the closure, so it drops normally (unpoisoned) if + /// the callback unwinds. Returns whether any callback ran and any errors. + fn execute_ready( + waitable: &mut Waitable, + ready: ReadyKind, + count: usize, + payload: &mut dyn Any, + ) -> (bool, Vec) { + let exec = std::panic::catch_unwind(AssertUnwindSafe(|| { + let mut ran = false; + let mut errors = Vec::new(); + for _ in 0..count.max(1) { + // SAFETY: `payload` is this worker's payload and `waitable` was + // registered on this worker, so its primitive expects exactly + // this payload type. + match unsafe { waitable.execute_with(ready, payload) } { + Ok(()) => ran = true, + Err(err) if err.is_take_failed() => break, + Err(err) => { + errors.push(err); + break; + } + } + } + (ran, errors) + })); + + exec.unwrap_or_else(|_| { + log_error!( + "rclrs.executor.tokio_executor", + "A callback panicked while spinning; the executor contained the \ + panic and continues. The worker's payload may now be in an \ + inconsistent state.", + ); + (false, Vec::new()) + }) + } + + /// Run this worker's activity listeners against the payload, containing any + /// panic so a bad listener cannot kill the worker task. + fn run_listeners_contained(&mut self) { + if std::panic::catch_unwind(AssertUnwindSafe(|| { + crate::worker::run_activity_listeners(&self.listeners, &mut *self.payload); + })) + .is_err() + { + log_error!( + "rclrs.executor.tokio_executor", + "A worker activity listener panicked; the executor contained the \ + panic and continues.", + ); + } + } +} + +/// A message delivered to a worker's task. +enum WorkerMsg { + /// The entity became ready, take and run its callback(s). At most one such + /// message is outstanding per entity at a time, and the worker takes all + /// pending events when it handles it (see [`EntityDispatch`]). + Ready { entity: EntityId }, + + /// Run a one-shot task against the worker's payload. + Payload(PayloadTask), +} + +/// Delivers an entity's readiness to its worker, combining repeated +/// notifications so the mailbox holds at most one pending `Ready` per entity. +/// +/// The middleware can fire many notifications for one entity in a row. Rather +/// than queue one mailbox message per notification, we combine them: `scheduled` +/// is set while a `Ready` for this entity is already queued and not yet handled, +/// so further notifications only add their event count to `pending` instead of +/// queueing another message. That keeps the mailbox to at most one pending +/// `Ready` per entity even while spinning is paused, and no work is lost: the +/// worker takes exactly `pending` items. `outstanding` is the executor-wide +/// in-flight counter `spin()` uses for quiescence. +/// +/// Shared (behind an `Arc`) by every path that can make an entity ready — the +/// push callback, the timer scheduler, and the worker that drains it. +pub(crate) struct EntityDispatch { + /// Identifies the entity in `Ready` messages. + id: EntityId, + /// The owning worker's mailbox. + mailbox: UnboundedSender, + /// Set while a `Ready` for this entity is queued and not yet handled. + scheduled: AtomicBool, + /// Ready events reported but not yet taken by the worker. + pending: AtomicUsize, + /// Executor-wide count of queued-but-unhandled messages, for `spin()`. + outstanding: Arc, +} + +impl EntityDispatch { + fn new( + id: EntityId, + mailbox: UnboundedSender, + outstanding: Arc, + ) -> Self { + Self { + id, + mailbox, + scheduled: AtomicBool::new(false), + pending: AtomicUsize::new(0), + outstanding, + } + } + + /// Record `events` ready events and queue a `Ready` if one isn't already in + /// flight. Returns `false` if the worker's mailbox is closed. + pub(crate) fn notify(&self, events: usize) -> bool { + self.pending.fetch_add(events, Ordering::AcqRel); + if self.scheduled.swap(true, Ordering::AcqRel) { + true + } else { + self.enqueue_ready() + } + } + + /// Like [`notify`][Self::notify] but counts at most one pending event: for + /// idempotent readiness (e.g. action-goal expiry) where a single run clears + /// everything, so a queued-but-unhandled `Ready` must not accumulate more. + pub(crate) fn notify_once(&self) -> bool { + if self.scheduled.swap(true, Ordering::AcqRel) { + true + } else { + self.pending.fetch_add(1, Ordering::AcqRel); + self.enqueue_ready() + } + } + + /// Whether a `Ready` is currently queued and not yet handled. + pub(crate) fn is_scheduled(&self) -> bool { + self.scheduled.load(Ordering::Acquire) + } + + /// Worker side: clear the queued flag and take all pending events. Clearing + /// *before* the take lets a notification that races the drain queue a fresh + /// `Ready`, so no wakeup is lost. + fn take_pending(&self) -> usize { + self.scheduled.store(false, Ordering::Release); + self.pending.swap(0, Ordering::AcqRel) + } + + /// Count one outstanding message and send the `Ready`. Returns `false` if the + /// mailbox is closed. + fn enqueue_ready(&self) -> bool { + self.outstanding.fetch_add(1, Ordering::AcqRel); + self.mailbox + .send(WorkerMsg::Ready { entity: self.id }) + .is_ok() + } +} + +/// An entity owned by a worker: registration inserts it, the worker task runs it. +struct WorkerEntity { + waitable: Mutex, + + /// Delivers this entity's readiness to the worker, combining repeated + /// notifications into at most one pending `Ready` (see [`EntityDispatch`]). + /// Shared with the entity's push callback and, for timers, the scheduler. + dispatch: Arc, + + /// Merged readiness for composite primitives (action servers/clients) whose + /// sub-entities report *different* [`ReadyKind`]s through the same entity. + /// Notifications OR their flags in; the worker swaps it out (resetting to the + /// kind's neutral value) and runs the primitive with it. `None` for primitives + /// with a single readiness path (subscriptions/services/clients/timers), which + /// are always [`ReadyKind::Basic`] — keeping their hot path lock-free. + ready: Option>>, + + /// Keeps the push callback registered; dropping it deregisters. `None` for + /// passive entities (e.g. guard conditions) or timers (driven separately). + /// Behind a `Mutex` so it can be filled in *after* the entity is inserted + /// into the registry: registering before insertion would let an early + /// middleware callback enqueue a `Ready` the worker can't resolve, which it + /// would drop, wedging the entity with its dispatch flag stuck set. + _on_ready: Mutex>>, +} + +/// OR the readiness flags of `new` into `acc`. Used to merge the +/// per-sub-entity readiness of an action server/client into one value before the +/// worker runs the primitive. Basic and mismatched variants leave `acc` as-is. +fn merge_ready(acc: &mut ReadyKind, new: ReadyKind) { + match (acc, new) { + (ReadyKind::ActionServer(a), ReadyKind::ActionServer(b)) => { + a.goal_request |= b.goal_request; + a.cancel_request |= b.cancel_request; + a.result_request |= b.result_request; + a.goal_expired |= b.goal_expired; + } + (ReadyKind::ActionClient(a), ReadyKind::ActionClient(b)) => { + a.feedback |= b.feedback; + a.status |= b.status; + a.goal_response |= b.goal_response; + a.cancel_response |= b.cancel_response; + a.result_response |= b.result_response; + } + _ => {} + } +} + +/// The "no readiness" value for `kind`'s variant, used to reset an accumulator +/// after the worker has taken its merged readiness. +fn neutral_ready(kind: &ReadyKind) -> ReadyKind { + match kind { + ReadyKind::Basic => ReadyKind::Basic, + ReadyKind::ActionServer(_) => ReadyKind::ActionServer(ActionServerReady::default()), + ReadyKind::ActionClient(_) => ReadyKind::ActionClient(ActionClientReady::default()), + } +} + +/// How often to poll an action server for expired goals. Goal expiration is +/// driven by an rcl-internal timer with no push callback, so we poll instead. +/// The interval only bounds how promptly a *completed* goal is cleaned up +/// (typically well after its multi-second result timeout), so it can be coarse. +const ACTION_EXPIRE_POLL: Duration = Duration::from_millis(100); + +/// Periodically nudge an action server to expire completed goals (there is no +/// rcl push callback for expiration). Enqueues a `goal_expired` readiness through +/// the same coalescing path as other events; the worker runs +/// `rcl_action_expire_goals`, which is a cheap no-op when nothing has expired. +/// Stops once the action server's owning entity is dropped. +async fn action_expire_driver( + in_use: Arc, + dispatch: Arc, + ready: Arc>, +) { + loop { + tokio::time::sleep(ACTION_EXPIRE_POLL).await; + if !in_use.load(Ordering::Acquire) { + return; + } + // Flag expiry as ready and deliver it through the dispatch. Expiry is + // idempotent (one run clears all expired goals), so `notify_once` keeps + // at most one pending take even while spinning is paused — otherwise the + // 100ms poll would accumulate redundant work for the whole pause. + merge_ready( + &mut ready.lock().unwrap(), + ReadyKind::ActionServer(ActionServerReady { + goal_expired: true, + ..Default::default() + }), + ); + if !dispatch.notify_once() { + return; // worker gone + } + } +} + +#[cfg(test)] +mod tests { + use crate::*; + use ros_env::{test_msgs, test_msgs::msg}; + use std::{ + sync::{ + atomic::{AtomicBool, AtomicUsize, Ordering}, + Arc, + }, + time::{Duration, Instant}, + }; + + /// The executor can adopt the current Tokio runtime (the `#[tokio::main]` + /// pattern) instead of owning one, and deliver messages while driven by + /// `spin_async().await` from within that runtime. + #[tokio::test(flavor = "multi_thread")] + async fn tokio_adopts_current_runtime() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor_on_current_runtime(); + let node = executor.create_node( + format!("test_tokio_adopt_mt_{}", line!()).start_parameter_services(false), + )?; + let qos = QoSProfile::default().reliable().keep_last(10); + + let received = Arc::new(AtomicUsize::new(0)); + let received_cb = Arc::clone(&received); + let _sub = node.create_subscription::( + "tokio_adopt_mt_topic".qos(qos), + move |_m: msg::Empty| { + received_cb.fetch_add(1, Ordering::Relaxed); + }, + )?; + let publisher = node.create_publisher::("tokio_adopt_mt_topic".qos(qos))?; + + // Drive the executor with `spin_async` on the current runtime (no separate + // runtime, no helper thread), republishing to ride out discovery until a + // message arrives. + let deadline = Instant::now() + Duration::from_secs(10); + while received.load(Ordering::Relaxed) == 0 && Instant::now() < deadline { + publisher.publish(msg::Empty::default())?; + let (exec, _) = executor + .spin_async(SpinOptions::spin_once().timeout(Duration::from_millis(200))) + .await; + executor = exec; + } + + assert!( + received.load(Ordering::Relaxed) > 0, + "no message delivered while running on the adopted runtime", + ); + Ok(()) + } + + /// Adopting a current-thread runtime works for a short non-blocking callback. + #[tokio::test(flavor = "current_thread")] + async fn tokio_adopts_current_thread_runtime() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor_on_current_runtime(); + let node = executor.create_node( + format!("test_tokio_adopt_ct_{}", line!()).start_parameter_services(false), + )?; + let qos = QoSProfile::default().reliable().keep_last(10); + + let received = Arc::new(AtomicUsize::new(0)); + let received_cb = Arc::clone(&received); + let _sub = node.create_subscription::( + "tokio_adopt_ct_topic".qos(qos), + move |_m: msg::Empty| { + received_cb.fetch_add(1, Ordering::Relaxed); + }, + )?; + let publisher = node.create_publisher::("tokio_adopt_ct_topic".qos(qos))?; + + let deadline = Instant::now() + Duration::from_secs(10); + while received.load(Ordering::Relaxed) == 0 && Instant::now() < deadline { + publisher.publish(msg::Empty::default())?; + let (exec, _) = executor + .spin_async(SpinOptions::spin_once().timeout(Duration::from_millis(200))) + .await; + executor = exec; + } + + assert!( + received.load(Ordering::Relaxed) > 0, + "no message delivered on an adopted current-thread runtime", + ); + Ok(()) + } + + /// Blocking `spin()` must not be called from within a Tokio runtime; it panics + /// with guidance to use `spin_async` instead. + /// + /// Uses the adopted constructor so there is no owned runtime to drop inside + /// this async test (dropping an owned Tokio runtime from within a runtime is + /// itself a panic, which is exactly why owned mode is not for `#[tokio::main]`). + #[tokio::test(flavor = "multi_thread")] + async fn tokio_blocking_spin_within_runtime_panics() { + let mut executor = Context::default().create_tokio_executor_on_current_runtime(); + let _node = executor + .create_node( + format!("test_tokio_spin_panic_{}", line!()).start_parameter_services(false), + ) + .unwrap(); + + // A scary panic message on stderr here is expected; the test asserts the + // panic happened. + let panicked = std::panic::catch_unwind(std::panic::AssertUnwindSafe(|| { + executor.spin(SpinOptions::default().timeout(Duration::from_millis(10))); + })) + .is_err(); + assert!( + panicked, + "blocking spin() inside a Tokio runtime should panic" + ); + } + + /// A spin with a timeout and no work reports a `Timeout` error, matching the + /// basic executor's contract (rather than returning silently). + #[test] + fn tokio_spin_timeout_reports_error() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let _node = executor.create_node( + format!("test_tokio_timeout_{}", line!()).start_parameter_services(false), + )?; + + let errors = executor.spin(SpinOptions::default().timeout(Duration::from_millis(20))); + assert!( + errors.iter().any(|e| matches!( + e, + RclrsError::RclError { + code: RclReturnCode::Timeout, + .. + } + )), + "expected a Timeout error from a timed-out spin, got {errors:?}", + ); + Ok(()) + } + + /// `only_next_available_work` (spin_once) drains the currently-available work + /// and returns promptly — it must not be ignored (loop forever) on the Tokio + /// path. We publish then spin_once until the message is delivered. + #[test] + fn tokio_spin_once_processes_available_work() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_spin_once_{}", line!()).start_parameter_services(false), + )?; + let qos = QoSProfile::default().reliable().keep_last(10); + + let received = Arc::new(AtomicUsize::new(0)); + let received_cb = Arc::clone(&received); + let _sub = node.create_subscription::( + "tokio_spin_once_topic".qos(qos), + move |_m: msg::Empty| { + received_cb.fetch_add(1, Ordering::Relaxed); + }, + )?; + let publisher = node.create_publisher::("tokio_spin_once_topic".qos(qos))?; + + // Each spin_once waits up to its timeout for work, drains it, and returns; + // republish to ride out discovery. A wedged/ignored spin_once would never + // deliver the message and this would time out at the outer deadline. + let deadline = Instant::now() + Duration::from_secs(10); + while received.load(Ordering::Relaxed) == 0 && Instant::now() < deadline { + publisher.publish(msg::Empty::default())?; + let _ = executor.spin(SpinOptions::spin_once().timeout(Duration::from_millis(200))); + } + + assert!( + received.load(Ordering::Relaxed) > 0, + "spin_once never delivered the message (only_next_available_work ignored?)", + ); + Ok(()) + } + + /// Regression for strict quiescence: once `spin()` returns, no callback may + /// still be running. The callback signals the moment it starts (resolving the + /// until-promise, so spinning is asked to stop *while it runs*) and then + /// blocks for 400ms. `spin()` must not return until it has finished — proven + /// by `completed` being set and by the elapsed time exceeding the callback's + /// duration. Using the start-signal (rather than a fixed sleep) makes the + /// test robust to discovery/delivery latency. + #[test] + fn tokio_spin_waits_for_in_flight_callback() -> Result<(), RclrsError> { + use futures::channel::oneshot; + use std::sync::Mutex; + + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_quiescence_{}", line!()).start_parameter_services(false), + )?; + let qos = QoSProfile::default().reliable().keep_last(10); + + let count = Arc::new(AtomicUsize::new(0)); + let completed = Arc::new(AtomicBool::new(false)); + // The long blocking body runs only once "armed", so discovery stays fast. + let armed = Arc::new(AtomicBool::new(false)); + // Sender the callback uses to announce that it has started running. + let start_tx = Arc::new(Mutex::new(None::>)); + + let (count_cb, completed_cb, armed_cb, tx_cb) = ( + Arc::clone(&count), + Arc::clone(&completed), + Arc::clone(&armed), + Arc::clone(&start_tx), + ); + let _sub = node.create_subscription::( + "tokio_quiescence_topic".qos(qos), + move |_m: msg::Empty| { + count_cb.fetch_add(1, Ordering::Relaxed); + if armed_cb.swap(false, Ordering::AcqRel) { + if let Some(tx) = tx_cb.lock().unwrap().take() { + let _ = tx.send(()); + } + std::thread::sleep(Duration::from_millis(400)); + completed_cb.store(true, Ordering::Release); + } + }, + )?; + let publisher = node.create_publisher::("tokio_quiescence_topic".qos(qos))?; + + // Discovery: spin_once (fast callback) until a message lands. + let deadline = Instant::now() + Duration::from_secs(10); + while count.load(Ordering::Relaxed) == 0 && Instant::now() < deadline { + publisher.publish(msg::Empty::default())?; + let _ = executor.spin(SpinOptions::spin_once().timeout(Duration::from_millis(200))); + } + assert!( + count.load(Ordering::Relaxed) > 0, + "discovery never delivered" + ); + + // Arm, then spin until the callback *starts* (the promise resolves from + // inside it). spin() waits for the message and the callback to begin, so + // there is no fixed-window race; the 10s timeout is just a safety net. + let (tx, rx) = oneshot::channel(); + *start_tx.lock().unwrap() = Some(tx); + armed.store(true, Ordering::Release); + let halt_on_start = executor.commands().run(async move { + let _ = rx.await; + }); + + publisher.publish(msg::Empty::default())?; + let start = Instant::now(); + executor.spin( + SpinOptions::default() + .until_promise_resolved(halt_on_start) + .timeout(Duration::from_secs(10)), + ); + let elapsed = start.elapsed(); + + assert!( + completed.load(Ordering::Acquire), + "spin() returned while a callback was still running (quiescence violated)", + ); + assert!( + elapsed >= Duration::from_millis(350), + "spin() returned after {elapsed:?}, before the in-flight callback finished", + ); + Ok(()) + } + + /// Regression for notification combining / no message loss: a burst of messages + /// published while the executor is NOT spinning must all be delivered once it + /// resumes (the per-entity `pending` accumulator preserves the count), and the + /// entity must not wedge. + #[test] + fn tokio_burst_while_paused_is_delivered() -> Result<(), RclrsError> { + const BURST: usize = 20; + + let mut executor = Context::default().create_tokio_executor(); + let node = executor + .create_node(format!("test_tokio_burst_{}", line!()).start_parameter_services(false))?; + let qos = QoSProfile::default().reliable().keep_last(100); + + let received = Arc::new(AtomicUsize::new(0)); + let received_cb = Arc::clone(&received); + let _sub = node.create_subscription::( + "tokio_burst_topic".qos(qos), + move |_m: msg::Empty| { + received_cb.fetch_add(1, Ordering::Relaxed); + }, + )?; + let publisher = node.create_publisher::("tokio_burst_topic".qos(qos))?; + + // Discovery: get one message through so pub/sub are matched. + let deadline = Instant::now() + Duration::from_secs(10); + while received.load(Ordering::Relaxed) == 0 && Instant::now() < deadline { + publisher.publish(msg::Empty::default())?; + let _ = executor.spin(SpinOptions::spin_once().timeout(Duration::from_millis(200))); + } + let baseline = received.load(Ordering::Relaxed); + + // Burst while NOT spinning: these fire push callbacks that combine into a + // single queued `Ready` whose accumulated count is BURST. + for _ in 0..BURST { + publisher.publish(msg::Empty::default())?; + } + std::thread::sleep(Duration::from_millis(300)); + + // Resume: a wedged entity or lost count would leave us short. + let target = baseline + BURST; + let deadline = Instant::now() + Duration::from_secs(10); + while received.load(Ordering::Relaxed) < target && Instant::now() < deadline { + let _ = executor.spin(SpinOptions::spin_once().timeout(Duration::from_millis(200))); + } + + assert!( + received.load(Ordering::Relaxed) >= target, + "only {} of {} messages delivered after a paused burst (combining lost work or wedged)", + received.load(Ordering::Relaxed), + target, + ); + Ok(()) + } + + /// A panicking callback must not wedge the executor: spin() must still return + /// (quiescence counters not leaked) and the worker must survive to run other + /// callbacks. Without panic containment the first spin would hang forever on + /// quiescence and this test would time out. + #[test] + fn tokio_panicking_callback_does_not_wedge() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor + .create_node(format!("test_tokio_panic_{}", line!()).start_parameter_services(false))?; + let qos = QoSProfile::default().reliable().keep_last(10); + + // A subscription whose callback always panics. + let _panic_sub = node.create_subscription::( + "tokio_panic_topic".qos(qos), + |_m: msg::Empty| panic!("intentional test panic in a callback"), + )?; + let panic_pub = node.create_publisher::("tokio_panic_topic".qos(qos))?; + + // A healthy subscription on the same worker — it must still run. + let healthy = Arc::new(AtomicUsize::new(0)); + let healthy_cb = Arc::clone(&healthy); + let _healthy_sub = node.create_subscription::( + "tokio_healthy_topic".qos(qos), + move |_m: msg::Empty| { + healthy_cb.fetch_add(1, Ordering::Relaxed); + }, + )?; + let healthy_pub = node.create_publisher::("tokio_healthy_topic".qos(qos))?; + + let deadline = Instant::now() + Duration::from_secs(10); + while healthy.load(Ordering::Relaxed) == 0 && Instant::now() < deadline { + // Each spin processes the panicking callback (contained) and the + // healthy one. If quiescence leaked, spin() here would never return. + let _ = panic_pub.publish(msg::Empty::default()); + let _ = healthy_pub.publish(msg::Empty::default()); + let _ = executor.spin(SpinOptions::spin_once().timeout(Duration::from_millis(200))); + } + + assert!( + healthy.load(Ordering::Relaxed) > 0, + "a panicking callback wedged the worker or spin() quiescence", + ); + Ok(()) + } + + /// End-to-end: a node-scoped subscription receives messages via the + /// event-driven path (push callback -> mailbox -> worker task -> callback). + #[test] + fn tokio_events_pubsub() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_events_pubsub_{}", line!()).start_parameter_services(false), + )?; + let qos = QoSProfile::default().reliable().keep_last(10); + + let publisher = node.create_publisher::("tokio_events_topic".qos(qos))?; + let received = Arc::new(AtomicUsize::new(0)); + let received_cb = Arc::clone(&received); + let _sub = node.create_subscription::( + "tokio_events_topic".qos(qos), + move |_: msg::Empty| { + received_cb.fetch_add(1, Ordering::Relaxed); + }, + )?; + + let deadline = Instant::now() + Duration::from_secs(10); + while received.load(Ordering::Relaxed) == 0 && Instant::now() < deadline { + publisher.publish(msg::Empty::default())?; + executor.spin(SpinOptions::new().timeout(Duration::from_millis(50))); + std::thread::sleep(Duration::from_millis(20)); + } + + assert!( + received.load(Ordering::Relaxed) > 0, + "subscription callback never ran via the event-driven path" + ); + Ok(()) + } + + /// Callbacks must NOT run before spinning (deferred-execution guarantee that + /// the spin gate provides). + #[test] + fn tokio_events_no_callbacks_before_spin() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_no_early_{}", line!()).start_parameter_services(false), + )?; + let qos = QoSProfile::default().reliable().keep_last(10); + + let publisher = node.create_publisher::("tokio_no_early_topic".qos(qos))?; + let received = Arc::new(AtomicUsize::new(0)); + let received_cb = Arc::clone(&received); + let _sub = node.create_subscription::( + "tokio_no_early_topic".qos(qos), + move |_: msg::Empty| { + received_cb.fetch_add(1, Ordering::Relaxed); + }, + )?; + + // Publish and wait WITHOUT spinning; the callback must not run. + for _ in 0..5 { + publisher.publish(msg::Empty::default())?; + } + std::thread::sleep(Duration::from_millis(300)); + assert_eq!( + received.load(Ordering::Relaxed), + 0, + "callback ran before the executor was spun" + ); + + // Now spin and confirm the buffered messages are delivered. + let deadline = Instant::now() + Duration::from_secs(10); + while received.load(Ordering::Relaxed) == 0 && Instant::now() < deadline { + publisher.publish(msg::Empty::default())?; + executor.spin(SpinOptions::new().timeout(Duration::from_millis(50))); + std::thread::sleep(Duration::from_millis(20)); + } + assert!( + received.load(Ordering::Relaxed) > 0, + "callback never ran while spinning" + ); + Ok(()) + } + + /// A dropped subscription must stop firing callbacks (its entity is pruned). + #[test] + fn tokio_events_dropped_subscription_stops() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor + .create_node(format!("test_tokio_drop_{}", line!()).start_parameter_services(false))?; + let qos = QoSProfile::default().reliable().keep_last(10); + + let publisher = node.create_publisher::("tokio_drop_topic".qos(qos))?; + let count = Arc::new(AtomicUsize::new(0)); + let count_cb = Arc::clone(&count); + let sub = node.create_subscription::( + "tokio_drop_topic".qos(qos), + move |_: msg::Empty| { + count_cb.fetch_add(1, Ordering::Relaxed); + }, + )?; + + // Confirm the subscription is delivering. + let deadline = Instant::now() + Duration::from_secs(10); + while count.load(Ordering::Relaxed) == 0 && Instant::now() < deadline { + publisher.publish(msg::Empty::default())?; + executor.spin(SpinOptions::new().timeout(Duration::from_millis(50))); + std::thread::sleep(Duration::from_millis(20)); + } + assert!( + count.load(Ordering::Relaxed) > 0, + "subscription never delivered" + ); + + // Drop it, then keep publishing + spinning: the callback must not fire again. + drop(sub); + let after_drop = count.load(Ordering::Relaxed); + for _ in 0..10 { + publisher.publish(msg::Empty::default())?; + executor.spin(SpinOptions::new().timeout(Duration::from_millis(50))); + } + assert_eq!( + count.load(Ordering::Relaxed), + after_drop, + "callback fired after the subscription was dropped" + ); + Ok(()) + } + + /// End-to-end service round-trip driven entirely by the event-driven executor. + #[test] + fn tokio_events_service_roundtrip() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_events_service_{}", line!()).start_parameter_services(false), + )?; + + let _service = node.create_service::( + "tokio_events_service", + |_request: test_msgs::srv::Empty_Request| test_msgs::srv::Empty_Response::default(), + )?; + let client = node.create_client::("tokio_events_service")?; + + let deadline = Instant::now() + Duration::from_secs(10); + while !client.service_is_ready()? { + assert!(Instant::now() < deadline, "service never became ready"); + std::thread::sleep(Duration::from_millis(20)); + } + + let response: Promise = + client.call(test_msgs::srv::Empty_Request::default())?; + let (mut response, notice) = executor.commands().create_notice(response); + executor.spin( + SpinOptions::new() + .until_promise_resolved(notice) + .timeout(Duration::from_secs(5)), + ); + + assert!( + response.try_recv().ok().flatten().is_some(), + "client never received the service response via the event-driven path" + ); + Ok(()) + } + + /// Timers fire on the event-driven executor. + #[test] + fn tokio_events_timer_fires() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_events_timer_{}", line!()).start_parameter_services(false), + )?; + + let count = Arc::new(AtomicUsize::new(0)); + let count_cb = Arc::clone(&count); + let _timer = node.create_timer_repeating(Duration::from_millis(10), move || { + count_cb.fetch_add(1, Ordering::Relaxed); + })?; + + executor.spin(SpinOptions::new().timeout(Duration::from_millis(300))); + + let fired = count.load(Ordering::Relaxed); + assert!( + fired >= 3, + "timer fired only {fired} times in ~300ms (expected several)" + ); + Ok(()) + } + + /// Worker-scoped subscription + `listen_until` activity listener on the + /// event-driven executor. + #[test] + fn tokio_events_worker() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_worker_{}", line!()).start_parameter_services(false), + )?; + + let worker = node.create_worker::(0); + let _sub = worker.create_subscription( + "tokio_worker_topic", + |payload: &mut usize, _msg: msg::Empty| { + *payload += 1; + }, + )?; + let promise = worker.listen_until(|payload: &mut usize| (*payload > 0).then_some(*payload)); + + let publisher = node.create_publisher::("tokio_worker_topic")?; + let stop = Arc::new(AtomicBool::new(false)); + let stop_pub = Arc::clone(&stop); + let pub_thread = std::thread::spawn(move || { + while !stop_pub.load(Ordering::Acquire) { + let _ = publisher.publish(msg::Empty::default()); + std::thread::sleep(Duration::from_millis(10)); + } + }); + + let (mut promise, notice) = executor.commands().create_notice(promise); + executor.spin( + SpinOptions::new() + .until_promise_resolved(notice) + .timeout(Duration::from_secs(5)), + ); + stop.store(true, Ordering::Release); + pub_thread.join().unwrap(); + + assert!( + promise.try_recv().ok().flatten().is_some(), + "worker subscription / activity listener never fired on the event-driven executor" + ); + Ok(()) + } + + /// A node with parameter services enabled drives cleanly on the executor. + #[test] + fn tokio_events_node_with_parameter_services() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let _node = executor.create_node(&format!("test_tokio_paramsvc_{}", line!()))?; + let errors = executor.spin(SpinOptions::new().timeout(Duration::from_millis(200))); + // A bare-timeout spin reports a `Timeout` error (matching the basic + // executor); assert nothing *other* than that was produced. + assert!( + errors.iter().all(|e| matches!( + e, + RclrsError::RclError { + code: RclReturnCode::Timeout, + .. + } + )), + "spinning a node with parameter services produced unexpected errors: {errors:?}" + ); + Ok(()) + } + + /// Async tasks run on the Tokio runtime (would panic on the basic executor). + #[test] + fn tokio_async_task_runs() { + let mut executor = Context::default().create_tokio_executor(); + let _node = executor + .create_node(&format!("test_tokio_async_task_{}", line!())) + .unwrap(); + + let done = Arc::new(AtomicBool::new(false)); + let done_clone = Arc::clone(&done); + + let promise = executor.commands().run(async move { + tokio::time::sleep(Duration::from_millis(1)).await; + done_clone.store(true, Ordering::Release); + }); + + let (_, notice) = executor.commands().create_notice(promise); + executor + .spin( + SpinOptions::new() + .until_promise_resolved(notice) + .timeout(Duration::from_secs(5)), + ) + .first_error() + .unwrap(); + + assert!(done.load(Ordering::Acquire)); + } + + /// A 1 kHz timer should fire close to its ideal rate with the shared scheduler. + #[test] + fn tokio_high_rate_timer() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_high_rate_{}", line!()).start_parameter_services(false), + )?; + + let count = Arc::new(AtomicUsize::new(0)); + let count_cb = Arc::clone(&count); + let _timer = node.create_timer_repeating(Duration::from_millis(1), move || { + count_cb.fetch_add(1, Ordering::Relaxed); + })?; + + executor.spin(SpinOptions::new().timeout(Duration::from_secs(1))); + + let fired = count.load(Ordering::Relaxed); + assert!( + fired >= 800, + "1 kHz timer fired only {fired} times in ~1s (expected >= 800)" + ); + Ok(()) + } + + /// Several timers at different rates each fire from one shared scheduler. + #[test] + fn tokio_multi_rate_timers() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_multi_rate_{}", line!()).start_parameter_services(false), + )?; + + let fast = Arc::new(AtomicUsize::new(0)); + let slow = Arc::new(AtomicUsize::new(0)); + let fast_cb = Arc::clone(&fast); + let slow_cb = Arc::clone(&slow); + + let _t1 = node.create_timer_repeating(Duration::from_millis(5), move || { + fast_cb.fetch_add(1, Ordering::Relaxed); + })?; + let _t2 = node.create_timer_repeating(Duration::from_millis(20), move || { + slow_cb.fetch_add(1, Ordering::Relaxed); + })?; + + executor.spin(SpinOptions::new().timeout(Duration::from_millis(500))); + + let fast_fired = fast.load(Ordering::Relaxed); + let slow_fired = slow.load(Ordering::Relaxed); + assert!( + fast_fired >= 60, + "fast timer fired only {fast_fired} times in ~500ms (expected ~100)" + ); + assert!( + slow_fired >= 15, + "slow timer fired only {slow_fired} times in ~500ms (expected ~25)" + ); + assert!( + fast_fired > slow_fired * 2, + "fast timer ({fast_fired}) did not outpace slow ({slow_fired})" + ); + Ok(()) + } + + /// Cancelled timers stop firing. + #[test] + fn tokio_timer_cancel_stops() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_timer_cancel_{}", line!()).start_parameter_services(false), + )?; + + let count = Arc::new(AtomicUsize::new(0)); + let count_cb = Arc::clone(&count); + let timer = node.create_timer_repeating(Duration::from_millis(5), move || { + count_cb.fetch_add(1, Ordering::Relaxed); + })?; + + executor.spin(SpinOptions::new().timeout(Duration::from_millis(100))); + assert!( + count.load(Ordering::Relaxed) > 0, + "timer never fired before cancel" + ); + + timer.cancel()?; + let after_cancel = count.load(Ordering::Relaxed); + executor.spin(SpinOptions::new().timeout(Duration::from_millis(100))); + assert_eq!( + count.load(Ordering::Relaxed), + after_cancel, + "timer kept firing after cancel" + ); + Ok(()) + } + + /// A slow callback causes dropped ticks, not unbounded pending growth. + #[test] + fn tokio_slow_timer_callback_drops_ticks() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_slow_timer_{}", line!()).start_parameter_services(false), + )?; + + let count = Arc::new(AtomicUsize::new(0)); + let count_cb = Arc::clone(&count); + let _timer = node.create_timer_repeating(Duration::from_millis(1), move || { + count_cb.fetch_add(1, Ordering::Relaxed); + std::thread::sleep(Duration::from_millis(10)); + })?; + + executor.spin(SpinOptions::new().timeout(Duration::from_millis(200))); + + let fired = count.load(Ordering::Relaxed); + assert!( + fired < 150, + "slow callback allowed {fired} fires in 200ms (pending burst?)" + ); + assert!(fired >= 5, "timer never fired at all ({fired})"); + Ok(()) + } + + /// Steady- and system-clock timers both fire on the shared scheduler. + #[test] + fn tokio_timer_steady_and_system_clocks() -> Result<(), RclrsError> { + use crate::IntoTimerOptions; + + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_timer_clocks_{}", line!()).start_parameter_services(false), + )?; + + let steady_count = Arc::new(AtomicUsize::new(0)); + let system_count = Arc::new(AtomicUsize::new(0)); + let steady_cb = Arc::clone(&steady_count); + let system_cb = Arc::clone(&system_count); + + let _steady = + node.create_timer_repeating(Duration::from_millis(10).steady_time(), move || { + steady_cb.fetch_add(1, Ordering::Relaxed); + })?; + let _system = + node.create_timer_repeating(Duration::from_millis(10).system_time(), move || { + system_cb.fetch_add(1, Ordering::Relaxed); + })?; + + executor.spin(SpinOptions::new().timeout(Duration::from_millis(200))); + + assert!( + steady_count.load(Ordering::Relaxed) >= 5, + "steady-clock timer did not fire" + ); + assert!( + system_count.load(Ordering::Relaxed) >= 5, + "system-clock timer did not fire" + ); + Ok(()) + } + + /// A cancelled timer fires again after reset(). + #[test] + fn tokio_timer_reset_after_cancel() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_timer_reset_cancel_{}", line!()).start_parameter_services(false), + )?; + + let count = Arc::new(AtomicUsize::new(0)); + let count_cb = Arc::clone(&count); + let timer = node.create_timer_repeating(Duration::from_millis(5), move || { + count_cb.fetch_add(1, Ordering::Relaxed); + })?; + + executor.spin(SpinOptions::new().timeout(Duration::from_millis(100))); + assert!( + count.load(Ordering::Relaxed) > 0, + "timer never fired before cancel" + ); + + timer.cancel()?; + let after_cancel = count.load(Ordering::Relaxed); + executor.spin(SpinOptions::new().timeout(Duration::from_millis(100))); + assert_eq!( + count.load(Ordering::Relaxed), + after_cancel, + "timer kept firing after cancel" + ); + + timer.reset()?; + executor.spin(SpinOptions::new().timeout(Duration::from_millis(100))); + assert!( + count.load(Ordering::Relaxed) > after_cancel, + "timer did not fire again after reset" + ); + Ok(()) + } + + /// reset() on an active timer keeps it firing without stopping it or + /// double-scheduling a burst. + #[test] + fn tokio_timer_reset_keeps_firing() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_timer_reset_reseed_{}", line!()).start_parameter_services(false), + )?; + + let count = Arc::new(AtomicUsize::new(0)); + let count_cb = Arc::clone(&count); + let timer = node.create_timer_repeating(Duration::from_millis(20), move || { + count_cb.fetch_add(1, Ordering::Relaxed); + })?; + + executor.spin(SpinOptions::new().timeout(Duration::from_millis(100))); + let before_reset = count.load(Ordering::Relaxed); + assert!(before_reset > 0, "timer never fired before reset"); + + timer.reset()?; + executor.spin(SpinOptions::new().timeout(Duration::from_millis(200))); + let after = count.load(Ordering::Relaxed) - before_reset; + assert!( + after >= 4, + "timer stopped firing after reset ({after} in ~200ms, expected ~10)" + ); + assert!( + after <= 20, + "reset() caused a burst ({after} in ~200ms, expected ~10)" + ); + Ok(()) + } + + /// Repeating timers stay near their ideal rate over several seconds. + #[test] + fn tokio_timer_no_drift() -> Result<(), RclrsError> { + let mut executor = Context::default().create_tokio_executor(); + let node = executor.create_node( + format!("test_tokio_timer_no_drift_{}", line!()).start_parameter_services(false), + )?; + + let count = Arc::new(AtomicUsize::new(0)); + let count_cb = Arc::clone(&count); + let _timer = node.create_timer_repeating(Duration::from_millis(10), move || { + count_cb.fetch_add(1, Ordering::Relaxed); + })?; + + executor.spin(SpinOptions::new().timeout(Duration::from_secs(2))); + + let fired = count.load(Ordering::Relaxed); + assert!( + fired >= 160, + "10 ms timer drifted low: {fired} fires in ~2s (expected ~200)" + ); + assert!( + fired <= 240, + "10 ms timer drifted high: {fired} fires in ~2s (expected ~200)" + ); + Ok(()) + } +} diff --git a/rclrs/src/executor/tokio_executor/timer_scheduler.rs b/rclrs/src/executor/tokio_executor/timer_scheduler.rs new file mode 100644 index 000000000..53c80cf6d --- /dev/null +++ b/rclrs/src/executor/tokio_executor/timer_scheduler.rs @@ -0,0 +1,595 @@ +//! Shared timer scheduler for the Tokio executor. +//! +//! One min-heap keyed by next deadline drives all timers. A dedicated OS thread +//! waits on a [`Condvar`] until the earliest deadline. Pacing is decoupled from +//! execution: the scheduler enqueues `Ready` messages and advances deadlines +//! without waiting for worker callbacks. +//! +//! ## Design +//! +//! The waiter is a plain OS thread blocking on a [`std::sync::Condvar`], not a +//! Tokio task. That buys precise pacing (a direct `wait_timeout`), isolation from +//! the worker tasks, and independence from the Tokio timer driver, at the cost +//! of having to guard the blocking wait by hand. +//! +//! The waiter computes its sleep from the heap under the state lock, then +//! *releases* that lock before blocking, so producers (`register` / `reschedule` +//! / `cancel`) stay responsive instead of being held off for the whole sleep. +//! +//! There is a possibility for a wakeup to be lost. A producer can mutate the heap and +//! signal in the gap between the waiter "deciding to sleep" and "actually +//! sleeping", and a bare condvar only wakes a thread that is *already* parked, so +//! the signal would be missed and the waiter would oversleep. +//! +//! [`TimerSchedulerState::generation`] closes that window. It is a version stamp +//! bumped on every change the waiter cares about (see [`bump_generation`]); the +//! waiter snapshots it under the state lock and, immediately before blocking, +//! re-checks it under the condvar's own lock (a separate, tiny lock so producers +//! never contend on the state lock during the wait). If it moved, the waiter +//! skips the wait and recomputes against the fresh state. +//! +//! This generation dance is the hand-rolled equivalent of what an async design +//! gets for free from a `tokio::select!` over a `sleep` and a `watch`/`Notify`: +//! the lost-wakeup is universal to notify-style signaling, but a *synchronous* +//! condvar is what forces us to reconstruct the "something changed, re-evaluate" +//! coalescing ourselves rather than leaning on the runtime's poll model. +//! +//! ## Practical limits (Linux, measured) +//! +//! - Single timer: ~16 kHz before worker round-trip latency caps throughput. +//! Recommended ≤ ~5 kHz with margin. +//! - Many timers: hundreds of kHz aggregate on one worker (empty callbacks). +//! - Heavy callbacks serialize: keep `rate × work_us` well below 1e6 µs/s. + +use std::{ + cmp::Ordering as CmpOrdering, + collections::{BinaryHeap, HashMap}, + sync::{ + atomic::{AtomicBool, Ordering}, + Arc, Condvar, Mutex, + }, + thread::{self, JoinHandle}, + time::{Duration, Instant}, +}; + +use crate::{ + rcl_bindings::{ + rcl_timer_get_period, rcl_timer_get_time_until_next_call, rcl_timer_is_canceled, + rcl_timer_is_ready, rcl_timer_t, + }, + ToResult, +}; + +use super::EntityDispatch; + +/// Drives every timer owned by a single Tokio executor. +/// +/// There is one `TimerScheduler` per executor. It owns the shared +/// [`TimerSchedulerState`] (the deadline heap and the timer table) and a +/// dedicated OS thread that sleeps until the earliest deadline and then enqueues +/// ticks on the owning workers' mailboxes. Timers register via [`register`] and +/// receive a [`TimerSchedulerNotify`] handle they use to reschedule/cancel +/// themselves; dropping the scheduler stops and joins the thread. +/// +/// [`register`]: TimerScheduler::register +pub(crate) struct TimerScheduler { + /// Heap + timer table, shared with the waiter thread and every notify handle. + state: Arc>, + /// Wakeup/shutdown signaling shared with the waiter thread. + wake: Arc, + /// The waiter thread; `Option` so [`Drop`] can `take()` and join it. + thread_waiter: Option>, +} + +impl TimerScheduler { + /// Create the scheduler and spawn its waiter thread (initially idle, with no + /// timers registered). + pub(crate) fn new() -> Self { + let state = Arc::new(Mutex::new(TimerSchedulerState::default())); + let wake = Arc::new(SharedWake::default()); + + let thread_waiter = Some(spawn_thread_waiter(Arc::clone(&state), Arc::clone(&wake))); + + Self { + state, + wake, + thread_waiter, + } + } + + /// Register a timer with the scheduler and arm its first deadline. + /// + /// Reads the timer's period and time-until-next-call from rcl, inserts an + /// entry plus a heap node, wakes the waiter thread, and returns a + /// [`TimerSchedulerNotify`] the caller stores on the timer so later + /// reset/cancel/drop can update the schedule. + pub(crate) fn register(&self, reg: TimerRegistration) -> TimerSchedulerNotify { + let period = read_period(®.rcl_timer); + let deadline = next_deadline(®.rcl_timer); + + let mut state = self.state.lock().unwrap(); + let timer_id = state.allocate_timer_id(); + + // Store the timer + state + .entries + .insert(timer_id, TimerEntry::new(®, period, deadline)); + + // Store the next deadline + state.heap.push(HeapKey { deadline, timer_id }); + bump_generation(&mut state, &self.wake); + + TimerSchedulerNotify { + rcl_timer: reg.rcl_timer, + timer_id, + state: Arc::clone(&self.state), + wake: Arc::clone(&self.wake), + } + } +} + +impl Drop for TimerScheduler { + /// Signal shutdown, wake the waiter thread, and join it. + fn drop(&mut self) { + self.wake.shutdown.store(true, Ordering::Release); + self.wake.condvar.notify_all(); + if let Some(handle) = self.thread_waiter.take() { + let _ = handle.join(); + } + } +} + +/// Per-timer handle into the scheduler, stored on the timer itself. +/// +/// Lets a timer's [`reset`][crate::TimerState::reset], +/// [`cancel`][crate::TimerState::cancel], and `Drop` update the shared schedule +/// (re-arm, remove from the heap, or drop the entry) and wake the waiter thread. +#[derive(Clone)] +pub(crate) struct TimerSchedulerNotify { + /// The timer this handle controls, used to re-read its next deadline. + rcl_timer: Arc>, + + /// Key into [`TimerSchedulerState::entries`]. + timer_id: u64, + + /// Shared scheduler state (heap + table). + state: Arc>, + + /// Wakeup signaling for the waiter thread. + wake: Arc, +} + +impl TimerSchedulerNotify { + /// Re-seed this timer's deadline from rcl and wake the waiter. + /// + /// Used by `reset()` to re-arm a (possibly cancelled) timer: it recomputes + /// the deadline, pushes a fresh heap node, and bumps the generation. + pub(crate) fn reschedule(&self) { + let deadline = next_deadline(&self.rcl_timer); + let mut state = self.state.lock().unwrap(); + if state.schedule_at(self.timer_id, deadline) { + bump_generation(&mut state, &self.wake); + } + } + + /// Remove this timer from the wait heap until it is rescheduled. + /// + /// Used by `cancel()`: the entry is kept (so a later `reset()` can re-arm it) + /// but marked out-of-heap so any stale heap node is ignored. + pub(crate) fn unschedule(&self) { + let mut state = self.state.lock().unwrap(); + let Some(entry) = state.entries.get_mut(&self.timer_id) else { + return; + }; + entry.in_heap = false; + bump_generation(&mut state, &self.wake); + } + + /// Drop this timer's entry entirely. Called when the timer is dropped so a + /// cancelled (and therefore heap-less) timer does not leak its entry. + pub(crate) fn remove(&self) { + let mut state = self.state.lock().unwrap(); + if state.entries.remove(&self.timer_id).is_some() { + bump_generation(&mut state, &self.wake); + } + } +} + +/// Registration inputs cloned from a worker entity at `add_to_wait_set` time. +/// +/// Carries everything the scheduler needs to pace a timer and deliver its ticks +/// to the owning worker. +pub(crate) struct TimerRegistration { + /// The rcl timer to pace. + pub rcl_timer: Arc>, + + /// Readiness dispatch shared with the owning worker entity; ticks are + /// delivered through it (notification-combining + outstanding accounting). + pub dispatch: Arc, + + /// False once the entity is being torn down; tells the scheduler to drop it. + pub in_use: Arc, +} + +/// Shared scheduler state guarded by a single mutex. +#[derive(Default)] +struct TimerSchedulerState { + /// Min-heap (via [`HeapKey`]'s reversed ordering) of next deadlines. May hold + /// stale nodes; staleness is detected against [`TimerEntry`] when popped. + heap: BinaryHeap, + + /// All registered timers, keyed by id. + entries: HashMap, + + /// Monotonic id allocator for new timers. + next_timer_id: u64, + + /// Version stamp of the state, bumped on every change the waiter cares about + /// so it can re-evaluate and not lose a wakeup. See the module-level doc. + generation: u64, +} + +impl TimerSchedulerState { + /// Allocate a fresh, unique timer id. + fn allocate_timer_id(&mut self) -> u64 { + let id = self.next_timer_id; + self.next_timer_id += 1; + id + } + + /// Arm `timer_id` to fire at `deadline`: update its entry and push a heap + /// node for it. Returns `false` (a no-op) if the timer has no entry. Does not + /// bump the generation — the caller wakes the waiter thread when appropriate. + fn schedule_at(&mut self, timer_id: u64, deadline: Instant) -> bool { + let Some(entry) = self.entries.get_mut(&timer_id) else { + return false; + }; + entry.deadline = deadline; + entry.in_heap = true; + self.heap.push(HeapKey { deadline, timer_id }); + true + } +} + +/// The scheduler's record for one timer. +struct TimerEntry { + /// The rcl timer, re-read each time it becomes due. + rcl_timer: Arc>, + + /// Cached period, used to advance the deadline without an rcl round-trip. + period: Duration, + + /// The authoritative next deadline for this timer. + deadline: Instant, + + /// Readiness dispatch to the owning worker entity: ticks are delivered + /// through it (notification-combining + outstanding accounting). + dispatch: Arc, + + /// False once the entity is torn down. + in_use: Arc, + + /// Whether a heap node with `deadline` is still authoritative. Lets the + /// scheduler invalidate heap nodes lazily (mark, don't remove). + in_heap: bool, +} + +impl TimerEntry { + /// Build an entry for a freshly registered timer, armed at `deadline` and + /// marked present in the heap. + fn new(reg: &TimerRegistration, period: Duration, deadline: Instant) -> Self { + Self { + rcl_timer: Arc::clone(®.rcl_timer), + period, + deadline, + dispatch: Arc::clone(®.dispatch), + in_use: Arc::clone(®.in_use), + in_heap: true, + } + } +} + +/// A node in the deadline heap: the timer id tagged with the deadline it was +/// pushed for. Compared so the heap behaves as a min-heap on `deadline`. +struct HeapKey { + deadline: Instant, + timer_id: u64, +} + +impl Clone for HeapKey { + fn clone(&self) -> Self { + Self { + deadline: self.deadline, + timer_id: self.timer_id, + } + } +} + +impl PartialEq for HeapKey { + fn eq(&self, other: &Self) -> bool { + self.deadline == other.deadline && self.timer_id == other.timer_id + } +} + +impl Eq for HeapKey {} + +impl PartialOrd for HeapKey { + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} + +impl Ord for HeapKey { + fn cmp(&self, other: &Self) -> CmpOrdering { + // Earliest deadline at the top of the max-heap. + other + .deadline + .cmp(&self.deadline) + .then_with(|| other.timer_id.cmp(&self.timer_id)) + } +} + +/// Wakeup and shutdown signaling between producers (register/notify/drop) and +/// the waiter thread. +#[derive(Default)] +struct SharedWake { + /// Set on drop; the waiter thread exits when it observes this. + shutdown: AtomicBool, + + /// Mirror of [`TimerSchedulerState::generation`] used for the condvar wait. + generation: Mutex, + + /// Signaled on every generation bump and on shutdown. + condvar: Condvar, +} + +/// Snapshot of an rcl timer's state, read together under the timer lock. +struct TimerRclStatus { + canceled: bool, + ready: bool, + time_until_next: Duration, +} + +/// Bump the generation (under the state lock) and signal the waiter thread. +/// +/// The generation mirror in [`SharedWake`] lets the waiter detect that state +/// changed between releasing the state lock and starting its condvar wait, +/// avoiding a lost wakeup. +fn bump_generation(state: &mut TimerSchedulerState, wake: &SharedWake) { + state.generation = state.generation.wrapping_add(1); + *wake.generation.lock().unwrap() = state.generation; + wake.condvar.notify_all(); +} + +/// Spawn the named waiter thread that drives the scheduler. +fn spawn_thread_waiter( + state: Arc>, + wake: Arc, +) -> JoinHandle<()> { + thread::Builder::new() + .name("rclrs-timer-scheduler".into()) + .spawn(move || run_thread_waiter(state, wake)) + .expect("failed to spawn timer scheduler thread") +} + +/// The waiter thread's main loop: sleep until the earliest deadline (or until +/// woken by a generation bump/shutdown), then fire due timers. +fn run_thread_waiter(state: Arc>, wake: Arc) { + #[cfg(windows)] + let _timer_resolution = WindowsTimerResolution::raise(); + + loop { + if wake.shutdown.load(Ordering::Acquire) { + return; + } + + let (next_deadline, generation) = { + let inner = state.lock().unwrap(); + (inner.heap.peek().map(|k| k.deadline), inner.generation) + }; + + // Wait under the generation lock, guarding against a lost wakeup: if a + // producer bumped the generation between our (released) state read and + // acquiring this lock, skip the wait and re-evaluate immediately. + match next_deadline { + None => { + let guard = wake.generation.lock().unwrap(); + + if *guard == generation { + let _ = wake.condvar.wait(guard).ok(); + } + } + Some(deadline) => { + let now = Instant::now(); + + if deadline > now { + let guard = wake.generation.lock().unwrap(); + + if *guard != generation { + continue; + } + + let (_guard, timeout) = + wake.condvar.wait_timeout(guard, deadline - now).unwrap(); + + if !timeout.timed_out() { + continue; + } + } + process_due_timers(&state, &wake, generation); + } + } + } +} + +/// Drain every timer whose deadline has passed, enqueueing ticks and advancing +/// (or removing) each entry. +/// +/// For each due timer it re-reads rcl to decide what to do: drop a torn-down +/// timer, leave a cancelled one as a tombstone, re-arm one that is not actually +/// ready yet, drop a tick when the worker is behind, or enqueue a tick and +/// advance by one period. `seen_generation` lets it bail out if the state was +/// mutated concurrently. +fn process_due_timers( + state: &Arc>, + wake: &Arc, + seen_generation: u64, +) { + let now = Instant::now(); + loop { + if wake.shutdown.load(Ordering::Acquire) { + return; + } + + let head = { + let inner = state.lock().unwrap(); + inner.heap.peek().cloned() + }; + + let Some(head) = head else { + break; + }; + + if head.deadline > now { + break; + } + + let mut reschedule = None; + let mut remove = false; + + { + let mut inner = state.lock().unwrap(); + if inner.generation != seen_generation { + break; + } + + inner.heap.pop(); + let Some(entry) = inner.entries.get_mut(&head.timer_id) else { + continue; + }; + + if !entry.in_heap || entry.deadline != head.deadline { + continue; + } + entry.in_heap = false; + + if !entry.in_use.load(Ordering::Acquire) { + remove = true; + } else { + let status = read_timer_status(&entry.rcl_timer); + if status.canceled { + // Tombstone: keep the entry (out of the heap) so reset() can re-arm. + } else if !status.ready { + reschedule = Some(Instant::now() + status.time_until_next); + } else if entry.dispatch.is_scheduled() { + // Worker is behind: drop this tick without growing `pending`. + reschedule = Some(entry.deadline + entry.period); + } else if !entry.dispatch.notify(1) { + // Worker's mailbox is closed: drop the timer. + remove = true; + } else { + reschedule = Some(entry.deadline + entry.period); + } + } + + if remove { + inner.entries.remove(&head.timer_id); + } else if let Some(dl) = reschedule { + inner.schedule_at(head.timer_id, dl); + } + } + } +} + +/// Read the timer's configured period, defaulting to 1 ms on error. +fn read_period(rcl_timer: &Arc>) -> Duration { + let timer = rcl_timer.lock().unwrap(); + let mut period_ns: i64 = 0; + // SAFETY: handle valid and locked. + let ret = unsafe { rcl_timer_get_period(&*timer, &mut period_ns) }; + ret.ok() + .ok() + .map(|()| Duration::from_nanos(period_ns.max(0) as u64)) + .unwrap_or(Duration::from_millis(1)) +} + +/// Read just the time until the timer's next call (see [`read_timer_status`]). +fn read_time_until_next_call(rcl_timer: &Arc>) -> Duration { + read_timer_status(rcl_timer).time_until_next +} + +/// The next absolute deadline for `rcl_timer`, derived from its current +/// time-until-next-call. Used when (re)arming a timer. +fn next_deadline(rcl_timer: &Arc>) -> Instant { + Instant::now() + read_time_until_next_call(rcl_timer) +} + +/// Read cancellation/readiness/time-until-next together under the timer lock. +/// +/// On error each field falls back conservatively: treated as canceled, not +/// ready, and a 50 ms retry delay so a transiently failing timer is re-polled +/// rather than busy-looped. +fn read_timer_status(rcl_timer: &Arc>) -> TimerRclStatus { + let timer = rcl_timer.lock().unwrap(); + + let mut canceled = false; + // SAFETY: handle valid and locked. + let canceled = unsafe { rcl_timer_is_canceled(&*timer, &mut canceled) } + .ok() + .map(|()| canceled) + .unwrap_or(true); + + let mut ready = false; + // SAFETY: handle valid and locked. + let ready = unsafe { rcl_timer_is_ready(&*timer, &mut ready) } + .ok() + .map(|()| ready) + .unwrap_or(false); + + let mut value: i64 = 0; + // SAFETY: handle valid and locked. + let time_until_next = + match unsafe { rcl_timer_get_time_until_next_call(&*timer, &mut value) }.ok() { + Ok(()) if value > 0 => Duration::from_nanos(value as u64), + Ok(()) => Duration::ZERO, + Err(_) => Duration::from_millis(50), + }; + + TimerRclStatus { + canceled, + ready, + time_until_next, + } +} + +/// On Windows the default scheduler tick is ~15.6 ms; request 1 ms for the +/// lifetime of the timer scheduler thread. +#[cfg(windows)] +struct WindowsTimerResolution { + active: bool, +} + +#[cfg(windows)] +impl WindowsTimerResolution { + fn raise() -> Self { + extern "system" { + fn timeBeginPeriod(u_period: u32) -> u32; + } + let active = unsafe { timeBeginPeriod(1) } == 0; + Self { active } + } +} + +#[cfg(windows)] +impl Drop for WindowsTimerResolution { + fn drop(&mut self) { + if self.active { + extern "system" { + fn timeEndPeriod(u_period: u32) -> u32; + } + unsafe { + timeEndPeriod(1); + } + } + } +} diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index a30ec8dc4..f72accb25 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -307,6 +307,31 @@ where fn handle(&self) -> RclPrimitiveHandle<'_> { RclPrimitiveHandle::Service(self.handle.lock()) } + + fn register_on_ready( + &self, + on_ready: Box, + ) -> Result>, RclrsError> { + // A service has a single readiness path; report it as `Basic`. + let on_ready = move |n| on_ready(ReadyKind::Basic, n); + let registration = crate::executor::event_callback::OnReadyRegistration::new( + Arc::clone(&self.handle), + set_service_on_new_request, + Box::new(on_ready), + )?; + Ok(Some(Box::new(registration))) + } +} + +/// Install (or, with a null callback/user_data, clear) the "on new request" +/// push callback used by the event-driven executor. Encapsulates the service +/// lock and the rcl call within this module. +pub(crate) unsafe fn set_service_on_new_request( + handle: &ServiceHandle, + callback: rcl_event_callback_t, + user_data: *const std::os::raw::c_void, +) -> rcl_ret_t { + rcl_service_set_on_new_request_callback(&*handle.lock(), callback, user_data) } // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 16a800a9c..e58767788 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -103,6 +103,16 @@ where self.handle.topic_name() } + /// Access the handle for this subscription's underlying `rcl_subscription_t`. + /// + /// Returns the subscription handle. Only the `event_callback` tests use this + /// accessor (production code reaches the handle through its own field), so it + /// is compiled only under test rather than carried as dead code. + #[cfg(test)] + pub(crate) fn handle(&self) -> &Arc { + &self.handle + } + /// Returns the QoS settings of the subscription. pub fn qos(&self) -> QoSProfile { let options = unsafe { @@ -294,6 +304,31 @@ where fn handle(&self) -> RclPrimitiveHandle<'_> { RclPrimitiveHandle::Subscription(self.handle.lock()) } + + fn register_on_ready( + &self, + on_ready: Box, + ) -> Result>, RclrsError> { + // A subscription has a single readiness path; report it as `Basic`. + let on_ready = move |n| on_ready(ReadyKind::Basic, n); + let registration = crate::executor::event_callback::OnReadyRegistration::new( + Arc::clone(&self.handle), + set_subscription_on_new_message, + Box::new(on_ready), + )?; + Ok(Some(Box::new(registration))) + } +} + +/// Install (or, with a null callback/user_data, clear) the "on new message" +/// push callback used by the event-driven executor. Encapsulates the +/// subscription lock and the rcl call within this module. +pub(crate) unsafe fn set_subscription_on_new_message( + handle: &SubscriptionHandle, + callback: rcl_event_callback_t, + user_data: *const std::os::raw::c_void, +) -> rcl_ret_t { + rcl_subscription_set_on_new_message_callback(&*handle.lock(), callback, user_data) } // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 9d64e99c2..0582169f3 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -120,6 +120,12 @@ impl TimerState { rcl_timer_cancel(&mut *rcl_timer) } .ok()?; + + #[cfg(feature = "tokio-executor")] + if let Some(notify) = self.handle.scheduler.lock().unwrap().as_ref() { + notify.unschedule(); + } + Ok(cancel_result) } @@ -193,17 +199,29 @@ impl TimerState { /// For all timers it will reset the last call time to now. For cancelled /// timers it will revert the timer to no longer being cancelled. pub fn reset(&self) -> Result<(), RclrsError> { - // SAFETY: The unwrap is safe here since we never use the rcl_timer - // in a way that could panic while the mutex is locked. - let mut rcl_timer = self.handle.rcl_timer.lock().unwrap(); + // Scope the rcl_timer lock so it is released before notifying the + // scheduler below: `reschedule` re-locks this same timer to read its + // next deadline, which would otherwise self-deadlock. + { + // SAFETY: The unwrap is safe here since we never use the rcl_timer + // in a way that could panic while the mutex is locked. + let mut rcl_timer = self.handle.rcl_timer.lock().unwrap(); - unsafe { - // SAFETY: The rcl_timer is kept valid by the TimerState. This C - // function call is thread-safe and only requires a valid rcl_timer - // to be passed in. - rcl_timer_reset(&mut *rcl_timer) + unsafe { + // SAFETY: The rcl_timer is kept valid by the TimerState. This C + // function call is thread-safe and only requires a valid rcl_timer + // to be passed in. + rcl_timer_reset(&mut *rcl_timer) + } + .ok()?; } - .ok() + + #[cfg(feature = "tokio-executor")] + if let Some(notify) = self.handle.scheduler.lock().unwrap().as_ref() { + notify.reschedule(); + } + + Ok(()) } /// Checks if the timer is ready (not canceled) @@ -321,7 +339,12 @@ impl TimerState { .ok()?; let timer = Arc::new(TimerState { - handle: Arc::new(TimerHandle { rcl_timer, clock }), + handle: Arc::new(TimerHandle { + rcl_timer, + clock, + #[cfg(feature = "tokio-executor")] + scheduler: Arc::new(Mutex::new(None)), + }), callback: Mutex::new(Some(callback)), last_elapse: Mutex::new(Duration::ZERO), lifecycle: Mutex::default(), @@ -509,6 +532,14 @@ impl RclPrimitive for TimerExecutable { fn handle(&self) -> RclPrimitiveHandle<'_> { RclPrimitiveHandle::Timer(self.handle.rcl_timer.lock().unwrap()) } + + #[cfg(feature = "tokio-executor")] + fn timer_scheduler_handles(&self) -> Option { + Some(crate::TimerSchedulerHandles { + rcl_timer: Arc::clone(&self.handle.rcl_timer), + notify_slot: Arc::clone(&self.handle.scheduler), + }) + } } impl PartialEq for TimerState { @@ -533,11 +564,20 @@ fn rcl_duration(duration_value_ns: i64) -> Result { pub(crate) struct TimerHandle { pub(crate) rcl_timer: Arc>, clock: Clock, + #[cfg(feature = "tokio-executor")] + pub(crate) scheduler: Arc>>, } /// 'Drop' trait implementation to be able to release the resources impl Drop for TimerHandle { fn drop(&mut self) { + // Drop the scheduler entry first (idempotent) so the scheduler thread + // can no longer touch this rcl_timer before we finalize it below. + #[cfg(feature = "tokio-executor")] + if let Some(notify) = self.scheduler.lock().unwrap().take() { + notify.remove(); + } + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); unsafe { // SAFETY: The lifecycle mutex is locked and the clock for the timer diff --git a/rclrs/src/wait_set/rcl_primitive.rs b/rclrs/src/wait_set/rcl_primitive.rs index c59efc54c..0ff4225f8 100644 --- a/rclrs/src/wait_set/rcl_primitive.rs +++ b/rclrs/src/wait_set/rcl_primitive.rs @@ -1,6 +1,12 @@ -use std::{any::Any, sync::MutexGuard}; +use std::{ + any::Any, + sync::{Arc, Mutex, MutexGuard}, +}; -use crate::{log_error, rcl_bindings::*, InnerGuardConditionHandle, RclrsError, ToResult}; +use crate::{ + executor::TimerSchedulerNotify, log_error, rcl_bindings::*, InnerGuardConditionHandle, + RclrsError, ToResult, +}; /// This provides the public API for executing a waitable item. pub trait RclPrimitive: Send + Sync { @@ -28,8 +34,48 @@ pub trait RclPrimitive: Send + Sync { /// Provide the handle for this primitive fn handle(&self) -> RclPrimitiveHandle<'_>; + + /// Register a push "on ready" callback so an event-driven executor can learn + /// this primitive has become ready without polling a wait set. The + /// middleware invokes `on_ready` with the [`ReadyKind`] describing *which* + /// part of the primitive became ready and the number of new events. + /// + /// Most primitives have a single readiness path and call `on_ready` with + /// [`ReadyKind::Basic`]. Composite primitives (action servers and clients) + /// register one callback per internal source and call `on_ready` with a + /// [`ReadyKind::ActionServer`]/[`ReadyKind::ActionClient`] value whose single + /// matching flag is set, so the executor knows which sub-entity to run. + /// + /// Returns `Ok(None)` for primitive kinds that have no rcl push-callback API + /// (e.g. timers and guard conditions); an event-driven executor drives those + /// by other means. The returned [`OnReadyHandle`] keeps the callback(s) + /// registered; dropping it detaches them. + fn register_on_ready( + &self, + on_ready: Box, + ) -> Result>, RclrsError> { + // Default: no push-callback support. Suppress the unused parameter. + let _ = on_ready; + Ok(None) + } + + /// For a timer primitive, the handles the Tokio timer scheduler needs to + /// drive it: the rcl timer and the slot for its notify handle (see + /// [`TimerSchedulerHandles`]). Timers have no rcl push-callback API, so the + /// scheduler drives them instead. Returns `None` for every other primitive + /// kind. + #[cfg(feature = "tokio-executor")] + fn timer_scheduler_handles(&self) -> Option { + None + } } +/// RAII handle that keeps a push "on ready" callback registered with the +/// middleware (see [`RclPrimitive::register_on_ready`]). Dropping it +/// unregisters the callback, before freeing the callback's context, so an +/// event-driven executor can detach an entity simply by dropping this handle. +pub trait OnReadyHandle: Send + Sync {} + /// Enum to describe the kind of an executable. #[derive(Clone, Copy, Hash, PartialEq, Eq, PartialOrd, Ord)] pub enum RclPrimitiveKind { @@ -288,3 +334,17 @@ impl Default for ActionClientReady { } } } + +/// The handles the Tokio timer scheduler needs to drive a timer: the rcl timer +/// (to read deadlines from) and the slot where the timer keeps the +/// [`TimerSchedulerNotify`] it receives at registration, so its `reset`/`cancel` +/// and drop can reach the scheduler. Returned by +/// [`RclPrimitive::timer_scheduler_handles`]; `None` for non-timer primitives. +#[cfg(feature = "tokio-executor")] +pub(crate) struct TimerSchedulerHandles { + /// The rcl timer the scheduler reads deadlines from. + pub rcl_timer: Arc>, + /// Slot where the timer stores the [`TimerSchedulerNotify`] it gets back from + /// registration, so reset/cancel/drop can notify the scheduler. + pub notify_slot: Arc>>, +} diff --git a/rclrs/src/wait_set/wait_set_runner.rs b/rclrs/src/wait_set/wait_set_runner.rs index 62d39c9fb..a422fd94e 100644 --- a/rclrs/src/wait_set/wait_set_runner.rs +++ b/rclrs/src/wait_set/wait_set_runner.rs @@ -13,8 +13,8 @@ use std::{ }; use crate::{ - log_debug, log_fatal, ActivityListenerCallback, Context, ExecutorWorkerOptions, GuardCondition, - PayloadTask, Promise, RclReturnCode, RclrsError, WaitSet, Waitable, WeakActivityListener, + log_debug, log_fatal, Context, ExecutorWorkerOptions, GuardCondition, PayloadTask, Promise, + RclReturnCode, RclrsError, WaitSet, Waitable, WeakActivityListener, }; /// This is a utility class that executors can use to easily run and manage @@ -135,7 +135,6 @@ impl WaitSetRunner { /// [1]: crate::SpinOptions::until_promise_resolved pub fn run_blocking(&mut self, conditions: WaitSetRunConditions) -> Result<(), RclrsError> { let mut first_spin = true; - let mut listeners = Vec::new(); loop { // TODO(@mxgrey): SmallVec would be better suited here if we are // okay with adding that as a dependency. @@ -193,47 +192,7 @@ impl WaitSetRunner { })?; if at_least_one { - // We drain all listeners from activity_listeners to ensure that we - // don't get a deadlock from double-locking the activity_listeners - // mutex while executing one of the listeners. If the listener has - // access to the Worker then it could attempt to add another - // listener while we have the vector locked, which would cause a - // deadlock. - listeners.extend( - self.activity_listeners - .lock() - .unwrap() - .drain(..) - .filter_map(|x| x.upgrade()), - ); - - for arc_listener in &listeners { - // We pull the callback out of its mutex entirely and release - // the lock on the mutex before executing the callback. Otherwise - // if the callback triggers its own WorkerActivity to change the - // callback then we would get a deadlock from double-locking the - // mutex. - let listener = { arc_listener.lock().unwrap().take() }; - if let Some(mut listener) = listener { - match &mut listener { - ActivityListenerCallback::Listen(listen) => { - listen(&mut *self.payload); - } - ActivityListenerCallback::Inert => { - // Do nothing - } - } - - // We replace instead of assigning in case the callback - // inserted its own - arc_listener.lock().unwrap().replace(listener); - } - } - - self.activity_listeners - .lock() - .unwrap() - .extend(listeners.drain(..).map(|x| Arc::downgrade(&x))); + crate::worker::run_activity_listeners(&self.activity_listeners, &mut *self.payload); } if let Some(stop_time) = conditions.stop_time { diff --git a/rclrs/src/wait_set/waitable.rs b/rclrs/src/wait_set/waitable.rs index be771736d..ec0a9b970 100644 --- a/rclrs/src/wait_set/waitable.rs +++ b/rclrs/src/wait_set/waitable.rs @@ -43,10 +43,67 @@ impl Waitable { self.index_in_wait_set.is_some() } - pub(super) fn in_use(&self) -> bool { + /// Whether this waitable is still in use (its owning entity has not been + /// dropped). Used by the wait set to drop finished entries, and by an + /// event-driven executor to know when to deregister. + pub(crate) fn in_use(&self) -> bool { self.in_use.load(Ordering::Relaxed) } + /// Register a push "on ready" callback for an event-driven executor. + /// Delegates to the wrapped primitive. See [`RclPrimitive::register_on_ready`]. + #[cfg(feature = "tokio-executor")] + pub(crate) fn register_on_ready( + &self, + on_ready: Box, + ) -> Result>, RclrsError> { + self.primitive.register_on_ready(on_ready) + } + + /// The kind of primitive this waitable wraps, so an event-driven executor can + /// special-case composite primitives (action servers/clients). + #[cfg(feature = "tokio-executor")] + pub(crate) fn kind(&self) -> RclPrimitiveKind { + self.primitive.kind() + } + + /// Execute the wrapped primitive once for an event-driven executor with the + /// given readiness, taking a single item (e.g. one message) and running its + /// callback. For most primitives `ready` is [`ReadyKind::Basic`]; for action + /// servers/clients it identifies which sub-entity became ready. + /// + /// # Safety + /// + /// `payload` must have the type the wrapped primitive expects (the type of + /// the [`Worker`][crate::Worker] that owns it). Passing a mismatched payload + /// is undefined behavior, since the primitive downcasts it. An event-driven + /// executor upholds this by only ever executing an entity against the payload + /// of the worker it was registered on. + #[cfg(feature = "tokio-executor")] + pub(crate) unsafe fn execute_with( + &mut self, + ready: ReadyKind, + payload: &mut dyn std::any::Any, + ) -> Result<(), RclrsError> { + // SAFETY: the payload-type obligation is forwarded to our caller via this + // function being `unsafe`. + unsafe { self.primitive.execute(ready, payload) } + } + + /// If this waitable wraps a timer, the handles the Tokio timer scheduler + /// needs to drive it (rcl timer + notify slot). `None` otherwise. + #[cfg(feature = "tokio-executor")] + pub(crate) fn timer_scheduler_handles(&self) -> Option { + self.primitive.timer_scheduler_handles() + } + + /// A clone of the "in use" flag, so an event-driven executor's timer driver + /// can stop once the owning entity has been dropped. + #[cfg(feature = "tokio-executor")] + pub(crate) fn in_use_handle(&self) -> Arc { + Arc::clone(&self.in_use) + } + pub(super) fn is_ready(&self, wait_set: &rcl_wait_set_t) -> Option { match self.primitive.kind() { RclPrimitiveKind::Subscription => { diff --git a/rclrs/src/worker.rs b/rclrs/src/worker.rs index dd411399a..6f508f5f4 100644 --- a/rclrs/src/worker.rs +++ b/rclrs/src/worker.rs @@ -740,6 +740,53 @@ pub enum ActivityListenerCallback { Inert, } +/// Run every activity listener against the worker `payload`. Executor runtimes +/// call this after a worker primitive has run, so listeners (e.g. those backing +/// [`WorkerState::listen_until`]) observe the possibly-updated payload. +/// +/// The listeners are drained, run, and re-inserted so that a listener which +/// mutates the listener set (by adding another listener while it runs) cannot +/// deadlock on the listener-set mutex; likewise each callback is taken out of +/// its own mutex before running. +pub(crate) fn run_activity_listeners( + activity_listeners: &Mutex>, + payload: &mut dyn Any, +) { + // We drain all listeners from activity_listeners so that we don't get a + // deadlock from double-locking the activity_listeners mutex while executing + // one of the listeners. If the listener has access to the Worker then it + // could attempt to add another listener while we have the vector locked, + // which would cause a deadlock. + let mut listeners: Vec>>> = activity_listeners + .lock() + .unwrap() + .drain(..) + .filter_map(|listener| listener.upgrade()) + .collect(); + + for arc_listener in &listeners { + // We pull the callback out of its mutex entirely and release the lock on + // the mutex before executing the callback. Otherwise, if the callback + // triggers its own WorkerActivity to change the callback, then we would + // get a deadlock from double-locking the mutex. + let listener = { arc_listener.lock().unwrap().take() }; + if let Some(mut listener) = listener { + match &mut listener { + ActivityListenerCallback::Listen(listen) => listen(payload), + ActivityListenerCallback::Inert => {} + } + // Replace instead of assigning, in case the callback inserted its own. + arc_listener.lock().unwrap().replace(listener); + } + } + + activity_listeners.lock().unwrap().extend( + listeners + .drain(..) + .map(|listener| Arc::downgrade(&listener)), + ); +} + /// This is used to determine what kind of payload a callback can accept, as /// well as what kind of callbacks can be used with it. Users should not implement /// this trait.