Skip to content

Commit

Permalink
Add WaitSet::new_for_node() (#276)
Browse files Browse the repository at this point in the history
This function is important for users who directly use wait sets, since the live_subscriptions() etc. functions are not public.
  • Loading branch information
nnmm authored Oct 5, 2022
1 parent 3fb8ea5 commit 4956c31
Show file tree
Hide file tree
Showing 2 changed files with 41 additions and 34 deletions.
34 changes: 1 addition & 33 deletions rclrs/src/lib.rs
Original file line number Diff line number Diff line change
Expand Up @@ -46,39 +46,7 @@ pub use wait::*;
///
/// [1]: crate::RclReturnCode
pub fn spin_once(node: &Node, timeout: Option<Duration>) -> 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 {
Expand Down
41 changes: 40 additions & 1 deletion rclrs/src/wait.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<Self, 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())?;
}
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
Expand Down

0 comments on commit 4956c31

Please sign in to comment.