diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 74c04647d..2c9c812e7 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -46,39 +46,7 @@ pub use wait::*; /// /// [1]: crate::RclReturnCode pub fn spin_once(node: &Node, timeout: Option) -> Result<(), RclrsError> { - let live_subscriptions = node.live_subscriptions(); - let live_clients = node.live_clients(); - let live_guard_conditions = node.live_guard_conditions(); - let live_services = node.live_services(); - let ctx = Context { - rcl_context_mtx: node.rcl_context_mtx.clone(), - }; - let mut wait_set = WaitSet::new( - live_subscriptions.len(), - live_guard_conditions.len(), - 0, - live_clients.len(), - live_services.len(), - 0, - &ctx, - )?; - - for live_subscription in &live_subscriptions { - wait_set.add_subscription(live_subscription.clone())?; - } - - for live_client in &live_clients { - wait_set.add_client(live_client.clone())?; - } - - for live_guard_condition in &live_guard_conditions { - wait_set.add_guard_condition(live_guard_condition.clone())?; - } - - for live_service in &live_services { - wait_set.add_service(live_service.clone())?; - } - + let mut wait_set = WaitSet::new_for_node(node)?; let ready_entities = wait_set.wait(timeout)?; for ready_subscription in ready_entities.subscriptions { diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 5b3c63e14..09284ed75 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -21,7 +21,7 @@ use std::vec::Vec; use crate::error::{to_rclrs_result, RclReturnCode, RclrsError, ToResult}; use crate::rcl_bindings::*; -use crate::{ClientBase, Context, ServiceBase, SubscriptionBase}; +use crate::{ClientBase, Context, Node, ServiceBase, SubscriptionBase}; mod exclusivity_guard; mod guard_condition; @@ -117,6 +117,45 @@ impl WaitSet { }) } + /// Creates a new wait set and adds all waitable entities in the node to it. + /// + /// The wait set is sized to fit the node exactly, so there is no capacity for adding other entities. + pub fn new_for_node(node: &Node) -> Result { + let live_subscriptions = node.live_subscriptions(); + let live_clients = node.live_clients(); + let live_guard_conditions = node.live_guard_conditions(); + let live_services = node.live_services(); + let ctx = Context { + rcl_context_mtx: node.rcl_context_mtx.clone(), + }; + let mut wait_set = WaitSet::new( + live_subscriptions.len(), + live_guard_conditions.len(), + 0, + live_clients.len(), + live_services.len(), + 0, + &ctx, + )?; + + for live_subscription in &live_subscriptions { + wait_set.add_subscription(live_subscription.clone())?; + } + + for live_client in &live_clients { + wait_set.add_client(live_client.clone())?; + } + + for live_guard_condition in &live_guard_conditions { + wait_set.add_guard_condition(live_guard_condition.clone())?; + } + + for live_service in &live_services { + wait_set.add_service(live_service.clone())?; + } + Ok(wait_set) + } + /// Removes all entities from the wait set. /// /// This effectively resets the wait set to the state it was in after being created by