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::*, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX};
+use crate::{
+    error::ToResult, rcl_bindings::*, wait::WaitableNumEntities, NodeHandle, 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,5 +1,9 @@
 use crate::{
-    action::{GoalResponse, GoalUuid, CancelResponse, ServerGoalHandle}, error::ToResult, rcl_bindings::*, Clock, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX,
+    action::{CancelResponse, GoalResponse, GoalUuid, ServerGoalHandle},
+    error::ToResult,
+    rcl_bindings::*,
+    wait::WaitableNumEntities,
+    Clock, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX,
 };
 use std::{
     ffi::CString,
@@ -46,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>;
 }
@@ -59,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>>,
@@ -123,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),
@@ -139,4 +159,8 @@ where
     fn handle(&self) -> &ActionServerHandle {
         &self.handle
     }
+
+    fn num_entities(&self) -> &WaitableNumEntities {
+        &self.num_entities
+    }
 }

rclrs/src/node.rs

@@ -13,11 +13,11 @@ use rosidl_runtime_rs::Message;
 
 pub use self::{builder::*, graph::*};
 use crate::{
-    rcl_bindings::*, ActionClient, ActionClientBase, ActionServer, ActionServerBase, CancelResponse, Client, ClientBase, Clock,
-    Context, ContextHandle, GoalResponse, GoalUuid, GuardCondition, ParameterBuilder,
-    ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError,
-    ServerGoalHandle, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback,
-    TimeSource, ENTITY_LIFECYCLE_MUTEX,
+    rcl_bindings::*, ActionClient, ActionClientBase, ActionServer, ActionServerBase,
+    CancelResponse, Client, ClientBase, Clock, Context, ContextHandle, GoalResponse, GoalUuid,
+    GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher,
+    QoSProfile, RclrsError, ServerGoalHandle, Service, ServiceBase, Subscription, SubscriptionBase,
+    SubscriptionCallback, TimeSource, ENTITY_LIFECYCLE_MUTEX,
 };
 
 // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
@@ -209,7 +209,10 @@ impl Node {
         T: rosidl_runtime_rs::Service,
     {
         let client = Arc::new(Client::<T>::new(Arc::clone(&self.handle), topic)?);
-        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)
     }
 
@@ -222,7 +225,9 @@ impl Node {
         T: rosidl_runtime_rs::Action,
     {
         let action_client = Arc::new(ActionClient::<T>::new(Arc::clone(&self.handle), 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)
     }
@@ -252,7 +257,9 @@ impl Node {
             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)
     }
@@ -271,7 +278,9 @@ impl Node {
             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
     }
@@ -293,7 +302,9 @@ impl Node {
             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
     }
@@ -332,7 +343,9 @@ impl Node {
             topic,
             callback,
         )?);
-        self.services_mtx.lock().unwrap()
+        self.services_mtx
+            .lock()
+            .unwrap()
             .push(Arc::downgrade(&service) as Weak<dyn ServiceBase>);
         Ok(service)
     }
@@ -356,36 +369,63 @@ impl Node {
             qos,
             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()