Fix action cancellation pipeline

mxgrey · mxgrey · commit ea27c3fe3a74 · 2025-10-10T22:58:21.000+08:00
Signed-off-by: Michael X. Grey &lt;greyxmike@gmail.com&gt;
diff --git a/rclrs/src/action.rs b/rclrs/src/action.rs
@@ -73,6 +73,8 @@ impl From<&[u8; RCL_ACTION_UUID_SIZE]> for GoalUuid {
 }
 
 /// The response returned by an [`ActionServer`]'s cancel callback when a goal is requested to be cancelled.
+#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
+#[cfg_attr(feature = "serde", serde(rename = "snake_case"))]
 #[repr(i8)]
 #[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
 pub enum CancelResponseCode {
@@ -118,6 +120,7 @@ impl From<i8> for CancelResponseCode {
 ///
 /// When a cancellation request might cancel multiple goals, [`MultiCancelResponse`]
 /// will be used.
+#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
 #[derive(Debug, Clone, PartialEq, PartialOrd)]
 pub struct CancelResponse {
     /// What kind of response was given.
@@ -154,6 +157,7 @@ impl MultiCancelResponse {
 
 /// Values defined by `action_msgs/msg/GoalStatus`
 #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
+#[cfg_attr(feature = "serde", serde(rename = "snake_case"))]
 #[repr(i8)]
 #[derive(Debug, Clone, Copy, Hash, PartialEq, Eq, PartialOrd, Ord)]
 pub enum GoalStatusCode {
@@ -240,15 +244,17 @@ mod tests {
     use tokio::sync::mpsc::unbounded_channel;
 
     #[test]
-    fn test_action_success() {
+    fn test_action_success_streaming() {
         let mut executor = Context::default().create_basic_executor();
 
         let node = executor
             .create_node(&format!("test_action_success_{}", line!()))
             .unwrap();
         let action_name = format!("test_action_success_{}_action", line!());
         let _action_server = node
-            .create_action_server(&action_name, fibonacci_action)
+            .create_action_server(&action_name, |handle| {
+                fibonacci_action(handle, Duration::from_micros(10))
+            })
             .unwrap();
 
         let client = node
@@ -302,7 +308,41 @@ mod tests {
         executor.spin(SpinOptions::default().until_promise_resolved(promise));
     }
 
-    async fn fibonacci_action(handle: RequestedGoal<Fibonacci>) -> TerminatedGoal {
+    #[test]
+    fn test_action_cancel() {
+        let mut executor = Context::default().create_basic_executor();
+
+        let node = executor
+            .create_node(&format!("test_action_cancel_{}", line!()))
+            .unwrap();
+        let action_name = format!("test_action_cancel_{}_slow_action", line!());
+        let _action_server = node
+            .create_action_server(&action_name, |handle| {
+                fibonacci_action(handle, Duration::from_secs(1))
+            })
+            .unwrap();
+
+        let client = node
+            .create_action_client::<Fibonacci>(&action_name)
+            .unwrap();
+
+        let request = client.request_goal(Fibonacci_Goal { order: 10 });
+
+        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);
+        });
+
+        executor.spin(SpinOptions::default().until_promise_resolved(promise));
+    }
+
+    async fn fibonacci_action(
+        handle: RequestedGoal<Fibonacci>,
+        period: Duration,
+    ) -> TerminatedGoal {
         let goal_order = handle.goal().order;
         if goal_order < 0 {
             return handle.reject();
@@ -331,7 +371,7 @@ mod tests {
                 let next = previous + current;
                 previous = current;
                 current = next;
-                std::thread::sleep(Duration::from_micros(10));
+                std::thread::sleep(period);
             }
         });
 
diff --git a/rclrs/src/action/action_client.rs b/rclrs/src/action/action_client.rs
@@ -2,14 +2,7 @@ use super::empty_goal_status_array;
 use crate::{
     log_warn,
     rcl_bindings::*,
-    vendor::{
-        action_msgs::{
-            msg::GoalInfo,
-            srv::{CancelGoal_Request, CancelGoal_Response},
-        },
-        builtin_interfaces::msg::Time,
-        unique_identifier_msgs::msg::UUID,
-    },
+    vendor::{action_msgs::srv::CancelGoal_Response, builtin_interfaces::msg::Time},
     CancelResponse, CancelResponseCode, DropGuard, GoalStatus, GoalStatusCode, GoalUuid,
     MultiCancelResponse, Node, NodeHandle, QoSProfile, RclPrimitive, RclPrimitiveHandle,
     RclPrimitiveKind, RclrsError, ReadyKind, TakeFailedAsNone, ToResult, Waitable,
@@ -880,15 +873,15 @@ impl ActionClientHandle {
     }
 
     fn send_cancel_goal(&self, goal_id: GoalUuid, stamp: Time) -> Result<i64, RclrsError> {
-        let cancel_request = CancelGoal_Request {
-            goal_info: GoalInfo {
-                goal_id: UUID { uuid: *goal_id },
-                stamp,
+        let cancel_request_rmw = action_msgs__srv__CancelGoal_Request {
+            goal_info: action_msgs__msg__GoalInfo {
+                goal_id: unique_identifier_msgs__msg__UUID { uuid: *goal_id },
+                stamp: builtin_interfaces__msg__Time {
+                    sec: stamp.sec,
+                    nanosec: stamp.nanosec,
+                },
             },
         };
-
-        let cancel_request_rmw =
-            <CancelGoal_Request as Message>::into_rmw_message(Cow::Owned(cancel_request));
         let mut seq: i64 = 0;
 
         unsafe {
diff --git a/rclrs/src/action/action_client/cancellation_client.rs b/rclrs/src/action/action_client/cancellation_client.rs
@@ -2,8 +2,11 @@ use super::GoalClientLifecycle;
 use crate::{ActionClient, CancelResponse, GoalUuid, MultiCancelResponse, RclrsError};
 use rosidl_runtime_rs::Action;
 use std::{
+    future::Future,
     ops::{Deref, DerefMut},
+    pin::Pin,
     sync::Arc,
+    task::{Context, Poll},
 };
 use tokio::sync::oneshot::Receiver;
 
@@ -88,6 +91,18 @@ impl<A: Action> CancelResponseClient<A> {
     }
 }
 
+impl<A: Action> Future for CancelResponseClient<A> {
+    type Output = CancelResponse;
+
+    fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
+        Future::poll(Pin::new(&mut self.get_mut().receiver), cx)
+            // SAFETY: The Receiver returns an Err if the sender is dropped, but
+            // the CancelResponseClient makes sure that the sender is alive in
+            // the ActionClient, so we can always safely unwrap this.
+            .map(|result| result.unwrap())
+    }
+}
+
 /// This will allow you to receive a response to a cancellation request that
 /// impacts multiple goals.
 ///
diff --git a/rclrs/src/action/action_client/requested_goal_client.rs b/rclrs/src/action/action_client/requested_goal_client.rs
@@ -65,7 +65,7 @@ impl<A: Action> Future for RequestedGoalClient<A> {
     fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
         Future::poll(Pin::new(&mut self.get_mut().receiver), cx)
             // SAFETY: The Receiver returns an Err if the sender is dropped, but
-            // the RegisteredGoalClient makes sure that the sender is alive in
+            // the RequestedGoalClient makes sure that the sender is alive in
             // the ActionClient, so we can always safely unwrap this.
             .map(|result| result.unwrap())
     }
diff --git a/rclrs/src/action/action_client/result_client.rs b/rclrs/src/action/action_client/result_client.rs
@@ -63,7 +63,7 @@ impl<A: Action> Future for ResultClient<A> {
     fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
         Future::poll(Pin::new(&mut self.get_mut().receiver), cx)
             // SAFETY: The Receiver returns an Err if the sender is dropped, but
-            // the RegisteredGoalClient makes sure that the sender is alive in
+            // the ResultClient makes sure that the sender is alive in
             // the ActionClient, so we can always safely unwrap this.
             .map(|result| result.unwrap())
     }
diff --git a/rclrs/src/action/action_server/cancellation_state.rs b/rclrs/src/action/action_server/cancellation_state.rs
@@ -70,9 +70,11 @@ impl<A: Action> CancellationState<A> {
             CancellationMode::None => {
                 let requests = Vec::from_iter([request]);
                 *mode = CancellationMode::CancelRequested(requests);
+                self.change_cancel_requested_status(true);
             }
             CancellationMode::CancelRequested(requests) => {
                 requests.push(request);
+                self.change_cancel_requested_status(true);
             }
             CancellationMode::Cancelling => {
                 request.accept(*uuid);
@@ -93,7 +95,7 @@ impl<A: Action> CancellationState<A> {
                 *mode = CancellationMode::None;
                 // We do not need to worry about errors from sending this state
                 // since it is okay for the receiver to be dropped.
-                let _ = self.sender.send(false);
+                let _ = self.change_cancel_requested_status(false);
             }
             CancellationMode::None => {
                 // Do nothing
@@ -120,21 +122,33 @@ impl<A: Action> CancellationState<A> {
                 // a true value in the cancel requested channel. We can ignore
                 // errors from this because it is okay for the receiver to be
                 // dropped.
-                let _ = self.sender.send(true);
+                let _ = self.change_cancel_requested_status(true);
             }
             CancellationMode::None => {
                 // Skip straight to cancellation mode since the user has accepted
                 // a cancellation even though it wasn't requested externally.
                 *mode = CancellationMode::Cancelling;
                 // Make sure the cancellation is signalled. We can ignore errors
                 // from this because it is okay for the receiver to be dropped.
-                let _ = self.sender.send(true);
+                let _ = self.change_cancel_requested_status(true);
             }
             CancellationMode::Cancelling => {
                 // Do nothing
             }
         }
     }
+
+    fn change_cancel_requested_status(&self, cancel_requested: bool) {
+        self.sender.send_if_modified(|status| {
+            let previously_requested = *status;
+            *status = cancel_requested;
+            // Only notify the listeners if a cancellation has been requested
+            // and was not previously requested. We do not need to notify when
+            // a cancellation has been rejected (i.e. reverting back to no
+            // cancellation requested).
+            cancel_requested && !previously_requested
+        });
+    }
 }
 
 impl<A: Action> Default for CancellationState<A> {
diff --git a/rclrs/src/vendor/example_interfaces/action.rs b/rclrs/src/vendor/example_interfaces/action.rs
diff --git a/rclrs/src/vendor/test_msgs/action.rs b/rclrs/src/vendor/test_msgs/action.rs

Original file line number	Diff line number	Diff line change
`@@ -65,7 +65,7 @@ impl<A: Action> Future for RequestedGoalClient<A> {`
`65`	`65`	`fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {`
`66`	`66`	`Future::poll(Pin::new(&mut self.get_mut().receiver), cx)`
`67`	`67`	`// SAFETY: The Receiver returns an Err if the sender is dropped, but`
`68`		`- // the RegisteredGoalClient makes sure that the sender is alive in`
	`68`	`+ // the RequestedGoalClient makes sure that the sender is alive in`
`69`	`69`	`// the ActionClient, so we can always safely unwrap this.`
`70`	`70`	`.map(\|result\| result.unwrap())`
`71`	`71`	`}`
Original file line number	Diff line number	Diff line change
`@@ -63,7 +63,7 @@ impl<A: Action> Future for ResultClient<A> {`
`63`	`63`	`fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {`
`64`	`64`	`Future::poll(Pin::new(&mut self.get_mut().receiver), cx)`
`65`	`65`	`// SAFETY: The Receiver returns an Err if the sender is dropped, but`
`66`		`- // the RegisteredGoalClient makes sure that the sender is alive in`
	`66`	`+ // the ResultClient makes sure that the sender is alive in`
`67`	`67`	`// the ActionClient, so we can always safely unwrap this.`
`68`	`68`	`.map(\|result\| result.unwrap())`
`69`	`69`	`}`