Action API corrections

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,
+    GoalStatusCode, GoalUuid, MultiCancelResponse, Node, NodeHandle, Promise, 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};
@@ -191,6 +191,16 @@ impl<A: Action> ActionClientState<A> {
         Ok(is_available)
     }
 
+    /// Get a promise that will be fulfilled when an action server is ready for
+    /// this client. You can `.await` the promise in an async function or use it
+    /// for `until_promise_resolved` in [`SpinOptions`][crate::SpinOptions].
+    pub fn notify_on_server_ready(self: &Arc<Self>) -> Promise<()> {
+        let client = Arc::clone(self);
+        self.board
+            .node
+            .notify_on_graph_change(move || client.server_is_available().is_ok_and(|r| r))
+    }
+
     /// Request the action server to execute a goal. You will receive a
     /// [`RequestedGoalClient`] which you can use to wait for the reply from the
     /// action server that indicates whether the goal was accepted.
@@ -480,7 +490,7 @@ impl<A: Action> ActionClientGoalBoard<A> {
         let accepted = A::get_goal_response_accepted(&response_rmw);
         let stamp = A::get_goal_response_stamp(&response_rmw);
 
-        let Some(pending) = self
+        let Some(mut pending) = self
             .pending_goal_clients
             .lock()
             .unwrap()
@@ -491,6 +501,31 @@ impl<A: Action> ActionClientGoalBoard<A> {
         };
 
         if accepted {
+            let status = GoalStatus {
+                code: GoalStatusCode::Accepted,
+                goal_id: pending.goal_id,
+                stamp: Time {
+                    sec: stamp.0,
+                    nanosec: stamp.1,
+                },
+            };
+
+            {
+                let all_status_senders = self.status_senders.lock().unwrap();
+                if let Some(senders) = all_status_senders.get(&pending.goal_id) {
+                    for sender in senders {
+                        let _ = sender.send(status.clone());
+                    }
+                }
+            }
+
+            {
+                let all_status_posters = self.status_posters.lock().unwrap();
+                if let Some(watcher) = all_status_posters.get(&pending.goal_id) {
+                    watcher.send_modify(|watched_status| *watched_status = status);
+                }
+            }
+
             let _ = pending.sender.send(Some(GoalClient {
                 feedback: pending.feedback,
                 status: pending.status,

rclrs/src/action/action_server.rs

@@ -29,7 +29,7 @@ mod cancellation_state;
 use cancellation_state::*;
 
 mod cancelling_goal;
-use cancelling_goal::*;
+pub use cancelling_goal::*;
 
 mod executing_goal;
 pub use executing_goal::*;

rclrs/src/action/action_server/requested_goal.rs

@@ -23,6 +23,12 @@ impl<A: Action> RequestedGoal<A> {
         &self.goal_request
     }
 
+    /// Get the unique ID of this goal
+    #[must_use]
+    pub fn goal_id(&self) -> &GoalUuid {
+        &self.uuid
+    }
+
     /// Accept the requested goal. The action client will be notified that the
     /// goal was accepted, and you will be able to begin executing.
     ///