Add action client/server entities to wait set

They still aren't handled in the .wait() method, though.

Author: nwn

rclrs/src/action/client.rs

@@ -1,4 +1,7 @@
-use crate::{error::ToResult, rcl_bindings::*, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX};
+use crate::{
+    error::ToResult, rcl_bindings::*, wait::WaitableNumEntities, Node, RclrsError,
+    ENTITY_LIFECYCLE_MUTEX,
+};
 use std::{
     ffi::CString,
     marker::PhantomData,
@@ -45,16 +48,18 @@ impl Drop for ActionClientHandle {
 pub trait ActionClientBase: Send + Sync {
     /// Internal function to get a reference to the `rcl` handle.
     fn handle(&self) -> &ActionClientHandle;
+    fn num_entities(&self) -> &WaitableNumEntities;
     // /// Tries to take a new request and run the callback with it.
     // fn execute(&self) -> Result<(), RclrsError>;
 }
 
-pub struct ActionClient<T>
+pub struct ActionClient<ActionT>
 where
-    T: rosidl_runtime_rs::Action,
+    ActionT: rosidl_runtime_rs::Action,
 {
-    _marker: PhantomData<fn() -> T>,
+    _marker: PhantomData<fn() -> ActionT>,
     pub(crate) handle: Arc<ActionClientHandle>,
+    num_entities: WaitableNumEntities,
 }
 
 impl<T> ActionClient<T>
@@ -106,9 +111,23 @@ where
             in_use_by_wait_set: Arc::new(AtomicBool::new(false)),
         });
 
+        let mut num_entities = WaitableNumEntities::default();
+        unsafe {
+            rcl_action_client_wait_set_get_num_entities(
+                &*handle.lock(),
+                &mut num_entities.num_subscriptions,
+                &mut num_entities.num_guard_conditions,
+                &mut num_entities.num_timers,
+                &mut num_entities.num_clients,
+                &mut num_entities.num_services,
+            )
+            .ok()?;
+        }
+
         Ok(Self {
             _marker: Default::default(),
             handle,
+            num_entities,
         })
     }
 }
@@ -120,4 +139,8 @@ where
     fn handle(&self) -> &ActionClientHandle {
         &self.handle
     }
+
+    fn num_entities(&self) -> &WaitableNumEntities {
+        &self.num_entities
+    }
 }

rclrs/src/action/server.rs

@@ -1,4 +1,10 @@
-use crate::{action::{GoalResponse, GoalUuid, CancelResponse, ServerGoalHandle}, error::ToResult, rcl_bindings::*, Clock, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX};
+use crate::{
+    action::{CancelResponse, GoalResponse, GoalUuid, ServerGoalHandle},
+    error::ToResult,
+    rcl_bindings::*,
+    wait::WaitableNumEntities,
+    Clock, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX,
+};
 use std::{
     ffi::CString,
     sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard},
@@ -44,6 +50,7 @@ impl Drop for ActionServerHandle {
 pub trait ActionServerBase: Send + Sync {
     /// Internal function to get a reference to the `rcl` handle.
     fn handle(&self) -> &ActionServerHandle;
+    fn num_entities(&self) -> &WaitableNumEntities;
     // /// Tries to take a new request and run the callback with it.
     // fn execute(&self) -> Result<(), RclrsError>;
 }
@@ -57,6 +64,7 @@ where
     ActionT: rosidl_runtime_rs::Action,
 {
     pub(crate) handle: Arc<ActionServerHandle>,
+    num_entities: WaitableNumEntities,
     goal_callback: Box<GoalCallback<ActionT>>,
     cancel_callback: Box<CancelCallback<ActionT>>,
     accepted_callback: Box<AcceptedCallback<ActionT>>,
@@ -121,8 +129,22 @@ where
             in_use_by_wait_set: Arc::new(AtomicBool::new(false)),
         });
 
+        let mut num_entities = WaitableNumEntities::default();
+        unsafe {
+            rcl_action_server_wait_set_get_num_entities(
+                &*handle.lock(),
+                &mut num_entities.num_subscriptions,
+                &mut num_entities.num_guard_conditions,
+                &mut num_entities.num_timers,
+                &mut num_entities.num_clients,
+                &mut num_entities.num_services,
+            )
+            .ok()?;
+        }
+
         Ok(Self {
             handle,
+            num_entities,
             goal_callback: Box::new(goal_callback),
             cancel_callback: Box::new(cancel_callback),
             accepted_callback: Box::new(accepted_callback),
@@ -137,4 +159,8 @@ where
     fn handle(&self) -> &ActionServerHandle {
         &self.handle
     }
+
+    fn num_entities(&self) -> &WaitableNumEntities {
+        &self.num_entities
+    }
 }

rclrs/src/node.rs

@@ -19,12 +19,13 @@ use std::{
 use rosidl_runtime_rs::Message;
 
 use crate::{
-    rcl_bindings::*, ActionClient, ActionClientBase, ActionServer, ActionServerBase, CancelResponse, Client, ClientBase, ClientOptions,
-    ClientState, Clock, ContextHandle, GoalResponse, GoalUuid, GuardCondition, LogParams, Logger,
-    ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher,
-    PublisherOptions, PublisherState, RclrsError, ServerGoalHandle, Service, ServiceBase,
-    ServiceOptions, ServiceState, Subscription, SubscriptionBase, SubscriptionCallback,
-    SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, ENTITY_LIFECYCLE_MUTEX,
+    rcl_bindings::*, ActionClient, ActionClientBase, ActionServer, ActionServerBase,
+    CancelResponse, Client, ClientBase, ClientOptions, ClientState, Clock, ContextHandle,
+    GoalResponse, GoalUuid, GuardCondition, LogParams, Logger, ParameterBuilder,
+    ParameterInterface, ParameterVariant, Parameters, Publisher, PublisherOptions, PublisherState,
+    RclrsError, ServerGoalHandle, Service, ServiceBase, ServiceOptions, ServiceState, Subscription,
+    SubscriptionBase, SubscriptionCallback, SubscriptionOptions, SubscriptionState, TimeSource,
+    ToLogParams, ENTITY_LIFECYCLE_MUTEX,
 };
 
 // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
@@ -284,7 +285,10 @@ impl NodeState {
         T: rosidl_runtime_rs::Service,
     {
         let client = Arc::new(ClientState::<T>::new(self, options)?);
-        self.clients_mtx.lock().unwrap().push(Arc::downgrade(&client) as Weak<dyn ClientBase>);
+        self.clients_mtx
+            .lock()
+            .unwrap()
+            .push(Arc::downgrade(&client) as Weak<dyn ClientBase>);
         Ok(client)
     }
 
@@ -300,7 +304,9 @@ impl NodeState {
         T: rosidl_runtime_rs::Action,
     {
         let action_client = Arc::new(ActionClient::<T>::new(self, topic)?);
-        self.action_clients_mtx.lock().unwrap()
+        self.action_clients_mtx
+            .lock()
+            .unwrap()
             .push(Arc::downgrade(&action_client) as Weak<dyn ActionClientBase>);
         Ok(action_client)
     }
@@ -330,7 +336,9 @@ impl NodeState {
             handle_cancel,
             handle_accepted,
         )?);
-        self.action_servers_mtx.lock().unwrap()
+        self.action_servers_mtx
+            .lock()
+            .unwrap()
             .push(Arc::downgrade(&action_server) as Weak<dyn ActionServerBase>);
         Ok(action_server)
     }
@@ -348,7 +356,9 @@ impl NodeState {
             Arc::clone(&self.handle.context_handle),
             None,
         ));
-        self.guard_conditions_mtx.lock().unwrap()
+        self.guard_conditions_mtx
+            .lock()
+            .unwrap()
             .push(Arc::downgrade(&guard_condition) as Weak<GuardCondition>);
         guard_condition
     }
@@ -369,7 +379,9 @@ impl NodeState {
             Arc::clone(&self.handle.context_handle),
             Some(Box::new(callback) as Box<dyn Fn() + Send + Sync>),
         ));
-        self.guard_conditions_mtx.lock().unwrap()
+        self.guard_conditions_mtx
+            .lock()
+            .unwrap()
             .push(Arc::downgrade(&guard_condition) as Weak<GuardCondition>);
         guard_condition
     }
@@ -469,7 +481,9 @@ impl NodeState {
         F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send,
     {
         let service = Arc::new(ServiceState::<T>::new(self, options, callback)?);
-        self.services_mtx.lock().unwrap()
+        self.services_mtx
+            .lock()
+            .unwrap()
             .push(Arc::downgrade(&service) as Weak<dyn ServiceBase>);
         Ok(service)
     }
@@ -524,36 +538,63 @@ impl NodeState {
         T: Message,
     {
         let subscription = Arc::new(SubscriptionState::<T>::new(self, options, callback)?);
-        self.subscriptions_mtx.lock()
+        self.subscriptions_mtx
+            .lock()
             .unwrap()
             .push(Arc::downgrade(&subscription) as Weak<dyn SubscriptionBase>);
         Ok(subscription)
     }
 
     /// Returns the subscriptions that have not been dropped yet.
     pub(crate) fn live_subscriptions(&self) -> Vec<Arc<dyn SubscriptionBase>> {
-        self.subscriptions_mtx.lock().unwrap()
+        self.subscriptions_mtx
+            .lock()
+            .unwrap()
             .iter()
             .filter_map(Weak::upgrade)
             .collect()
     }
 
     pub(crate) fn live_clients(&self) -> Vec<Arc<dyn ClientBase>> {
-        self.clients_mtx.lock().unwrap()
+        self.clients_mtx
+            .lock()
+            .unwrap()
             .iter()
             .filter_map(Weak::upgrade)
             .collect()
     }
 
     pub(crate) fn live_guard_conditions(&self) -> Vec<Arc<GuardCondition>> {
-        self.guard_conditions_mtx.lock().unwrap()
+        self.guard_conditions_mtx
+            .lock()
+            .unwrap()
             .iter()
             .filter_map(Weak::upgrade)
             .collect()
     }
 
     pub(crate) fn live_services(&self) -> Vec<Arc<dyn ServiceBase>> {
-        self.services_mtx.lock().unwrap()
+        self.services_mtx
+            .lock()
+            .unwrap()
+            .iter()
+            .filter_map(Weak::upgrade)
+            .collect()
+    }
+
+    pub(crate) fn live_action_clients(&self) -> Vec<Arc<dyn ActionClientBase>> {
+        self.action_clients_mtx
+            .lock()
+            .unwrap()
+            .iter()
+            .filter_map(Weak::upgrade)
+            .collect()
+    }
+
+    pub(crate) fn live_action_servers(&self) -> Vec<Arc<dyn ActionServerBase>> {
+        self.action_servers_mtx
+            .lock()
+            .unwrap()
             .iter()
             .filter_map(Weak::upgrade)
             .collect()