feat: support actions on the Tokio executor

azerupi · cursoragent · azerupi · commit 0dd9e5b6990f · 2026-06-21T19:56:40.000+02:00
Drive action servers and clients with rcl's action push callbacks
(rcl_action_*_set_*_callback) on the event-driven executor.

An action is a composite primitive: each internal source (the server's
goal/cancel/result services; the client's feedback/status subscriptions
and goal/cancel/result clients) registers its own push callback tagged
with the ReadyKind flag it satisfies. Because the notifications for one
action coalesce into a single mailbox wakeup, the executor accumulates a
merged ReadyKind per entity and runs the primitive with it. Goal
expiration has no push callback, so the server polls it on a coarse
interval.

Adds end-to-end tests for a goal round-trip (feedback + result) and for
cancellation on the Tokio executor.

Co-authored-by: Cursor &lt;cursoragent@cursor.com&gt;
diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
@@ -443,6 +443,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::<Fibonacci>(&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::<Fibonacci>(&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
@@ -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<A: Action> RclPrimitive for ActionClientExecutable<A> {
     fn handle(&self) -> crate::RclPrimitiveHandle<'_> {
         RclPrimitiveHandle::ActionClient(self.board.handle.lock())
     }
+
+    fn register_on_ready(
+        &self,
+        on_ready: Box<dyn Fn(ReadyKind, usize) + Send + Sync>,
+    ) -> Result<Option<Box<dyn crate::OnReadyHandle>>, 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<dyn Fn(usize) + Send + Sync> {
+            let on_ready = Arc::clone(&on_ready);
+            Box::new(move |n| on_ready(ReadyKind::ActionClient(bits), n))
+        };
+
+        let regs: Vec<Box<dyn crate::OnReadyHandle>> = 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
@@ -1,9 +1,9 @@
 use super::empty_goal_status_array;
 use crate::{
-    action::GoalUuid, error::ToResult, rcl_bindings::*, ActionGoalReceiver, CancelResponseCode,
-    Clock, 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, Clock, 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;
@@ -669,6 +669,81 @@ impl<A: Action> RclPrimitive for ActionServerExecutable<A> {
     fn handle(&self) -> RclPrimitiveHandle<'_> {
         RclPrimitiveHandle::ActionServer(self.board.handle.lock())
     }
+
+    fn register_on_ready(
+        &self,
+        on_ready: Box<dyn Fn(ReadyKind, usize) + Send + Sync>,
+    ) -> Result<Option<Box<dyn crate::OnReadyHandle>>, 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<dyn Fn(usize) + Send + Sync> {
+            let on_ready = Arc::clone(&on_ready);
+            Box::new(move |n| on_ready(ReadyKind::ActionServer(bits), n))
+        };
+
+        let regs: Vec<Box<dyn crate::OnReadyHandle>> = vec![
+            Box::new(OnReadyRegistration::new(
+                Arc::clone(handle),
+                set_action_server_goal_callback::<A>,
+                mk(ActionServerReady {
+                    goal_request: true,
+                    ..Default::default()
+                }),
+            )?),
+            Box::new(OnReadyRegistration::new(
+                Arc::clone(handle),
+                set_action_server_cancel_callback::<A>,
+                mk(ActionServerReady {
+                    cancel_request: true,
+                    ..Default::default()
+                }),
+            )?),
+            Box::new(OnReadyRegistration::new(
+                Arc::clone(handle),
+                set_action_server_result_callback::<A>,
+                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<A: Action>(
+    handle: &ActionServerHandle<A>,
+    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<A: Action>(
+    handle: &ActionServerHandle<A>,
+    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<A: Action>(
+    handle: &ActionServerHandle<A>,
+    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/executor/event_callback.rs b/rclrs/src/executor/event_callback.rs
@@ -77,6 +77,15 @@ pub(crate) struct OnReadyRegistration<H: Send + Sync + 'static> {
 
 impl<H: Send + Sync + 'static> OnReadyHandle for OnReadyRegistration<H> {}
 
+/// 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<Box<dyn OnReadyHandle>>);
+
+impl OnReadyHandle for CompositeOnReady {}
+
 impl<H: Send + Sync + 'static> OnReadyRegistration<H> {
     /// Register `on_ready` to be called by the middleware whenever the entity
     /// becomes ready. `set_callback` locks `handle` and installs the trampoline.
diff --git a/rclrs/src/executor/tokio_executor.rs b/rclrs/src/executor/tokio_executor.rs
diff --git a/rclrs/src/wait_set/waitable.rs b/rclrs/src/wait_set/waitable.rs
