diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index 3e2bf53b2..f8cc6431d 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -1,4 +1,4 @@ -use std::{convert::TryInto, env, sync::Arc}; +use std::convert::TryInto; use anyhow::{Error, Result}; use rosidl_runtime_rs::{seq, BoundedSequence, Message, Sequence}; @@ -138,38 +138,32 @@ fn demonstrate_sequences() { fn demonstrate_pubsub() -> Result<(), Error> { println!("================== Interoperability demo =================="); // Demonstrate interoperability between idiomatic and RMW-native message types - let context = rclrs::Context::new(env::args())?; - let node = rclrs::create_node(&context, "message_demo")?; + let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); + let node = executor.create_node("message_demo")?; - let idiomatic_publisher = node.create_publisher::( - "topic", - rclrs::QOS_PROFILE_DEFAULT, - )?; - let direct_publisher = node.create_publisher::( - "topic", - rclrs::QOS_PROFILE_DEFAULT, - )?; + let idiomatic_publisher = + node.create_publisher::("topic")?; + let direct_publisher = + node.create_publisher::("topic")?; let _idiomatic_subscription = node .create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, move |_msg: rclrs_example_msgs::msg::VariousTypes| println!("Got idiomatic message!"), )?; let _direct_subscription = node .create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, move |_msg: rclrs_example_msgs::msg::rmw::VariousTypes| { println!("Got RMW-native message!") }, )?; println!("Sending idiomatic message."); idiomatic_publisher.publish(rclrs_example_msgs::msg::VariousTypes::default())?; - rclrs::spin_once(Arc::clone(&node), None)?; + executor.spin(rclrs::SpinOptions::spin_once())?; println!("Sending RMW-native message."); direct_publisher.publish(rclrs_example_msgs::msg::rmw::VariousTypes::default())?; - rclrs::spin_once(Arc::clone(&node), None)?; + executor.spin(rclrs::SpinOptions::spin_once())?; Ok(()) } diff --git a/examples/minimal_client_service/src/minimal_client.rs b/examples/minimal_client_service/src/minimal_client.rs index 915541d54..d1b35c0d9 100644 --- a/examples/minimal_client_service/src/minimal_client.rs +++ b/examples/minimal_client_service/src/minimal_client.rs @@ -1,34 +1,32 @@ -use std::env; - use anyhow::{Error, Result}; +use rclrs::{Context, SpinOptions, Promise}; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let mut executor = Context::default_from_env()?.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_client")?; + let node = executor.create_node("minimal_client")?; let client = node.create_client::("add_two_ints")?; - let request = example_interfaces::srv::AddTwoInts_Request { a: 41, b: 1 }; - println!("Starting client"); while !client.service_is_ready()? { std::thread::sleep(std::time::Duration::from_millis(10)); } - client.async_send_request_with_callback( - &request, - move |response: example_interfaces::srv::AddTwoInts_Response| { - println!( - "Result of {} + {} is: {}", - request.a, request.b, response.sum - ); - }, - )?; + let request = example_interfaces::srv::AddTwoInts_Request { a: 41, b: 1 }; + + let response: Promise = client.call(&request).unwrap(); - std::thread::sleep(std::time::Duration::from_millis(500)); + let promise = executor.commands().run(async move { + let response = response.await.unwrap(); + println!( + "Result of {} + {} is: {}", + request.a, request.b, response.sum, + ); + }); println!("Waiting for response"); - rclrs::spin(node).map_err(|err| err.into()) + executor.spin(SpinOptions::new().until_promise_resolved(promise))?; + Ok(()) } diff --git a/examples/minimal_client_service/src/minimal_client_async.rs b/examples/minimal_client_service/src/minimal_client_async.rs index 0eeb87f4d..b4bca1372 100644 --- a/examples/minimal_client_service/src/minimal_client_async.rs +++ b/examples/minimal_client_service/src/minimal_client_async.rs @@ -1,12 +1,10 @@ -use std::env; - use anyhow::{Error, Result}; +use rclrs::{Context, SpinOptions}; -#[tokio::main] -async fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; +fn main() -> Result<(), Error> { + let mut executor = Context::default_from_env()?.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_client")?; + let node = executor.create_node("minimal_client")?; let client = node.create_client::("add_two_ints")?; @@ -18,18 +16,17 @@ async fn main() -> Result<(), Error> { let request = example_interfaces::srv::AddTwoInts_Request { a: 41, b: 1 }; - let future = client.call_async(&request); + let promise = client.call_then( + &request, + move |response: example_interfaces::srv::AddTwoInts_Response| { + println!( + "Result of {} + {} is: {}", + request.a, request.b, response.sum, + ); + } + ).unwrap(); println!("Waiting for response"); - - let rclrs_spin = tokio::task::spawn_blocking(move || rclrs::spin(node)); - - let response = future.await?; - println!( - "Result of {} + {} is: {}", - request.a, request.b, response.sum - ); - - rclrs_spin.await.ok(); + executor.spin(SpinOptions::new().until_promise_resolved(promise))?; Ok(()) } diff --git a/examples/minimal_client_service/src/minimal_service.rs b/examples/minimal_client_service/src/minimal_service.rs index b4149c817..f249940bf 100644 --- a/examples/minimal_client_service/src/minimal_service.rs +++ b/examples/minimal_client_service/src/minimal_service.rs @@ -1,25 +1,30 @@ -use std::env; - use anyhow::{Error, Result}; +use rclrs::{Context, ServiceInfo, SpinOptions}; fn handle_service( - _request_header: &rclrs::rmw_request_id_t, request: example_interfaces::srv::AddTwoInts_Request, + info: ServiceInfo, ) -> example_interfaces::srv::AddTwoInts_Response { - println!("request: {} + {}", request.a, request.b); + let timestamp = info + .received_timestamp + .map(|t| format!(" at [{t:?}]")) + .unwrap_or(String::new()); + + println!("request{timestamp}: {} + {}", request.a, request.b); example_interfaces::srv::AddTwoInts_Response { sum: request.a + request.b, } } fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let mut executor = Context::default_from_env()?.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_service")?; + let node = executor.create_node("minimal_service")?; let _server = node .create_service::("add_two_ints", handle_service)?; println!("Starting server"); - rclrs::spin(node).map_err(|err| err.into()) + executor.spin(SpinOptions::default())?; + Ok(()) } diff --git a/examples/minimal_pub_sub/src/minimal_publisher.rs b/examples/minimal_pub_sub/src/minimal_publisher.rs index 720086917..be88b0f5a 100644 --- a/examples/minimal_pub_sub/src/minimal_publisher.rs +++ b/examples/minimal_pub_sub/src/minimal_publisher.rs @@ -1,14 +1,12 @@ -use std::env; - use anyhow::{Error, Result}; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let context = rclrs::Context::default_from_env()?; + let executor = context.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_publisher")?; + let node = executor.create_node("minimal_publisher")?; - let publisher = - node.create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + let publisher = node.create_publisher::("topic")?; let mut message = std_msgs::msg::String::default(); diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index ebc5fc194..59fa37b1e 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -1,23 +1,25 @@ -use std::env; - use anyhow::{Error, Result}; +use std::sync::Mutex; +use rclrs::{Context, SpinOptions}; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; - - let node = rclrs::create_node(&context, "minimal_subscriber")?; + let context = Context::default_from_env()?; + let mut executor = context.create_basic_executor(); - let mut num_messages: usize = 0; + let node = executor.create_node("minimal_subscriber")?; + let num_messages = Mutex::new(0usize); let _subscription = node.create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, move |msg: std_msgs::msg::String| { - num_messages += 1; + let mut num = num_messages.lock().unwrap(); + *num += 1; println!("I heard: '{}'", msg.data); - println!("(Got {} messages so far)", num_messages); + println!("(Got {} messages so far)", num); }, )?; - rclrs::spin(node).map_err(|err| err.into()) + println!("Waiting for messages..."); + executor.spin(SpinOptions::default())?; + Ok(()) } diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index fb03574a2..46bd9780c 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -1,23 +1,23 @@ -use std::{ - env, - sync::{ - atomic::{AtomicU32, Ordering}, - Arc, Mutex, - }, +use std::sync::{ + atomic::{AtomicU32, Ordering}, + Arc, Mutex, }; use anyhow::{Error, Result}; struct MinimalSubscriber { num_messages: AtomicU32, - node: Arc, - subscription: Mutex>>>, + node: rclrs::Node, + subscription: Mutex>>, } impl MinimalSubscriber { - pub fn new(name: &str, topic: &str) -> Result, rclrs::RclrsError> { - let context = rclrs::Context::new(env::args())?; - let node = rclrs::create_node(&context, name)?; + pub fn new( + executor: &rclrs::Executor, + name: &str, + topic: &str, + ) -> Result, rclrs::RclrsError> { + let node = executor.create_node(name)?; let minimal_subscriber = Arc::new(MinimalSubscriber { num_messages: 0.into(), node, @@ -29,7 +29,6 @@ impl MinimalSubscriber { .node .create_subscription::( topic, - rclrs::QOS_PROFILE_DEFAULT, move |msg: std_msgs::msg::String| { minimal_subscriber_aux.callback(msg); }, @@ -50,14 +49,15 @@ impl MinimalSubscriber { } fn main() -> Result<(), Error> { - let publisher_context = rclrs::Context::new(env::args())?; - let publisher_node = rclrs::create_node(&publisher_context, "minimal_publisher")?; + let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); + let publisher_node = executor.create_node("minimal_publisher")?; - let subscriber_node_one = MinimalSubscriber::new("minimal_subscriber_one", "topic")?; - let subscriber_node_two = MinimalSubscriber::new("minimal_subscriber_two", "topic")?; + let _subscriber_node_one = + MinimalSubscriber::new(&executor, "minimal_subscriber_one", "topic")?; + let _subscriber_node_two = + MinimalSubscriber::new(&executor, "minimal_subscriber_two", "topic")?; - let publisher = publisher_node - .create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + let publisher = publisher_node.create_publisher::("topic")?; std::thread::spawn(move || -> Result<(), rclrs::RclrsError> { let mut message = std_msgs::msg::String::default(); @@ -71,11 +71,7 @@ fn main() -> Result<(), Error> { } }); - let executor = rclrs::SingleThreadedExecutor::new(); - - executor.add_node(&publisher_node)?; - executor.add_node(&subscriber_node_one.node)?; - executor.add_node(&subscriber_node_two.node)?; - - executor.spin().map_err(|err| err.into()) + executor + .spin(rclrs::SpinOptions::default()) + .map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/zero_copy_publisher.rs b/examples/minimal_pub_sub/src/zero_copy_publisher.rs index 5e73b5de7..d495f90bb 100644 --- a/examples/minimal_pub_sub/src/zero_copy_publisher.rs +++ b/examples/minimal_pub_sub/src/zero_copy_publisher.rs @@ -1,14 +1,12 @@ -use std::env; - use anyhow::{Error, Result}; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let context = rclrs::Context::default_from_env()?; + let executor = context.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_publisher")?; + let node = executor.create_node("minimal_publisher")?; - let publisher = - node.create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + let publisher = node.create_publisher::("topic")?; let mut publish_count: u32 = 1; diff --git a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs index 9551dba0e..4769e9f12 100644 --- a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -1,23 +1,24 @@ -use std::env; - use anyhow::{Error, Result}; +use std::sync::Mutex; +use rclrs::ReadOnlyLoanedMessage; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_subscriber")?; + let node = executor.create_node("minimal_subscriber")?; - let mut num_messages: usize = 0; + let num_messages = Mutex::new(0usize); let _subscription = node.create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, - move |msg: rclrs::ReadOnlyLoanedMessage<'_, std_msgs::msg::UInt32>| { - num_messages += 1; + move |msg: ReadOnlyLoanedMessage| { + let mut num = num_messages.lock().unwrap(); + *num += 1; println!("I heard: '{}'", msg.data); - println!("(Got {} messages so far)", num_messages); + println!("(Got {} messages so far)", *num); }, )?; - rclrs::spin(node).map_err(|err| err.into()) + executor.spin(rclrs::SpinOptions::default())?; + Ok(()) } diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index 98d0e0f74..8b5b467a7 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -1,36 +1,36 @@ -use rclrs::{create_node, Context, Node, Publisher, RclrsError, QOS_PROFILE_DEFAULT}; -use std::{sync::Arc, thread, time::Duration}; +use rclrs::{Context, Executor, Publisher, RclrsError, SpinOptions}; +use std::{thread, time::Duration}; use std_msgs::msg::String as StringMsg; + struct SimplePublisherNode { - node: Arc, - _publisher: Arc>, + publisher: Publisher, } + impl SimplePublisherNode { - fn new(context: &Context) -> Result { - let node = create_node(context, "simple_publisher").unwrap(); - let _publisher = node - .create_publisher("publish_hello", QOS_PROFILE_DEFAULT) + fn new(executor: &Executor) -> Result { + let node = executor.create_node("simple_publisher").unwrap(); + let publisher = node + .create_publisher("publish_hello") .unwrap(); - Ok(Self { node, _publisher }) + Ok(Self { publisher }) } fn publish_data(&self, increment: i32) -> Result { let msg: StringMsg = StringMsg { data: format!("Hello World {}", increment), }; - self._publisher.publish(msg).unwrap(); + self.publisher.publish(msg).unwrap(); Ok(increment + 1_i32) } } fn main() -> Result<(), RclrsError> { - let context = Context::new(std::env::args()).unwrap(); - let publisher = Arc::new(SimplePublisherNode::new(&context).unwrap()); - let publisher_other_thread = Arc::clone(&publisher); + let mut executor = Context::default_from_env().unwrap().create_basic_executor(); + let node = SimplePublisherNode::new(&executor).unwrap(); let mut count: i32 = 0; thread::spawn(move || loop { thread::sleep(Duration::from_millis(1000)); - count = publisher_other_thread.publish_data(count).unwrap(); + count = node.publish_data(count).unwrap(); }); - rclrs::spin(publisher.node.clone()) + executor.spin(SpinOptions::default()) } diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index a0d02bb4c..5e11f5fda 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -1,35 +1,31 @@ -use rclrs::{create_node, Context, Node, RclrsError, Subscription, QOS_PROFILE_DEFAULT}; +use rclrs::{Context, Executor, RclrsError, SpinOptions, Subscription}; use std::{ - env, sync::{Arc, Mutex}, thread, time::Duration, }; use std_msgs::msg::String as StringMsg; + pub struct SimpleSubscriptionNode { - node: Arc, - _subscriber: Arc>, + #[allow(unused)] + subscriber: Subscription, data: Arc>>, } + impl SimpleSubscriptionNode { - fn new(context: &Context) -> Result { - let node = create_node(context, "simple_subscription").unwrap(); + fn new(executor: &Executor) -> Result { + let node = executor.create_node("simple_subscription").unwrap(); let data: Arc>> = Arc::new(Mutex::new(None)); let data_mut: Arc>> = Arc::clone(&data); - let _subscriber = node + let subscriber = node .create_subscription::( "publish_hello", - QOS_PROFILE_DEFAULT, move |msg: StringMsg| { *data_mut.lock().unwrap() = Some(msg); }, ) .unwrap(); - Ok(Self { - node, - _subscriber, - data, - }) + Ok(Self { subscriber, data }) } fn data_callback(&self) -> Result<(), RclrsError> { if let Some(data) = self.data.lock().unwrap().as_ref() { @@ -41,12 +37,11 @@ impl SimpleSubscriptionNode { } } fn main() -> Result<(), RclrsError> { - let context = Context::new(env::args()).unwrap(); - let subscription = Arc::new(SimpleSubscriptionNode::new(&context).unwrap()); - let subscription_other_thread = Arc::clone(&subscription); + let mut executor = Context::default_from_env().unwrap().create_basic_executor(); + let node = SimpleSubscriptionNode::new(&executor).unwrap(); thread::spawn(move || loop { thread::sleep(Duration::from_millis(1000)); - subscription_other_thread.data_callback().unwrap() + node.data_callback().unwrap() }); - rclrs::spin(subscription.node.clone()) + executor.spin(SpinOptions::default()) } diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index fe17cc990..a47e90a5d 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -23,6 +23,9 @@ cfg-if = "1.0.0" # Needed for clients futures = "0.3" +# Needed for the runtime-agnostic timeout feature +async-std = "1.13" + # Needed for dynamic messages libloading = { version = "0.8", optional = true } @@ -39,7 +42,7 @@ tempfile = "3.3.0" # Needed for publisher and subscriber tests test_msgs = {version = "*"} # Needed for parameter service tests -tokio = { version = "*", features = ["rt", "time", "macros"] } +tokio = { version = "1", features = ["rt", "time", "macros"] } [build-dependencies] # Needed for FFI diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index b308f1de2..15baf57f6 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -1,108 +1,214 @@ use std::{ - boxed::Box, collections::HashMap, ffi::CString, - sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, + sync::{Arc, Mutex, MutexGuard}, }; -use futures::channel::oneshot; use rosidl_runtime_rs::Message; use crate::{ - error::{RclReturnCode, ToResult}, - rcl_bindings::*, - MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + error::ToResult, rcl_bindings::*, IntoPrimitiveOptions, MessageCow, Node, Promise, QoSProfile, + RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclReturnCode, RclrsError, ServiceInfo, + Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, }; -// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread -// they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for rcl_client_t {} - -/// Manage the lifecycle of an `rcl_client_t`, including managing its dependencies -/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are -/// [dropped after][1] the `rcl_client_t`. -/// -/// [1]: -pub struct ClientHandle { - rcl_client: Mutex, - node_handle: Arc, - pub(crate) in_use_by_wait_set: Arc, -} +mod client_async_callback; +pub use client_async_callback::*; -impl ClientHandle { - pub(crate) fn lock(&self) -> MutexGuard { - self.rcl_client.lock().unwrap() - } -} +mod client_callback; +pub use client_callback::*; -impl Drop for ClientHandle { - fn drop(&mut self) { - let rcl_client = self.rcl_client.get_mut().unwrap(); - let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); - let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); - // SAFETY: The entity lifecycle mutex is locked to protect against the risk of - // global variables in the rmw implementation being unsafely modified during cleanup. - unsafe { - rcl_client_fini(rcl_client, &mut *rcl_node); - } - } -} +mod client_output; +pub use client_output::*; -/// Trait to be implemented by concrete Client structs. +/// Main class responsible for sending requests to a ROS service. /// -/// See [`Client`] for an example. -pub trait ClientBase: Send + Sync { - /// Internal function to get a reference to the `rcl` handle. - fn handle(&self) -> &ClientHandle; - /// Tries to take a new response and run the callback or future with it. - fn execute(&self) -> Result<(), RclrsError>; -} - -type RequestValue = Box; - -type RequestId = i64; +/// Create a client using [`Node::create_client`][1]. +/// +/// Receiving responses requires the node's executor to [spin][2]. +/// +/// [1]: crate::NodeState::create_client +/// [2]: crate::spin +pub type Client = Arc>; -/// Main class responsible for sending requests to a ROS service. +/// The inner state of a [`Client`]. /// -/// The only available way to instantiate clients is via [`Node::create_client`][1], this is to -/// ensure that [`Node`][2]s can track all the clients that have been created. +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Client`] in a non-owning way. It is +/// generally recommended to manage the `ClientState` inside of an [`Arc`], +/// and [`Client`] is provided as a convenience alias for that. /// -/// [1]: crate::Node::create_client -/// [2]: crate::Node -pub struct Client +/// The public API of the [`Client`] type is implemented via `ClientState`. +/// +/// [1]: std::sync::Weak +pub struct ClientState where T: rosidl_runtime_rs::Service, { - pub(crate) handle: Arc, - requests: Mutex>>, - futures: Arc>>>, + handle: Arc, + board: Arc>>, + #[allow(unused)] + lifecycle: WaitableLifecycle, } -impl Client +impl ClientState where T: rosidl_runtime_rs::Service, { + /// Send out a request for this service client. + /// + /// If the call to rcl succeeds, you will receive a [`Promise`] of the + /// service response. You can choose what kind of metadata you receive. The + /// promise can provide any of the following: + /// - `Response` + /// - `(Response, `[`RequestId`][1]`)` + /// - `(Response, `[`ServiceInfo`][2]`)` + /// + /// Dropping the [`Promise`] that this returns will not cancel the request. + /// Once this function is called, the service provider will receive the + /// request and respond to it no matter what. + /// + /// [1]: crate::RequestId + /// [2]: crate::ServiceInfo + pub fn call<'a, Req, Out>(&self, request: Req) -> Result, RclrsError> + where + Req: MessageCow<'a, T::Request>, + Out: ClientOutput, + { + let (sender, promise) = Out::create_channel(); + let rmw_message = T::Request::into_rmw_message(request.into_cow()); + let mut sequence_number = -1; + unsafe { + // SAFETY: The client handle ensures the rcl_client is valid and + // our generic system ensures it has the correct type. + rcl_send_request( + &*self.handle.lock() as *const _, + rmw_message.as_ref() as *const ::RmwMsg as *mut _, + &mut sequence_number, + ) + } + .ok()?; + + // TODO(@mxgrey): Log errors here when logging becomes available. + self.board + .lock() + .unwrap() + .new_request(sequence_number, sender); + + Ok(promise) + } + + /// Call this service and then handle its response with a regular callback. + /// + /// You do not need to retain the [`Promise`] that this returns, even if the + /// compiler warns you that you need to. You can use the [`Promise`] to know + /// when the response is finished being processed, but otherwise you can + /// safely discard it. + // + // TODO(@mxgrey): Add documentation to show what callback signatures are supported + pub fn call_then<'a, Req, Args>( + &self, + request: Req, + callback: impl ClientCallback, + ) -> Result, RclrsError> + where + Req: MessageCow<'a, T::Request>, + { + let callback = move |response, info| async { + callback.run_client_callback(response, info); + }; + self.call_then_async(request, callback) + } + + /// Call this service and then handle its response with an async callback. + /// + /// You do not need to retain the [`Promise`] that this returns, even if the + /// compiler warns you that you need to. You can use the [`Promise`] to know + /// when the response is finished being processed, but otherwise you can + /// safely discard it. + // + // TODO(@mxgrey): Add documentation to show what callback signatures are supported + pub fn call_then_async<'a, Req, Args>( + &self, + request: Req, + callback: impl ClientAsyncCallback, + ) -> Result, RclrsError> + where + Req: MessageCow<'a, T::Request>, + { + let response: Promise<(T::Response, ServiceInfo)> = self.call(request)?; + let promise = self.handle.node.commands().run(async move { + match response.await { + Ok((response, info)) => { + callback.run_client_async_callback(response, info).await; + } + Err(_) => { + // TODO(@mxgrey): Log this error when logging becomes available + } + } + }); + + Ok(promise) + } + + /// Check if a service server is available. + /// + /// Will return true if there is a service server available, false if unavailable. + /// + /// Consider using [`Self::notify_on_service_ready`] if you want to wait + /// until a service for this client is ready. + pub fn service_is_ready(&self) -> Result { + let mut is_ready = false; + let client = &mut *self.handle.rcl_client.lock().unwrap(); + let node = &mut *self.handle.node.handle().rcl_node.lock().unwrap(); + + unsafe { + // SAFETY both node and client are guaranteed to be valid here + // client is guaranteed to have been generated with node + rcl_service_server_is_available(node as *const _, client as *const _, &mut is_ready) + } + .ok()?; + Ok(is_ready) + } + + /// Get a promise that will be fulfilled when a service 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_service_ready(self: &Arc) -> Promise<()> { + let client = Arc::clone(self); + self.handle.node.notify_on_graph_change( + // TODO(@mxgrey): Log any errors here once logging is available + move || client.service_is_ready().is_ok_and(|r| r), + ) + } + /// Creates a new client. - pub(crate) fn new(node_handle: Arc, topic: &str) -> Result + pub(crate) fn create<'a>( + node: &Node, + options: impl Into>, + ) -> Result, RclrsError> // This uses pub(crate) visibility to avoid instantiating this struct outside // [`Node::create_client`], see the struct's documentation for the rationale where T: rosidl_runtime_rs::Service, { + let ClientOptions { service_name, qos } = options.into(); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_client = unsafe { rcl_get_zero_initialized_client() }; let type_support = ::get_type_support() as *const rosidl_service_type_support_t; - let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { - err, - s: topic.into(), - })?; + let topic_c_string = + CString::new(service_name).map_err(|err| RclrsError::StringContainsNul { + err, + s: service_name.into(), + })?; // SAFETY: No preconditions for this function. - let client_options = unsafe { rcl_client_get_default_options() }; + let mut client_options = unsafe { rcl_client_get_default_options() }; + client_options.qos = qos.into(); { - let rcl_node = node_handle.rcl_node.lock().unwrap(); + let rcl_node = node.handle().rcl_node.lock().unwrap(); let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); // SAFETY: @@ -126,187 +232,220 @@ where let handle = Arc::new(ClientHandle { rcl_client: Mutex::new(rcl_client), - node_handle, - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + node: Arc::clone(&node), }); - Ok(Self { + let commands = node.commands(); + let board = Arc::new(Mutex::new(ClientRequestBoard::new())); + + let (waitable, lifecycle) = Waitable::new( + Box::new(ClientExecutable { + handle: Arc::clone(&handle), + board: Arc::clone(&board), + }), + Some(Arc::clone(&commands.get_guard_condition())), + ); + commands.add_to_wait_set(waitable); + + Ok(Arc::new(Self { handle, - requests: Mutex::new(HashMap::new()), - futures: Arc::new(Mutex::new( - HashMap::>::new(), - )), - }) + board, + lifecycle, + })) } +} - /// Sends a request with a callback to be called with the response. - /// - /// The [`MessageCow`] trait is implemented by any - /// [`Message`] as well as any reference to a `Message`. - /// - /// The reason for allowing owned messages is that publishing owned messages can be more - /// efficient in the case of idiomatic messages[^note]. - /// - /// [^note]: See the [`Message`] trait for an explanation of "idiomatic". - /// - /// Hence, when a message will not be needed anymore after publishing, pass it by value. - /// When a message will be needed again after publishing, pass it by reference, instead of cloning and passing by value. - pub fn async_send_request_with_callback<'a, M: MessageCow<'a, T::Request>, F>( - &self, - message: M, - callback: F, - ) -> Result<(), RclrsError> - where - F: FnOnce(T::Response) + 'static + Send, - { - let rmw_message = T::Request::into_rmw_message(message.into_cow()); - let mut sequence_number = -1; - unsafe { - // SAFETY: The request type is guaranteed to match the client type by the type system. - rcl_send_request( - &*self.handle.lock() as *const _, - rmw_message.as_ref() as *const ::RmwMsg as *mut _, - &mut sequence_number, - ) +/// `ClientOptions` are used by [`Node::create_client`][1] to initialize a +/// [`Client`] for a service. +/// +/// [1]: crate::Node::create_client +#[derive(Debug, Clone)] +#[non_exhaustive] +pub struct ClientOptions<'a> { + /// The name of the service that this client will send requests to + pub service_name: &'a str, + /// The quality of the service profile for this client + pub qos: QoSProfile, +} + +impl<'a> ClientOptions<'a> { + /// Initialize a new [`ClientOptions`] with default settings. + pub fn new(service_name: &'a str) -> Self { + Self { + service_name, + qos: QoSProfile::services_default(), } - .ok()?; - let requests = &mut *self.requests.lock().unwrap(); - requests.insert(sequence_number, Box::new(callback)); - Ok(()) } +} - /// Sends a request and returns the response as a `Future`. - /// - /// The [`MessageCow`] trait is implemented by any - /// [`Message`] as well as any reference to a `Message`. - /// - /// The reason for allowing owned messages is that publishing owned messages can be more - /// efficient in the case of idiomatic messages[^note]. - /// - /// [^note]: See the [`Message`] trait for an explanation of "idiomatic". - /// - /// Hence, when a message will not be needed anymore after publishing, pass it by value. - /// When a message will be needed again after publishing, pass it by reference, instead of cloning and passing by value. - pub async fn call_async<'a, R: MessageCow<'a, T::Request>>( - &self, - request: R, - ) -> Result - where - T: rosidl_runtime_rs::Service, - { - let rmw_message = T::Request::into_rmw_message(request.into_cow()); - let mut sequence_number = -1; - unsafe { - // SAFETY: The request type is guaranteed to match the client type by the type system. - rcl_send_request( - &*self.handle.lock() as *const _, - rmw_message.as_ref() as *const ::RmwMsg as *mut _, - &mut sequence_number, - ) - } - .ok()?; - let (tx, rx) = oneshot::channel::(); - self.futures.lock().unwrap().insert(sequence_number, tx); - // It is safe to call unwrap() here since the `Canceled` error will only happen when the - // `Sender` is dropped - // https://docs.rs/futures/latest/futures/channel/oneshot/struct.Canceled.html - Ok(rx.await.unwrap()) +impl<'a, T: IntoPrimitiveOptions<'a>> From for ClientOptions<'a> { + fn from(value: T) -> Self { + let primitive = value.into_primitive_options(); + let mut options = Self::new(primitive.name); + primitive.apply(&mut options.qos); + options } +} - /// Fetches a new response. - /// - /// When there is no new message, this will return a - /// [`ClientTakeFailed`][1]. - /// - /// [1]: crate::RclrsError - // - // ```text - // +----------------------+ - // | rclrs::take_response | - // +----------+-----------+ - // | - // | - // +----------v-----------+ - // | rcl_take_response | - // +----------+-----------+ - // | - // | - // +----------v----------+ - // | rmw_take | - // +---------------------+ - // ``` - pub fn take_response(&self) -> Result<(T::Response, rmw_request_id_t), RclrsError> { - let mut request_id_out = rmw_request_id_t { - writer_guid: [0; 16], - sequence_number: 0, - }; - type RmwMsg = - <::Response as rosidl_runtime_rs::Message>::RmwMsg; - let mut response_out = RmwMsg::::default(); - let handle = &*self.handle.lock(); - unsafe { - // SAFETY: The three pointers are valid/initialized - rcl_take_response( - handle, - &mut request_id_out, - &mut response_out as *mut RmwMsg as *mut _, - ) - } - .ok()?; - Ok((T::Response::from_rmw_message(response_out), request_id_out)) +struct ClientExecutable +where + T: rosidl_runtime_rs::Service, +{ + handle: Arc, + board: Arc>>, +} + +impl RclPrimitive for ClientExecutable +where + T: rosidl_runtime_rs::Service, +{ + fn execute(&mut self) -> Result<(), RclrsError> { + self.board.lock().unwrap().execute(&self.handle) } - /// Check if a service server is available. - /// - /// Will return true if there is a service server available, false if unavailable. - /// - pub fn service_is_ready(&self) -> Result { - let mut is_ready = false; - let client = &mut *self.handle.rcl_client.lock().unwrap(); - let node = &mut *self.handle.node_handle.rcl_node.lock().unwrap(); + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::Client(self.handle.lock()) + } - unsafe { - // SAFETY both node and client are guaranteed to be valid here - // client is guaranteed to have been generated with node - rcl_service_server_is_available(node as *const _, client as *const _, &mut is_ready) - } - .ok()?; - Ok(is_ready) + fn kind(&self) -> RclPrimitiveKind { + RclPrimitiveKind::Client } } -impl ClientBase for Client +type SequenceNumber = i64; + +/// This is used internally to monitor the state of active requests, as well as +/// responses that have arrived without a known request. +struct ClientRequestBoard +where + T: rosidl_runtime_rs::Service, +{ + // This stores all active requests that have not received a response yet + active_requests: HashMap>, + // This holds responses that came in when no active request matched the + // sequence number. This could happen if take_response is triggered before + // the new_request for the same sequence number. That is extremely unlikely + // to ever happen but is theoretically possible on systems that may exhibit + // very strange CPU scheduling patterns, so we should account for it. + loose_responses: HashMap, +} + +impl ClientRequestBoard where T: rosidl_runtime_rs::Service, { - fn handle(&self) -> &ClientHandle { - &self.handle + fn new() -> Self { + Self { + active_requests: Default::default(), + loose_responses: Default::default(), + } + } + + fn new_request( + &mut self, + sequence_number: SequenceNumber, + sender: AnyClientOutputSender, + ) { + if let Some((response, info)) = self.loose_responses.remove(&sequence_number) { + // Weirdly the response for this request already arrived, so we'll + // send it off immediately. + sender.send_response(response, info); + } else { + self.active_requests.insert(sequence_number, sender); + } } - fn execute(&self) -> Result<(), RclrsError> { - let (res, req_id) = match self.take_response() { - Ok((res, req_id)) => (res, req_id), - Err(RclrsError::RclError { - code: RclReturnCode::ClientTakeFailed, - .. - }) => { - // Spurious wakeup – this may happen even when a waitset indicated that this - // client was ready, so it shouldn't be an error. - return Ok(()); + fn execute(&mut self, handle: &Arc) -> Result<(), RclrsError> { + match self.take_response(handle) { + Ok((response, info)) => { + let seq = info.request_id.sequence_number; + if let Some(sender) = self.active_requests.remove(&seq) { + // The active request is available, so send this response off + sender.send_response(response, info); + } else { + // Weirdly there isn't an active request for this, so save + // it in the loose responses map. + self.loose_responses.insert(seq, (response, info)); + } + } + Err(err) => { + match err { + RclrsError::RclError { + code: RclReturnCode::ClientTakeFailed, + .. + } => { + // This is okay, it means a spurious wakeup happened + } + err => { + // TODO(@mxgrey): Log the error here once logging is available + eprintln!("Error while taking a response for a client: {err}"); + } + } } - Err(e) => return Err(e), - }; - let requests = &mut *self.requests.lock().unwrap(); - let futures = &mut *self.futures.lock().unwrap(); - if let Some(callback) = requests.remove(&req_id.sequence_number) { - callback(res); - } else if let Some(future) = futures.remove(&req_id.sequence_number) { - let _ = future.send(res); } Ok(()) } + + fn take_response( + &self, + handle: &Arc, + ) -> Result<(T::Response, rmw_service_info_t), RclrsError> { + let mut service_info_out = ServiceInfo::zero_initialized_rmw(); + let mut response_out = ::RmwMsg::default(); + let handle = &*handle.lock(); + unsafe { + // SAFETY: The three pointers are all kept valid by the handle + rcl_take_response_with_info( + handle, + &mut service_info_out, + &mut response_out as *mut ::RmwMsg as *mut _, + ) + } + .ok() + .map(|_| { + ( + T::Response::from_rmw_message(response_out), + service_info_out, + ) + }) + } +} + +/// Manage the lifecycle of an `rcl_client_t`, including managing its dependencies +/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are +/// [dropped after][1] the `rcl_client_t`. +/// +/// [1]: +struct ClientHandle { + rcl_client: Mutex, + /// We store the whole node here because we use some of its user-facing API + /// in some of the Client methods. + node: Node, +} + +impl ClientHandle { + fn lock(&self) -> MutexGuard { + self.rcl_client.lock().unwrap() + } } +impl Drop for ClientHandle { + fn drop(&mut self) { + let rcl_client = self.rcl_client.get_mut().unwrap(); + let mut rcl_node = self.node.handle().rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of + // global variables in the rmw implementation being unsafely modified during cleanup. + unsafe { + rcl_client_fini(rcl_client, &mut *rcl_node); + } + } +} + +// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread +// they are running in. Therefore, this type can be safely sent to another thread. +unsafe impl Send for rcl_client_t {} + #[cfg(test)] mod tests { use super::*; diff --git a/rclrs/src/client/client_async_callback.rs b/rclrs/src/client/client_async_callback.rs new file mode 100644 index 000000000..537ae1bbf --- /dev/null +++ b/rclrs/src/client/client_async_callback.rs @@ -0,0 +1,57 @@ +use rosidl_runtime_rs::Service; + +use std::future::Future; + +use crate::{RequestId, ServiceInfo}; + +/// A trait to deduce async callbacks of service clients. +/// +/// Users of rclrs never need to use this trait directly. +// +// TODO(@mxgrey): Add a description of what callback signatures are supported +pub trait ClientAsyncCallback: Send + 'static +where + T: Service, +{ + /// This represents the type of task (Future) that will be produced by the callback + type Task: Future + Send; + + /// Trigger the callback to run + fn run_client_async_callback(self, response: T::Response, info: ServiceInfo) -> Self::Task; +} + +impl ClientAsyncCallback for Func +where + T: Service, + Func: FnOnce(T::Response) -> Fut + Send + 'static, + Fut: Future + Send, +{ + type Task = Fut; + fn run_client_async_callback(self, response: T::Response, _info: ServiceInfo) -> Fut { + self(response) + } +} + +impl ClientAsyncCallback for Func +where + T: Service, + Func: FnOnce(T::Response, RequestId) -> Fut + Send + 'static, + Fut: Future + Send, +{ + type Task = Fut; + fn run_client_async_callback(self, response: T::Response, info: ServiceInfo) -> Fut { + self(response, info.request_id) + } +} + +impl ClientAsyncCallback for Func +where + T: Service, + Func: FnOnce(T::Response, ServiceInfo) -> Fut + Send + 'static, + Fut: Future + Send, +{ + type Task = Fut; + fn run_client_async_callback(self, response: T::Response, info: ServiceInfo) -> Fut { + self(response, info) + } +} diff --git a/rclrs/src/client/client_callback.rs b/rclrs/src/client/client_callback.rs new file mode 100644 index 000000000..d05a83de3 --- /dev/null +++ b/rclrs/src/client/client_callback.rs @@ -0,0 +1,46 @@ +use rosidl_runtime_rs::Service; + +use crate::{RequestId, ServiceInfo}; + +/// A trait to deduce regular callbacks of service clients. +/// +/// Users of rclrs never need to use this trait directly. +// +// TODO(@mxgrey): Add a description of what callback signatures are supported +pub trait ClientCallback: Send + 'static +where + T: Service, +{ + /// Trigger the callback to run + fn run_client_callback(self, response: T::Response, info: ServiceInfo); +} + +impl ClientCallback for Func +where + T: Service, + Func: FnOnce(T::Response) + Send + 'static, +{ + fn run_client_callback(self, response: T::Response, _info: ServiceInfo) { + self(response) + } +} + +impl ClientCallback for Func +where + T: Service, + Func: FnOnce(T::Response, RequestId) + Send + 'static, +{ + fn run_client_callback(self, response: T::Response, info: ServiceInfo) { + self(response, info.request_id) + } +} + +impl ClientCallback for Func +where + T: Service, + Func: FnOnce(T::Response, ServiceInfo) + Send + 'static, +{ + fn run_client_callback(self, response: T::Response, info: ServiceInfo) { + self(response, info) + } +} diff --git a/rclrs/src/client/client_output.rs b/rclrs/src/client/client_output.rs new file mode 100644 index 000000000..f0c2b2314 --- /dev/null +++ b/rclrs/src/client/client_output.rs @@ -0,0 +1,66 @@ +use rosidl_runtime_rs::Message; + +use futures::channel::oneshot::{channel, Sender}; + +use crate::{rcl_bindings::rmw_service_info_t, Promise, RequestId, ServiceInfo}; + +/// This trait allows us to deduce how much information a user wants to receive +/// from a client call. A user can choose to receive only the response from the +/// service or may include the [`RequestId`] or [`ServiceInfo`] metadata. +/// +/// Users never need to use this trait directly. +pub trait ClientOutput: Sized { + /// Create the appropriate type of channel to send the information that the + /// user asked for. + fn create_channel() -> (AnyClientOutputSender, Promise); +} + +impl ClientOutput for Response { + fn create_channel() -> (AnyClientOutputSender, Promise) { + let (sender, receiver) = channel(); + (AnyClientOutputSender::ResponseOnly(sender), receiver) + } +} + +impl ClientOutput for (Response, RequestId) { + fn create_channel() -> (AnyClientOutputSender, Promise) { + let (sender, receiver) = channel(); + (AnyClientOutputSender::WithId(sender), receiver) + } +} + +impl ClientOutput for (Response, ServiceInfo) { + fn create_channel() -> (AnyClientOutputSender, Promise) { + let (sender, receiver) = channel(); + (AnyClientOutputSender::WithServiceInfo(sender), receiver) + } +} + +/// Can send any kind of response for a client call. +pub enum AnyClientOutputSender { + /// The user only asked for the response. + ResponseOnly(Sender), + /// The user also asked for the RequestId + WithId(Sender<(Response, RequestId)>), + /// The user also asked for the ServiceInfo + WithServiceInfo(Sender<(Response, ServiceInfo)>), +} + +impl AnyClientOutputSender { + pub(super) fn send_response(self, response: Response, service_info: rmw_service_info_t) { + match self { + Self::ResponseOnly(sender) => { + let _ = sender.send(response); + } + Self::WithId(sender) => { + let _ = sender.send(( + response, + RequestId::from_rmw_request_id(&service_info.request_id), + )); + } + Self::WithServiceInfo(sender) => { + let _ = sender.send((response, ServiceInfo::from_rmw_service_info(&service_info))); + } + } + } +} diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 524169bb2..09407996a 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -6,7 +6,10 @@ use std::{ vec::Vec, }; -use crate::{rcl_bindings::*, LoggingLifecycle, RclrsError, ToResult}; +use crate::{ + rcl_bindings::*, BasicExecutorRuntime, Executor, ExecutorRuntime, LoggingLifecycle, RclrsError, + ToResult, +}; /// This is locked whenever initializing or dropping any middleware entity /// because we have found issues in RCL and some RMW implementations that @@ -78,34 +81,40 @@ pub(crate) struct ContextHandle { logging: Arc, } +impl Default for Context { + fn default() -> Self { + // SAFETY: It should always be valid to instantiate a context with no + // arguments, no parameters, no options, etc. + Self::new([], InitOptions::default()).expect("Failed to instantiate a default context") + } +} + impl Context { /// Creates a new context. /// - /// Usually this would be called with `std::env::args()`, analogously to `rclcpp::init()`. - /// See also the official "Passing ROS arguments to nodes via the command-line" tutorial. + /// * `args` - A sequence of strings that resembles command line arguments + /// that users can pass into a ROS executable. See [the official tutorial][1] + /// to know what these arguments may look like. To simply pass in the arguments + /// that the user has provided from the command line, call [`Self::from_env`] + /// or [`Self::default_from_env`] instead. /// - /// Creating a context will fail if the args contain invalid ROS arguments. + /// * `options` - Additional options that your application can use to override + /// settings that would otherwise be determined by the environment. /// - /// # Example - /// ``` - /// # use rclrs::Context; - /// assert!(Context::new([]).is_ok()); - /// let invalid_remapping = ["--ros-args", "-r", ":=:*/]"].map(String::from); - /// assert!(Context::new(invalid_remapping).is_err()); - /// ``` - pub fn new(args: impl IntoIterator) -> Result { - Self::new_with_options(args, InitOptions::new()) - } - - /// Same as [`Context::new`] except you can additionally provide initialization options. + /// Creating a context will fail if `args` contains invalid ROS arguments. /// /// # Example /// ``` /// use rclrs::{Context, InitOptions}; - /// let context = Context::new_with_options([], InitOptions::new().with_domain_id(Some(5))).unwrap(); + /// let context = Context::new( + /// std::env::args(), + /// InitOptions::new().with_domain_id(Some(5)), + /// ).unwrap(); /// assert_eq!(context.domain_id(), 5); - /// ```` - pub fn new_with_options( + /// ``` + /// + /// [1]: https://docs.ros.org/en/rolling/How-To-Guides/Node-arguments.html + pub fn new( args: impl IntoIterator, options: InitOptions, ) -> Result { @@ -165,6 +174,35 @@ impl Context { }) } + /// Same as [`Self::new`] but [`std::env::args`] is automatically passed in + /// for `args`. + pub fn from_env(options: InitOptions) -> Result { + Self::new(std::env::args(), options) + } + + /// Same as [`Self::from_env`] but the default [`InitOptions`] is passed in + /// for `options`. + pub fn default_from_env() -> Result { + Self::new(std::env::args(), InitOptions::default()) + } + + /// Create an executor that uses the [basic executor runtime][1] that comes + /// built into rclrs. + /// + /// [1]: BasicExecutorRuntime + pub fn create_basic_executor(&self) -> Executor { + let runtime = BasicExecutorRuntime::new(self); + self.create_executor(runtime) + } + + /// Create an [`Executor`] for this context. + pub fn create_executor(&self, runtime: E) -> Executor + where + E: 'static + ExecutorRuntime + Send, + { + Executor::new(Arc::clone(&self.handle), runtime) + } + /// Returns the ROS domain ID that the context is using. /// /// The domain ID controls which nodes can send messages to each other, see the [ROS 2 concept article][1]. @@ -265,14 +303,14 @@ mod tests { #[test] fn test_create_context() -> Result<(), RclrsError> { // If the context fails to be created, this will cause a panic - let _ = Context::new(vec![])?; + let _ = Context::new(vec![], InitOptions::default())?; Ok(()) } #[test] fn test_context_ok() -> Result<(), RclrsError> { // If the context fails to be created, this will cause a panic - let created_context = Context::new(vec![]).unwrap(); + let created_context = Context::new(vec![], InitOptions::default()).unwrap(); assert!(created_context.ok()); Ok(()) diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index 3eba2549f..34175b2d9 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -32,6 +32,9 @@ pub enum RclrsError { }, /// It was attempted to add a waitable to a wait set twice. AlreadyAddedToWaitSet, + /// The guard condition that you tried to trigger is not owned by the + /// [`GuardCondition`][crate::GuardCondition] instance. + UnownedGuardCondition, } impl Display for RclrsError { @@ -48,6 +51,12 @@ impl Display for RclrsError { "Could not add entity to wait set because it was already added to a wait set" ) } + RclrsError::UnownedGuardCondition => { + write!( + f, + "Could not trigger guard condition because it is not owned by rclrs" + ) + } } } } @@ -79,7 +88,10 @@ impl Error for RclrsError { RclrsError::RclError { msg, .. } => msg.as_ref().map(|e| e as &dyn Error), RclrsError::UnknownRclError { msg, .. } => msg.as_ref().map(|e| e as &dyn Error), RclrsError::StringContainsNul { err, .. } => Some(err).map(|e| e as &dyn Error), + // TODO(@mxgrey): We should provide source information for these other types. + // It should be easy to do this using the thiserror crate. RclrsError::AlreadyAddedToWaitSet => None, + RclrsError::UnownedGuardCondition => None, } } } @@ -352,3 +364,30 @@ impl ToResult for rcl_ret_t { to_rclrs_result(*self) } } + +/// A helper trait to disregard timeouts as not an error. +pub trait RclrsErrorFilter { + /// If the result was a timeout error, change it to `Ok(())`. + fn timeout_ok(self) -> Result<(), RclrsError>; +} + +impl RclrsErrorFilter for Result<(), RclrsError> { + fn timeout_ok(self) -> Result<(), RclrsError> { + match self { + Ok(()) => Ok(()), + Err(err) => { + if matches!( + err, + RclrsError::RclError { + code: RclReturnCode::Timeout, + .. + } + ) { + return Ok(()); + } + + Err(err) + } + } + } +} diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 37c43a68e..af2cfe632 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,84 +1,318 @@ -use crate::{rcl_bindings::rcl_context_is_valid, Node, RclReturnCode, RclrsError, WaitSet}; +mod basic_executor; +pub use self::basic_executor::*; + +use crate::{Context, ContextHandle, GuardCondition, IntoNodeOptions, Node, RclrsError, Waitable}; +pub use futures::channel::oneshot::Receiver as Promise; +use futures::{ + channel::oneshot, + future::{select, BoxFuture, Either}, +}; use std::{ - sync::{Arc, Mutex, Weak}, + future::Future, + sync::{ + atomic::{AtomicBool, Ordering}, + Arc, + }, time::Duration, }; -/// Single-threaded executor implementation. -pub struct SingleThreadedExecutor { - nodes_mtx: Mutex>>, +/// An executor that can be used to create nodes and run their callbacks. +pub struct Executor { + context: Arc, + commands: Arc, + runtime: Box, } -impl Default for SingleThreadedExecutor { - fn default() -> Self { - Self::new() +impl Executor { + /// Access the commands interface for this executor. Use the returned + /// [`ExecutorCommands`] to create [nodes][Node]. + pub fn commands(&self) -> &Arc { + &self.commands } -} -impl SingleThreadedExecutor { - /// Creates a new executor. - pub fn new() -> Self { - SingleThreadedExecutor { - nodes_mtx: Mutex::new(Vec::new()), + /// Create a [`Node`] that will run on this Executor. + pub fn create_node<'a>( + &'a self, + options: impl IntoNodeOptions<'a>, + ) -> Result { + let options = options.into_node_options(); + let node = options.build(&self.commands)?; + Ok(node) + } + + /// Spin the Executor. The current thread will be blocked until the Executor + /// stops spinning. + /// + /// [`SpinOptions`] can be used to automatically stop the spinning when + /// certain conditions are met. Use `SpinOptions::default()` to allow the + /// Executor to keep spinning indefinitely. + pub fn spin(&mut self, options: SpinOptions) -> Result<(), RclrsError> { + let conditions = self.make_spin_conditions(options); + self.runtime.spin(conditions) + } + + /// Spin the Executor as an async task. This does not block the current thread. + /// It also does not prevent your `main` function from exiting while it spins, + /// so make sure you have a way to keep the application running. + /// + /// This will consume the Executor so that the task can run on other threads. + /// + /// The async task will run until the [`SpinConditions`] stop the Executor + /// from spinning. The output of the async task will be the restored Executor, + /// which you can use to resume spinning after the task is finished. + pub async fn spin_async(self, options: SpinOptions) -> (Self, Result<(), RclrsError>) { + let conditions = self.make_spin_conditions(options); + let Self { + context, + commands, + runtime, + } = self; + + let (runtime, result) = runtime.spin_async(conditions).await; + ( + Self { + context, + commands, + runtime, + }, + result, + ) + } + + /// Creates a new executor using the provided runtime. Users of rclrs should + /// use [`Context::create_executor`]. + pub(crate) fn new(context: Arc, runtime: E) -> Self + where + E: 'static + ExecutorRuntime + Send, + { + let (wakeup_wait_set, waitable) = GuardCondition::new(&context, None); + let commands = Arc::new(ExecutorCommands { + context: Context { + handle: Arc::clone(&context), + }, + channel: runtime.channel(), + halt_spinning: Arc::new(AtomicBool::new(false)), + wakeup_wait_set: Arc::new(wakeup_wait_set), + }); + + commands.add_to_wait_set(waitable); + + Self { + context, + commands, + runtime: Box::new(runtime), } } - /// Add a node to the executor. - pub fn add_node(&self, node: &Arc) -> Result<(), RclrsError> { - { self.nodes_mtx.lock().unwrap() }.push(Arc::downgrade(node)); - Ok(()) + fn make_spin_conditions(&self, options: SpinOptions) -> SpinConditions { + self.commands.halt_spinning.store(false, Ordering::Release); + SpinConditions { + options, + halt_spinning: Arc::clone(&self.commands.halt_spinning), + context: Context { + handle: Arc::clone(&self.context), + }, + guard_condition: Arc::clone(&self.commands.wakeup_wait_set), + } } +} - /// Remove a node from the executor. - pub fn remove_node(&self, node: Arc) -> Result<(), RclrsError> { - { self.nodes_mtx.lock().unwrap() } - .retain(|n| !n.upgrade().map(|n| Arc::ptr_eq(&n, &node)).unwrap_or(false)); - Ok(()) +/// This allows commands, such as creating a new node, to be run on the executor +/// while the executor is spinning. +pub struct ExecutorCommands { + context: Context, + channel: Box, + halt_spinning: Arc, + wakeup_wait_set: Arc, +} + +impl ExecutorCommands { + /// Create a new node that will run on the [`Executor`] that is being commanded. + pub fn create_node<'a>( + self: &Arc, + options: impl IntoNodeOptions<'a>, + ) -> Result { + let options = options.into_node_options(); + options.build(self) } - /// Polls the nodes for new messages and executes the corresponding callbacks. + /// Tell the [`Executor`] to halt its spinning. + pub fn halt_spinning(&self) { + self.halt_spinning.store(true, Ordering::Release); + // TODO(@mxgrey): Log errors here when logging becomes available + self.wakeup_wait_set.trigger().ok(); + } + + /// Run a task on the [`Executor`]. If the returned [`Promise`] is dropped + /// then the task will be dropped, which means it might not run to + /// completion. /// - /// This function additionally checks that the context is still valid. - pub fn spin_once(&self, timeout: Option) -> Result<(), RclrsError> { - for node in { self.nodes_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .filter(|node| unsafe { - rcl_context_is_valid(&*node.handle.context_handle.rcl_context.lock().unwrap()) - }) - { - let wait_set = WaitSet::new_for_node(&node)?; - let ready_entities = wait_set.wait(timeout)?; - - for ready_subscription in ready_entities.subscriptions { - ready_subscription.execute()?; - } - - for ready_client in ready_entities.clients { - ready_client.execute()?; - } - - for ready_service in ready_entities.services { - ready_service.execute()?; - } - } + /// This differs from [`run`][Self::run] because [`run`][Self::run] will + /// always run to completion, even if you discard the [`Promise`] that gets + /// returned. If dropping the [`Promise`] means that you don't need the task + /// to finish, then this `query` method is what you want. + /// + /// You have two ways to obtain the output of the promise: + /// - `.await` the output of the promise in an async scope + /// - use [`Promise::try_recv`] to get the output if it is available + pub fn query(&self, f: F) -> Promise + where + F: 'static + Future + Send, + F::Output: Send, + { + let (mut sender, receiver) = oneshot::channel(); + self.channel.add_async_task(Box::pin(async move { + let cancellation = sender.cancellation(); + let output = match select(cancellation, std::pin::pin!(f)).await { + // The task was cancelled + Either::Left(_) => return, + // The task completed + Either::Right((output, _)) => output, + }; + sender.send(output).ok(); + })); + + receiver + } + + /// Run a task on the [`Executor`]. The task will run to completion even if + /// you drop the returned [`Promise`]. + /// + /// This differs from [`query`][Self::query] because [`query`][Self::query] + /// will automatically stop running the task if you drop the [`Promise`]. + /// If you want to ensure that the task always runs to completion, then this + /// `run` method is what you want. + /// + /// You can safely discard the promise that is returned to you even if the + /// compiler gives you a warning about it. Use `let _ = promise;` to suppress + /// the warning. + /// + /// If you choose to keep the promise, you have two ways to obtain its output: + /// - `.await` the output of the promise in an async scope + /// - use [`Promise::try_recv`] to get the output if it is available + pub fn run(&self, f: F) -> Promise + where + F: 'static + Future + Send, + F::Output: Send, + { + let (sender, receiver) = oneshot::channel(); + self.channel.add_async_task(Box::pin(async move { + sender.send(f.await).ok(); + })); + receiver + } + + /// Get the context that the executor is associated with. + pub fn context(&self) -> &Context { + &self.context + } + + pub(crate) fn add_to_wait_set(&self, waitable: Waitable) { + self.channel.add_to_waitset(waitable); + } - Ok(()) + /// Get a guard condition that can be used to wake up the wait set of the executor. + pub(crate) fn get_guard_condition(&self) -> &Arc { + &self.wakeup_wait_set } +} + +/// This trait defines the interface for passing new items into an executor to +/// run. +pub trait ExecutorChannel: Send + Sync { + /// Add a new item for the executor to run. + fn add_async_task(&self, f: BoxFuture<'static, ()>); + + /// Add new entities to the waitset of the executor. + fn add_to_waitset(&self, new_entity: Waitable); +} + +/// This trait defines the interface for having an executor run. +pub trait ExecutorRuntime: Send { + /// Get a channel that can add new items for the executor to run. + fn channel(&self) -> Box; + + /// Tell the runtime to spin while blocking any further execution until the + /// spinning is complete. + fn spin(&mut self, conditions: SpinConditions) -> Result<(), RclrsError>; - /// Convenience function for calling [`SingleThreadedExecutor::spin_once`] in a loop. - pub fn spin(&self) -> Result<(), RclrsError> { - while !{ self.nodes_mtx.lock().unwrap() }.is_empty() { - match self.spin_once(None) { - Ok(_) - | Err(RclrsError::RclError { - code: RclReturnCode::Timeout, - .. - }) => std::thread::yield_now(), - error => return error, - } + /// Tell the runtime to spin asynchronously, not blocking the current + /// thread. The runtime instance will be consumed by this function, but it + /// must return itself as the output of the [`Future`] that this function + /// returns. + fn spin_async( + self: Box, + conditions: SpinConditions, + ) -> BoxFuture<'static, (Box, Result<(), RclrsError>)>; +} + +/// A bundle of optional conditions that a user may want to impose on how long +/// an executor spins for. +/// +/// By default the executor will be allowed to spin indefinitely. +#[non_exhaustive] +#[derive(Default)] +pub struct SpinOptions { + /// Only perform the next available work. This is similar to spin_once in + /// rclcpp and rclpy. + /// + /// To only process work that is immediately available without waiting at all, + /// set a timeout of zero. + pub only_next_available_work: bool, + /// The executor will stop spinning if the promise is resolved. The promise + /// does not need to be fulfilled (i.e. a value was sent), it could also be + /// cancelled (i.e. the Sender was dropped) and spinning will nevertheless + /// stop. + pub until_promise_resolved: Option>, + /// Stop waiting after this duration of time has passed. Use `Some(0)` to not + /// wait any amount of time. Use `None` to wait an infinite amount of time. + pub timeout: Option, +} + +impl SpinOptions { + /// Use default spin options. + pub fn new() -> Self { + Self::default() + } + + /// Behave like spin_once in rclcpp and rclpy. + pub fn spin_once() -> Self { + Self { + only_next_available_work: true, + ..Default::default() } + } + + /// Stop spinning once this promise is resolved. + pub fn until_promise_resolved(mut self, promise: Promise<()>) -> Self { + self.until_promise_resolved = Some(promise); + self + } - Ok(()) + /// Stop spinning once this durtion of time is reached. + pub fn timeout(mut self, timeout: Duration) -> Self { + self.timeout = Some(timeout); + self } } + +/// A bundle of conditions that tell the [`ExecutorRuntime`] how long to keep +/// spinning. This combines conditions that users specify with [`SpinOptions`] +/// and standard conditions that are set by the [`Executor`]. +/// +/// This struct is only for users who are implementing custom executors. Users +/// who are writing applications should use [`SpinOptions`]. +#[non_exhaustive] +pub struct SpinConditions { + /// User-specified optional conditions for spinning. + pub options: SpinOptions, + /// Halt trigger that gets set by [`ExecutorCommands`]. + pub halt_spinning: Arc, + /// Use this to check [`Context::ok`] to make sure that the context is still + /// valid. When the context is invalid, the executor runtime should stop + /// spinning. + pub context: Context, + /// This is a guard condition which is present in the wait set. The executor + /// can use this to wake up the wait set. + pub guard_condition: Arc, +} diff --git a/rclrs/src/executor/basic_executor.rs b/rclrs/src/executor/basic_executor.rs new file mode 100644 index 000000000..1ee2c0ad7 --- /dev/null +++ b/rclrs/src/executor/basic_executor.rs @@ -0,0 +1,245 @@ +use futures::{ + channel::{mpsc::UnboundedSender, oneshot}, + future::BoxFuture, + task::{waker_ref, ArcWake}, +}; +use std::{ + sync::{ + atomic::{AtomicBool, Ordering}, + mpsc::{channel, Receiver, Sender}, + Arc, Mutex, + }, + task::Context as TaskContext, +}; + +use crate::{ + executor::{ExecutorChannel, ExecutorRuntime, SpinConditions}, + Context, RclrsError, WaitSetRunner, Waitable, +}; + +/// The implementation of this runtime is based off of the async Rust reference book: +/// https://rust-lang.github.io/async-book/02_execution/04_executor.html +/// +/// This implements a single-threaded async executor. This means the execution of +/// all async tasks will be interlaced on a single thread. This is good for +/// minimizing context switching overhead and preventing the application from +/// consuming more CPU threads than it really needs. +/// +/// If you need high-throughput multi-threaded execution, then consider using +/// a different executor. +// +// TODO(@mxgrey): Implement a multi-threaded executor using tokio in a downstream +// crate and refer to it in this documentation. +pub struct BasicExecutorRuntime { + ready_queue: Receiver>, + task_sender: TaskSender, + /// We use an Option here because we need to hand the WaitSetRunner off to + /// another thread while spinning. It should only be None while the spin + /// function is active. At any other time this should contain Some. + wait_set_runner: Option, +} + +impl ExecutorRuntime for BasicExecutorRuntime { + fn spin(&mut self, mut conditions: SpinConditions) -> Result<(), RclrsError> { + self.process_spin_conditions(&mut conditions); + + let wait_set_runner = self.wait_set_runner.take().expect( + "The wait set runner of the basic executor is missing while beginning to spin. \ + This is a critical bug in rclrs. \ + Please report this bug to the maintainers of rclrs by providing a minimum reproduction of the problem." + ); + + let wait_set_promise = wait_set_runner.run(conditions); + // futures::channel::oneshot::Receiver is only suitable for async, but + // we need to block this function from exiting until the WaitSetRunner + // is returned to self. Therefore we create this blocking channel to + // prevent the function from returning until the WaitSetRunner has been + // re-obtained. + let (wait_set_sender, wait_set_receiver) = channel(); + + // Use this atomic bool to recognize when we should stop spinning. + let wait_set_finished = Arc::new(AtomicBool::new(false)); + + // Use this to terminate the spinning once the wait set is finished. + let wait_set_finished_clone = Arc::clone(&wait_set_finished); + self.task_sender.add_async_task(Box::pin(async move { + let wait_set_runner = wait_set_promise.await.expect( + "The wait set thread of the basic executor dropped prematurely. \ + This is a critical bug in rclrs. \ + Please report this bug to the maintainers of rclrs by providing a minimum reproduction of the problem." + ); + // TODO(@mxgrey): Log errors here when logging becomes available. + wait_set_sender.send(wait_set_runner).ok(); + + // Notify the main loop that it should stop + wait_set_finished_clone.store(true, Ordering::Release); + })); + + while let Ok(task) = self.next_task(&wait_set_finished) { + // SAFETY: If the mutex is poisoned then we have unrecoverable situation. + let mut future_slot = task.future.lock().unwrap(); + if let Some(mut future) = future_slot.take() { + let waker = waker_ref(&task); + let task_context = &mut TaskContext::from_waker(&waker); + // Poll the future inside the task so it can do some work and + // tell us its state. + if future.as_mut().poll(task_context).is_pending() { + // The task is still pending, so return the future to its + // task so it can be processed again when it's ready to + // continue. + *future_slot = Some(future); + } + } + } + + let (runner, result) = wait_set_receiver.recv().expect( + "Basic executor failed to receive the WaitSetRunner at the end of its spinning. \ + This is a critical bug in rclrs. \ + Please report this bug to the maintainers of rclrs by providing a minimum reproduction of the problem." + ); + + self.wait_set_runner = Some(runner); + result + } + + fn spin_async( + mut self: Box, + conditions: SpinConditions, + ) -> BoxFuture<'static, (Box, Result<(), RclrsError>)> { + let (sender, receiver) = oneshot::channel(); + // Create a thread to run the executor. We should not run the executor + // as an async task because it blocks its current thread while running. + // If its future were passed into a different single-threaded async + // executor then it would block anything else from running on that + // executor. + // + // Theoretically we could design this executor to use async-compatible + // channels. Then it could run safely inside of a different async + // executor. But that would probably require us to introduce a new + // dependency such as tokio. + std::thread::spawn(move || { + let result = self.spin(conditions); + sender.send((self as Box, result)).ok(); + }); + + Box::pin(async move { + receiver.await.expect( + "The basic executor async spin thread was dropped without finishing. \ + This is a critical bug in rclrs. \ + Please report this bug to the maintainers of rclrs by providing a minimum reproduction of the problem." + ) + }) + } + + fn channel(&self) -> Box { + let waitable_sender = self.wait_set_runner.as_ref().expect( + "The wait set runner of the basic executor is missing while creating a channel. \ + This is a critical bug in rclrs. \ + Please report this bug to the maintainers of rclrs by providing a minimum reproduction of the problem." + ) + .sender(); + + Box::new(BasicExecutorChannel { + task_sender: self.task_sender.clone(), + waitable_sender, + }) + } +} + +impl BasicExecutorRuntime { + pub(crate) fn new(context: &Context) -> Self { + let (task_sender, ready_queue) = channel(); + Self { + ready_queue, + task_sender: TaskSender { task_sender }, + wait_set_runner: Some(WaitSetRunner::new(context)), + } + } + + fn process_spin_conditions(&self, conditions: &mut SpinConditions) { + if let Some(promise) = conditions.options.until_promise_resolved.take() { + let guard_condition = Arc::clone(&conditions.guard_condition); + let (sender, receiver) = oneshot::channel(); + self.task_sender.add_async_task(Box::pin(async move { + if let Err(err) = promise.await { + // TODO(@mxgrey): We should change this to a log when logging + // becomes available. + eprintln!( + "Sender for SpinOptions::until_promise_resolved was \ + dropped, so the Promise will never be fulfilled. \ + Spinning will stop now. Error message: {err}" + ); + } + // TODO(@mxgrey): Log errors here when logging becomes available. + guard_condition.trigger().ok(); + sender.send(()).ok(); + })); + + conditions.options.until_promise_resolved = Some(receiver); + } + } + + fn next_task(&mut self, wait_set_finished: &AtomicBool) -> Result, ()> { + if wait_set_finished.load(Ordering::Acquire) { + // The wait set is done spinning, so we should only pull tasks if + // they are immediately ready to be performed. + self.ready_queue.try_recv().map_err(|_| ()) + } else { + self.ready_queue.recv().map_err(|_| ()) + } + } +} + +struct BasicExecutorChannel { + task_sender: TaskSender, + waitable_sender: UnboundedSender, +} + +impl ExecutorChannel for BasicExecutorChannel { + fn add_async_task(&self, f: BoxFuture<'static, ()>) { + self.task_sender.add_async_task(f); + } + + fn add_to_waitset(&self, new_entity: Waitable) { + // TODO(@mxgrey): Log errors here once logging becomes available. + self.waitable_sender.unbounded_send(new_entity).ok(); + } +} + +#[derive(Clone)] +struct TaskSender { + task_sender: Sender>, +} + +impl TaskSender { + fn add_async_task(&self, f: BoxFuture<'static, ()>) { + let task = Arc::new(Task { + future: Mutex::new(Some(f)), + task_sender: self.task_sender.clone(), + }); + + // TODO(@mxgrey): Consider logging errors here once logging is available. + self.task_sender.send(task).ok(); + } +} + +struct Task { + /// This future is held inside an Option because we need to move it in and + /// out of this `Task` instance without destructuring the `Task` because the + /// [`ArcWake`] wakeup behavior relies on `Task` having a single instance + /// that is managed by an Arc. + /// + /// We wrap the Option in Mutex because we need to mutate the Option from a + /// shared borrow that comes from the Arc. + future: Mutex>>, + task_sender: Sender>, +} + +/// Implementing this trait gives us a very easy implementation of waking +/// behavior for Task on our BasicExecutorRuntime. +impl ArcWake for Task { + fn wake_by_ref(arc_self: &Arc) { + let cloned = Arc::clone(arc_self); + arc_self.task_sender.send(cloned).ok(); + } +} diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 3a22c6da8..fddf3ac50 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -21,7 +21,7 @@ mod subscription; mod time; mod time_source; mod vendor; -mod wait; +mod wait_set; #[cfg(test)] mod test_helpers; @@ -31,8 +31,6 @@ mod rcl_bindings; #[cfg(feature = "dyn_msg")] pub mod dynamic_message; -use std::{sync::Arc, time::Duration}; - pub use arguments::*; pub use client::*; pub use clock::*; @@ -49,67 +47,4 @@ pub use service::*; pub use subscription::*; pub use time::*; use time_source::*; -pub use wait::*; - -/// Polls the node for new messages and executes the corresponding callbacks. -/// -/// See [`WaitSet::wait`] for the meaning of the `timeout` parameter. -/// -/// This may under some circumstances return -/// [`SubscriptionTakeFailed`][1], [`ClientTakeFailed`][1], [`ServiceTakeFailed`][1] when the wait -/// set spuriously wakes up. -/// This can usually be ignored. -/// -/// [1]: crate::RclReturnCode -pub fn spin_once(node: Arc, timeout: Option) -> Result<(), RclrsError> { - let executor = SingleThreadedExecutor::new(); - executor.add_node(&node)?; - executor.spin_once(timeout) -} - -/// Convenience function for calling [`spin_once`] in a loop. -pub fn spin(node: Arc) -> Result<(), RclrsError> { - let executor = SingleThreadedExecutor::new(); - executor.add_node(&node)?; - executor.spin() -} - -/// Creates a new node in the empty namespace. -/// -/// Convenience function equivalent to [`Node::new`][1]. -/// Please see that function's documentation. -/// -/// [1]: crate::Node::new -/// -/// # Example -/// ``` -/// # use rclrs::{Context, RclrsError}; -/// let ctx = Context::new([])?; -/// let node = rclrs::create_node(&ctx, "my_node"); -/// assert!(node.is_ok()); -/// # Ok::<(), RclrsError>(()) -/// ``` -pub fn create_node(context: &Context, node_name: &str) -> Result, RclrsError> { - Node::new(context, node_name) -} - -/// Creates a [`NodeBuilder`]. -/// -/// Convenience function equivalent to [`NodeBuilder::new()`][1] and [`Node::builder()`][2]. -/// Please see that function's documentation. -/// -/// [1]: crate::NodeBuilder::new -/// [2]: crate::Node::builder -/// -/// # Example -/// ``` -/// # use rclrs::{Context, RclrsError}; -/// let context = Context::new([])?; -/// let node_builder = rclrs::create_node_builder(&context, "my_node"); -/// let node = node_builder.build()?; -/// assert_eq!(node.name(), "my_node"); -/// # Ok::<(), RclrsError>(()) -/// ``` -pub fn create_node_builder(context: &Context, node_name: &str) -> NodeBuilder { - Node::builder(context, node_name) -} +pub use wait_set::*; diff --git a/rclrs/src/logging.rs b/rclrs/src/logging.rs index 5143ae35c..5d2b2ac19 100644 --- a/rclrs/src/logging.rs +++ b/rclrs/src/logging.rs @@ -30,8 +30,8 @@ pub use logger::*; /// use std::time::Duration; /// use std::env; /// -/// let context = rclrs::Context::new(env::args()).unwrap(); -/// let node = rclrs::Node::new(&context, "test_node").unwrap(); +/// let executor = rclrs::Context::default().create_basic_executor(); +/// let node = executor.create_node("test_node").unwrap(); /// /// log!(node.debug(), "Simple debug message"); /// let some_variable = 43; @@ -473,7 +473,10 @@ macro_rules! function { #[cfg(test)] mod tests { use crate::{log_handler::*, test_helpers::*, *}; - use std::sync::Mutex; + use std::{ + sync::{Arc, Mutex}, + time::Duration, + }; #[test] fn test_logging_macros() -> Result<(), RclrsError> { diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index b51b59817..8be556458 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,36 +1,54 @@ -mod builder; +mod node_options; +pub use node_options::*; + +mod primitive_options; +pub use primitive_options::*; + mod graph; +pub use graph::*; + +mod node_graph_task; +use node_graph_task::*; + use std::{ cmp::PartialEq, ffi::CStr, fmt, os::raw::c_char, - sync::{atomic::AtomicBool, Arc, Mutex, Weak}, - vec::Vec, + sync::{atomic::AtomicBool, Arc, Mutex}, + time::Duration, }; +use futures::{ + channel::mpsc::{unbounded, UnboundedSender}, + StreamExt, +}; + +use async_std::future::timeout; + use rosidl_runtime_rs::Message; -pub use self::{builder::*, graph::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, LogParams, - Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, - QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, - SubscriptionCallback, TimeSource, ToLogParams, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands, + LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Promise, + Publisher, PublisherOptions, PublisherState, RclrsError, Service, ServiceAsyncCallback, + ServiceCallback, ServiceOptions, ServiceState, Subscription, SubscriptionAsyncCallback, + SubscriptionCallback, SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, + ENTITY_LIFECYCLE_MUTEX, }; -// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread -// they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for rcl_node_t {} - /// A processing unit that can communicate with other nodes. /// /// Nodes are a core concept in ROS 2. Refer to the official ["Understanding ROS 2 nodes"][1] /// tutorial for an introduction. /// -/// Ownership of the node is shared with all [`Publisher`]s and [`Subscription`]s created from it. -/// That means that even after the node itself is dropped, it will continue to exist and be -/// displayed by e.g. `ros2 topic` as long as its publishers and subscriptions are not dropped. +/// Ownership of the node is shared with all the primitives such as [`Publisher`]s and [`Subscription`]s +/// that are created from it. That means that even after the `Node` itself is dropped, it will continue +/// to exist and be displayed by e.g. `ros2 topic` as long as any one of its primitives is not dropped. +/// +/// # Creating +/// Use [`Executor::create_node`][7] to create a new node. Pass in [`NodeOptions`] to set all the different +/// options for node creation, or just pass in a string for the node's name if the default options are okay. /// /// # Naming /// A node has a *name* and a *namespace*. @@ -48,25 +66,38 @@ unsafe impl Send for rcl_node_t {} /// In that sense, the parameters to the node creation functions are only the _default_ namespace and /// name. /// See also the [official tutorial][1] on the command line arguments for ROS nodes, and the -/// [`Node::namespace()`] and [`Node::name()`] functions for examples. +/// [`Node::namespace()`][3] and [`Node::name()`][4] functions for examples. /// /// ## Rules for valid names /// The rules for valid node names and node namespaces are explained in -/// [`NodeBuilder::new()`][3] and [`NodeBuilder::namespace()`][4]. +/// [`NodeOptions::new()`][5] and [`NodeOptions::namespace()`][6]. /// /// [1]: https://docs.ros.org/en/rolling/Tutorials/Understanding-ROS2-Nodes.html /// [2]: https://docs.ros.org/en/rolling/How-To-Guides/Node-arguments.html -/// [3]: crate::NodeBuilder::new -/// [4]: crate::NodeBuilder::namespace -pub struct Node { - pub(crate) clients_mtx: Mutex>>, - pub(crate) guard_conditions_mtx: Mutex>>, - pub(crate) services_mtx: Mutex>>, - pub(crate) subscriptions_mtx: Mutex>>, +/// [3]: Node::namespace +/// [4]: Node::name +/// [5]: crate::NodeOptions::new +/// [6]: crate::NodeOptions::namespace +/// [7]: crate::Executor::create_node + +pub type Node = Arc; + +/// The inner state of a [`Node`]. +/// +/// This is public so that you can choose to put it inside a [`Weak`] if you +/// want to be able to refer to a [`Node`] in a non-owning way. It is generally +/// recommended to manage the [`NodeState`] inside of an [`Arc`], and [`Node`] +/// recommended to manage the `NodeState` inside of an [`Arc`], and [`Node`] +/// is provided as convenience alias for that. +/// +/// The public API of the [`Node`] type is implemented via `NodeState`. +pub struct NodeState { time_source: TimeSource, parameter: ParameterInterface, - pub(crate) handle: Arc, logger: Logger, + commands: Arc, + graph_change_action: UnboundedSender, + handle: Arc, } /// This struct manages the lifetime of an `rcl_node_t`, and accounts for its @@ -114,15 +145,15 @@ impl Drop for NodeHandle { } } -impl Eq for Node {} +impl Eq for NodeState {} -impl PartialEq for Node { +impl PartialEq for NodeState { fn eq(&self, other: &Self) -> bool { Arc::ptr_eq(&self.handle, &other.handle) } } -impl fmt::Debug for Node { +impl fmt::Debug for NodeState { fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { f.debug_struct("Node") .field("fully_qualified_name", &self.fully_qualified_name()) @@ -130,15 +161,7 @@ impl fmt::Debug for Node { } } -impl Node { - /// Creates a new node in the empty namespace. - /// - /// See [`NodeBuilder::new()`] for documentation. - #[allow(clippy::new_ret_no_self)] - pub fn new(context: &Context, node_name: &str) -> Result, RclrsError> { - Self::builder(context, node_name).build() - } - +impl NodeState { /// Returns the clock associated with this node. pub fn get_clock(&self) -> Clock { self.time_source.get_clock() @@ -151,15 +174,15 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError}; + /// # use rclrs::{Context, InitOptions, RclrsError}; /// // Without remapping - /// let context = Context::new([])?; - /// let node = rclrs::create_node(&context, "my_node")?; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node("my_node")?; /// assert_eq!(node.name(), "my_node"); /// // With remapping /// let remapping = ["--ros-args", "-r", "__node:=your_node"].map(String::from); - /// let context_r = Context::new(remapping)?; - /// let node_r = rclrs::create_node(&context_r, "my_node")?; + /// let executor_r = Context::new(remapping, InitOptions::default())?.create_basic_executor(); + /// let node_r = executor_r.create_node("my_node")?; /// assert_eq!(node_r.name(), "your_node"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -174,18 +197,18 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError}; + /// # use rclrs::{Context, InitOptions, RclrsError, IntoNodeOptions}; /// // Without remapping - /// let context = Context::new([])?; - /// let node = - /// rclrs::create_node_builder(&context, "my_node") - /// .namespace("/my/namespace") - /// .build()?; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node( + /// "my_node" + /// .namespace("/my/namespace") + /// )?; /// assert_eq!(node.namespace(), "/my/namespace"); /// // With remapping /// let remapping = ["--ros-args", "-r", "__ns:=/your_namespace"].map(String::from); - /// let context_r = Context::new(remapping)?; - /// let node_r = rclrs::create_node(&context_r, "my_node")?; + /// let executor_r = Context::new(remapping, InitOptions::default())?.create_basic_executor(); + /// let node_r = executor_r.create_node("my_node")?; /// assert_eq!(node_r.namespace(), "/your_namespace"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -196,16 +219,16 @@ impl Node { /// Returns the fully qualified name of the node. /// /// The fully qualified name of the node is the node namespace combined with the node name. - /// It is subject to the remappings shown in [`Node::name()`] and [`Node::namespace()`]. + /// It is subject to the remappings shown in [`NodeState::name()`] and [`NodeState::namespace()`]. /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError}; - /// let context = Context::new([])?; - /// let node = - /// rclrs::create_node_builder(&context, "my_node") - /// .namespace("/my/namespace") - /// .build()?; + /// # use rclrs::{Context, RclrsError, IntoNodeOptions}; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node( + /// "my_node" + /// .namespace("/my/namespace") + /// )?; /// assert_eq!(node.fully_qualified_name(), "/my/namespace/my_node"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -213,183 +236,332 @@ impl Node { self.call_string_getter(rcl_node_get_fully_qualified_name) } - // Helper for name(), namespace(), fully_qualified_name() - fn call_string_getter( - &self, - getter: unsafe extern "C" fn(*const rcl_node_t) -> *const c_char, - ) -> String { - let rcl_node = self.handle.rcl_node.lock().unwrap(); - unsafe { call_string_getter_with_rcl_node(&rcl_node, getter) } - } - /// Creates a [`Client`][1]. /// - /// [1]: crate::Client - // TODO: make client's lifetime depend on node's lifetime - pub fn create_client(&self, topic: &str) -> Result>, RclrsError> + /// Pass in only the service name for the `options` argument to use all default client options: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let client = node.create_client::( + /// "my_service" + /// ) + /// .unwrap(); + /// ``` + /// + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the + /// client options: + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let client = node.create_client::( + /// "my_service" + /// .keep_all() + /// .transient_local() + /// ) + /// .unwrap(); + /// ``` + /// + /// Any quality of service options that you explicitly specify will override + /// the default service options. Any that you do not explicitly specify will + /// remain the default service options. Note that clients are generally + /// expected to use [reliable][1], so it's best not to change the reliability + /// setting unless you know what you are doing. + /// + /// [1]: crate::QoSReliabilityPolicy::Reliable + pub fn create_client<'a, T>( + self: &Arc, + options: impl Into>, + ) -> Result, RclrsError> where T: rosidl_runtime_rs::Service, { - let client = Arc::new(Client::::new(Arc::clone(&self.handle), topic)?); - { self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); - Ok(client) - } - - /// Creates a [`GuardCondition`][1] with no callback. - /// - /// A weak pointer to the `GuardCondition` is stored within this node. - /// When this node is added to a wait set (e.g. when calling `spin_once`[2] - /// with this node as an argument), the guard condition can be used to - /// interrupt the wait. - /// - /// [1]: crate::GuardCondition - /// [2]: crate::spin_once - pub fn create_guard_condition(&self) -> Arc { - let guard_condition = Arc::new(GuardCondition::new_with_context_handle( - Arc::clone(&self.handle.context_handle), - None, - )); - { self.guard_conditions_mtx.lock().unwrap() } - .push(Arc::downgrade(&guard_condition) as Weak); - guard_condition + ClientState::::create(self, options) } - /// Creates a [`GuardCondition`][1] with a callback. + /// Creates a [`Publisher`][1]. + /// + /// Pass in only the topic name for the `options` argument to use all default publisher options: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let publisher = node.create_publisher::( + /// "my_topic" + /// ) + /// .unwrap(); + /// ``` + /// + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the + /// publisher options: + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let publisher = node.create_publisher::( + /// "my_topic" + /// .keep_last(100) + /// .transient_local() + /// ) + /// .unwrap(); /// - /// A weak pointer to the `GuardCondition` is stored within this node. - /// When this node is added to a wait set (e.g. when calling `spin_once`[2] - /// with this node as an argument), the guard condition can be used to - /// interrupt the wait. + /// let reliable_publisher = node.create_publisher::( + /// "my_topic" + /// .reliable() + /// ) + /// .unwrap(); + /// ``` /// - /// [1]: crate::GuardCondition - /// [2]: crate::spin_once - pub fn create_guard_condition_with_callback(&mut self, callback: F) -> Arc + pub fn create_publisher<'a, T>( + &self, + options: impl Into>, + ) -> Result, RclrsError> where - F: Fn() + Send + Sync + 'static, + T: Message, { - let guard_condition = Arc::new(GuardCondition::new_with_context_handle( - Arc::clone(&self.handle.context_handle), - Some(Box::new(callback) as Box), - )); - { self.guard_conditions_mtx.lock().unwrap() } - .push(Arc::downgrade(&guard_condition) as Weak); - guard_condition + PublisherState::::create(Arc::clone(&self.handle), options) } - /// Creates a [`Publisher`][1]. + /// Creates a [`Service`] with an ordinary callback. + /// + /// # Behavior + /// + /// Even though this takes in a blocking (non-async) function, the callback + /// may run in parallel with other callbacks. This callback may even run + /// multiple times simultaneously with different incoming requests. + /// + /// Any internal state that needs to be mutated will need to be wrapped in + /// [`Mutex`] to ensure it is synchronized across multiple simultaneous runs + /// of the callback. To share internal state outside of the callback you will + /// need to wrap it in [`Arc`] or `Arc>`. + /// + /// # Usage + /// + /// Pass in only the service name for the `options` argument to use all default service options: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let service = node.create_service::( + /// "my_service", + /// |_request: test_msgs::srv::Empty_Request| { + /// println!("Received request!"); + /// test_msgs::srv::Empty_Response::default() + /// }, + /// ); + /// ``` + /// + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the + /// service options: + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let service = node.create_service::( + /// "my_service" + /// .keep_all() + /// .transient_local(), + /// |_request: test_msgs::srv::Empty_Request| { + /// println!("Received request!"); + /// test_msgs::srv::Empty_Response::default() + /// }, + /// ); + /// ``` + /// + /// Any quality of service options that you explicitly specify will override + /// the default service options. Any that you do not explicitly specify will + /// remain the default service options. Note that services are generally + /// expected to use [reliable][2], so it's best not to change the reliability + /// setting unless you know what you are doing. /// - /// [1]: crate::Publisher - // TODO: make publisher's lifetime depend on node's lifetime - pub fn create_publisher( + /// [1]: crate::Service + /// [2]: crate::QoSReliabilityPolicy::Reliable + // + // TODO(@mxgrey): Add examples showing each supported signature + pub fn create_service<'a, T, Args>( &self, - topic: &str, - qos: QoSProfile, - ) -> Result>, RclrsError> + options: impl Into>, + callback: impl ServiceCallback, + ) -> Result, RclrsError> where - T: Message, + T: rosidl_runtime_rs::Service, { - let publisher = Arc::new(Publisher::::new(Arc::clone(&self.handle), topic, qos)?); - Ok(publisher) + ServiceState::::create( + options, + callback.into_service_callback(), + &self.handle, + &self.commands, + ) } - /// Creates a [`Service`][1]. + /// Creates a [`Service`] with an async callback. /// - /// [1]: crate::Service - // TODO: make service's lifetime depend on node's lifetime - pub fn create_service( + /// # Behavior + /// + /// This callback may run in parallel with other callbacks. It may even run + /// multiple times simultaneously with different incoming requests. This + /// parallelism will depend on the executor that is being used. When the + /// callback uses `.await`, it will not block anything else from running. + /// + /// Any internal state that needs to be mutated will need to be wrapped in + /// [`Mutex`] to ensure it is synchronized across multiple runs of the + /// callback. To share internal state outside of the callback you will need + /// to wrap it in [`Arc`] (immutable) or `Arc>` (mutable). + /// + /// # Usage + /// + /// See [create_service][Node::create_service#Usage] for usage. + // + // TODO(@mxgrey): Add examples showing each supported signature + pub fn create_async_service<'a, T, Args>( &self, - topic: &str, - callback: F, - ) -> Result>, RclrsError> + options: impl Into>, + callback: impl ServiceAsyncCallback, + ) -> Result, RclrsError> where T: rosidl_runtime_rs::Service, - F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send, { - let service = Arc::new(Service::::new( - Arc::clone(&self.handle), - topic, - callback, - )?); - { self.services_mtx.lock().unwrap() } - .push(Arc::downgrade(&service) as Weak); - Ok(service) + ServiceState::::create( + options, + callback.into_service_async_callback(), + &self.handle, + &self.commands, + ) } - /// Creates a [`Subscription`][1]. + /// Creates a [`Subscription`] with an ordinary callback. + /// + /// # Behavior + /// + /// Even though this takes in a blocking (non-async) function, the callback + /// may run in parallel with other callbacks. This callback may even run + /// multiple times simultaneously with different incoming messages. This + /// parallelism will depend on the executor that is being used. + /// + /// Any internal state that needs to be mutated will need to be wrapped in + /// [`Mutex`] to ensure it is synchronized across multiple simultaneous runs + /// of the callback. To share internal state outside of the callback you will + /// need to wrap it in [`Arc`] or `Arc>`. + /// + /// # Usage + /// + /// Pass in only the topic name for the `options` argument to use all default subscription options: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let subscription = node.create_subscription( + /// "my_topic", + /// |_msg: test_msgs::msg::Empty| { + /// println!("Received message!"); + /// }, + /// ); + /// ``` + /// + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the + /// subscription options: /// - /// [1]: crate::Subscription - // TODO: make subscription's lifetime depend on node's lifetime - pub fn create_subscription( + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let subscription = node.create_subscription( + /// "my_topic" + /// .keep_last(100) + /// .transient_local(), + /// |_msg: test_msgs::msg::Empty| { + /// println!("Received message!"); + /// }, + /// ); + /// + /// let reliable_subscription = node.create_subscription( + /// "my_reliable_topic" + /// .reliable(), + /// |_msg: test_msgs::msg::Empty| { + /// println!("Received message!"); + /// }, + /// ); + /// ``` + /// + // + // TODO(@mxgrey): Add examples showing each supported callback signatures + pub fn create_subscription<'a, T, Args>( &self, - topic: &str, - qos: QoSProfile, + options: impl Into>, callback: impl SubscriptionCallback, - ) -> Result>, RclrsError> + ) -> Result, RclrsError> where T: Message, { - let subscription = Arc::new(Subscription::::new( - Arc::clone(&self.handle), - topic, - qos, - callback, - )?); - { self.subscriptions_mtx.lock() } - .unwrap() - .push(Arc::downgrade(&subscription) as Weak); - Ok(subscription) - } - - /// Returns the subscriptions that have not been dropped yet. - pub(crate) fn live_subscriptions(&self) -> Vec> { - { self.subscriptions_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .collect() - } - - pub(crate) fn live_clients(&self) -> Vec> { - { self.clients_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .collect() + SubscriptionState::::create( + options, + callback.into_subscription_callback(), + &self.handle, + &self.commands, + ) } - pub(crate) fn live_guard_conditions(&self) -> Vec> { - { self.guard_conditions_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .collect() - } - - pub(crate) fn live_services(&self) -> Vec> { - { self.services_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .collect() + /// Creates a [`Subscription`] with an async callback. + /// + /// # Behavior + /// + /// This callback may run in parallel with other callbacks. It may even run + /// multiple times simultaneously with different incoming messages. This + /// parallelism will depend on the executor that is being used. When the + /// callback uses `.await`, it will not block anything else from running. + /// + /// Any internal state that needs to be mutated will need to be wrapped in + /// [`Mutex`] to ensure it is synchronized across multiple runs of the + /// callback. To share internal state outside of the callback you will need + /// to wrap it in [`Arc`] or `Arc>`. + /// + /// # Usage + /// + /// See [create_subscription][Node::create_subscription#Usage] for usage. + // + // TODO(@mxgrey): Add examples showing each supported signature + pub fn create_async_subscription<'a, T, Args>( + &self, + options: impl Into>, + callback: impl SubscriptionAsyncCallback, + ) -> Result, RclrsError> + where + T: Message, + { + SubscriptionState::::create( + options, + callback.into_subscription_async_callback(), + &self.handle, + &self.commands, + ) } /// Returns the ROS domain ID that the node is using. /// /// The domain ID controls which nodes can send messages to each other, see the [ROS 2 concept article][1]. - /// It can be set through the `ROS_DOMAIN_ID` environment variable. + /// It can be set through the `ROS_DOMAIN_ID` environment variable or by + /// passing custom [`InitOptions`][2] into [`Context::new`][3] or [`Context::from_env`][4]. /// /// [1]: https://docs.ros.org/en/rolling/Concepts/About-Domain-ID.html + /// [2]: crate::InitOptions + /// [3]: crate::Context::new + /// [4]: crate::Context::from_env /// /// # Example /// ``` /// # use rclrs::{Context, RclrsError}; /// // Set default ROS domain ID to 10 here /// std::env::set_var("ROS_DOMAIN_ID", "10"); - /// let context = Context::new([])?; - /// let node = rclrs::create_node(&context, "domain_id_node")?; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node("domain_id_node")?; /// let domain_id = node.domain_id(); /// assert_eq!(domain_id, 10); /// # Ok::<(), RclrsError>(()) /// ``` - // TODO: If node option is supported, - // add description about this function is for getting actual domain_id - // and about override of domain_id via node option pub fn domain_id(&self) -> usize { let rcl_node = self.handle.rcl_node.lock().unwrap(); let mut domain_id: usize = 0; @@ -410,8 +582,8 @@ impl Node { /// # Example /// ``` /// # use rclrs::{Context, ParameterRange, RclrsError}; - /// let context = Context::new([])?; - /// let node = rclrs::create_node(&context, "domain_id_node")?; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node("domain_id_node")?; /// // Set it to a range of 0-100, with a step of 2 /// let range = ParameterRange { /// lower: Some(0), @@ -447,32 +619,90 @@ impl Node { } } - /// Creates a [`NodeBuilder`][1] with the given name. + /// Same as [`Self::notify_on_graph_change_with_period`] but uses a + /// recommended default period of 100ms. + pub fn notify_on_graph_change( + &self, + condition: impl FnMut() -> bool + Send + 'static, + ) -> Promise<()> { + self.notify_on_graph_change_with_period(Duration::from_millis(100), condition) + } + + /// This function allows you to track when a specific graph change happens. /// - /// Convenience function equivalent to [`NodeBuilder::new()`][2]. + /// Provide a function that will be called each time a graph change occurs. + /// You will be given a [`Promise`] that will be fulfilled when the condition + /// returns true. The condition will be checked under these conditions: + /// - once immediately as this function is run + /// - each time rcl notifies us that a graph change has happened + /// - each time the period elapses /// - /// [1]: crate::NodeBuilder - /// [2]: crate::NodeBuilder::new + /// We specify a period because it is possible that race conditions at the + /// rcl layer could trigger a notification of a graph change before your + /// API calls will be able to observe it. /// - /// # Example - /// ``` - /// # use rclrs::{Context, Node, RclrsError}; - /// let context = Context::new([])?; - /// let node = Node::builder(&context, "my_node").build()?; - /// assert_eq!(node.name(), "my_node"); - /// # Ok::<(), RclrsError>(()) - /// ``` - pub fn builder(context: &Context, node_name: &str) -> NodeBuilder { - NodeBuilder::new(context, node_name) + /// + pub fn notify_on_graph_change_with_period( + &self, + period: Duration, + mut condition: impl FnMut() -> bool + Send + 'static, + ) -> Promise<()> { + let (listener, mut on_graph_change_receiver) = unbounded(); + let promise = self.commands.query(async move { + loop { + match timeout(period, on_graph_change_receiver.next()).await { + Ok(Some(_)) | Err(_) => { + // Either we received a notification that there was a + // graph change, or the timeout elapsed. Either way, we + // want to check the condition and break out of the loop + // if the condition is true. + if condition() { + return; + } + } + Ok(None) => { + // We've been notified that the graph change sender is + // closed which means we will never receive another + // graph change update. This only happens when a node + // is being torn down, so go ahead and exit this loop. + return; + } + } + } + }); + + self.graph_change_action + .unbounded_send(NodeGraphAction::NewGraphListener(listener)) + .ok(); + + promise + } + + /// Get the [`ExecutorCommands`] used by this Node. + pub fn commands(&self) -> &Arc { + &self.commands } /// Get the logger associated with this Node. pub fn logger(&self) -> &Logger { &self.logger } + + // Helper for name(), namespace(), fully_qualified_name() + fn call_string_getter( + &self, + getter: unsafe extern "C" fn(*const rcl_node_t) -> *const c_char, + ) -> String { + let rcl_node = self.handle.rcl_node.lock().unwrap(); + unsafe { call_string_getter_with_rcl_node(&rcl_node, getter) } + } + + pub(crate) fn handle(&self) -> &Arc { + &self.handle + } } -impl<'a> ToLogParams<'a> for &'a Node { +impl<'a> ToLogParams<'a> for &'a NodeState { fn to_log_params(self) -> LogParams<'a> { self.logger().to_log_params() } @@ -495,10 +725,13 @@ pub(crate) unsafe fn call_string_getter_with_rcl_node( cstr.to_string_lossy().into_owned() } +// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread +// they are running in. Therefore, this type can be safely sent to another thread. +unsafe impl Send for rcl_node_t {} + #[cfg(test)] mod tests { - use super::*; - use crate::test_helpers::*; + use crate::{test_helpers::*, *}; #[test] fn traits() { @@ -508,25 +741,20 @@ mod tests { #[test] fn test_topic_names_and_types() -> Result<(), RclrsError> { - use crate::QOS_PROFILE_SYSTEM_DEFAULT; use test_msgs::msg; let graph = construct_test_graph("test_topics_graph")?; let _node_1_defaults_subscription = graph.node1.create_subscription::( "graph_test_topic_3", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::Defaults| {}, )?; - let _node_2_empty_subscription = graph.node2.create_subscription::( - "graph_test_topic_1", - QOS_PROFILE_SYSTEM_DEFAULT, - |_msg: msg::Empty| {}, - )?; + let _node_2_empty_subscription = graph + .node2 + .create_subscription::("graph_test_topic_1", |_msg: msg::Empty| {})?; let _node_2_basic_types_subscription = graph.node2.create_subscription::( "graph_test_topic_2", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::BasicTypes| {}, )?; diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 639a38e38..948d55621 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -3,7 +3,7 @@ use std::{ ffi::{CStr, CString}, }; -use crate::{rcl_bindings::*, Node, RclrsError, ToResult}; +use crate::{rcl_bindings::*, NodeState, RclrsError, ToResult}; impl Drop for rmw_names_and_types_t { fn drop(&mut self) { @@ -57,7 +57,7 @@ pub struct TopicEndpointInfo { pub topic_type: String, } -impl Node { +impl NodeState { /// Returns a list of topic names and types for publishers associated with a node. pub fn get_publisher_names_and_types_by_node( &self, @@ -482,11 +482,11 @@ mod tests { .map(|value: usize| if value != 99 { 99 } else { 98 }) .unwrap_or(99); - let context = - Context::new_with_options([], InitOptions::new().with_domain_id(Some(domain_id))) - .unwrap(); + let executor = Context::new([], InitOptions::new().with_domain_id(Some(domain_id))) + .unwrap() + .create_basic_executor(); let node_name = "test_publisher_names_and_types"; - let node = Node::new(&context, node_name).unwrap(); + let node = executor.create_node(node_name).unwrap(); let check_rosout = |topics: HashMap>| { // rosout shows up in humble and iron, even if the graph is empty @@ -558,9 +558,9 @@ mod tests { #[test] fn test_node_names() { - let context = Context::new([]).unwrap(); + let executor = Context::default().create_basic_executor(); let node_name = "test_node_names"; - let node = Node::new(&context, node_name).unwrap(); + let node = executor.create_node(node_name).unwrap(); let names_and_namespaces = node.get_node_names().unwrap(); @@ -574,9 +574,9 @@ mod tests { #[test] fn test_node_names_with_enclaves() { - let context = Context::new([]).unwrap(); + let executor = Context::default().create_basic_executor(); let node_name = "test_node_names_with_enclaves"; - let node = Node::new(&context, node_name).unwrap(); + let node = executor.create_node(node_name).unwrap(); let names_and_namespaces = node.get_node_names_with_enclaves().unwrap(); diff --git a/rclrs/src/node/node_graph_task.rs b/rclrs/src/node/node_graph_task.rs new file mode 100644 index 000000000..523d2e491 --- /dev/null +++ b/rclrs/src/node/node_graph_task.rs @@ -0,0 +1,39 @@ +use futures::{ + channel::mpsc::{UnboundedReceiver, UnboundedSender}, + StreamExt, +}; + +use crate::GuardCondition; + +pub(super) enum NodeGraphAction { + NewGraphListener(UnboundedSender<()>), + GraphChange, +} + +// We take in the GuardCondition to ensure that its Waitable remains in the wait +// set for as long as this task is running. The task will be kept running as long +// as the Node that started it is alive. +pub(super) async fn node_graph_task( + mut receiver: UnboundedReceiver, + #[allow(unused)] guard_condition: GuardCondition, +) { + let mut listeners = Vec::new(); + while let Some(action) = receiver.next().await { + match action { + NodeGraphAction::NewGraphListener(listener) => { + if listener.unbounded_send(()).is_ok() { + // The listener might or might not still be relevant, so + // keep it until we see that the receiver is dropped. + listeners.push(listener); + } + } + NodeGraphAction::GraphChange => { + // We should let all listeners know that a graph event happened. + // If we see that the listener's receiver has dropped (i.e. + // unbounded_send returns an Err) then we remove it from the + // container. + listeners.retain(|listener| listener.unbounded_send(()).is_ok()); + } + } + } +} diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/node_options.rs similarity index 59% rename from rclrs/src/node/builder.rs rename to rclrs/src/node/node_options.rs index 1e7a9fc63..08f6bebbf 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/node_options.rs @@ -1,110 +1,26 @@ use std::{ + borrow::Borrow, ffi::{CStr, CString}, sync::{atomic::AtomicBool, Arc, Mutex}, }; +use futures::channel::mpsc::unbounded; + use crate::{ - rcl_bindings::*, ClockType, Context, ContextHandle, Logger, Node, NodeHandle, + node::node_graph_task::{node_graph_task, NodeGraphAction}, + rcl_bindings::*, + ClockType, ExecutorCommands, GuardCondition, Logger, Node, NodeHandle, NodeState, ParameterInterface, QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, }; -/// A builder for creating a [`Node`][1]. -/// -/// The builder pattern allows selectively setting some fields, and leaving all others at their default values. -/// This struct instance can be created via [`Node::builder()`][2]. -/// -/// The default values for optional fields are: -/// - `namespace: "/"` -/// - `use_global_arguments: true` -/// - `arguments: []` -/// - `enable_rosout: true` -/// - `start_parameter_services: true` -/// - `clock_type: ClockType::RosTime` -/// - `clock_qos: QOS_PROFILE_CLOCK` +/// This trait helps to build [`NodeOptions`] which can be passed into +/// [`Executor::create_node`][1]. /// -/// # Example -/// ``` -/// # use rclrs::{Context, NodeBuilder, Node, RclrsError}; -/// let context = Context::new([])?; -/// // Building a node in a single expression -/// let node = NodeBuilder::new(&context, "foo_node").namespace("/bar").build()?; -/// assert_eq!(node.name(), "foo_node"); -/// assert_eq!(node.namespace(), "/bar"); -/// // Building a node via Node::builder() -/// let node = Node::builder(&context, "bar_node").build()?; -/// assert_eq!(node.name(), "bar_node"); -/// // Building a node step-by-step -/// let mut builder = Node::builder(&context, "goose"); -/// builder = builder.namespace("/duck/duck"); -/// let node = builder.build()?; -/// assert_eq!(node.fully_qualified_name(), "/duck/duck/goose"); -/// # Ok::<(), RclrsError>(()) -/// ``` -/// -/// [1]: crate::Node -/// [2]: crate::Node::builder -pub struct NodeBuilder { - context: Arc, - name: String, - namespace: String, - use_global_arguments: bool, - arguments: Vec, - enable_rosout: bool, - start_parameter_services: bool, - clock_type: ClockType, - clock_qos: QoSProfile, -} - -impl NodeBuilder { - /// Creates a builder for a node with the given name. - /// - /// See the [`Node` docs][1] for general information on node names. - /// - /// # Rules for valid node names - /// - /// The rules for a valid node name are checked by the [`rmw_validate_node_name()`][2] - /// function. They are: - /// - Must contain only the `a-z`, `A-Z`, `0-9`, and `_` characters - /// - Must not be empty and not be longer than `RMW_NODE_NAME_MAX_NAME_LENGTH` - /// - Must not start with a number - /// - /// Note that node name validation is delayed until [`NodeBuilder::build()`][3]. - /// - /// # Example - /// ``` - /// # use rclrs::{Context, NodeBuilder, RclrsError, RclReturnCode}; - /// let context = Context::new([])?; - /// // This is a valid node name - /// assert!(NodeBuilder::new(&context, "my_node").build().is_ok()); - /// // This is another valid node name (although not a good one) - /// assert!(NodeBuilder::new(&context, "_______").build().is_ok()); - /// // This is an invalid node name - /// assert!(matches!( - /// NodeBuilder::new(&context, "röböt") - /// .build() - /// .unwrap_err(), - /// RclrsError::RclError { code: RclReturnCode::NodeInvalidName, .. } - /// )); - /// # Ok::<(), RclrsError>(()) - /// ``` - /// - /// [1]: crate::Node#naming - /// [2]: https://docs.ros2.org/latest/api/rmw/validate__node__name_8h.html#a5690a285aed9735f89ef11950b6e39e3 - /// [3]: NodeBuilder::build - pub fn new(context: &Context, name: &str) -> NodeBuilder { - NodeBuilder { - context: Arc::clone(&context.handle), - name: name.to_string(), - namespace: "/".to_string(), - use_global_arguments: true, - arguments: vec![], - enable_rosout: true, - start_parameter_services: true, - clock_type: ClockType::RosTime, - clock_qos: QOS_PROFILE_CLOCK, - } - } +/// [1]: crate::Executor::create_node +pub trait IntoNodeOptions<'a>: Sized { + /// Conver the object into [`NodeOptions`] with default settings. + fn into_node_options(self) -> NodeOptions<'a>; /// Sets the node namespace. /// @@ -123,29 +39,29 @@ impl NodeBuilder { /// - Must not contain two or more `/` characters in a row /// - Must not have a `/` character at the end, except if `/` is the full namespace /// - /// Note that namespace validation is delayed until [`NodeBuilder::build()`][4]. + /// Note that namespace validation is delayed until [`Executor::create_node`][4]. /// /// # Example /// ``` - /// # use rclrs::{Context, Node, RclrsError, RclReturnCode}; - /// let context = Context::new([])?; + /// # use rclrs::{Context, Node, IntoNodeOptions, RclrsError, RclReturnCode}; + /// let executor = Context::default().create_basic_executor(); /// // This is a valid namespace - /// let builder_ok_ns = Node::builder(&context, "my_node").namespace("/some/nested/namespace"); - /// assert!(builder_ok_ns.build().is_ok()); + /// let options_ok_ns = "my_node".namespace("/some/nested/namespace"); + /// assert!(executor.create_node(options_ok_ns).is_ok()); /// // This is an invalid namespace /// assert!(matches!( - /// Node::builder(&context, "my_node") + /// executor.create_node( + /// "my_node" /// .namespace("/10_percent_luck/20_percent_skill") - /// .build() - /// .unwrap_err(), + /// ).unwrap_err(), /// RclrsError::RclError { code: RclReturnCode::NodeInvalidNamespace, .. } /// )); /// // A missing forward slash at the beginning is automatically added /// assert_eq!( - /// Node::builder(&context, "my_node") + /// executor.create_node( + /// "my_node" /// .namespace("foo") - /// .build()? - /// .namespace(), + /// )?.namespace(), /// "/foo" /// ); /// # Ok::<(), RclrsError>(()) @@ -154,10 +70,11 @@ impl NodeBuilder { /// [1]: crate::Node#naming /// [2]: http://design.ros2.org/articles/topic_and_service_names.html /// [3]: https://docs.ros2.org/latest/api/rmw/validate__namespace_8h.html#a043f17d240cf13df01321b19a469ee49 - /// [4]: NodeBuilder::build - pub fn namespace(mut self, namespace: &str) -> Self { - self.namespace = namespace.to_string(); - self + /// [4]: crate::Executor::create_node + fn namespace(self, namespace: &'a str) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.namespace = namespace; + options } /// Enables or disables using global arguments. @@ -166,29 +83,30 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, NodeBuilder, RclrsError}; + /// # use rclrs::{Context, InitOptions, Node, IntoNodeOptions, RclrsError}; /// let context_args = ["--ros-args", "--remap", "__node:=your_node"] /// .map(String::from); - /// let context = Context::new(context_args)?; + /// let executor = Context::new(context_args, InitOptions::default())?.create_basic_executor(); /// // Ignore the global arguments: - /// let node_without_global_args = - /// rclrs::create_node_builder(&context, "my_node") - /// .use_global_arguments(false) - /// .build()?; + /// let node_without_global_args = executor.create_node( + /// "my_node" + /// .use_global_arguments(false) + /// )?; /// assert_eq!(node_without_global_args.name(), "my_node"); /// // Do not ignore the global arguments: - /// let node_with_global_args = - /// rclrs::create_node_builder(&context, "my_other_node") - /// .use_global_arguments(true) - /// .build()?; + /// let node_with_global_args = executor.create_node( + /// "my_other_node" + /// .use_global_arguments(true) + /// )?; /// assert_eq!(node_with_global_args.name(), "your_node"); /// # Ok::<(), RclrsError>(()) /// ``` /// /// [1]: crate::Context::new - pub fn use_global_arguments(mut self, enable: bool) -> Self { - self.use_global_arguments = enable; - self + fn use_global_arguments(self, enable: bool) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.use_global_arguments = enable; + options } /// Sets node-specific command line arguments. @@ -201,27 +119,31 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, NodeBuilder, RclrsError}; + /// # use rclrs::{Context, InitOptions, IntoNodeOptions, Node, RclrsError}; /// // Usually, this would change the name of "my_node" to "context_args_node": /// let context_args = ["--ros-args", "--remap", "my_node:__node:=context_args_node"] /// .map(String::from); - /// let context = Context::new(context_args)?; + /// let executor = Context::new(context_args, InitOptions::default())?.create_basic_executor(); /// // But the node arguments will change it to "node_args_node": /// let node_args = ["--ros-args", "--remap", "my_node:__node:=node_args_node"] /// .map(String::from); - /// let node = - /// rclrs::create_node_builder(&context, "my_node") - /// .arguments(node_args) - /// .build()?; + /// let node = executor.create_node( + /// "my_node" + /// .arguments(node_args) + /// )?; /// assert_eq!(node.name(), "node_args_node"); /// # Ok::<(), RclrsError>(()) /// ``` /// /// [1]: crate::Context::new /// [2]: https://design.ros2.org/articles/ros_command_line_arguments.html - pub fn arguments(mut self, arguments: impl IntoIterator) -> Self { - self.arguments = arguments.into_iter().collect(); - self + fn arguments(self, arguments: Args) -> NodeOptions<'a> + where + Args::Item: ToString, + { + let mut options = self.into_node_options(); + options.arguments = arguments.into_iter().map(|item| item.to_string()).collect(); + options } /// Enables or disables logging to rosout. @@ -230,57 +152,161 @@ impl NodeBuilder { /// standard output. /// /// This option is currently unused in `rclrs`. - pub fn enable_rosout(mut self, enable: bool) -> Self { - self.enable_rosout = enable; - self + fn enable_rosout(self, enable: bool) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.enable_rosout = enable; + options } /// Enables or disables parameter services. /// /// Parameter services can be used to allow external nodes to list, get and set /// parameters for this node. - pub fn start_parameter_services(mut self, start: bool) -> Self { - self.start_parameter_services = start; - self + fn start_parameter_services(self, start: bool) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.start_parameter_services = start; + options } /// Sets the node's clock type. - pub fn clock_type(mut self, clock_type: ClockType) -> Self { - self.clock_type = clock_type; - self + fn clock_type(self, clock_type: ClockType) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.clock_type = clock_type; + options } /// Sets the QoSProfile for the clock subscription. - pub fn clock_qos(mut self, clock_qos: QoSProfile) -> Self { - self.clock_qos = clock_qos; - self + fn clock_qos(self, clock_qos: QoSProfile) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.clock_qos = clock_qos; + options } +} - /// Builds the node instance. +/// A set of options for creating a [`Node`][1]. +/// +/// The builder pattern, implemented through [`IntoNodeOptions`], allows +/// selectively setting some fields, and leaving all others at their default values. +/// +/// The default values for optional fields are: +/// - `namespace: "/"` +/// - `use_global_arguments: true` +/// - `arguments: []` +/// - `enable_rosout: true` +/// - `start_parameter_services: true` +/// - `clock_type: ClockType::RosTime` +/// - `clock_qos: QOS_PROFILE_CLOCK` +/// +/// # Example +/// ``` +/// # use rclrs::{ClockType, Context, IntoNodeOptions, NodeOptions, Node, RclrsError}; +/// let executor = Context::default().create_basic_executor(); +/// +/// // Building a node with default options +/// let node = executor.create_node("foo_node"); +/// +/// // Building a node with a namespace +/// let node = executor.create_node("bar_node".namespace("/bar"))?; +/// assert_eq!(node.name(), "bar_node"); +/// assert_eq!(node.namespace(), "/bar"); +/// +/// // Building a node with a namespace and no parameter services +/// let node = executor.create_node( +/// "baz" +/// .namespace("qux") +/// .start_parameter_services(false) +/// )?; +/// +/// // Building node options step-by-step +/// let mut options = NodeOptions::new("goose"); +/// options = options.namespace("/duck/duck"); +/// options = options.clock_type(ClockType::SteadyTime); +/// +/// let node = executor.create_node(options)?; +/// assert_eq!(node.fully_qualified_name(), "/duck/duck/goose"); +/// # Ok::<(), RclrsError>(()) +/// ``` +/// +/// [1]: crate::Node +pub struct NodeOptions<'a> { + name: &'a str, + namespace: &'a str, + use_global_arguments: bool, + arguments: Vec, + enable_rosout: bool, + start_parameter_services: bool, + clock_type: ClockType, + clock_qos: QoSProfile, +} + +impl<'a> NodeOptions<'a> { + /// Creates a builder for a node with the given name. /// - /// Node name and namespace validation is performed in this method. + /// See the [`Node` docs][1] for general information on node names. /// - /// For example usage, see the [`NodeBuilder`][1] docs. + /// # Rules for valid node names /// - /// [1]: crate::NodeBuilder - pub fn build(&self) -> Result, RclrsError> { - let node_name = - CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul { - err, - s: self.name.clone(), - })?; + /// The rules for a valid node name are checked by the [`rmw_validate_node_name()`][2] + /// function. They are: + /// - Must contain only the `a-z`, `A-Z`, `0-9`, and `_` characters + /// - Must not be empty and not be longer than `RMW_NODE_NAME_MAX_NAME_LENGTH` + /// - Must not start with a number + /// + /// Note that node name validation is delayed until [`Executor::create_node`][3]. + /// + /// # Example + /// ``` + /// # use rclrs::{Context, NodeOptions, RclrsError, RclReturnCode}; + /// let executor = Context::default().create_basic_executor(); + /// // This is a valid node name + /// assert!(executor.create_node(NodeOptions::new("my_node")).is_ok()); + /// // This is another valid node name (although not a good one) + /// assert!(executor.create_node(NodeOptions::new("_______")).is_ok()); + /// // This is an invalid node name + /// assert!(matches!( + /// executor.create_node(NodeOptions::new("röböt")).unwrap_err(), + /// RclrsError::RclError { code: RclReturnCode::NodeInvalidName, .. } + /// )); + /// # Ok::<(), RclrsError>(()) + /// ``` + /// + /// [1]: crate::Node#naming + /// [2]: https://docs.ros2.org/latest/api/rmw/validate__node__name_8h.html#a5690a285aed9735f89ef11950b6e39e3 + /// [3]: crate::Executor::create_node + pub fn new(name: &'a str) -> NodeOptions<'a> { + NodeOptions { + name, + namespace: "/", + use_global_arguments: true, + arguments: vec![], + enable_rosout: true, + start_parameter_services: true, + clock_type: ClockType::RosTime, + clock_qos: QOS_PROFILE_CLOCK, + } + } + + /// Builds the node instance. + /// + /// Only used internally. Downstream users should call + /// [`Executor::create_node`]. + pub(crate) fn build(self, commands: &Arc) -> Result { + let node_name = CString::new(self.name).map_err(|err| RclrsError::StringContainsNul { + err, + s: self.name.to_owned(), + })?; let node_namespace = - CString::new(self.namespace.as_str()).map_err(|err| RclrsError::StringContainsNul { + CString::new(self.namespace).map_err(|err| RclrsError::StringContainsNul { err, - s: self.namespace.clone(), + s: self.namespace.to_owned(), })?; let rcl_node_options = self.create_rcl_node_options()?; - let rcl_context = &mut *self.context.rcl_context.lock().unwrap(); + let rcl_context = &mut *commands.context().handle.rcl_context.lock().unwrap(); let handle = Arc::new(NodeHandle { // SAFETY: Getting a zero-initialized value is always safe. rcl_node: Mutex::new(unsafe { rcl_get_zero_initialized_node() }), - context_handle: Arc::clone(&self.context), + context_handle: Arc::clone(&commands.context().handle), initialized: AtomicBool::new(false), }); @@ -334,23 +360,51 @@ impl NodeBuilder { } }; - let node = Arc::new(Node { - handle, - clients_mtx: Mutex::new(vec![]), - guard_conditions_mtx: Mutex::new(vec![]), - services_mtx: Mutex::new(vec![]), - subscriptions_mtx: Mutex::new(vec![]), + // --- Set up guard condition for graph change events --- + let (graph_change_action, graph_change_receiver) = unbounded(); + let graph_change_execute_sender = graph_change_action.clone(); + + let rcl_graph_change_guard_condition = unsafe { + // SAFETY: The node is valid because we just instantiated it. + rcl_node_get_graph_guard_condition(&*handle.rcl_node.lock().unwrap()) + }; + let (graph_change_guard_condition, graph_change_waitable) = unsafe { + // SAFETY: The guard condition is owned by the rcl_node and will + // remain valid for as long as the rcl_node is alive, so we set the + // owner to be the Arc for the NodeHandle. + GuardCondition::from_rcl( + &commands.context().handle, + rcl_graph_change_guard_condition, + Box::new(Arc::clone(&handle)), + Some(Box::new(move || { + graph_change_execute_sender + .unbounded_send(NodeGraphAction::GraphChange) + .ok(); + })), + ) + }; + commands.add_to_wait_set(graph_change_waitable); + let _ = commands.run(node_graph_task( + graph_change_receiver, + graph_change_guard_condition, + )); + + let node = Arc::new(NodeState { time_source: TimeSource::builder(self.clock_type) .clock_qos(self.clock_qos) .build(), parameter, logger: Logger::new(logger_name)?, + graph_change_action, + commands: Arc::clone(&commands), + handle, }); - node.time_source.attach_node(&node); + if self.start_parameter_services { node.parameter.create_services(&node)?; } + Ok(node) } @@ -395,6 +449,24 @@ impl NodeBuilder { } } +impl<'a> IntoNodeOptions<'a> for NodeOptions<'a> { + fn into_node_options(self) -> NodeOptions<'a> { + self + } +} + +impl<'a, T: Borrow> IntoNodeOptions<'a> for &'a T { + fn into_node_options(self) -> NodeOptions<'a> { + NodeOptions::new(self.borrow()) + } +} + +impl<'a> IntoNodeOptions<'a> for &'a str { + fn into_node_options(self) -> NodeOptions<'a> { + NodeOptions::new(self) + } +} + impl Drop for rcl_node_options_t { fn drop(&mut self) { // SAFETY: Do not finish this struct except here. diff --git a/rclrs/src/node/primitive_options.rs b/rclrs/src/node/primitive_options.rs new file mode 100644 index 000000000..0299c70f0 --- /dev/null +++ b/rclrs/src/node/primitive_options.rs @@ -0,0 +1,253 @@ +use crate::{ + QoSDurabilityPolicy, QoSDuration, QoSHistoryPolicy, QoSLivelinessPolicy, QoSProfile, + QoSReliabilityPolicy, +}; + +use std::{borrow::Borrow, time::Duration}; + +/// `PrimitiveOptions` are the subset of options that are relevant across all +/// primitives (e.g. [`Subscription`][1], [`Publisher`][2], [`Client`][3], and +/// [`Service`][4]). +/// +/// Each different primitive type may have its own defaults for the overall +/// quality of service settings, and we cannot know what the default will be +/// until the `PrimitiveOptions` gets converted into the more specific set of +/// options. Therefore we store each quality of service field separately so that +/// we will only override the settings that the user explicitly asked for, and +/// the rest will be determined by the default settings for each primitive. +/// +/// [1]: crate::Subscription +/// [2]: crate::Publisher +/// [3]: crate::Client +/// [4]: crate::Service +#[derive(Debug, Clone, Copy)] +#[non_exhaustive] +pub struct PrimitiveOptions<'a> { + /// The name that will be used for the primitive + pub name: &'a str, + /// Override the default [`QoSProfile::history`] for the primitive. + pub history: Option, + /// Override the default [`QoSProfile::reliability`] for the primitive. + pub reliability: Option, + /// Override the default [`QoSProfile::durability`] for the primitive. + pub durability: Option, + /// Override the default [`QoSProfile::deadline`] for the primitive. + pub deadline: Option, + /// Override the default [`QoSProfile::lifespan`] for the primitive. + pub lifespan: Option, + /// Override the default [`QoSProfile::liveliness`] for the primitive. + pub liveliness: Option, + /// Override the default [`QoSProfile::liveliness_lease`] for the primitive. + pub liveliness_lease: Option, + /// Override the default [`QoSProfile::avoid_ros_namespace_conventions`] for the primitive. + pub avoid_ros_namespace_conventions: Option, +} + +/// Trait to implicitly convert a compatible object into [`PrimitiveOptions`]. +pub trait IntoPrimitiveOptions<'a>: Sized { + /// Convert the object into [`PrimitiveOptions`] with default settings. + fn into_primitive_options(self) -> PrimitiveOptions<'a>; + + /// Override all the quality of service settings for the primitive. + fn qos(self, profile: QoSProfile) -> PrimitiveOptions<'a> { + self.into_primitive_options().history(profile.history) + } + + /// Use the default topics quality of service profile. + fn topics_qos(self) -> PrimitiveOptions<'a> { + self.qos(QoSProfile::topics_default()) + } + + /// Use the default sensor data quality of service profile. + fn sensor_data_qos(self) -> PrimitiveOptions<'a> { + self.qos(QoSProfile::sensor_data_default()) + } + + /// Use the default services quality of service profile. + fn services_qos(self) -> PrimitiveOptions<'a> { + self.qos(QoSProfile::services_default()) + } + + /// Use the system-defined default quality of service profile. This profile + /// is determined by the underlying RMW implementation, so you cannot rely + /// on this profile being consistent or appropriate for your needs. + fn system_qos(self) -> PrimitiveOptions<'a> { + self.qos(QoSProfile::system_default()) + } + + /// Override the default [`QoSProfile::history`] for the primitive. + fn history(self, history: QoSHistoryPolicy) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.history = Some(history); + options + } + + /// Keep the last `depth` messages for the primitive. + fn keep_last(self, depth: u32) -> PrimitiveOptions<'a> { + self.history(QoSHistoryPolicy::KeepLast { depth }) + } + + /// Keep all messages for the primitive. + fn keep_all(self) -> PrimitiveOptions<'a> { + self.history(QoSHistoryPolicy::KeepAll) + } + + /// Override the default [`QoSProfile::reliability`] for the primitive. + fn reliability(self, reliability: QoSReliabilityPolicy) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.reliability = Some(reliability); + options + } + + /// Set the primitive to have [reliable][QoSReliabilityPolicy::Reliable] communication. + fn reliable(self) -> PrimitiveOptions<'a> { + self.reliability(QoSReliabilityPolicy::Reliable) + } + + /// Set the primitive to have [best-effort][QoSReliabilityPolicy::BestEffort] communication. + fn best_effort(self) -> PrimitiveOptions<'a> { + self.reliability(QoSReliabilityPolicy::BestEffort) + } + + /// Override the default [`QoSProfile::durability`] for the primitive. + fn durability(self, durability: QoSDurabilityPolicy) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.durability = Some(durability); + options + } + + /// Set the primitive to have [volatile][QoSDurabilityPolicy::Volatile] durability. + fn volatile(self) -> PrimitiveOptions<'a> { + self.durability(QoSDurabilityPolicy::Volatile) + } + + /// Set the primitive to have [transient local][QoSDurabilityPolicy::TransientLocal] durability. + fn transient_local(self) -> PrimitiveOptions<'a> { + self.durability(QoSDurabilityPolicy::TransientLocal) + } + + /// Override the default [`QoSProfile::lifespan`] for the primitive. + fn lifespan(self, lifespan: QoSDuration) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.lifespan = Some(lifespan); + options + } + + /// Set a custom duration for the [lifespan][QoSProfile::lifespan] of the primitive. + fn lifespan_duration(self, duration: Duration) -> PrimitiveOptions<'a> { + self.lifespan(QoSDuration::Custom(duration)) + } + + /// Make the [lifespan][QoSProfile::lifespan] of the primitive infinite. + fn infinite_lifespan(self) -> PrimitiveOptions<'a> { + self.lifespan(QoSDuration::Infinite) + } + + /// Override the default [`QoSProfile::deadline`] for the primitive. + fn deadline(self, deadline: QoSDuration) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.deadline = Some(deadline); + options + } + + /// Set the [`QoSProfile::deadline`] to a custom finite value. + fn deadline_duration(self, duration: Duration) -> PrimitiveOptions<'a> { + self.deadline(QoSDuration::Custom(duration)) + } + + /// Do not use a deadline for liveliness for this primitive. + fn no_deadline(self) -> PrimitiveOptions<'a> { + self.deadline(QoSDuration::Infinite) + } + + /// Override the default [`QoSProfile::liveliness_lease`] for the primitive. + fn liveliness_lease(self, lease: QoSDuration) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.liveliness_lease = Some(lease); + options + } + + /// Set a custom duration for the [liveliness lease][QoSProfile::liveliness_lease]. + fn liveliness_lease_duration(self, duration: Duration) -> PrimitiveOptions<'a> { + self.liveliness_lease(QoSDuration::Custom(duration)) + } + + /// [Avoid the ROS namespace conventions][1] for the primitive. + /// + /// [1]: QoSProfile::avoid_ros_namespace_conventions + fn avoid_ros_namespace_conventions(self) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.avoid_ros_namespace_conventions = Some(true); + options + } +} + +impl<'a> IntoPrimitiveOptions<'a> for PrimitiveOptions<'a> { + fn into_primitive_options(self) -> PrimitiveOptions<'a> { + self + } +} + +impl<'a> IntoPrimitiveOptions<'a> for &'a str { + fn into_primitive_options(self) -> PrimitiveOptions<'a> { + PrimitiveOptions::new(self) + } +} + +impl<'a, T: Borrow> IntoPrimitiveOptions<'a> for &'a T { + fn into_primitive_options(self) -> PrimitiveOptions<'a> { + self.borrow().into_primitive_options() + } +} + +impl<'a> PrimitiveOptions<'a> { + /// Begin building a new set of `PrimitiveOptions` with only the name set. + pub fn new(name: &'a str) -> Self { + Self { + name, + history: None, + reliability: None, + durability: None, + deadline: None, + lifespan: None, + liveliness: None, + liveliness_lease: None, + avoid_ros_namespace_conventions: None, + } + } + + /// Apply the user-specified options to a pre-initialized [`QoSProfile`]. + pub fn apply(&self, qos: &mut QoSProfile) { + if let Some(history) = self.history { + qos.history = history; + } + + if let Some(reliability) = self.reliability { + qos.reliability = reliability; + } + + if let Some(durability) = self.durability { + qos.durability = durability; + } + + if let Some(deadline) = self.deadline { + qos.deadline = deadline; + } + + if let Some(lifespan) = self.lifespan { + qos.lifespan = lifespan; + } + + if let Some(liveliness) = self.liveliness { + qos.liveliness = liveliness; + } + + if let Some(liveliness_lease) = self.liveliness_lease { + qos.liveliness_lease = liveliness_lease; + } + + if let Some(convention) = self.avoid_ros_namespace_conventions { + qos.avoid_ros_namespace_conventions = convention; + } + } +} diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index 2a0829eac..2a6913a5f 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -84,7 +84,7 @@ enum DeclaredValue { } /// Builder used to declare a parameter. Obtain this by calling -/// [`crate::Node::declare_parameter`]. +/// [`crate::NodeState::declare_parameter`]. #[must_use] pub struct ParameterBuilder<'a, T: ParameterVariant> { name: Arc, @@ -874,18 +874,25 @@ impl ParameterInterface { #[cfg(test)] mod tests { use super::*; - use crate::{create_node, Context}; + use crate::{Context, InitOptions}; #[test] fn test_parameter_override_errors() { // Create a new node with a few parameter overrides - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("declared_int:=10"), - ]) - .unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); // Declaring a parameter with a different type than what was overridden should return an // error @@ -931,19 +938,26 @@ mod tests { #[test] fn test_parameter_setting_declaring() { // Create a new node with a few parameter overrides - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("declared_int:=10"), - String::from("-p"), - String::from("double_array:=[1.0, 2.0]"), - String::from("-p"), - String::from("optional_bool:=true"), - String::from("-p"), - String::from("non_declared_string:='param'"), - ]) - .unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + String::from("-p"), + String::from("double_array:=[1.0, 2.0]"), + String::from("-p"), + String::from("optional_bool:=true"), + String::from("-p"), + String::from("non_declared_string:='param'"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); let overridden_int = node .declare_parameter("declared_int") @@ -1087,13 +1101,20 @@ mod tests { #[test] fn test_override_undeclared_set_priority() { - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("declared_int:=10"), - ]) - .unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); // If a parameter was set as an override and as an undeclared parameter, the undeclared // value should get priority node.use_undeclared_parameters() @@ -1109,13 +1130,20 @@ mod tests { #[test] fn test_parameter_scope_redeclaring() { - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("declared_int:=10"), - ]) - .unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); { // Setting a parameter with an override let param = node @@ -1160,8 +1188,10 @@ mod tests { #[test] fn test_parameter_ranges() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); // Setting invalid ranges should fail let range = ParameterRange { lower: Some(10), @@ -1288,8 +1318,10 @@ mod tests { #[test] fn test_readonly_parameters() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); let param = node .declare_parameter("int_param") .default(100) @@ -1315,8 +1347,10 @@ mod tests { #[test] fn test_preexisting_value_error() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); node.use_undeclared_parameters() .set("int_param", 100) .unwrap(); @@ -1368,8 +1402,10 @@ mod tests { #[test] fn test_optional_parameter_apis() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); node.declare_parameter::("int_param") .optional() .unwrap(); diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 7c8ffe62d..304f78f98 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -9,24 +9,24 @@ use rosidl_runtime_rs::Sequence; use super::ParameterMap; use crate::{ parameter::{DeclaredValue, ParameterKind, ParameterStorage}, - rmw_request_id_t, Node, RclrsError, Service, + IntoPrimitiveOptions, Node, QoSProfile, RclrsError, Service, }; // The variables only exist to keep a strong reference to the services and are technically unused. // What is used is the Weak that is stored in the node, and is upgraded when spinning. pub struct ParameterService { #[allow(dead_code)] - describe_parameters_service: Arc>, + describe_parameters_service: Service, #[allow(dead_code)] - get_parameter_types_service: Arc>, + get_parameter_types_service: Service, #[allow(dead_code)] - get_parameters_service: Arc>, + get_parameters_service: Service, #[allow(dead_code)] - list_parameters_service: Arc>, + list_parameters_service: Service, #[allow(dead_code)] - set_parameters_service: Arc>, + set_parameters_service: Service, #[allow(dead_code)] - set_parameters_atomically_service: Arc>, + set_parameters_atomically_service: Service, } fn describe_parameters( @@ -247,47 +247,48 @@ impl ParameterService { // destruction is made for the parameter map. let map = parameter_map.clone(); let describe_parameters_service = node.create_service( - &(fqn.clone() + "/describe_parameters"), - move |_req_id: &rmw_request_id_t, req: DescribeParameters_Request| { + (fqn.clone() + "/describe_parameters").qos(QoSProfile::parameter_services_default()), + move |req: DescribeParameters_Request| { let map = map.lock().unwrap(); describe_parameters(req, &map) }, )?; let map = parameter_map.clone(); let get_parameter_types_service = node.create_service( - &(fqn.clone() + "/get_parameter_types"), - move |_req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { + (fqn.clone() + "/get_parameter_types").qos(QoSProfile::parameter_services_default()), + move |req: GetParameterTypes_Request| { let map = map.lock().unwrap(); get_parameter_types(req, &map) }, )?; let map = parameter_map.clone(); let get_parameters_service = node.create_service( - &(fqn.clone() + "/get_parameters"), - move |_req_id: &rmw_request_id_t, req: GetParameters_Request| { + (fqn.clone() + "/get_parameters").qos(QoSProfile::parameter_services_default()), + move |req: GetParameters_Request| { let map = map.lock().unwrap(); get_parameters(req, &map) }, )?; let map = parameter_map.clone(); let list_parameters_service = node.create_service( - &(fqn.clone() + "/list_parameters"), - move |_req_id: &rmw_request_id_t, req: ListParameters_Request| { + (fqn.clone() + "/list_parameters").qos(QoSProfile::parameter_services_default()), + move |req: ListParameters_Request| { let map = map.lock().unwrap(); list_parameters(req, &map) }, )?; let map = parameter_map.clone(); let set_parameters_service = node.create_service( - &(fqn.clone() + "/set_parameters"), - move |_req_id: &rmw_request_id_t, req: SetParameters_Request| { + (fqn.clone() + "/set_parameters").qos(QoSProfile::parameter_services_default()), + move |req: SetParameters_Request| { let mut map = map.lock().unwrap(); set_parameters(req, &mut map) }, )?; let set_parameters_atomically_service = node.create_service( - &(fqn.clone() + "/set_parameters_atomically"), - move |_req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { + (fqn.clone() + "/set_parameters_atomically") + .qos(QoSProfile::parameter_services_default()), + move |req: SetParametersAtomically_Request| { let mut map = parameter_map.lock().unwrap(); set_parameters_atomically(req, &mut map) }, @@ -312,39 +313,30 @@ mod tests { }, srv::rmw::*, }, - Context, MandatoryParameter, Node, NodeBuilder, ParameterRange, ParameterValue, RclrsError, - ReadOnlyParameter, + Context, Executor, IntoNodeOptions, MandatoryParameter, Node, NodeOptions, ParameterRange, + ParameterValue, RclrsError, ReadOnlyParameter, SpinOptions, }; use rosidl_runtime_rs::{seq, Sequence}; - use std::sync::{Arc, RwLock}; + use std::{ + sync::{ + atomic::{AtomicBool, Ordering}, + Arc, + }, + time::Duration, + }; struct TestNode { - node: Arc, + node: Node, bool_param: MandatoryParameter, _ns_param: MandatoryParameter, _read_only_param: ReadOnlyParameter, dynamic_param: MandatoryParameter, } - async fn try_until_timeout(f: F) -> Result<(), ()> - where - F: FnOnce() -> bool + Copy, - { - let mut retry_count = 0; - while !f() { - if retry_count > 50 { - return Err(()); - } - tokio::time::sleep(tokio::time::Duration::from_millis(10)).await; - retry_count += 1; - } - Ok(()) - } - - fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { - let node = NodeBuilder::new(context, "node") - .namespace(ns) - .build() + fn construct_test_nodes(ns: &str) -> (Executor, TestNode, Node) { + let executor = Context::default().create_basic_executor(); + let node = executor + .create_node(NodeOptions::new("node").namespace(ns)) .unwrap(); let range = ParameterRange { lower: Some(0), @@ -375,12 +367,12 @@ mod tests { .mandatory() .unwrap(); - let client = NodeBuilder::new(context, "client") - .namespace(ns) - .build() + let client = executor + .create_node(NodeOptions::new("client").namespace(ns)) .unwrap(); ( + executor, TestNode { node, bool_param, @@ -394,552 +386,542 @@ mod tests { #[test] fn test_parameter_services_names_and_types() -> Result<(), RclrsError> { - let context = Context::new([]).unwrap(); - let (node, _client) = construct_test_nodes(&context, "names_types"); + let (mut executor, test, _client) = construct_test_nodes("names_types"); + + // Avoid flakiness while also finishing faster in most cases by giving + // this more maximum time but checking each time a graph change is detected. + let timeout = Duration::from_secs(1); + let initial_time = std::time::Instant::now(); + + let node = Arc::clone(&test.node); + let promise = + test.node + .notify_on_graph_change_with_period(Duration::from_millis(1), move || { + let mut not_finished = false; + let max_time_reached = initial_time.elapsed() > timeout; + let mut check = |condition: bool| { + if max_time_reached { + assert!(condition); + } else { + not_finished &= !condition; + } + }; + + let names_and_types = node.get_service_names_and_types().unwrap(); + let types = names_and_types + .get("/names_types/node/describe_parameters") + .unwrap(); + check(!types.contains(&"rcl_interfaces/srv/DescribeParameters".to_string())); + let types = names_and_types + .get("/names_types/node/get_parameters") + .unwrap(); + check(!types.contains(&"rcl_interfaces/srv/GetParameters".to_string())); + let types = names_and_types + .get("/names_types/node/set_parameters") + .unwrap(); + check(!types.contains(&"rcl_interfaces/srv/SetParameters".to_string())); + let types = names_and_types + .get("/names_types/node/set_parameters_atomically") + .unwrap(); + check( + !types.contains(&"rcl_interfaces/srv/SetParametersAtomically".to_string()), + ); + let types = names_and_types + .get("/names_types/node/list_parameters") + .unwrap(); + check(types.contains(&"rcl_interfaces/srv/ListParameters".to_string())); + let types = names_and_types + .get("/names_types/node/get_parameter_types") + .unwrap(); + check(types.contains(&"rcl_interfaces/srv/GetParameterTypes".to_string())); + !not_finished + }); - std::thread::sleep(std::time::Duration::from_millis(100)); + executor + .spin( + SpinOptions::new() + .until_promise_resolved(promise) + .timeout(Duration::from_secs(1)), + ) + .unwrap(); + + Ok(()) + } - let names_and_types = node.node.get_service_names_and_types()?; - let types = names_and_types - .get("/names_types/node/describe_parameters") + #[test] + fn test_list_parameters_service() -> Result<(), RclrsError> { + let (mut executor, _test, client_node) = construct_test_nodes("list"); + let list_client = + client_node.create_client::("/list/node/list_parameters")?; + + // return Ok(()); + executor + .spin( + SpinOptions::default() + .until_promise_resolved(list_client.notify_on_service_ready()) + .timeout(Duration::from_secs(2)), + ) .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/DescribeParameters".to_string())); - let types = names_and_types - .get("/names_types/node/get_parameters") + + // List all parameters + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let request = ListParameters_Request { + prefixes: seq![], + depth: 0, + }; + let promise = list_client + .call_then(&request, move |response: ListParameters_Response| { + // use_sim_time + all the manually defined ones + let names = response.result.names; + assert_eq!(names.len(), 5); + // Parameter names are returned in alphabetical order + assert_eq!(names[0].to_string(), "bool"); + assert_eq!(names[1].to_string(), "dynamic"); + assert_eq!(names[2].to_string(), "ns1.ns2.ns3.int"); + assert_eq!(names[3].to_string(), "read_only"); + assert_eq!(names[4].to_string(), "use_sim_time"); + // Only one prefix + assert_eq!(response.result.prefixes.len(), 1); + assert_eq!(response.result.prefixes[0].to_string(), "ns1.ns2.ns3"); + callback_ran_inner.store(true, Ordering::Release); + }) .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/GetParameters".to_string())); - let types = names_and_types - .get("/names_types/node/set_parameters") + + executor + .spin( + SpinOptions::default() + .until_promise_resolved(promise) + .timeout(Duration::from_secs(5)), + ) + .unwrap(); + assert!(callback_ran.load(Ordering::Acquire)); + + // Limit depth, namespaced parameter is not returned + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let request = ListParameters_Request { + prefixes: seq![], + depth: 1, + }; + let promise = list_client + .call_then(&request, move |response: ListParameters_Response| { + let names = response.result.names; + assert_eq!(names.len(), 4); + assert!(names.iter().all(|n| n.to_string() != "ns1.ns2.ns3.int")); + assert_eq!(response.result.prefixes.len(), 0); + callback_ran_inner.store(true, Ordering::Release); + }) .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/SetParameters".to_string())); - let types = names_and_types - .get("/names_types/node/set_parameters_atomically") + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/SetParametersAtomically".to_string())); - let types = names_and_types - .get("/names_types/node/list_parameters") + assert!(callback_ran.load(Ordering::Acquire)); + + // Filter by prefix, just return the requested one with the right prefix + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let request = ListParameters_Request { + prefixes: seq!["ns1.ns2".into()], + depth: 0, + }; + let promise = list_client + .call_then(&request, move |response: ListParameters_Response| { + let names = response.result.names; + assert_eq!(names.len(), 1); + assert_eq!(names[0].to_string(), "ns1.ns2.ns3.int"); + assert_eq!(response.result.prefixes.len(), 1); + assert_eq!(response.result.prefixes[0].to_string(), "ns1.ns2.ns3"); + callback_ran_inner.store(true, Ordering::Release); + }) .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/ListParameters".to_string())); - let types = names_and_types - .get("/names_types/node/get_parameter_types") + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/GetParameterTypes".to_string())); + assert!(callback_ran.load(Ordering::Acquire)); + + // If prefix is equal to names, parameters should be returned + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let request = ListParameters_Request { + prefixes: seq!["use_sim_time".into(), "bool".into()], + depth: 0, + }; + let promise = list_client + .call_then(&request, move |response: ListParameters_Response| { + let names = response.result.names; + assert_eq!(names.len(), 2); + assert_eq!(names[0].to_string(), "bool"); + assert_eq!(names[1].to_string(), "use_sim_time"); + assert_eq!(response.result.prefixes.len(), 0); + callback_ran_inner.store(true, Ordering::Release); + }) + .unwrap(); + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); + assert!(callback_ran.load(Ordering::Acquire)); + Ok(()) } - #[tokio::test] - async fn test_list_parameters_service() -> Result<(), RclrsError> { - let context = Context::new([]).unwrap(); - let (node, client) = construct_test_nodes(&context, "list"); - let list_client = client.create_client::("/list/node/list_parameters")?; + #[test] + fn test_get_set_parameters_service() -> Result<(), RclrsError> { + let (mut executor, test, client_node) = construct_test_nodes("get_set"); + let get_client = + client_node.create_client::("/get_set/node/get_parameters")?; + let set_client = + client_node.create_client::("/get_set/node/set_parameters")?; + let set_atomically_client = client_node + .create_client::("/get_set/node/set_parameters_atomically")?; + + let get_client_inner = Arc::clone(&get_client); + let set_client_inner = Arc::clone(&set_client); + let set_atomically_client_inner = Arc::clone(&set_atomically_client); + let clients_ready_condition = move || { + get_client_inner.service_is_ready().unwrap() + && set_client_inner.service_is_ready().unwrap() + && set_atomically_client_inner.service_is_ready().unwrap() + }; + + let clients_ready = client_node + .notify_on_graph_change_with_period(Duration::from_millis(1), clients_ready_condition); - try_until_timeout(|| list_client.service_is_ready().unwrap()) - .await + executor + .spin(SpinOptions::default().until_promise_resolved(clients_ready)) .unwrap(); - let done = Arc::new(RwLock::new(false)); + // Get an existing parameter + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let request = GetParameters_Request { + names: seq!["bool".into()], + }; + let promise = get_client + .call_then(&request, move |response: GetParameters_Response| { + assert_eq!(response.values.len(), 1); + let param = &response.values[0]; + assert_eq!(param.type_, ParameterType::PARAMETER_BOOL); + assert!(param.bool_value); + callback_ran_inner.store(true, Ordering::Release); + }) + .unwrap(); - let inner_done = done.clone(); - let rclrs_spin = tokio::task::spawn(async move { - try_until_timeout(|| { - crate::spin_once(node.node.clone(), Some(std::time::Duration::ZERO)).ok(); - crate::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); - *inner_done.read().unwrap() + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); + assert!(callback_ran.load(Ordering::Acquire)); + + // Getting both existing and non existing parameters, missing one should return + // PARAMETER_NOT_SET + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let request = GetParameters_Request { + names: seq!["bool".into(), "non_existing".into()], + }; + let promise = get_client + .call_then(&request, move |response: GetParameters_Response| { + assert_eq!(response.values.len(), 2); + let param = &response.values[0]; + assert_eq!(param.type_, ParameterType::PARAMETER_BOOL); + assert!(param.bool_value); + assert_eq!(response.values[1].type_, ParameterType::PARAMETER_NOT_SET); + callback_ran_inner.store(true, Ordering::Release); }) - .await .unwrap(); - }); - let res = tokio::task::spawn(async move { - // List all parameters - let request = ListParameters_Request { - prefixes: seq![], - depth: 0, - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - list_client - .async_send_request_with_callback( - &request, - move |response: ListParameters_Response| { - // use_sim_time + all the manually defined ones - *call_done.write().unwrap() = true; - let names = response.result.names; - assert_eq!(names.len(), 5); - // Parameter names are returned in alphabetical order - assert_eq!(names[0].to_string(), "bool"); - assert_eq!(names[1].to_string(), "dynamic"); - assert_eq!(names[2].to_string(), "ns1.ns2.ns3.int"); - assert_eq!(names[3].to_string(), "read_only"); - assert_eq!(names[4].to_string(), "use_sim_time"); - // Only one prefix - assert_eq!(response.result.prefixes.len(), 1); - assert_eq!(response.result.prefixes[0].to_string(), "ns1.ns2.ns3"); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - // Limit depth, namespaced parameter is not returned - let request = ListParameters_Request { - prefixes: seq![], - depth: 1, - }; - let call_done = client_finished.clone(); - *call_done.write().unwrap() = false; - list_client - .async_send_request_with_callback( - &request, - move |response: ListParameters_Response| { - *call_done.write().unwrap() = true; - let names = response.result.names; - assert_eq!(names.len(), 4); - assert!(names.iter().all(|n| n.to_string() != "ns1.ns2.ns3.int")); - assert_eq!(response.result.prefixes.len(), 0); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - // Filter by prefix, just return the requested one with the right prefix - let request = ListParameters_Request { - prefixes: seq!["ns1.ns2".into()], - depth: 0, - }; - let call_done = client_finished.clone(); - *call_done.write().unwrap() = false; - list_client - .async_send_request_with_callback( - &request, - move |response: ListParameters_Response| { - *call_done.write().unwrap() = true; - let names = response.result.names; - assert_eq!(names.len(), 1); - assert_eq!(names[0].to_string(), "ns1.ns2.ns3.int"); - assert_eq!(response.result.prefixes.len(), 1); - assert_eq!(response.result.prefixes[0].to_string(), "ns1.ns2.ns3"); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - // If prefix is equal to names, parameters should be returned - let request = ListParameters_Request { - prefixes: seq!["use_sim_time".into(), "bool".into()], - depth: 0, - }; - let call_done = client_finished.clone(); - *call_done.write().unwrap() = false; - list_client - .async_send_request_with_callback( - &request, - move |response: ListParameters_Response| { - *call_done.write().unwrap() = true; - let names = response.result.names; - dbg!(&names); - assert_eq!(names.len(), 2); - assert_eq!(names[0].to_string(), "bool"); - assert_eq!(names[1].to_string(), "use_sim_time"); - assert_eq!(response.result.prefixes.len(), 0); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - *done.write().unwrap() = true; - }); - - res.await.unwrap(); - rclrs_spin.await.unwrap(); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); + assert!(callback_ran.load(Ordering::Acquire)); + + // Set a mix of existing, non existing, dynamic and out of range parameters + let bool_parameter = RmwParameter { + name: "bool".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_BOOL, + bool_value: false, + ..Default::default() + }, + }; + let bool_parameter_mismatched = RmwParameter { + name: "bool".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_INTEGER, + integer_value: 42, + ..Default::default() + }, + }; + let read_only_parameter = RmwParameter { + name: "read_only".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_DOUBLE, + double_value: 3.45, + ..Default::default() + }, + }; + let dynamic_parameter = RmwParameter { + name: "dynamic".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_BOOL, + bool_value: true, + ..Default::default() + }, + }; + let out_of_range_parameter = RmwParameter { + name: "ns1.ns2.ns3.int".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_INTEGER, + integer_value: 1000, + ..Default::default() + }, + }; + let invalid_parameter_type = RmwParameter { + name: "dynamic".into(), + value: RmwParameterValue { + type_: 200, + integer_value: 1000, + ..Default::default() + }, + }; + let undeclared_bool = RmwParameter { + name: "undeclared_bool".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_BOOL, + bool_value: true, + ..Default::default() + }, + }; + let request = SetParameters_Request { + parameters: seq![ + bool_parameter.clone(), + read_only_parameter.clone(), + bool_parameter_mismatched, + dynamic_parameter, + out_of_range_parameter, + invalid_parameter_type, + undeclared_bool.clone() + ], + }; - Ok(()) - } + // Parameter is assigned a default of true at declaration time + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + assert!(test.bool_param.get()); + let promise = set_client + .call_then(&request, move |response: SetParameters_Response| { + assert_eq!(response.results.len(), 7); + // Setting a bool value set for a bool parameter + assert!(response.results[0].successful); + // Value was set to false, node parameter get should reflect this + assert!(!test.bool_param.get()); + // Setting a parameter to the wrong type + assert!(!response.results[1].successful); + // Setting a read only parameter + assert!(!response.results[2].successful); + // Setting a dynamic parameter to a new type + assert!(response.results[3].successful); + assert_eq!(test.dynamic_param.get(), ParameterValue::Bool(true)); + // Setting a value out of range + assert!(!response.results[4].successful); + // Setting an invalid type + assert!(!response.results[5].successful); + // Setting an undeclared parameter, without allowing undeclared parameters + assert!(!response.results[6].successful); + callback_ran_inner.store(true, Ordering::Release); + }) + .unwrap(); - #[tokio::test] - async fn test_get_set_parameters_service() -> Result<(), RclrsError> { - let context = Context::new([]).unwrap(); - let (node, client) = construct_test_nodes(&context, "get_set"); - let get_client = client.create_client::("/get_set/node/get_parameters")?; - let set_client = client.create_client::("/get_set/node/set_parameters")?; - let set_atomically_client = client - .create_client::("/get_set/node/set_parameters_atomically")?; + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); + assert!(callback_ran.load(Ordering::Acquire)); + + // Set the node to use undeclared parameters and try to set one + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + test.node.use_undeclared_parameters(); + let request = SetParameters_Request { + parameters: seq![undeclared_bool], + }; - try_until_timeout(|| { - get_client.service_is_ready().unwrap() - && set_client.service_is_ready().unwrap() - && set_atomically_client.service_is_ready().unwrap() - }) - .await - .unwrap(); - - let done = Arc::new(RwLock::new(false)); - - let inner_node = node.node.clone(); - let inner_done = done.clone(); - let rclrs_spin = tokio::task::spawn(async move { - try_until_timeout(|| { - crate::spin_once(inner_node.clone(), Some(std::time::Duration::ZERO)).ok(); - crate::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); - *inner_done.read().unwrap() + // Clone test.node here so that we don't move the whole test bundle into + // the closure, which would cause the test node to be fully dropped + // after the closure is called. + let test_node = Arc::clone(&test.node); + + let promise = set_client + .call_then(&request, move |response: SetParameters_Response| { + assert_eq!(response.results.len(), 1); + // Setting the undeclared parameter is now allowed + assert!(response.results[0].successful); + assert_eq!( + test_node.use_undeclared_parameters().get("undeclared_bool"), + Some(ParameterValue::Bool(true)) + ); + callback_ran_inner.store(true, Ordering::Release); }) - .await .unwrap(); - }); - let res = tokio::task::spawn(async move { - // Get an existing parameter - let request = GetParameters_Request { - names: seq!["bool".into()], - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - get_client - .async_send_request_with_callback( - &request, - move |response: GetParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.values.len(), 1); - let param = &response.values[0]; - assert_eq!(param.type_, ParameterType::PARAMETER_BOOL); - assert!(param.bool_value); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - // Getting both existing and non existing parameters, missing one should return - // PARAMETER_NOT_SET - let request = GetParameters_Request { - names: seq!["bool".into(), "non_existing".into()], - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - get_client - .async_send_request_with_callback( - &request, - move |response: GetParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.values.len(), 2); - let param = &response.values[0]; - assert_eq!(param.type_, ParameterType::PARAMETER_BOOL); - assert!(param.bool_value); - assert_eq!(response.values[1].type_, ParameterType::PARAMETER_NOT_SET); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - // Set a mix of existing, non existing, dynamic and out of range parameters - let bool_parameter = RmwParameter { - name: "bool".into(), - value: RmwParameterValue { - type_: ParameterType::PARAMETER_BOOL, - bool_value: false, - ..Default::default() - }, - }; - let bool_parameter_mismatched = RmwParameter { - name: "bool".into(), - value: RmwParameterValue { - type_: ParameterType::PARAMETER_INTEGER, - integer_value: 42, - ..Default::default() - }, - }; - let read_only_parameter = RmwParameter { - name: "read_only".into(), - value: RmwParameterValue { - type_: ParameterType::PARAMETER_DOUBLE, - double_value: 3.45, - ..Default::default() - }, - }; - let dynamic_parameter = RmwParameter { - name: "dynamic".into(), - value: RmwParameterValue { - type_: ParameterType::PARAMETER_BOOL, - bool_value: true, - ..Default::default() - }, - }; - let out_of_range_parameter = RmwParameter { - name: "ns1.ns2.ns3.int".into(), - value: RmwParameterValue { - type_: ParameterType::PARAMETER_INTEGER, - integer_value: 1000, - ..Default::default() - }, - }; - let invalid_parameter_type = RmwParameter { - name: "dynamic".into(), - value: RmwParameterValue { - type_: 200, - integer_value: 1000, - ..Default::default() - }, - }; - let undeclared_bool = RmwParameter { - name: "undeclared_bool".into(), - value: RmwParameterValue { - type_: ParameterType::PARAMETER_BOOL, - bool_value: true, - ..Default::default() + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); + assert!(callback_ran.load(Ordering::Acquire)); + + // With set_parameters_atomically, if one fails all should fail + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let request = SetParametersAtomically_Request { + parameters: seq![bool_parameter, read_only_parameter], + }; + let promise = set_atomically_client + .call_then( + &request, + move |response: SetParametersAtomically_Response| { + assert!(!response.result.successful); + callback_ran_inner.store(true, Ordering::Release); }, - }; - let request = SetParameters_Request { - parameters: seq![ - bool_parameter.clone(), - read_only_parameter.clone(), - bool_parameter_mismatched, - dynamic_parameter, - out_of_range_parameter, - invalid_parameter_type, - undeclared_bool.clone() - ], - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - // Parameter is assigned a default of true at declaration time - assert!(node.bool_param.get()); - set_client - .async_send_request_with_callback( - &request, - move |response: SetParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.results.len(), 7); - // Setting a bool value set for a bool parameter - assert!(response.results[0].successful); - // Value was set to false, node parameter get should reflect this - assert!(!node.bool_param.get()); - // Setting a parameter to the wrong type - assert!(!response.results[1].successful); - // Setting a read only parameter - assert!(!response.results[2].successful); - // Setting a dynamic parameter to a new type - assert!(response.results[3].successful); - assert_eq!(node.dynamic_param.get(), ParameterValue::Bool(true)); - // Setting a value out of range - assert!(!response.results[4].successful); - // Setting an invalid type - assert!(!response.results[5].successful); - // Setting an undeclared parameter, without allowing undeclared parameters - assert!(!response.results[6].successful); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - // Set the node to use undeclared parameters and try to set one - node.node.use_undeclared_parameters(); - let request = SetParameters_Request { - parameters: seq![undeclared_bool], - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - set_client - .async_send_request_with_callback( - &request, - move |response: SetParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.results.len(), 1); - // Setting the undeclared parameter is now allowed - assert!(response.results[0].successful); - assert_eq!( - node.node.use_undeclared_parameters().get("undeclared_bool"), - Some(ParameterValue::Bool(true)) - ); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - // With set_parameters_atomically, if one fails all should fail - let request = SetParametersAtomically_Request { - parameters: seq![bool_parameter, read_only_parameter], - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - set_atomically_client - .async_send_request_with_callback( - &request, - move |response: SetParametersAtomically_Response| { - *call_done.write().unwrap() = true; - assert!(!response.result.successful); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - *done.write().unwrap() = true; - }); - - res.await.unwrap(); - rclrs_spin.await.unwrap(); + ) + .unwrap(); + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); + assert!(callback_ran.load(Ordering::Acquire)); Ok(()) } - #[tokio::test] - async fn test_describe_get_types_parameters_service() -> Result<(), RclrsError> { - let context = Context::new([]).unwrap(); - let (node, client) = construct_test_nodes(&context, "describe"); - let describe_client = - client.create_client::("/describe/node/describe_parameters")?; + #[test] + fn test_describe_get_types_parameters_service() -> Result<(), RclrsError> { + let (mut executor, _test, client_node) = construct_test_nodes("describe"); + let describe_client = client_node + .create_client::("/describe/node/describe_parameters")?; let get_types_client = - client.create_client::("/describe/node/get_parameter_types")?; + client_node.create_client::("/describe/node/get_parameter_types")?; - try_until_timeout(|| { - describe_client.service_is_ready().unwrap() - && get_types_client.service_is_ready().unwrap() - }) - .await - .unwrap(); - - let done = Arc::new(RwLock::new(false)); - - let inner_done = done.clone(); - let inner_node = node.node.clone(); - let rclrs_spin = tokio::task::spawn(async move { - try_until_timeout(|| { - crate::spin_once(inner_node.clone(), Some(std::time::Duration::ZERO)).ok(); - crate::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); - *inner_done.read().unwrap() + let describe_client_inner = Arc::clone(&describe_client); + let get_types_client_inner = Arc::clone(&get_types_client); + let clients_ready_condition = move || { + describe_client_inner.service_is_ready().unwrap() + && get_types_client_inner.service_is_ready().unwrap() + }; + + let promise = client_node + .notify_on_graph_change_with_period(Duration::from_millis(1), clients_ready_condition); + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); + + // Describe all parameters + let request = DescribeParameters_Request { + names: seq![ + "bool".into(), + "ns1.ns2.ns3.int".into(), + "read_only".into(), + "dynamic".into() + ], + }; + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let promise = describe_client + .call_then(&request, move |response: DescribeParameters_Response| { + let desc = response.descriptors; + assert_eq!(desc.len(), 4); + // Descriptors are returned in the requested order + assert_eq!(desc[0].name.to_string(), "bool"); + assert_eq!(desc[0].type_, ParameterType::PARAMETER_BOOL); + assert_eq!(desc[0].description.to_string(), "A boolean value"); + assert!(!desc[0].read_only); + assert!(!desc[0].dynamic_typing); + assert_eq!(desc[1].name.to_string(), "ns1.ns2.ns3.int"); + assert_eq!(desc[1].type_, ParameterType::PARAMETER_INTEGER); + assert_eq!(desc[1].integer_range.len(), 1); + assert_eq!(desc[1].integer_range[0].from_value, 0); + assert_eq!(desc[1].integer_range[0].to_value, 100); + assert_eq!(desc[1].integer_range[0].step, 0); + assert!(!desc[1].read_only); + assert!(!desc[1].dynamic_typing); + assert_eq!( + desc[1].additional_constraints.to_string(), + "Only the answer" + ); + assert_eq!(desc[2].name.to_string(), "read_only"); + assert_eq!(desc[2].type_, ParameterType::PARAMETER_DOUBLE); + assert!(desc[2].read_only); + assert!(!desc[2].dynamic_typing); + assert_eq!(desc[3].name.to_string(), "dynamic"); + assert_eq!(desc[3].type_, ParameterType::PARAMETER_STRING); + assert!(desc[3].dynamic_typing); + assert!(!desc[3].read_only); + callback_ran_inner.store(true, Ordering::Release); }) - .await - .unwrap(); - }); - - let res = tokio::task::spawn(async move { - // Describe all parameters - let request = DescribeParameters_Request { - names: seq![ - "bool".into(), - "ns1.ns2.ns3.int".into(), - "read_only".into(), - "dynamic".into() - ], - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - describe_client - .async_send_request_with_callback( - &request, - move |response: DescribeParameters_Response| { - *call_done.write().unwrap() = true; - let desc = response.descriptors; - assert_eq!(desc.len(), 4); - // Descriptors are returned in the requested order - assert_eq!(desc[0].name.to_string(), "bool"); - assert_eq!(desc[0].type_, ParameterType::PARAMETER_BOOL); - assert_eq!(desc[0].description.to_string(), "A boolean value"); - assert!(!desc[0].read_only); - assert!(!desc[0].dynamic_typing); - assert_eq!(desc[1].name.to_string(), "ns1.ns2.ns3.int"); - assert_eq!(desc[1].type_, ParameterType::PARAMETER_INTEGER); - assert_eq!(desc[1].integer_range.len(), 1); - assert_eq!(desc[1].integer_range[0].from_value, 0); - assert_eq!(desc[1].integer_range[0].to_value, 100); - assert_eq!(desc[1].integer_range[0].step, 0); - assert!(!desc[1].read_only); - assert!(!desc[1].dynamic_typing); - assert_eq!( - desc[1].additional_constraints.to_string(), - "Only the answer" - ); - assert_eq!(desc[2].name.to_string(), "read_only"); - assert_eq!(desc[2].type_, ParameterType::PARAMETER_DOUBLE); - assert!(desc[2].read_only); - assert!(!desc[2].dynamic_typing); - assert_eq!(desc[3].name.to_string(), "dynamic"); - assert_eq!(desc[3].type_, ParameterType::PARAMETER_STRING); - assert!(desc[3].dynamic_typing); - assert!(!desc[3].read_only); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - // If a describe parameters request is sent with a non existing parameter, an empty - // response should be returned - let request = DescribeParameters_Request { - names: seq!["bool".into(), "non_existing".into()], - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - describe_client - .async_send_request_with_callback( - &request, - move |response: DescribeParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.descriptors[0].name.to_string(), "bool"); - assert_eq!(response.descriptors[0].type_, ParameterType::PARAMETER_BOOL); - assert_eq!(response.descriptors.len(), 2); - assert_eq!(response.descriptors[1].name.to_string(), "non_existing"); - assert_eq!( - response.descriptors[1].type_, - ParameterType::PARAMETER_NOT_SET - ); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - // Get all parameter types, including a non existing one that will be NOT_SET - let request = GetParameterTypes_Request { - names: seq![ - "bool".into(), - "ns1.ns2.ns3.int".into(), - "read_only".into(), - "dynamic".into(), - "non_existing".into() - ], - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - get_types_client - .async_send_request_with_callback( - &request, - move |response: GetParameterTypes_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.types.len(), 5); - // Types are returned in the requested order - assert_eq!(response.types[0], ParameterType::PARAMETER_BOOL); - assert_eq!(response.types[1], ParameterType::PARAMETER_INTEGER); - assert_eq!(response.types[2], ParameterType::PARAMETER_DOUBLE); - assert_eq!(response.types[3], ParameterType::PARAMETER_STRING); - assert_eq!(response.types[4], ParameterType::PARAMETER_NOT_SET); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - *done.write().unwrap() = true; - }); - - res.await.unwrap(); - rclrs_spin.await.unwrap(); + .unwrap(); + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); + assert!(callback_ran.load(Ordering::Acquire)); + + // If a describe parameters request is sent with a non existing parameter, an empty + // response should be returned + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let request = DescribeParameters_Request { + names: seq!["bool".into(), "non_existing".into()], + }; + let promise = describe_client + .call_then(&request, move |response: DescribeParameters_Response| { + assert_eq!(response.descriptors[0].name.to_string(), "bool"); + assert_eq!(response.descriptors[0].type_, ParameterType::PARAMETER_BOOL); + assert_eq!(response.descriptors.len(), 2); + assert_eq!(response.descriptors[1].name.to_string(), "non_existing"); + assert_eq!( + response.descriptors[1].type_, + ParameterType::PARAMETER_NOT_SET + ); + callback_ran_inner.store(true, Ordering::Release); + }) + .unwrap(); + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); + assert!(callback_ran.load(Ordering::Acquire)); + + // Get all parameter types, including a non existing one that will be NOT_SET + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let request = GetParameterTypes_Request { + names: seq![ + "bool".into(), + "ns1.ns2.ns3.int".into(), + "read_only".into(), + "dynamic".into(), + "non_existing".into() + ], + }; + let promise = get_types_client + .call_then(&request, move |response: GetParameterTypes_Response| { + assert_eq!(response.types.len(), 5); + // Types are returned in the requested order + assert_eq!(response.types[0], ParameterType::PARAMETER_BOOL); + assert_eq!(response.types[1], ParameterType::PARAMETER_INTEGER); + assert_eq!(response.types[2], ParameterType::PARAMETER_DOUBLE); + assert_eq!(response.types[3], ParameterType::PARAMETER_STRING); + assert_eq!(response.types[4], ParameterType::PARAMETER_NOT_SET); + callback_ran_inner.store(true, Ordering::Release); + }) + .unwrap(); + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); + assert!(callback_ran.load(Ordering::Acquire)); Ok(()) } diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index 82fe31ebb..ff0c86c46 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -537,7 +537,7 @@ impl ParameterValue { #[cfg(test)] mod tests { use super::*; - use crate::{Context, RclrsError, ToResult}; + use crate::{Context, InitOptions, RclrsError, ToResult}; // TODO(luca) tests for all from / to ParameterVariant functions @@ -565,11 +565,14 @@ mod tests { ), ]; for pair in input_output_pairs { - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - format!("foo:={}", pair.0), - ])?; + let ctx = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + format!("foo:={}", pair.0), + ], + InitOptions::default(), + )?; let mut rcl_params = std::ptr::null_mut(); unsafe { rcl_arguments_get_param_overrides( diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index b1cdd93b9..967cb9c62 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -11,7 +11,7 @@ use crate::{ error::{RclrsError, ToResult}, qos::QoSProfile, rcl_bindings::*, - NodeHandle, ENTITY_LIFECYCLE_MUTEX, + IntoPrimitiveOptions, NodeHandle, ENTITY_LIFECYCLE_MUTEX, }; mod loaned_message; @@ -45,15 +45,32 @@ impl Drop for PublisherHandle { /// Struct for sending messages of type `T`. /// +/// Create a publisher using [`Node::create_publisher`][1]. +/// /// Multiple publishers can be created for the same topic, in different nodes or the same node. +/// A clone of a `Publisher` will refer to the same publisher instance as the original. +/// The underlying instance is tied to [`PublisherState`] which implements the [`Publisher`] API. /// /// The underlying RMW will decide on the concrete delivery mechanism (network stack, shared /// memory, or intraprocess). /// -/// Sending messages does not require calling [`spin`][1] on the publisher's node. +/// Sending messages does not require the node's executor to [spin][2]. +/// +/// [1]: crate::NodeState::create_publisher +/// [2]: crate::Executor::spin +pub type Publisher = Arc>; + +/// The inner state of a [`Publisher`]. +/// +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Publisher`] in a non-owning way. It is +/// generally recommended to manage the `PublisherState` inside of an [`Arc`], +/// and [`Publisher`] is provided as a convenience alias for that. +/// +/// The public API of the [`Publisher`] type is implemented via `PublisherState`. /// -/// [1]: crate::spin -pub struct Publisher +/// [1]: std::sync::Weak +pub struct PublisherState where T: Message, { @@ -66,26 +83,26 @@ where // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread // they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for Publisher where T: Message {} +unsafe impl Send for PublisherState where T: Message {} // SAFETY: The type_support_ptr prevents the default Sync impl. // rosidl_message_type_support_t is a read-only type without interior mutability. -unsafe impl Sync for Publisher where T: Message {} +unsafe impl Sync for PublisherState where T: Message {} -impl Publisher +impl PublisherState where T: Message, { /// Creates a new `Publisher`. /// /// Node and namespace changes are always applied _before_ topic remapping. - pub(crate) fn new( + pub(crate) fn create<'a>( node_handle: Arc, - topic: &str, - qos: QoSProfile, - ) -> Result + options: impl Into>, + ) -> Result, RclrsError> where T: Message, { + let PublisherOptions { topic, qos } = options.into(); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_publisher = unsafe { rcl_get_zero_initialized_publisher() }; let type_support_ptr = @@ -120,14 +137,14 @@ where } } - Ok(Self { + Ok(Arc::new(Self { type_support_ptr, message: PhantomData, handle: PublisherHandle { rcl_publisher: Mutex::new(rcl_publisher), node_handle, }, - }) + })) } /// Returns the topic name of the publisher. @@ -179,7 +196,7 @@ where } } -impl Publisher +impl PublisherState where T: RmwMessage, { @@ -236,7 +253,39 @@ where } } -/// Convenience trait for [`Publisher::publish`]. +/// `PublisherOptions` are used by [`Node::create_publisher`][1] to initialize +/// a [`Publisher`]. +/// +/// [1]: crate::Node::create_publisher +#[derive(Debug, Clone)] +#[non_exhaustive] +pub struct PublisherOptions<'a> { + /// The topic name for the publisher. + pub topic: &'a str, + /// The quality of service settings for the publisher. + pub qos: QoSProfile, +} + +impl<'a> PublisherOptions<'a> { + /// Initialize a new [`PublisherOptions`] with default settings. + pub fn new(topic: &'a str) -> Self { + Self { + topic, + qos: QoSProfile::topics_default(), + } + } +} + +impl<'a, T: IntoPrimitiveOptions<'a>> From for PublisherOptions<'a> { + fn from(value: T) -> Self { + let primitive = value.into_primitive_options(); + let mut options = Self::new(primitive.name); + primitive.apply(&mut options.qos); + options + } +} + +/// Convenience trait for [`PublisherState::publish`]. pub trait MessageCow<'a, T: Message> { /// Wrap the owned or borrowed message in a `Cow`. fn into_cow(self) -> Cow<'a, T>; @@ -267,7 +316,7 @@ mod tests { #[test] fn test_publishers() -> Result<(), RclrsError> { - use crate::{TopicEndpointInfo, QOS_PROFILE_SYSTEM_DEFAULT}; + use crate::TopicEndpointInfo; use test_msgs::msg; let namespace = "/test_publishers_graph"; @@ -275,16 +324,15 @@ mod tests { let node_1_empty_publisher = graph .node1 - .create_publisher::("graph_test_topic_1", QOS_PROFILE_SYSTEM_DEFAULT)?; + .create_publisher::("graph_test_topic_1")?; let topic1 = node_1_empty_publisher.topic_name(); - let node_1_basic_types_publisher = graph.node1.create_publisher::( - "graph_test_topic_2", - QOS_PROFILE_SYSTEM_DEFAULT, - )?; + let node_1_basic_types_publisher = graph + .node1 + .create_publisher::("graph_test_topic_2")?; let topic2 = node_1_basic_types_publisher.topic_name(); let node_2_default_publisher = graph .node2 - .create_publisher::("graph_test_topic_3", QOS_PROFILE_SYSTEM_DEFAULT)?; + .create_publisher::("graph_test_topic_3")?; let topic3 = node_2_default_publisher.topic_name(); std::thread::sleep(std::time::Duration::from_millis(100)); diff --git a/rclrs/src/publisher/loaned_message.rs b/rclrs/src/publisher/loaned_message.rs index 7d29122dc..924e7d21e 100644 --- a/rclrs/src/publisher/loaned_message.rs +++ b/rclrs/src/publisher/loaned_message.rs @@ -2,13 +2,13 @@ use std::ops::{Deref, DerefMut}; use rosidl_runtime_rs::RmwMessage; -use crate::{rcl_bindings::*, Publisher, RclrsError, ToResult}; +use crate::{rcl_bindings::*, PublisherState, RclrsError, ToResult}; /// A message that is owned by the middleware, loaned for publishing. /// /// It dereferences to a `&mut T`. /// -/// This type is returned by [`Publisher::borrow_loaned_message()`], see the documentation of +/// This type is returned by [`PublisherState::borrow_loaned_message()`], see the documentation of /// that function for more information. /// /// The loan is returned by dropping the message or [publishing it][1]. @@ -19,7 +19,7 @@ where T: RmwMessage, { pub(super) msg_ptr: *mut T, - pub(super) publisher: &'a Publisher, + pub(super) publisher: &'a PublisherState, } impl Deref for LoanedMessage<'_, T> diff --git a/rclrs/src/qos.rs b/rclrs/src/qos.rs index b26f01ef8..699576964 100644 --- a/rclrs/src/qos.rs +++ b/rclrs/src/qos.rs @@ -166,7 +166,7 @@ pub struct QoSProfile { /// The time within which the RMW publisher must show that it is alive. /// /// If this is `Infinite`, liveliness is not enforced. - pub liveliness_lease_duration: QoSDuration, + pub liveliness_lease: QoSDuration, /// If true, any ROS specific namespacing conventions will be circumvented. /// /// In the case of DDS and topics, for example, this means the typical @@ -200,7 +200,7 @@ impl From for rmw_qos_profile_t { deadline: qos.deadline.into(), lifespan: qos.lifespan.into(), liveliness: qos.liveliness.into(), - liveliness_lease_duration: qos.liveliness_lease_duration.into(), + liveliness_lease_duration: qos.liveliness_lease.into(), avoid_ros_namespace_conventions: qos.avoid_ros_namespace_conventions, } } @@ -244,22 +244,54 @@ impl QoSProfile { } /// Sets the QoS profile deadline to the specified `Duration`. - pub fn deadline(mut self, deadline: Duration) -> Self { + pub fn deadline_duration(mut self, deadline: Duration) -> Self { self.deadline = QoSDuration::Custom(deadline); self } /// Sets the QoS profile liveliness lease duration to the specified `Duration`. pub fn liveliness_lease_duration(mut self, lease_duration: Duration) -> Self { - self.liveliness_lease_duration = QoSDuration::Custom(lease_duration); + self.liveliness_lease = QoSDuration::Custom(lease_duration); self } /// Sets the QoS profile lifespan to the specified `Duration`. - pub fn lifespan(mut self, lifespan: Duration) -> Self { + pub fn lifespan_duration(mut self, lifespan: Duration) -> Self { self.lifespan = QoSDuration::Custom(lifespan); self } + + /// Get the default QoS profile for ordinary topics. + pub fn topics_default() -> Self { + QOS_PROFILE_DEFAULT + } + + /// Get the default QoS profile for topics that transmit sensor data. + pub fn sensor_data_default() -> Self { + QOS_PROFILE_SENSOR_DATA + } + + /// Get the default QoS profile for services. + pub fn services_default() -> Self { + QOS_PROFILE_SERVICES_DEFAULT + } + + /// Get the default QoS profile for parameter services. + pub fn parameter_services_default() -> Self { + QOS_PROFILE_PARAMETERS + } + + /// Get the default QoS profile for parameter event topics. + pub fn parameter_events_default() -> Self { + QOS_PROFILE_PARAMETER_EVENTS + } + + /// Get the system-defined default quality of service profile. This profile + /// is determined by the underlying RMW implementation, so you cannot rely + /// on this profile being consistent or appropriate for your needs. + pub fn system_default() -> Self { + QOS_PROFILE_SYSTEM_DEFAULT + } } impl From for rmw_qos_history_policy_t { @@ -355,7 +387,7 @@ pub const QOS_PROFILE_SENSOR_DATA: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -370,7 +402,7 @@ pub const QOS_PROFILE_CLOCK: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -384,7 +416,7 @@ pub const QOS_PROFILE_PARAMETERS: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -398,7 +430,7 @@ pub const QOS_PROFILE_DEFAULT: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -412,7 +444,7 @@ pub const QOS_PROFILE_SERVICES_DEFAULT: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -426,7 +458,7 @@ pub const QOS_PROFILE_PARAMETER_EVENTS: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -440,6 +472,6 @@ pub const QOS_PROFILE_SYSTEM_DEFAULT: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index ac43e51a8..9430c0383 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -1,107 +1,129 @@ use std::{ boxed::Box, - ffi::CString, - sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, + ffi::{CStr, CString}, + sync::{Arc, Mutex, MutexGuard}, }; -use rosidl_runtime_rs::Message; - use crate::{ - error::{RclReturnCode, ToResult}, - rcl_bindings::*, - MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + error::ToResult, rcl_bindings::*, ExecutorCommands, IntoPrimitiveOptions, NodeHandle, + QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, Waitable, + WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, }; -// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread -// they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for rcl_service_t {} +mod any_service_callback; +pub use any_service_callback::*; -/// Manage the lifecycle of an `rcl_service_t`, including managing its dependencies -/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are -/// [dropped after][1] the `rcl_service_t`. -/// -/// [1]: -pub struct ServiceHandle { - rcl_service: Mutex, - node_handle: Arc, - pub(crate) in_use_by_wait_set: Arc, -} +mod service_async_callback; +pub use service_async_callback::*; -impl ServiceHandle { - pub(crate) fn lock(&self) -> MutexGuard { - self.rcl_service.lock().unwrap() - } -} +mod service_callback; +pub use service_callback::*; -impl Drop for ServiceHandle { - fn drop(&mut self) { - let rcl_service = self.rcl_service.get_mut().unwrap(); - let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); - let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); - // SAFETY: The entity lifecycle mutex is locked to protect against the risk of - // global variables in the rmw implementation being unsafely modified during cleanup. - unsafe { - rcl_service_fini(rcl_service, &mut *rcl_node); - } - } -} +mod service_info; +pub use service_info::*; -/// Trait to be implemented by concrete Service structs. +/// Provide a service that can respond to requests sent by ROS service clients. /// -/// See [`Service`] for an example -pub trait ServiceBase: Send + Sync { - /// Internal function to get a reference to the `rcl` handle. - fn handle(&self) -> &ServiceHandle; - /// Tries to take a new request and run the callback with it. - fn execute(&self) -> Result<(), RclrsError>; -} - -type ServiceCallback = - Box Response + 'static + Send>; - -/// Main class responsible for responding to requests sent by ROS clients. +/// Create a service using [`Node::create_service`][1] +/// or [`Node::create_async_service`][2]. /// -/// The only available way to instantiate services is via [`Node::create_service()`][1], this is to -/// ensure that [`Node`][2]s can track all the services that have been created. +/// ROS only supports having one service provider for any given fully-qualified +/// service name. "Fully-qualified" means the namespace is also taken into account +/// for uniqueness. A clone of a `Service` will refer to the same service provider +/// instance as the original. The underlying instance is tied to [`ServiceState`] +/// which implements the [`Service`] API. +/// +/// Responding to requests requires the node's executor to [spin][3]. /// /// [1]: crate::Node::create_service -/// [2]: crate::Node -pub struct Service +/// [2]: crate::Node::create_async_service +/// [3]: crate::Executor::spin +/// +pub type Service = Arc>; + +/// The inner state of a [`Service`]. +/// +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Service`] in a non-owning way. It is +/// generally recommended to manage the `ServiceState` inside of an [`Arc`], +/// and [`Service`] is provided as a convenience alias for that. +/// +/// The public API of the [`Service`] type is implemented via `ServiceState`. +/// +/// [1]: std::sync::Weak +pub struct ServiceState where T: rosidl_runtime_rs::Service, { - pub(crate) handle: Arc, - /// The callback function that runs when a request was received. - pub callback: Mutex>, + /// This handle is used to access the data that rcl holds for this service. + handle: Arc, + /// This is the callback that will be executed each time a request arrives. + callback: Arc>>, + /// Holding onto this keeps the waiter for this service alive in the wait + /// set of the executor. + #[allow(unused)] + lifecycle: WaitableLifecycle, } -impl Service +impl ServiceState where T: rosidl_runtime_rs::Service, { - /// Creates a new service. - pub(crate) fn new( - node_handle: Arc, - topic: &str, - callback: F, - ) -> Result - // This uses pub(crate) visibility to avoid instantiating this struct outside - // [`Node::create_service`], see the struct's documentation for the rationale - where - T: rosidl_runtime_rs::Service, - F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send, - { + /// Returns the name of the service. + /// + /// This returns the service name after remapping, so it is not necessarily the + /// service name which was used when creating the service. + pub fn service_name(&self) -> String { + // SAFETY: The service handle is valid because its lifecycle is managed by an Arc. + // The unsafe variables get converted to safe types before being returned + unsafe { + let raw_service_pointer = rcl_service_get_service_name(&*self.handle.lock()); + CStr::from_ptr(raw_service_pointer) + } + .to_string_lossy() + .into_owned() + } + + /// Set the callback of this service, replacing the callback that was + /// previously set. + /// + /// This can be used even if the service previously used an async callback. + pub fn set_callback(&self, callback: impl ServiceCallback) { + let callback = callback.into_service_callback(); + // TODO(@mxgrey): Log any errors here when logging becomes available + *self.callback.lock().unwrap() = callback; + } + + /// Set the callback of this service, replacing the callback that was + /// previously set. + /// + /// This can be used even if the service previously used a non-async callback. + pub fn set_async_callback(&self, callback: impl ServiceAsyncCallback) { + let callback = callback.into_service_async_callback(); + *self.callback.lock().unwrap() = callback; + } + + /// Used by [`Node`][crate::Node] to create a new service + pub(crate) fn create<'a>( + options: impl Into>, + callback: AnyServiceCallback, + node_handle: &Arc, + commands: &Arc, + ) -> Result, RclrsError> { + let ServiceOptions { name, qos } = options.into(); + let callback = Arc::new(Mutex::new(callback)); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_service = unsafe { rcl_get_zero_initialized_service() }; let type_support = ::get_type_support() as *const rosidl_service_type_support_t; - let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { + let topic_c_string = CString::new(name).map_err(|err| RclrsError::StringContainsNul { err, - s: topic.into(), + s: name.into(), })?; // SAFETY: No preconditions for this function. - let service_options = unsafe { rcl_service_get_default_options() }; + let mut service_options = unsafe { rcl_service_get_default_options() }; + service_options.qos = qos.into(); { let rcl_node = node_handle.rcl_node.lock().unwrap(); @@ -127,93 +149,121 @@ where let handle = Arc::new(ServiceHandle { rcl_service: Mutex::new(rcl_service), - node_handle, - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + node_handle: Arc::clone(&node_handle), }); - Ok(Self { + let (waitable, lifecycle) = Waitable::new( + Box::new(ServiceExecutable { + handle: Arc::clone(&handle), + callback: Arc::clone(&callback), + commands: Arc::clone(&commands), + }), + Some(Arc::clone(commands.get_guard_condition())), + ); + + let service = Arc::new(Self { handle, - callback: Mutex::new(Box::new(callback)), - }) + callback, + lifecycle, + }); + commands.add_to_wait_set(waitable); + + Ok(service) } +} - /// Fetches a new request. - /// - /// When there is no new message, this will return a - /// [`ServiceTakeFailed`][1]. - /// - /// [1]: crate::RclrsError - // - // ```text - // +---------------------+ - // | rclrs::take_request | - // +----------+----------+ - // | - // | - // +----------v----------+ - // | rcl_take_request | - // +----------+----------+ - // | - // | - // +----------v----------+ - // | rmw_take | - // +---------------------+ - // ``` - pub fn take_request(&self) -> Result<(T::Request, rmw_request_id_t), RclrsError> { - let mut request_id_out = rmw_request_id_t { - writer_guid: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], - sequence_number: 0, - }; - type RmwMsg = - <::Request as rosidl_runtime_rs::Message>::RmwMsg; - let mut request_out = RmwMsg::::default(); - let handle = &*self.handle.lock(); - unsafe { - // SAFETY: The three pointers are valid/initialized - rcl_take_request( - handle, - &mut request_id_out, - &mut request_out as *mut RmwMsg as *mut _, - ) +/// `ServiceOptions are used by [`Node::create_service`][1] to initialize a +/// [`Service`] provider. +#[derive(Debug, Clone)] +#[non_exhaustive] +pub struct ServiceOptions<'a> { + /// The name for the service + pub name: &'a str, + /// The quality of service profile for the service. + pub qos: QoSProfile, +} + +impl<'a> ServiceOptions<'a> { + /// Initialize a new [`ServiceOptions`] with default settings. + pub fn new(name: &'a str) -> Self { + Self { + name, + qos: QoSProfile::services_default(), } - .ok()?; - Ok((T::Request::from_rmw_message(request_out), request_id_out)) } } -impl ServiceBase for Service +impl<'a, T: IntoPrimitiveOptions<'a>> From for ServiceOptions<'a> { + fn from(value: T) -> Self { + let primitive = value.into_primitive_options(); + let mut options = Self::new(primitive.name); + primitive.apply(&mut options.qos); + options + } +} + +struct ServiceExecutable { + handle: Arc, + callback: Arc>>, + commands: Arc, +} + +impl RclPrimitive for ServiceExecutable where T: rosidl_runtime_rs::Service, { - fn handle(&self) -> &ServiceHandle { - &self.handle + fn execute(&mut self) -> Result<(), RclrsError> { + if let Err(err) = self + .callback + .lock() + .unwrap() + .execute(&self.handle, &self.commands) + { + // TODO(@mxgrey): Log the error here once logging is implemented + eprintln!("Error while executing a service callback: {err}"); + } + Ok(()) } - fn execute(&self) -> Result<(), RclrsError> { - let (req, mut req_id) = match self.take_request() { - Ok((req, req_id)) => (req, req_id), - Err(RclrsError::RclError { - code: RclReturnCode::ServiceTakeFailed, - .. - }) => { - // Spurious wakeup – this may happen even when a waitset indicated that this - // service was ready, so it shouldn't be an error. - return Ok(()); - } - Err(e) => return Err(e), - }; - let res = (*self.callback.lock().unwrap())(&req_id, req); - let rmw_message = ::into_rmw_message(res.into_cow()); - let handle = &*self.handle.lock(); + fn kind(&self) -> crate::RclPrimitiveKind { + RclPrimitiveKind::Service + } + + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::Service(self.handle.lock()) + } +} + +// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread +// they are running in. Therefore, this type can be safely sent to another thread. +unsafe impl Send for rcl_service_t {} + +/// Manage the lifecycle of an `rcl_service_t`, including managing its dependencies +/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are +/// [dropped after][1] the `rcl_service_t`. +/// +/// [1]: +pub struct ServiceHandle { + rcl_service: Mutex, + node_handle: Arc, +} + +impl ServiceHandle { + pub(crate) fn lock(&self) -> MutexGuard { + self.rcl_service.lock().unwrap() + } +} + +impl Drop for ServiceHandle { + fn drop(&mut self) { + let rcl_service = self.rcl_service.get_mut().unwrap(); + let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of + // global variables in the rmw implementation being unsafely modified during cleanup. unsafe { - // SAFETY: The response type is guaranteed to match the service type by the type system. - rcl_send_response( - handle, - &mut req_id, - rmw_message.as_ref() as *const ::RmwMsg as *mut _, - ) + rcl_service_fini(rcl_service, &mut *rcl_node); } - .ok() } } @@ -242,14 +292,12 @@ mod tests { assert!(types.contains(&"test_msgs/srv/Empty".to_string())); }; - let _node_1_empty_service = - graph - .node1 - .create_service::("graph_test_topic_4", |_, _| { - srv::Empty_Response { - structure_needs_at_least_one_member: 0, - } - })?; + let _node_1_empty_service = graph.node1.create_service::( + "graph_test_topic_4", + |_: srv::Empty_Request| srv::Empty_Response { + structure_needs_at_least_one_member: 0, + }, + )?; let _node_2_empty_client = graph .node2 .create_client::("graph_test_topic_4")?; diff --git a/rclrs/src/service/any_service_callback.rs b/rclrs/src/service/any_service_callback.rs new file mode 100644 index 000000000..f2cc4a021 --- /dev/null +++ b/rclrs/src/service/any_service_callback.rs @@ -0,0 +1,169 @@ +use rosidl_runtime_rs::{Message, Service}; + +use crate::{ + error::ToResult, + rcl_bindings::{ + rcl_send_response, rcl_take_request, rcl_take_request_with_info, rmw_request_id_t, + rmw_service_info_t, + }, + ExecutorCommands, MessageCow, RclReturnCode, RclrsError, RequestId, ServiceHandle, ServiceInfo, +}; + +use futures::future::BoxFuture; + +use std::sync::Arc; + +/// An enum capturing the various possible function signatures for service callbacks. +pub enum AnyServiceCallback +where + T: Service, +{ + /// A callback that only takes in the request value + OnlyRequest(Box BoxFuture<'static, T::Response> + Send>), + /// A callback that takes in the request value and the ID of the request + WithId(Box BoxFuture<'static, T::Response> + Send>), + /// A callback that takes in the request value and all available + WithInfo(Box BoxFuture<'static, T::Response> + Send>), +} + +impl AnyServiceCallback { + pub(super) fn execute( + &mut self, + handle: &Arc, + commands: &Arc, + ) -> Result<(), RclrsError> { + let mut evaluate = || { + match self { + AnyServiceCallback::OnlyRequest(cb) => { + let (msg, mut rmw_request_id) = Self::take_request(handle)?; + let handle = Arc::clone(&handle); + let response = cb(msg); + let _ = commands.run(async move { + if let Err(err) = Self::send_response(&handle, &mut rmw_request_id, response.await) { + // TODO(@mxgrey): Use logging instead when it becomes available + eprintln!("Error while sending service response for {rmw_request_id:?}: {err}"); + } + }); + } + AnyServiceCallback::WithId(cb) => { + let (msg, mut rmw_request_id) = Self::take_request(handle)?; + let request_id = RequestId::from_rmw_request_id(&rmw_request_id); + let handle = Arc::clone(&handle); + let response = cb(msg, request_id); + let _ = commands.run(async move { + if let Err(err) = Self::send_response(&handle, &mut rmw_request_id, response.await) { + // TODO(@mxgrey): Use logging instead when it becomes available + eprintln!("Error while sending service response for {rmw_request_id:?}: {err}"); + } + }); + } + AnyServiceCallback::WithInfo(cb) => { + let (msg, rmw_service_info) = Self::take_request_with_info(handle)?; + let mut rmw_request_id = rmw_request_id_t { + writer_guid: rmw_service_info.request_id.writer_guid, + sequence_number: rmw_service_info.request_id.sequence_number, + }; + let service_info = ServiceInfo::from_rmw_service_info(&rmw_service_info); + let handle = Arc::clone(&handle); + let response = cb(msg, service_info); + let _ = commands.run(async move { + if let Err(err) = Self::send_response(&handle, &mut rmw_request_id, response.await) { + // TODO(@mxgrey): Use logging instead when it becomes available + eprintln!("Error while sending service response for {rmw_request_id:?}: {err}"); + } + }); + } + } + + Ok(()) + }; + + match evaluate() { + Err(RclrsError::RclError { + code: RclReturnCode::ServiceTakeFailed, + .. + }) => { + // Spurious wakeup - this may happen even when a waitlist indicated that this + // subscription was ready, so it shouldn't be an error. + Ok(()) + } + other => other, + } + } + + /// Fetches a new request. + /// + /// When there is no new message, this will return a + /// [`ServiceTakeFailed`][1]. + /// + /// [1]: crate::RclrsError + // + // ```text + // +---------------------+ + // | rclrs::take_request | + // +----------+----------+ + // | + // | + // +----------v----------+ + // | rcl_take_request | + // +----------+----------+ + // | + // | + // +----------v----------+ + // | rmw_take | + // +---------------------+ + // ``` + fn take_request(handle: &ServiceHandle) -> Result<(T::Request, rmw_request_id_t), RclrsError> { + let mut request_id_out = RequestId::zero_initialized_rmw(); + type RmwMsg = <::Request as Message>::RmwMsg; + let mut request_out = RmwMsg::::default(); + let handle = &*handle.lock(); + unsafe { + // SAFETY: The three pointers are valid and initialized + rcl_take_request( + handle, + &mut request_id_out, + &mut request_out as *mut RmwMsg as *mut _, + ) + } + .ok()?; + Ok((T::Request::from_rmw_message(request_out), request_id_out)) + } + + /// Same as [`Self::take_request`] but includes additional info about the service + fn take_request_with_info( + handle: &ServiceHandle, + ) -> Result<(T::Request, rmw_service_info_t), RclrsError> { + let mut service_info_out = ServiceInfo::zero_initialized_rmw(); + type RmwMsg = <::Request as Message>::RmwMsg; + let mut request_out = RmwMsg::::default(); + let handle = &*handle.lock(); + unsafe { + // SAFETY: The three pointers are valid and initialized + rcl_take_request_with_info( + handle, + &mut service_info_out, + &mut request_out as *mut RmwMsg as *mut _, + ) + } + .ok()?; + Ok((T::Request::from_rmw_message(request_out), service_info_out)) + } + + fn send_response( + handle: &Arc, + request_id: &mut rmw_request_id_t, + response: T::Response, + ) -> Result<(), RclrsError> { + let rmw_message = ::into_rmw_message(response.into_cow()); + let handle = &*handle.lock(); + unsafe { + rcl_send_response( + handle, + request_id, + rmw_message.as_ref() as *const ::RmwMsg as *mut _, + ) + } + .ok() + } +} diff --git a/rclrs/src/service/service_async_callback.rs b/rclrs/src/service/service_async_callback.rs new file mode 100644 index 000000000..a609a25eb --- /dev/null +++ b/rclrs/src/service/service_async_callback.rs @@ -0,0 +1,55 @@ +use rosidl_runtime_rs::Service; + +use super::{any_service_callback::AnyServiceCallback, RequestId, ServiceInfo}; + +use std::future::Future; + +/// A trait for async callbacks of services. +/// +// TODO(@mxgrey): Add a description of what callbacks signatures are supported +pub trait ServiceAsyncCallback: Send + 'static +where + T: Service, +{ + /// Converts the callback into an enum. + /// + /// User code never needs to call this function. + fn into_service_async_callback(self) -> AnyServiceCallback; +} + +impl ServiceAsyncCallback for Func +where + T: Service, + Func: FnMut(T::Request) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_service_async_callback(mut self) -> AnyServiceCallback { + AnyServiceCallback::OnlyRequest(Box::new(move |request| Box::pin(self(request)))) + } +} + +impl ServiceAsyncCallback for Func +where + T: Service, + Func: FnMut(T::Request, RequestId) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_service_async_callback(mut self) -> AnyServiceCallback { + AnyServiceCallback::WithId(Box::new(move |request, request_id| { + Box::pin(self(request, request_id)) + })) + } +} + +impl ServiceAsyncCallback for Func +where + T: Service, + Func: FnMut(T::Request, ServiceInfo) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_service_async_callback(mut self) -> AnyServiceCallback { + AnyServiceCallback::WithInfo(Box::new(move |request, service_info| { + Box::pin(self(request, service_info)) + })) + } +} diff --git a/rclrs/src/service/service_callback.rs b/rclrs/src/service/service_callback.rs new file mode 100644 index 000000000..8d28a36a2 --- /dev/null +++ b/rclrs/src/service/service_callback.rs @@ -0,0 +1,62 @@ +use rosidl_runtime_rs::Service; + +use crate::{service::any_service_callback::AnyServiceCallback, RequestId, ServiceInfo}; + +use std::sync::Arc; + +/// A trait to deduce regular callbacks of services. +/// +/// Users of rclrs never need to use this trait directly. +// +// TODO(@mxgrey): Add a description of what callbacks signatures are supported +pub trait ServiceCallback: Send + 'static +where + T: Service, +{ + /// Converts the callback into an enum. + /// + /// User code never needs to call this function. + fn into_service_callback(self) -> AnyServiceCallback; +} + +impl ServiceCallback for Func +where + T: Service, + Func: Fn(T::Request) -> T::Response + Send + Sync + 'static, +{ + fn into_service_callback(self) -> AnyServiceCallback { + let func = Arc::new(self); + AnyServiceCallback::OnlyRequest(Box::new(move |request| { + let f = Arc::clone(&func); + Box::pin(async move { f(request) }) + })) + } +} + +impl ServiceCallback for Func +where + T: Service, + Func: Fn(T::Request, RequestId) -> T::Response + Send + Sync + 'static, +{ + fn into_service_callback(self) -> AnyServiceCallback { + let func = Arc::new(self); + AnyServiceCallback::WithId(Box::new(move |request, request_id| { + let f = Arc::clone(&func); + Box::pin(async move { f(request, request_id) }) + })) + } +} + +impl ServiceCallback for Func +where + T: Service, + Func: Fn(T::Request, ServiceInfo) -> T::Response + Send + Sync + 'static, +{ + fn into_service_callback(self) -> AnyServiceCallback { + let func = Arc::new(self); + AnyServiceCallback::WithInfo(Box::new(move |request, service_info| { + let f = Arc::clone(&func); + Box::pin(async move { f(request, service_info) }) + })) + } +} diff --git a/rclrs/src/service/service_info.rs b/rclrs/src/service/service_info.rs new file mode 100644 index 000000000..5a56d5079 --- /dev/null +++ b/rclrs/src/service/service_info.rs @@ -0,0 +1,69 @@ +use std::time::SystemTime; + +use crate::{rcl_bindings::*, timestamp_to_system_time}; + +/// Information about an incoming service request. +#[derive(Debug, Clone, Hash, PartialEq, Eq, PartialOrd, Ord)] +pub struct ServiceInfo { + /// Time when the message was published by the publisher. + /// + /// The `rmw` layer does not specify the exact point at which the RMW implementation + /// must take the timestamp, but it should be taken consistently at the same point in the + /// process of publishing a message. + pub source_timestamp: Option, + /// Time when the message was received by the service node. + /// + /// The `rmw` layer does not specify the exact point at which the RMW implementation + /// must take the timestamp, but it should be taken consistently at the same point in the + /// process of receiving a message. + pub received_timestamp: Option, + /// Unique identifier for the request. + pub request_id: RequestId, +} + +impl ServiceInfo { + pub(crate) fn from_rmw_service_info(rmw_service_info: &rmw_service_info_t) -> Self { + Self { + source_timestamp: timestamp_to_system_time(rmw_service_info.source_timestamp), + received_timestamp: timestamp_to_system_time(rmw_service_info.received_timestamp), + request_id: RequestId::from_rmw_request_id(&rmw_service_info.request_id), + } + } + + pub(crate) fn zero_initialized_rmw() -> rmw_service_info_t { + rmw_service_info_t { + source_timestamp: 0, + received_timestamp: 0, + request_id: RequestId::zero_initialized_rmw(), + } + } +} + +/// Unique identifier for a service request. +/// +/// Individually each field in the `RequestId` may be repeated across different +/// requests, but the combination of both values will be unique per request. +#[derive(Debug, Clone, Hash, PartialEq, Eq, PartialOrd, Ord)] +pub struct RequestId { + /// A globally unique identifier for the writer of the request. + pub writer_guid: [i8; 16usize], + /// A number assigned to the request which is unique for the writer who + /// wrote the request. + pub sequence_number: i64, +} + +impl RequestId { + pub(crate) fn from_rmw_request_id(rmw_request_id: &rmw_request_id_t) -> Self { + Self { + writer_guid: rmw_request_id.writer_guid, + sequence_number: rmw_request_id.sequence_number, + } + } + + pub(crate) fn zero_initialized_rmw() -> rmw_request_id_t { + rmw_request_id_t { + writer_guid: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + sequence_number: 0, + } + } +} diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index fbd518c21..9d639acbb 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -1,109 +1,125 @@ use std::{ ffi::{CStr, CString}, - marker::PhantomData, - sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, + sync::{Arc, Mutex, MutexGuard}, }; use rosidl_runtime_rs::{Message, RmwMessage}; use crate::{ - error::{RclReturnCode, ToResult}, - qos::QoSProfile, - rcl_bindings::*, - NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + error::ToResult, qos::QoSProfile, rcl_bindings::*, ExecutorCommands, IntoPrimitiveOptions, + NodeHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, Waitable, + WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, }; -mod callback; -mod message_info; -mod readonly_loaned_message; -pub use callback::*; -pub use message_info::*; -pub use readonly_loaned_message::*; - -// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread -// they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for rcl_subscription_t {} +mod any_subscription_callback; +pub use any_subscription_callback::*; -/// Manage the lifecycle of an `rcl_subscription_t`, including managing its dependencies -/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are -/// [dropped after][1] the `rcl_subscription_t`. -/// -/// [1]: -pub struct SubscriptionHandle { - rcl_subscription: Mutex, - node_handle: Arc, - pub(crate) in_use_by_wait_set: Arc, -} +mod subscription_async_callback; +pub use subscription_async_callback::*; -impl SubscriptionHandle { - pub(crate) fn lock(&self) -> MutexGuard { - self.rcl_subscription.lock().unwrap() - } -} +mod subscription_callback; +pub use subscription_callback::*; -impl Drop for SubscriptionHandle { - fn drop(&mut self) { - let rcl_subscription = self.rcl_subscription.get_mut().unwrap(); - let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); - let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); - // SAFETY: The entity lifecycle mutex is locked to protect against the risk of - // global variables in the rmw implementation being unsafely modified during cleanup. - unsafe { - rcl_subscription_fini(rcl_subscription, &mut *rcl_node); - } - } -} +mod message_info; +pub use message_info::*; -/// Trait to be implemented by concrete [`Subscription`]s. -pub trait SubscriptionBase: Send + Sync { - /// Internal function to get a reference to the `rcl` handle. - fn handle(&self) -> &SubscriptionHandle; - /// Tries to take a new message and run the callback with it. - fn execute(&self) -> Result<(), RclrsError>; -} +mod readonly_loaned_message; +pub use readonly_loaned_message::*; /// Struct for receiving messages of type `T`. /// +/// Create a subscription using [`Node::create_subscription()`][2] +/// or [`Node::create_async_subscription`][3]. +/// /// There can be multiple subscriptions for the same topic, in different nodes or the same node. +/// A clone of a `Subscription` will refer to the same subscription instance as the original. +/// The underlying instance is tied to [`SubscriptionState`] which implements the [`Subscription`] API. /// -/// Receiving messages requires calling [`spin_once`][1] or [`spin`][2] on the subscription's node. +/// Receiving messages requires calling [`spin`][1] on the `Executor` of subscription's [Node][4]. /// /// When a subscription is created, it may take some time to get "matched" with a corresponding /// publisher. /// -/// The only available way to instantiate subscriptions is via [`Node::create_subscription()`][3], this -/// is to ensure that [`Node`][4]s can track all the subscriptions that have been created. -/// -/// [1]: crate::spin_once -/// [2]: crate::spin -/// [3]: crate::Node::create_subscription +/// [1]: crate::Executor::spin +/// [2]: crate::Node::create_subscription +/// [3]: crate::Node::create_async_subscription /// [4]: crate::Node -pub struct Subscription +pub type Subscription = Arc>; + +/// The inner state of a [`Subscription`]. +/// +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Subscription`] in a non-owning way. It is +/// generally recommended to manage the `SubscriptionState` inside of an [`Arc`], +/// and [`Subscription`] is provided as a convenience alias for that. +/// +/// The public API of the [`Subscription`] type is implemented via `SubscriptionState`. +/// +/// [1]: std::sync::Weak +pub struct SubscriptionState where T: Message, { - pub(crate) handle: Arc, - /// The callback function that runs when a message was received. - pub callback: Mutex>, - message: PhantomData, + /// This handle is used to access the data that rcl holds for this subscription. + handle: Arc, + /// This allows us to replace the callback in the subscription task. + /// + /// Holding onto this sender will keep the subscription task alive. Once + /// this sender is dropped, the subscription task will end itself. + callback: Arc>>, + /// Holding onto this keeps the waiter for this subscription alive in the + /// wait set of the executor. + #[allow(unused)] + lifecycle: WaitableLifecycle, } -impl Subscription +impl SubscriptionState where T: Message, { - /// Creates a new subscription. - pub(crate) fn new( - node_handle: Arc, - topic: &str, - qos: QoSProfile, - callback: impl SubscriptionCallback, - ) -> Result - // This uses pub(crate) visibility to avoid instantiating this struct outside - // [`Node::create_subscription`], see the struct's documentation for the rationale - where - T: Message, - { + /// Returns the topic name of the subscription. + /// + /// This returns the topic name after remapping, so it is not necessarily the + /// topic name which was used when creating the subscription. + pub fn topic_name(&self) -> String { + // SAFETY: The subscription handle is valid because its lifecycle is managed by an Arc. + // The unsafe variables get converted to safe types before being returned + unsafe { + let raw_topic_pointer = rcl_subscription_get_topic_name(&*self.handle.lock()); + CStr::from_ptr(raw_topic_pointer) + } + .to_string_lossy() + .into_owned() + } + + /// Set the callback of this subscription, replacing the callback that was + /// previously set. + /// + /// This can be used even if the subscription previously used an async callback. + pub fn set_callback(&self, callback: impl SubscriptionCallback) { + let callback = callback.into_subscription_callback(); + *self.callback.lock().unwrap() = callback; + } + + /// Set the callback of this subscription, replacing the callback that was + /// previously set. + /// + /// This can be used even if the subscription previously used a non-async callback. + pub fn set_async_callback(&self, callback: impl SubscriptionAsyncCallback) { + let callback = callback.into_subscription_async_callback(); + *self.callback.lock().unwrap() = callback; + } + + /// Used by [`Node`][crate::Node] to create a new subscription. + pub(crate) fn create<'a>( + options: impl Into>, + callback: AnySubscriptionCallback, + node_handle: &Arc, + commands: &Arc, + ) -> Result, RclrsError> { + let SubscriptionOptions { topic, qos } = options.into(); + let callback = Arc::new(Mutex::new(callback)); + // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_subscription = unsafe { rcl_get_zero_initialized_subscription() }; let type_support = @@ -114,8 +130,8 @@ where })?; // SAFETY: No preconditions for this function. - let mut subscription_options = unsafe { rcl_subscription_get_default_options() }; - subscription_options.qos = qos.into(); + let mut rcl_subscription_options = unsafe { rcl_subscription_get_default_options() }; + rcl_subscription_options.qos = qos.into(); { let rcl_node = node_handle.rcl_node.lock().unwrap(); @@ -132,7 +148,7 @@ where &*rcl_node, type_support, topic_c_string.as_ptr(), - &subscription_options, + &rcl_subscription_options, ) .ok()?; } @@ -140,180 +156,114 @@ where let handle = Arc::new(SubscriptionHandle { rcl_subscription: Mutex::new(rcl_subscription), - node_handle, - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + node_handle: Arc::clone(node_handle), }); - Ok(Self { + let (waitable, lifecycle) = Waitable::new( + Box::new(SubscriptionExecutable { + handle: Arc::clone(&handle), + callback: Arc::clone(&callback), + commands: Arc::clone(commands), + }), + Some(Arc::clone(commands.get_guard_condition())), + ); + commands.add_to_wait_set(waitable); + + Ok(Arc::new(Self { handle, - callback: Mutex::new(callback.into_callback()), - message: PhantomData, - }) + callback, + lifecycle, + })) } +} - /// Returns the topic name of the subscription. - /// - /// This returns the topic name after remapping, so it is not necessarily the - /// topic name which was used when creating the subscription. - pub fn topic_name(&self) -> String { - // SAFETY: No preconditions for the function used - // The unsafe variables get converted to safe types before being returned - unsafe { - let raw_topic_pointer = rcl_subscription_get_topic_name(&*self.handle.lock()); - CStr::from_ptr(raw_topic_pointer) - .to_string_lossy() - .into_owned() +/// `SubscriptionOptions` are used by [`Node::create_subscription`][1] to initialize +/// a [`Subscription`]. +/// +/// [1]: crate::Node::create_subscription +#[derive(Debug, Clone)] +#[non_exhaustive] +pub struct SubscriptionOptions<'a> { + /// The topic name for the subscription. + pub topic: &'a str, + /// The quality of service settings for the subscription. + pub qos: QoSProfile, +} + +impl<'a> SubscriptionOptions<'a> { + /// Initialize a new [`SubscriptionOptions`] with default settings. + pub fn new(topic: &'a str) -> Self { + Self { + topic, + qos: QoSProfile::topics_default(), } } +} - /// Fetches a new message. - /// - /// When there is no new message, this will return a - /// [`SubscriptionTakeFailed`][1]. - /// - /// [1]: crate::RclrsError - // - // ```text - // +-------------+ - // | rclrs::take | - // +------+------+ - // | - // | - // +------v------+ - // | rcl_take | - // +------+------+ - // | - // | - // +------v------+ - // | rmw_take | - // +-------------+ - // ``` - pub fn take(&self) -> Result<(T, MessageInfo), RclrsError> { - let mut rmw_message = ::RmwMsg::default(); - let message_info = self.take_inner(&mut rmw_message)?; - Ok((T::from_rmw_message(rmw_message), message_info)) +impl<'a, T: IntoPrimitiveOptions<'a>> From for SubscriptionOptions<'a> { + fn from(value: T) -> Self { + let primitive = value.into_primitive_options(); + let mut options = Self::new(primitive.name); + primitive.apply(&mut options.qos); + options } +} - /// This is a version of take() that returns a boxed message. - /// - /// This can be more efficient for messages containing large arrays. - pub fn take_boxed(&self) -> Result<(Box, MessageInfo), RclrsError> { - let mut rmw_message = Box::<::RmwMsg>::default(); - let message_info = self.take_inner(&mut *rmw_message)?; - // TODO: This will still use the stack in general. Change signature of - // from_rmw_message to allow placing the result in a Box directly. - let message = Box::new(T::from_rmw_message(*rmw_message)); - Ok((message, message_info)) +struct SubscriptionExecutable { + handle: Arc, + callback: Arc>>, + commands: Arc, +} + +impl RclPrimitive for SubscriptionExecutable +where + T: Message, +{ + fn execute(&mut self) -> Result<(), RclrsError> { + self.callback + .lock() + .unwrap() + .execute(&self.handle, &self.commands) } - // Inner function, to be used by both regular and boxed versions. - fn take_inner( - &self, - rmw_message: &mut ::RmwMsg, - ) -> Result { - let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; - let rcl_subscription = &mut *self.handle.lock(); - unsafe { - // SAFETY: The first two pointers are valid/initialized, and do not need to be valid - // beyond the function call. - // The latter two pointers are explicitly allowed to be NULL. - rcl_take( - rcl_subscription, - rmw_message as *mut ::RmwMsg as *mut _, - &mut message_info, - std::ptr::null_mut(), - ) - .ok()? - }; - Ok(MessageInfo::from_rmw_message_info(&message_info)) + fn kind(&self) -> crate::RclPrimitiveKind { + RclPrimitiveKind::Subscription } - /// Obtains a read-only handle to a message owned by the middleware. - /// - /// When there is no new message, this will return a - /// [`SubscriptionTakeFailed`][1]. - /// - /// This is the counterpart to [`Publisher::borrow_loaned_message()`][2]. See its documentation - /// for more information. - /// - /// [1]: crate::RclrsError - /// [2]: crate::Publisher::borrow_loaned_message - pub fn take_loaned(&self) -> Result<(ReadOnlyLoanedMessage<'_, T>, MessageInfo), RclrsError> { - let mut msg_ptr = std::ptr::null_mut(); - let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; - unsafe { - // SAFETY: The third argument (message_info) and fourth argument (allocation) may be null. - // The second argument (loaned_message) contains a null ptr as expected. - rcl_take_loaned_message( - &*self.handle.lock(), - &mut msg_ptr, - &mut message_info, - std::ptr::null_mut(), - ) - .ok()?; - } - let read_only_loaned_msg = ReadOnlyLoanedMessage { - msg_ptr: msg_ptr as *const T::RmwMsg, - subscription: self, - }; - Ok(( - read_only_loaned_msg, - MessageInfo::from_rmw_message_info(&message_info), - )) + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::Subscription(self.handle.lock()) } } -impl SubscriptionBase for Subscription -where - T: Message, -{ - fn handle(&self) -> &SubscriptionHandle { - &self.handle +// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread +// they are running in. Therefore, this type can be safely sent to another thread. +unsafe impl Send for rcl_subscription_t {} + +/// Manage the lifecycle of an `rcl_subscription_t`, including managing its dependencies +/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are +/// [dropped after][1] the `rcl_subscription_t`. +/// +/// [1]: +struct SubscriptionHandle { + rcl_subscription: Mutex, + node_handle: Arc, +} + +impl SubscriptionHandle { + fn lock(&self) -> MutexGuard { + self.rcl_subscription.lock().unwrap() } +} - fn execute(&self) -> Result<(), RclrsError> { - let evaluate = || { - match &mut *self.callback.lock().unwrap() { - AnySubscriptionCallback::Regular(cb) => { - let (msg, _) = self.take()?; - cb(msg) - } - AnySubscriptionCallback::RegularWithMessageInfo(cb) => { - let (msg, msg_info) = self.take()?; - cb(msg, msg_info) - } - AnySubscriptionCallback::Boxed(cb) => { - let (msg, _) = self.take_boxed()?; - cb(msg) - } - AnySubscriptionCallback::BoxedWithMessageInfo(cb) => { - let (msg, msg_info) = self.take_boxed()?; - cb(msg, msg_info) - } - AnySubscriptionCallback::Loaned(cb) => { - let (msg, _) = self.take_loaned()?; - cb(msg) - } - AnySubscriptionCallback::LoanedWithMessageInfo(cb) => { - let (msg, msg_info) = self.take_loaned()?; - cb(msg, msg_info) - } - } - Ok(()) - }; - - // Immediately evaluated closure, to handle SubscriptionTakeFailed - // outside this match - match evaluate() { - Err(RclrsError::RclError { - code: RclReturnCode::SubscriptionTakeFailed, - .. - }) => { - // Spurious wakeup – this may happen even when a waitset indicated that this - // subscription was ready, so it shouldn't be an error. - Ok(()) - } - other => other, +impl Drop for SubscriptionHandle { + fn drop(&mut self) { + let rcl_subscription = self.rcl_subscription.get_mut().unwrap(); + let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of + // global variables in the rmw implementation being unsafely modified during cleanup. + unsafe { + rcl_subscription_fini(rcl_subscription, &mut *rcl_node); } } } @@ -326,33 +276,29 @@ mod tests { #[test] fn traits() { - assert_send::>(); - assert_sync::>(); + assert_send::>(); + assert_sync::>(); } #[test] fn test_subscriptions() -> Result<(), RclrsError> { - use crate::{TopicEndpointInfo, QOS_PROFILE_SYSTEM_DEFAULT}; + use crate::TopicEndpointInfo; let namespace = "/test_subscriptions_graph"; let graph = construct_test_graph(namespace)?; - let node_2_empty_subscription = graph.node2.create_subscription::( - "graph_test_topic_1", - QOS_PROFILE_SYSTEM_DEFAULT, - |_msg: msg::Empty| {}, - )?; + let node_2_empty_subscription = graph + .node2 + .create_subscription::("graph_test_topic_1", |_msg: msg::Empty| {})?; let topic1 = node_2_empty_subscription.topic_name(); let node_2_basic_types_subscription = graph.node2.create_subscription::( "graph_test_topic_2", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::BasicTypes| {}, )?; let topic2 = node_2_basic_types_subscription.topic_name(); let node_1_defaults_subscription = graph.node1.create_subscription::( "graph_test_topic_3", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::Defaults| {}, )?; let topic3 = node_1_defaults_subscription.topic_name(); diff --git a/rclrs/src/subscription/any_subscription_callback.rs b/rclrs/src/subscription/any_subscription_callback.rs new file mode 100644 index 000000000..19732d67c --- /dev/null +++ b/rclrs/src/subscription/any_subscription_callback.rs @@ -0,0 +1,193 @@ +use rosidl_runtime_rs::Message; + +use super::{MessageInfo, SubscriptionHandle}; +use crate::{ + error::ToResult, rcl_bindings::*, ExecutorCommands, RclReturnCode, RclrsError, + ReadOnlyLoanedMessage, +}; + +use futures::future::BoxFuture; + +use std::sync::Arc; + +/// An enum capturing the various possible function signatures for subscription callbacks. +/// +/// The correct enum variant is deduced by the [`SubscriptionCallback`][1] or +/// [`SubscriptionAsyncCallback`][2] trait. +/// +/// [1]: crate::SubscriptionCallback +/// [2]: crate::SubscriptionAsyncCallback +pub enum AnySubscriptionCallback +where + T: Message, +{ + /// A callback with only the message as an argument. + Regular(Box BoxFuture<'static, ()> + Send>), + /// A callback with the message and the message info as arguments. + RegularWithMessageInfo(Box BoxFuture<'static, ()> + Send>), + /// A callback with only the boxed message as an argument. + Boxed(Box) -> BoxFuture<'static, ()> + Send>), + /// A callback with the boxed message and the message info as arguments. + BoxedWithMessageInfo(Box, MessageInfo) -> BoxFuture<'static, ()> + Send>), + /// A callback with only the loaned message as an argument. + #[allow(clippy::type_complexity)] + Loaned(Box FnMut(ReadOnlyLoanedMessage) -> BoxFuture<'static, ()> + Send>), + /// A callback with the loaned message and the message info as arguments. + #[allow(clippy::type_complexity)] + LoanedWithMessageInfo( + Box< + dyn for<'a> FnMut(ReadOnlyLoanedMessage, MessageInfo) -> BoxFuture<'static, ()> + + Send, + >, + ), +} + +impl AnySubscriptionCallback { + pub(super) fn execute( + &mut self, + handle: &Arc, + commands: &Arc, + ) -> Result<(), RclrsError> { + // Immediately evaluated closure, to handle SubscriptionTakeFailed + // outside this match + let mut evaluate = || { + match self { + AnySubscriptionCallback::Regular(cb) => { + let (msg, _) = Self::take(handle)?; + let _ = commands.run(cb(msg)); + } + AnySubscriptionCallback::RegularWithMessageInfo(cb) => { + let (msg, msg_info) = Self::take(handle)?; + let _ = commands.run(cb(msg, msg_info)); + } + AnySubscriptionCallback::Boxed(cb) => { + let (msg, _) = Self::take_boxed(handle)?; + let _ = commands.run(cb(msg)); + } + AnySubscriptionCallback::BoxedWithMessageInfo(cb) => { + let (msg, msg_info) = Self::take_boxed(handle)?; + let _ = commands.run(cb(msg, msg_info)); + } + AnySubscriptionCallback::Loaned(cb) => { + let (msg, _) = Self::take_loaned(handle)?; + let _ = commands.run(cb(msg)); + } + AnySubscriptionCallback::LoanedWithMessageInfo(cb) => { + let (msg, msg_info) = Self::take_loaned(handle)?; + let _ = commands.run(cb(msg, msg_info)); + } + } + Ok(()) + }; + + match evaluate() { + Err(RclrsError::RclError { + code: RclReturnCode::SubscriptionTakeFailed, + .. + }) => { + // Spurious wakeup – this may happen even when a waitset indicated that this + // subscription was ready, so it shouldn't be an error. + Ok(()) + } + other => other, + } + } + + /// Fetches a new message. + /// + /// When there is no new message, this will return a + /// [`SubscriptionTakeFailed`][1]. + /// + /// [1]: crate::RclrsError + // + // ```text + // +-------------+ + // | rclrs::take | + // +------+------+ + // | + // | + // +------v------+ + // | rcl_take | + // +------+------+ + // | + // | + // +------v------+ + // | rmw_take | + // +-------------+ + // ``` + fn take(handle: &SubscriptionHandle) -> Result<(T, MessageInfo), RclrsError> { + let mut rmw_message = ::RmwMsg::default(); + let message_info = Self::take_inner(handle, &mut rmw_message)?; + Ok((T::from_rmw_message(rmw_message), message_info)) + } + + /// This is a version of take() that returns a boxed message. + /// + /// This can be more efficient for messages containing large arrays. + fn take_boxed(handle: &SubscriptionHandle) -> Result<(Box, MessageInfo), RclrsError> { + let mut rmw_message = Box::<::RmwMsg>::default(); + let message_info = Self::take_inner(handle, &mut *rmw_message)?; + // TODO: This will still use the stack in general. Change signature of + // from_rmw_message to allow placing the result in a Box directly. + let message = Box::new(T::from_rmw_message(*rmw_message)); + Ok((message, message_info)) + } + + // Inner function, to be used by both regular and boxed versions. + fn take_inner( + handle: &SubscriptionHandle, + rmw_message: &mut ::RmwMsg, + ) -> Result { + let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; + let rcl_subscription = &mut *handle.lock(); + unsafe { + // SAFETY: The first two pointers are valid/initialized, and do not need to be valid + // beyond the function call. + // The latter two pointers are explicitly allowed to be NULL. + rcl_take( + rcl_subscription, + rmw_message as *mut ::RmwMsg as *mut _, + &mut message_info, + std::ptr::null_mut(), + ) + .ok()? + }; + Ok(MessageInfo::from_rmw_message_info(&message_info)) + } + + /// Obtains a read-only handle to a message owned by the middleware. + /// + /// When there is no new message, this will return a + /// [`SubscriptionTakeFailed`][1]. + /// + /// This is the counterpart to [`Publisher::borrow_loaned_message()`][2]. See its documentation + /// for more information. + /// + /// [1]: crate::RclrsError + /// [2]: crate::Publisher::borrow_loaned_message + fn take_loaned( + handle: &Arc, + ) -> Result<(ReadOnlyLoanedMessage, MessageInfo), RclrsError> { + let mut msg_ptr = std::ptr::null_mut(); + let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; + unsafe { + // SAFETY: The third argument (message_info) and fourth argument (allocation) may be null. + // The second argument (loaned_message) contains a null ptr as expected. + rcl_take_loaned_message( + &*handle.lock(), + &mut msg_ptr, + &mut message_info, + std::ptr::null_mut(), + ) + .ok()?; + } + let read_only_loaned_msg = ReadOnlyLoanedMessage { + msg_ptr: msg_ptr as *const T::RmwMsg, + handle: Arc::clone(handle), + }; + Ok(( + read_only_loaned_msg, + MessageInfo::from_rmw_message_info(&message_info), + )) + } +} diff --git a/rclrs/src/subscription/callback.rs b/rclrs/src/subscription/callback.rs deleted file mode 100644 index d5e9fba8e..000000000 --- a/rclrs/src/subscription/callback.rs +++ /dev/null @@ -1,174 +0,0 @@ -use rosidl_runtime_rs::Message; - -use super::MessageInfo; -use crate::ReadOnlyLoanedMessage; - -/// A trait for allowed callbacks for subscriptions. -/// -/// See [`AnySubscriptionCallback`] for a list of possible callback signatures. -pub trait SubscriptionCallback: Send + 'static -where - T: Message, -{ - /// Converts the callback into an enum. - /// - /// User code never needs to call this function. - fn into_callback(self) -> AnySubscriptionCallback; -} - -/// An enum capturing the various possible function signatures for subscription callbacks. -/// -/// The correct enum variant is deduced by the [`SubscriptionCallback`] trait. -pub enum AnySubscriptionCallback -where - T: Message, -{ - /// A callback with only the message as an argument. - Regular(Box), - /// A callback with the message and the message info as arguments. - RegularWithMessageInfo(Box), - /// A callback with only the boxed message as an argument. - Boxed(Box) + Send>), - /// A callback with the boxed message and the message info as arguments. - BoxedWithMessageInfo(Box, MessageInfo) + Send>), - /// A callback with only the loaned message as an argument. - #[allow(clippy::type_complexity)] - Loaned(Box FnMut(ReadOnlyLoanedMessage<'a, T>) + Send>), - /// A callback with the loaned message and the message info as arguments. - #[allow(clippy::type_complexity)] - LoanedWithMessageInfo(Box FnMut(ReadOnlyLoanedMessage<'a, T>, MessageInfo) + Send>), -} - -// We need one implementation per arity. This was inspired by Bevy's systems. -impl SubscriptionCallback for Func -where - Func: FnMut(A0) + Send + 'static, - (A0,): ArgTuple, - T: Message, -{ - fn into_callback(self) -> AnySubscriptionCallback { - <(A0,) as ArgTuple>::into_callback_with_args(self) - } -} - -impl SubscriptionCallback for Func -where - Func: FnMut(A0, A1) + Send + 'static, - (A0, A1): ArgTuple, - T: Message, -{ - fn into_callback(self) -> AnySubscriptionCallback { - <(A0, A1) as ArgTuple>::into_callback_with_args(self) - } -} - -// Helper trait for SubscriptionCallback. -// -// For each tuple of args, it provides conversion from a function with -// these args to the correct enum variant. -trait ArgTuple -where - T: Message, -{ - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback; -} - -impl ArgTuple for (T,) -where - T: Message, - Func: FnMut(T) + Send + 'static, -{ - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback { - AnySubscriptionCallback::Regular(Box::new(func)) - } -} - -impl ArgTuple for (T, MessageInfo) -where - T: Message, - Func: FnMut(T, MessageInfo) + Send + 'static, -{ - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback { - AnySubscriptionCallback::RegularWithMessageInfo(Box::new(func)) - } -} - -impl ArgTuple for (Box,) -where - T: Message, - Func: FnMut(Box) + Send + 'static, -{ - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback { - AnySubscriptionCallback::Boxed(Box::new(func)) - } -} - -impl ArgTuple for (Box, MessageInfo) -where - T: Message, - Func: FnMut(Box, MessageInfo) + Send + 'static, -{ - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback { - AnySubscriptionCallback::BoxedWithMessageInfo(Box::new(func)) - } -} - -impl ArgTuple for (ReadOnlyLoanedMessage<'_, T>,) -where - T: Message, - Func: for<'b> FnMut(ReadOnlyLoanedMessage<'b, T>) + Send + 'static, -{ - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback { - AnySubscriptionCallback::Loaned(Box::new(func)) - } -} - -impl ArgTuple for (ReadOnlyLoanedMessage<'_, T>, MessageInfo) -where - T: Message, - Func: for<'b> FnMut(ReadOnlyLoanedMessage<'b, T>, MessageInfo) + Send + 'static, -{ - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback { - AnySubscriptionCallback::LoanedWithMessageInfo(Box::new(func)) - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn callback_conversion() { - type Message = test_msgs::msg::BoundedSequences; - let cb = |_msg: Message| {}; - assert!(matches!( - cb.into_callback(), - AnySubscriptionCallback::::Regular(_) - )); - let cb = |_msg: Message, _info: MessageInfo| {}; - assert!(matches!( - cb.into_callback(), - AnySubscriptionCallback::::RegularWithMessageInfo(_) - )); - let cb = |_msg: Box| {}; - assert!(matches!( - cb.into_callback(), - AnySubscriptionCallback::::Boxed(_) - )); - let cb = |_msg: Box, _info: MessageInfo| {}; - assert!(matches!( - cb.into_callback(), - AnySubscriptionCallback::::BoxedWithMessageInfo(_) - )); - let cb = |_msg: ReadOnlyLoanedMessage<'_, Message>| {}; - assert!(matches!( - cb.into_callback(), - AnySubscriptionCallback::::Loaned(_) - )); - let cb = |_msg: ReadOnlyLoanedMessage<'_, Message>, _info: MessageInfo| {}; - assert!(matches!( - cb.into_callback(), - AnySubscriptionCallback::::LoanedWithMessageInfo(_) - )); - } -} diff --git a/rclrs/src/subscription/message_info.rs b/rclrs/src/subscription/message_info.rs index 1adecd3ec..4dd518093 100644 --- a/rclrs/src/subscription/message_info.rs +++ b/rclrs/src/subscription/message_info.rs @@ -26,7 +26,7 @@ use crate::rcl_bindings::*; /// > and should be unlikely to happen in practice. /// /// [1]: https://docs.ros.org/en/rolling/p/rmw/generated/structrmw__message__info__s.html#_CPPv4N18rmw_message_info_s13publisher_gidE -#[derive(Clone, Debug, PartialEq, Eq)] +#[derive(Clone, Debug, Hash, PartialEq, Eq, PartialOrd, Ord)] pub struct PublisherGid { /// Bytes identifying a publisher in the RMW implementation. pub data: [u8; RMW_GID_STORAGE_SIZE], @@ -48,7 +48,7 @@ unsafe impl Send for PublisherGid {} unsafe impl Sync for PublisherGid {} /// Additional information about a received message. -#[derive(Clone, Debug, PartialEq, Eq)] +#[derive(Debug, Clone, Hash, PartialEq, Eq, PartialOrd, Ord)] pub struct MessageInfo { /// Time when the message was published by the publisher. /// @@ -106,30 +106,27 @@ pub struct MessageInfo { impl MessageInfo { pub(crate) fn from_rmw_message_info(rmw_message_info: &rmw_message_info_t) -> Self { - let source_timestamp = match rmw_message_info.source_timestamp { - 0 => None, - ts if ts < 0 => Some(SystemTime::UNIX_EPOCH - Duration::from_nanos(ts.unsigned_abs())), - ts => Some(SystemTime::UNIX_EPOCH + Duration::from_nanos(ts.unsigned_abs())), - }; - let received_timestamp = match rmw_message_info.received_timestamp { - 0 => None, - ts if ts < 0 => Some(SystemTime::UNIX_EPOCH - Duration::from_nanos(ts.unsigned_abs())), - ts => Some(SystemTime::UNIX_EPOCH + Duration::from_nanos(ts.unsigned_abs())), - }; - let publisher_gid = PublisherGid { - data: rmw_message_info.publisher_gid.data, - implementation_identifier: rmw_message_info.publisher_gid.implementation_identifier, - }; Self { - source_timestamp, - received_timestamp, + source_timestamp: timestamp_to_system_time(rmw_message_info.source_timestamp), + received_timestamp: timestamp_to_system_time(rmw_message_info.received_timestamp), publication_sequence_number: rmw_message_info.publication_sequence_number, reception_sequence_number: rmw_message_info.reception_sequence_number, - publisher_gid, + publisher_gid: PublisherGid { + data: rmw_message_info.publisher_gid.data, + implementation_identifier: rmw_message_info.publisher_gid.implementation_identifier, + }, } } } +pub(crate) fn timestamp_to_system_time(timestamp: rmw_time_point_value_t) -> Option { + match timestamp { + 0 => None, + ts if ts < 0 => Some(SystemTime::UNIX_EPOCH - Duration::from_nanos(ts.unsigned_abs())), + ts => Some(SystemTime::UNIX_EPOCH + Duration::from_nanos(ts.unsigned_abs())), + } +} + #[cfg(test)] mod tests { use super::*; diff --git a/rclrs/src/subscription/readonly_loaned_message.rs b/rclrs/src/subscription/readonly_loaned_message.rs index c6f52e280..9fae1db11 100644 --- a/rclrs/src/subscription/readonly_loaned_message.rs +++ b/rclrs/src/subscription/readonly_loaned_message.rs @@ -1,8 +1,8 @@ -use std::ops::Deref; +use std::{ops::Deref, sync::Arc}; use rosidl_runtime_rs::Message; -use crate::{rcl_bindings::*, Subscription, ToResult}; +use crate::{rcl_bindings::*, subscription::SubscriptionHandle, ToResult}; /// A message that is owned by the middleware, loaned out for reading. /// @@ -14,15 +14,15 @@ use crate::{rcl_bindings::*, Subscription, ToResult}; /// subscription callbacks. /// /// The loan is returned by dropping the `ReadOnlyLoanedMessage`. -pub struct ReadOnlyLoanedMessage<'a, T> +pub struct ReadOnlyLoanedMessage where T: Message, { pub(super) msg_ptr: *const T::RmwMsg, - pub(super) subscription: &'a Subscription, + pub(super) handle: Arc, } -impl Deref for ReadOnlyLoanedMessage<'_, T> +impl Deref for ReadOnlyLoanedMessage where T: Message, { @@ -32,14 +32,14 @@ where } } -impl Drop for ReadOnlyLoanedMessage<'_, T> +impl Drop for ReadOnlyLoanedMessage where T: Message, { fn drop(&mut self) { unsafe { rcl_return_loaned_message_from_subscription( - &*self.subscription.handle.lock(), + &*self.handle.lock(), self.msg_ptr as *mut _, ) .ok() @@ -50,9 +50,9 @@ where // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread // they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for ReadOnlyLoanedMessage<'_, T> where T: Message {} +unsafe impl Send for ReadOnlyLoanedMessage where T: Message {} // SAFETY: This type has no interior mutability, in fact it has no mutability at all. -unsafe impl Sync for ReadOnlyLoanedMessage<'_, T> where T: Message {} +unsafe impl Sync for ReadOnlyLoanedMessage where T: Message {} #[cfg(test)] mod tests { diff --git a/rclrs/src/subscription/subscription_async_callback.rs b/rclrs/src/subscription/subscription_async_callback.rs new file mode 100644 index 000000000..b60838712 --- /dev/null +++ b/rclrs/src/subscription/subscription_async_callback.rs @@ -0,0 +1,205 @@ +use rosidl_runtime_rs::Message; + +use super::{any_subscription_callback::AnySubscriptionCallback, MessageInfo}; +use crate::ReadOnlyLoanedMessage; + +use std::future::Future; + +/// A trait for async callbacks of subscriptions. +/// +// TODO(@mxgrey): Add a description of what callback signatures are supported +pub trait SubscriptionAsyncCallback: Send + 'static +where + T: Message, +{ + /// Converts the callback into an enum. + /// + /// User code never needs to call this function. + fn into_subscription_async_callback(self) -> AnySubscriptionCallback; +} + +// We need one implementation per arity. This was inspired by Bevy's systems. +impl SubscriptionAsyncCallback for Func +where + T: Message, + (A0,): SubscriptionAsyncArgs, + Out: Future + Send + 'static, + Func: FnMut(A0) -> Out + Send + 'static, +{ + fn into_subscription_async_callback(self) -> AnySubscriptionCallback { + <(A0,) as SubscriptionAsyncArgs>::into_any_callback(self) + } +} + +impl SubscriptionAsyncCallback for Func +where + T: Message, + (A0, A1): SubscriptionAsyncArgs, + Out: Future + Send + 'static, + Func: FnMut(A0, A1) -> Out + Send + 'static, +{ + fn into_subscription_async_callback(self) -> AnySubscriptionCallback { + <(A0, A1) as SubscriptionAsyncArgs>::into_any_callback(self) + } +} + +/// Helper trait for SubscriptionCallback. +/// +/// For each tuple of args, it provides conversion from a function with +/// these args to the correct enum variant. +trait SubscriptionAsyncArgs +where + T: Message, +{ + fn into_any_callback(func: Func) -> AnySubscriptionCallback; +} + +impl SubscriptionAsyncArgs for (T,) +where + T: Message, + Func: FnMut(T) -> Out + Send + 'static, + Out: Future + Send + 'static, +{ + fn into_any_callback(mut func: Func) -> AnySubscriptionCallback { + AnySubscriptionCallback::Regular(Box::new(move |message| Box::pin(func(message)))) + } +} + +impl SubscriptionAsyncArgs for (T, MessageInfo) +where + T: Message, + Func: FnMut(T, MessageInfo) -> Out + Send + 'static, + Out: Future + Send + 'static, +{ + fn into_any_callback(mut func: Func) -> AnySubscriptionCallback { + AnySubscriptionCallback::RegularWithMessageInfo(Box::new(move |message, info| { + Box::pin(func(message, info)) + })) + } +} + +impl SubscriptionAsyncArgs for (Box,) +where + T: Message, + Func: FnMut(Box) -> Out + Send + 'static, + Out: Future + Send + 'static, +{ + fn into_any_callback(mut func: Func) -> AnySubscriptionCallback { + AnySubscriptionCallback::Boxed(Box::new(move |message| Box::pin(func(message)))) + } +} + +impl SubscriptionAsyncArgs for (Box, MessageInfo) +where + T: Message, + Func: FnMut(Box, MessageInfo) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_any_callback(mut func: Func) -> AnySubscriptionCallback { + AnySubscriptionCallback::BoxedWithMessageInfo(Box::new(move |message, info| { + Box::pin(func(message, info)) + })) + } +} + +impl SubscriptionAsyncArgs for (ReadOnlyLoanedMessage,) +where + T: Message, + Func: FnMut(ReadOnlyLoanedMessage) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_any_callback(mut func: Func) -> AnySubscriptionCallback { + AnySubscriptionCallback::Loaned(Box::new(move |message| Box::pin(func(message)))) + } +} + +impl SubscriptionAsyncArgs for (ReadOnlyLoanedMessage, MessageInfo) +where + T: Message, + Func: FnMut(ReadOnlyLoanedMessage, MessageInfo) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_any_callback(mut func: Func) -> AnySubscriptionCallback { + AnySubscriptionCallback::LoanedWithMessageInfo(Box::new(move |message, info| { + Box::pin(func(message, info)) + })) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + type TestMessage = test_msgs::msg::BoundedSequences; + + #[test] + fn callback_conversion() { + let cb = |_msg: TestMessage| async {}; + assert!(matches!( + cb.into_subscription_async_callback(), + AnySubscriptionCallback::::Regular(_) + )); + let cb = |_msg: TestMessage, _info: MessageInfo| async {}; + assert!(matches!( + cb.into_subscription_async_callback(), + AnySubscriptionCallback::::RegularWithMessageInfo(_) + )); + let cb = |_msg: Box| async {}; + assert!(matches!( + cb.into_subscription_async_callback(), + AnySubscriptionCallback::::Boxed(_) + )); + let cb = |_msg: Box, _info: MessageInfo| async {}; + assert!(matches!( + cb.into_subscription_async_callback(), + AnySubscriptionCallback::::BoxedWithMessageInfo(_) + )); + let cb = |_msg: ReadOnlyLoanedMessage| async {}; + assert!(matches!( + cb.into_subscription_async_callback(), + AnySubscriptionCallback::::Loaned(_) + )); + let cb = |_msg: ReadOnlyLoanedMessage, _info: MessageInfo| async {}; + assert!(matches!( + cb.into_subscription_async_callback(), + AnySubscriptionCallback::::LoanedWithMessageInfo(_) + )); + + assert!(matches!( + test_regular.into_subscription_async_callback(), + AnySubscriptionCallback::::Regular(_), + )); + assert!(matches!( + test_regular_with_info.into_subscription_async_callback(), + AnySubscriptionCallback::::RegularWithMessageInfo(_), + )); + assert!(matches!( + test_boxed.into_subscription_async_callback(), + AnySubscriptionCallback::::Boxed(_), + )); + assert!(matches!( + test_boxed_with_info.into_subscription_async_callback(), + AnySubscriptionCallback::::BoxedWithMessageInfo(_), + )); + assert!(matches!( + test_loaned.into_subscription_async_callback(), + AnySubscriptionCallback::::Loaned(_), + )); + assert!(matches!( + test_loaned_with_info.into_subscription_async_callback(), + AnySubscriptionCallback::::LoanedWithMessageInfo(_), + )); + } + + async fn test_regular(_msg: TestMessage) {} + + async fn test_regular_with_info(_msg: TestMessage, _info: MessageInfo) {} + + async fn test_boxed(_msg: Box) {} + + async fn test_boxed_with_info(_msg: Box, _info: MessageInfo) {} + + async fn test_loaned(_msg: ReadOnlyLoanedMessage) {} + + async fn test_loaned_with_info(_msg: ReadOnlyLoanedMessage, _info: MessageInfo) {} +} diff --git a/rclrs/src/subscription/subscription_callback.rs b/rclrs/src/subscription/subscription_callback.rs new file mode 100644 index 000000000..d1a2bb45d --- /dev/null +++ b/rclrs/src/subscription/subscription_callback.rs @@ -0,0 +1,223 @@ +use rosidl_runtime_rs::Message; + +use super::{any_subscription_callback::AnySubscriptionCallback, MessageInfo}; +use crate::ReadOnlyLoanedMessage; + +use std::sync::Arc; + +/// A trait for regular callbacks of subscriptions. +/// +// TODO(@mxgrey): Add a description of what callbacks signatures are supported +pub trait SubscriptionCallback: Send + 'static +where + T: Message, +{ + /// Converts the callback into an enum. + /// + /// User code never needs to call this function. + fn into_subscription_callback(self) -> AnySubscriptionCallback; +} + +// We need one implementation per arity. This was inspired by Bevy's systems. +impl SubscriptionCallback for Func +where + T: Message, + (A0,): SubscriptionArgs, + Func: Fn(A0) + Send + Sync + 'static, +{ + fn into_subscription_callback(self) -> AnySubscriptionCallback { + <(A0,) as SubscriptionArgs>::into_any_callback(self) + } +} + +impl SubscriptionCallback for Func +where + T: Message, + (A0, A1): SubscriptionArgs, + Func: Fn(A0, A1) + Clone + Send + 'static, +{ + fn into_subscription_callback(self) -> AnySubscriptionCallback { + <(A0, A1) as SubscriptionArgs>::into_any_callback(self) + } +} + +trait SubscriptionArgs +where + T: Message, +{ + fn into_any_callback(func: Func) -> AnySubscriptionCallback; +} + +impl SubscriptionArgs for (T,) +where + T: Message, + Func: Fn(T) + Send + Sync + 'static, +{ + fn into_any_callback(func: Func) -> AnySubscriptionCallback { + let func = Arc::new(func); + AnySubscriptionCallback::Regular(Box::new(move |message| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message); + }) + })) + } +} + +impl SubscriptionArgs for (T, MessageInfo) +where + T: Message, + Func: Fn(T, MessageInfo) + Send + Sync + 'static, +{ + fn into_any_callback(func: Func) -> AnySubscriptionCallback { + let func = Arc::new(func); + AnySubscriptionCallback::RegularWithMessageInfo(Box::new(move |message, info| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message, info); + }) + })) + } +} + +impl SubscriptionArgs for (Box,) +where + T: Message, + Func: Fn(Box) + Send + Sync + 'static, +{ + fn into_any_callback(func: Func) -> AnySubscriptionCallback { + let func = Arc::new(func); + AnySubscriptionCallback::Boxed(Box::new(move |message| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message); + }) + })) + } +} + +impl SubscriptionArgs for (Box, MessageInfo) +where + T: Message, + Func: Fn(Box, MessageInfo) + Send + Sync + 'static, +{ + fn into_any_callback(func: Func) -> AnySubscriptionCallback { + let func = Arc::new(func); + AnySubscriptionCallback::BoxedWithMessageInfo(Box::new(move |message, info| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message, info); + }) + })) + } +} + +impl SubscriptionArgs for (ReadOnlyLoanedMessage,) +where + T: Message, + Func: Fn(ReadOnlyLoanedMessage) + Send + Sync + 'static, +{ + fn into_any_callback(func: Func) -> AnySubscriptionCallback { + let func = Arc::new(func); + AnySubscriptionCallback::Loaned(Box::new(move |message| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message); + }) + })) + } +} + +impl SubscriptionArgs for (ReadOnlyLoanedMessage, MessageInfo) +where + T: Message, + Func: Fn(ReadOnlyLoanedMessage, MessageInfo) + Send + Sync + 'static, +{ + fn into_any_callback(func: Func) -> AnySubscriptionCallback { + let func = Arc::new(func); + AnySubscriptionCallback::LoanedWithMessageInfo(Box::new(move |message, info| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message, info); + }) + })) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + type TestMessage = test_msgs::msg::BoundedSequences; + + #[test] + fn callback_conversion() { + let cb = |_msg: TestMessage| {}; + assert!(matches!( + cb.into_subscription_callback(), + AnySubscriptionCallback::::Regular(_) + )); + let cb = |_msg: TestMessage, _info: MessageInfo| {}; + assert!(matches!( + cb.into_subscription_callback(), + AnySubscriptionCallback::::RegularWithMessageInfo(_) + )); + let cb = |_msg: Box| {}; + assert!(matches!( + cb.into_subscription_callback(), + AnySubscriptionCallback::::Boxed(_) + )); + let cb = |_msg: Box, _info: MessageInfo| {}; + assert!(matches!( + cb.into_subscription_callback(), + AnySubscriptionCallback::::BoxedWithMessageInfo(_) + )); + let cb = |_msg: ReadOnlyLoanedMessage| {}; + assert!(matches!( + cb.into_subscription_callback(), + AnySubscriptionCallback::::Loaned(_) + )); + let cb = |_msg: ReadOnlyLoanedMessage, _info: MessageInfo| {}; + assert!(matches!( + cb.into_subscription_callback(), + AnySubscriptionCallback::::LoanedWithMessageInfo(_) + )); + + assert!(matches!( + test_regular.into_subscription_callback(), + AnySubscriptionCallback::::Regular(_), + )); + assert!(matches!( + test_regular_with_info.into_subscription_callback(), + AnySubscriptionCallback::::RegularWithMessageInfo(_), + )); + assert!(matches!( + test_boxed.into_subscription_callback(), + AnySubscriptionCallback::::Boxed(_), + )); + assert!(matches!( + test_boxed_with_info.into_subscription_callback(), + AnySubscriptionCallback::::BoxedWithMessageInfo(_), + )); + assert!(matches!( + test_loaned.into_subscription_callback(), + AnySubscriptionCallback::::Loaned(_), + )); + assert!(matches!( + test_loaned_with_info.into_subscription_callback(), + AnySubscriptionCallback::::LoanedWithMessageInfo(_), + )); + } + + fn test_regular(_msg: TestMessage) {} + + fn test_regular_with_info(_msg: TestMessage, _info: MessageInfo) {} + + fn test_boxed(_msg: Box) {} + + fn test_boxed_with_info(_msg: Box, _info: MessageInfo) {} + + fn test_loaned(_msg: ReadOnlyLoanedMessage) {} + + fn test_loaned_with_info(_msg: ReadOnlyLoanedMessage, _info: MessageInfo) {} +} diff --git a/rclrs/src/test_helpers/graph_helpers.rs b/rclrs/src/test_helpers/graph_helpers.rs index 1e9b581ae..f61f5db96 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -1,19 +1,14 @@ -use crate::{Context, Node, NodeBuilder, RclrsError}; -use std::sync::Arc; +use crate::{Context, IntoNodeOptions, Node, RclrsError}; pub(crate) struct TestGraph { - pub node1: Arc, - pub node2: Arc, + pub node1: Node, + pub node2: Node, } pub(crate) fn construct_test_graph(namespace: &str) -> Result { - let context = Context::new([])?; + let executor = Context::default().create_basic_executor(); Ok(TestGraph { - node1: NodeBuilder::new(&context, "graph_test_node_1") - .namespace(namespace) - .build()?, - node2: NodeBuilder::new(&context, "graph_test_node_2") - .namespace(namespace) - .build()?, + node1: executor.create_node("graph_test_node_1".namespace(namespace))?, + node2: executor.create_node("graph_test_node_2".namespace(namespace))?, }) } diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index 0be0c07ec..d8165b937 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -1,7 +1,8 @@ use crate::{ clock::{Clock, ClockSource, ClockType}, vendor::rosgraph_msgs::msg::Clock as ClockMsg, - Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, + IntoPrimitiveOptions, Node, NodeState, QoSProfile, ReadOnlyParameter, Subscription, + QOS_PROFILE_CLOCK, }; use std::sync::{Arc, Mutex, RwLock, Weak}; @@ -9,12 +10,12 @@ use std::sync::{Arc, Mutex, RwLock, Weak}; /// If the node's `use_sim_time` parameter is set to `true`, the `TimeSource` will subscribe /// to the `/clock` topic and drive the attached clock pub(crate) struct TimeSource { - node: Mutex>, + node: Mutex>, clock: RwLock, clock_source: Arc>>, requested_clock_type: ClockType, clock_qos: QoSProfile, - clock_subscription: Mutex>>>, + clock_subscription: Mutex>>, last_received_time: Arc>>, // TODO(luca) Make this parameter editable when we have parameter callbacks implemented and can // safely change clock type at runtime @@ -85,7 +86,7 @@ impl TimeSource { /// Attaches the given node to to the `TimeSource`, using its interface to read the /// `use_sim_time` parameter and create the clock subscription. - pub(crate) fn attach_node(&self, node: &Arc) { + pub(crate) fn attach_node(&self, node: &Node) { // TODO(luca) Make this parameter editable and register a parameter callback // that calls set_ros_time(bool) once parameter callbacks are implemented. let param = node @@ -122,7 +123,7 @@ impl TimeSource { clock.set_ros_time_override(nanoseconds); } - fn create_clock_sub(&self) -> Arc> { + fn create_clock_sub(&self) -> Subscription { let clock = self.clock_source.clone(); let last_received_time = self.last_received_time.clone(); // Safe to unwrap since the function will only fail if invalid arguments are provided @@ -131,42 +132,51 @@ impl TimeSource { .unwrap() .upgrade() .unwrap() - .create_subscription::("/clock", self.clock_qos, move |msg: ClockMsg| { - let nanoseconds: i64 = - (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64; - *last_received_time.lock().unwrap() = Some(nanoseconds); - if let Some(clock) = clock.lock().unwrap().as_mut() { - Self::update_clock(clock, nanoseconds); - } - }) + .create_subscription::( + "/clock".qos(self.clock_qos), + move |msg: ClockMsg| { + let nanoseconds: i64 = + (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64; + *last_received_time.lock().unwrap() = Some(nanoseconds); + if let Some(clock) = clock.lock().unwrap().as_mut() { + Self::update_clock(clock, nanoseconds); + } + }, + ) .unwrap() } } #[cfg(test)] mod tests { - use crate::{create_node, Context}; + use crate::{Context, InitOptions}; #[test] fn time_source_default_clock() { - let node = create_node( - &Context::new([]).unwrap(), - &format!("time_source_test_node_{}", line!()), - ) - .unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node(&format!("time_source_test_node_{}", line!())) + .unwrap(); // Default clock should be above 0 (use_sim_time is default false) assert!(node.get_clock().now().nsec > 0); } #[test] fn time_source_sim_time() { - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("use_sim_time:=true"), - ]) - .unwrap(); - let node = create_node(&ctx, &format!("time_source_test_node_{}", line!())).unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("use_sim_time:=true"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor + .create_node(&format!("time_source_test_node_{}", line!())) + .unwrap(); // Default sim time value should be 0 (no message received) assert_eq!(node.get_clock().now().nsec, 0); } diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs deleted file mode 100644 index 243c9d857..000000000 --- a/rclrs/src/wait.rs +++ /dev/null @@ -1,443 +0,0 @@ -// Copyright 2020 DCS Corporation, All Rights Reserved. - -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at - -// http://www.apache.org/licenses/LICENSE-2.0 - -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// DISTRIBUTION A. Approved for public release; distribution unlimited. -// OPSEC #4584. - -use std::{sync::Arc, time::Duration, vec::Vec}; - -use crate::{ - error::{to_rclrs_result, RclReturnCode, RclrsError, ToResult}, - rcl_bindings::*, - ClientBase, Context, ContextHandle, Node, ServiceBase, SubscriptionBase, -}; - -mod exclusivity_guard; -mod guard_condition; -use exclusivity_guard::*; -pub use guard_condition::*; - -/// Manage the lifecycle of an `rcl_wait_set_t`, including managing its dependency -/// on `rcl_context_t` by ensuring that this dependency is [dropped after][1] the -/// `rcl_wait_set_t`. -/// -/// [1]: -struct WaitSetHandle { - rcl_wait_set: rcl_wait_set_t, - // Used to ensure the context is alive while the wait set is alive. - #[allow(dead_code)] - context_handle: Arc, -} - -/// A struct for waiting on subscriptions and other waitable entities to become ready. -pub struct WaitSet { - // The subscriptions that are currently registered in the wait set. - // This correspondence is an invariant that must be maintained by all functions, - // even in the error case. - subscriptions: Vec>>, - clients: Vec>>, - // The guard conditions that are currently registered in the wait set. - guard_conditions: Vec>>, - services: Vec>>, - handle: WaitSetHandle, -} - -/// A list of entities that are ready, returned by [`WaitSet::wait`]. -pub struct ReadyEntities { - /// A list of subscriptions that have potentially received messages. - pub subscriptions: Vec>, - /// A list of clients that have potentially received responses. - pub clients: Vec>, - /// A list of guard conditions that have been triggered. - pub guard_conditions: Vec>, - /// A list of services that have potentially received requests. - pub services: Vec>, -} - -impl Drop for rcl_wait_set_t { - fn drop(&mut self) { - // SAFETY: No preconditions for this function (besides passing in a valid wait set). - let rc = unsafe { rcl_wait_set_fini(self) }; - if let Err(e) = to_rclrs_result(rc) { - panic!("Unable to release WaitSet. {:?}", e) - } - } -} - -// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread -// they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for rcl_wait_set_t {} - -// SAFETY: While the rcl_wait_set_t does have some interior mutability (because it has -// members of non-const pointer type), this interior mutability is hidden/not used by -// the WaitSet type. Therefore, sharing &WaitSet between threads does not risk data races. -unsafe impl Sync for WaitSet {} - -impl WaitSet { - /// Creates a new wait set. - /// - /// The given number of subscriptions is a capacity, corresponding to how often - /// [`WaitSet::add_subscription`] may be called. - pub fn new( - number_of_subscriptions: usize, - number_of_guard_conditions: usize, - number_of_timers: usize, - number_of_clients: usize, - number_of_services: usize, - number_of_events: usize, - context: &Context, - ) -> Result { - let rcl_wait_set = unsafe { - // SAFETY: Getting a zero-initialized value is always safe - let mut rcl_wait_set = rcl_get_zero_initialized_wait_set(); - let mut rcl_context = context.handle.rcl_context.lock().unwrap(); - // SAFETY: We're passing in a zero-initialized wait set and a valid context. - // There are no other preconditions. - rcl_wait_set_init( - &mut rcl_wait_set, - number_of_subscriptions, - number_of_guard_conditions, - number_of_timers, - number_of_clients, - number_of_services, - number_of_events, - &mut *rcl_context, - rcutils_get_default_allocator(), - ) - .ok()?; - rcl_wait_set - }; - Ok(Self { - subscriptions: Vec::new(), - guard_conditions: Vec::new(), - clients: Vec::new(), - services: Vec::new(), - handle: WaitSetHandle { - rcl_wait_set, - context_handle: Arc::clone(&context.handle), - }, - }) - } - - /// 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 { - handle: Arc::clone(&node.handle.context_handle), - }; - 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 - /// [`WaitSet::new`]. - pub fn clear(&mut self) { - self.subscriptions.clear(); - self.guard_conditions.clear(); - self.clients.clear(); - self.services.clear(); - // This cannot fail – the rcl_wait_set_clear function only checks that the input handle is - // valid, which it always is in our case. Hence, only debug_assert instead of returning - // Result. - // SAFETY: No preconditions for this function (besides passing in a valid wait set). - let ret = unsafe { rcl_wait_set_clear(&mut self.handle.rcl_wait_set) }; - debug_assert_eq!(ret, 0); - } - - /// Adds a subscription to the wait set. - /// - /// # Errors - /// - If the subscription was already added to this wait set or another one, - /// [`AlreadyAddedToWaitSet`][1] will be returned - /// - If the number of subscriptions in the wait set is larger than the - /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned - /// - /// [1]: crate::RclrsError - /// [2]: crate::RclReturnCode - pub fn add_subscription( - &mut self, - subscription: Arc, - ) -> Result<(), RclrsError> { - let exclusive_subscription = ExclusivityGuard::new( - Arc::clone(&subscription), - Arc::clone(&subscription.handle().in_use_by_wait_set), - )?; - unsafe { - // SAFETY: I'm not sure if it's required, but the subscription pointer will remain valid - // for as long as the wait set exists, because it's stored in self.subscriptions. - // Passing in a null pointer for the third argument is explicitly allowed. - rcl_wait_set_add_subscription( - &mut self.handle.rcl_wait_set, - &*subscription.handle().lock(), - std::ptr::null_mut(), - ) - } - .ok()?; - self.subscriptions.push(exclusive_subscription); - Ok(()) - } - - /// Adds a guard condition to the wait set. - /// - /// # Errors - /// - If the guard condition was already added to this wait set or another one, - /// [`AlreadyAddedToWaitSet`][1] will be returned - /// - If the number of guard conditions in the wait set is larger than the - /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned - /// - /// [1]: crate::RclrsError - /// [2]: crate::RclReturnCode - pub fn add_guard_condition( - &mut self, - guard_condition: Arc, - ) -> Result<(), RclrsError> { - let exclusive_guard_condition = ExclusivityGuard::new( - Arc::clone(&guard_condition), - Arc::clone(&guard_condition.in_use_by_wait_set), - )?; - - unsafe { - // SAFETY: Safe if the wait set and guard condition are initialized - rcl_wait_set_add_guard_condition( - &mut self.handle.rcl_wait_set, - &*guard_condition.handle.rcl_guard_condition.lock().unwrap(), - std::ptr::null_mut(), - ) - .ok()?; - } - self.guard_conditions.push(exclusive_guard_condition); - Ok(()) - } - - /// Adds a client to the wait set. - /// - /// # Errors - /// - If the client was already added to this wait set or another one, - /// [`AlreadyAddedToWaitSet`][1] will be returned - /// - If the number of clients in the wait set is larger than the - /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned - /// - /// [1]: crate::RclrsError - /// [2]: crate::RclReturnCode - pub fn add_client(&mut self, client: Arc) -> Result<(), RclrsError> { - let exclusive_client = ExclusivityGuard::new( - Arc::clone(&client), - Arc::clone(&client.handle().in_use_by_wait_set), - )?; - unsafe { - // SAFETY: I'm not sure if it's required, but the client pointer will remain valid - // for as long as the wait set exists, because it's stored in self.clients. - // Passing in a null pointer for the third argument is explicitly allowed. - rcl_wait_set_add_client( - &mut self.handle.rcl_wait_set, - &*client.handle().lock() as *const _, - core::ptr::null_mut(), - ) - } - .ok()?; - self.clients.push(exclusive_client); - Ok(()) - } - - /// Adds a service to the wait set. - /// - /// # Errors - /// - If the service was already added to this wait set or another one, - /// [`AlreadyAddedToWaitSet`][1] will be returned - /// - If the number of services in the wait set is larger than the - /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned - /// - /// [1]: crate::RclrsError - /// [2]: crate::RclReturnCode - pub fn add_service(&mut self, service: Arc) -> Result<(), RclrsError> { - let exclusive_service = ExclusivityGuard::new( - Arc::clone(&service), - Arc::clone(&service.handle().in_use_by_wait_set), - )?; - unsafe { - // SAFETY: I'm not sure if it's required, but the service pointer will remain valid - // for as long as the wait set exists, because it's stored in self.services. - // Passing in a null pointer for the third argument is explicitly allowed. - rcl_wait_set_add_service( - &mut self.handle.rcl_wait_set, - &*service.handle().lock() as *const _, - core::ptr::null_mut(), - ) - } - .ok()?; - self.services.push(exclusive_service); - Ok(()) - } - - /// Blocks until the wait set is ready, or until the timeout has been exceeded. - /// - /// If the timeout is `None` then this function will block indefinitely until - /// something in the wait set is valid or it is interrupted. - /// - /// If the timeout is [`Duration::ZERO`][1] then this function will be non-blocking; checking what's - /// ready now, but not waiting if nothing is ready yet. - /// - /// If the timeout is greater than [`Duration::ZERO`][1] then this function will return after - /// that period of time has elapsed or the wait set becomes ready, which ever - /// comes first. - /// - /// This function does not change the entities registered in the wait set. - /// - /// # Errors - /// - /// - Passing a wait set with no wait-able items in it will return an error. - /// - The timeout must not be so large so as to overflow an `i64` with its nanosecond - /// representation, or an error will occur. - /// - /// This list is not comprehensive, since further errors may occur in the `rmw` or `rcl` layers. - /// - /// [1]: std::time::Duration::ZERO - pub fn wait(mut self, timeout: Option) -> Result { - let timeout_ns = match timeout.map(|d| d.as_nanos()) { - None => -1, - Some(ns) if ns <= i64::MAX as u128 => ns as i64, - _ => { - return Err(RclrsError::RclError { - code: RclReturnCode::InvalidArgument, - msg: None, - }) - } - }; - // SAFETY: The comments in rcl mention "This function cannot operate on the same wait set - // in multiple threads, and the wait sets may not share content." - // We cannot currently guarantee that the wait sets may not share content, but it is - // mentioned in the doc comment for `add_subscription`. - // Also, the rcl_wait_set is obviously valid. - match unsafe { rcl_wait(&mut self.handle.rcl_wait_set, timeout_ns) }.ok() { - Ok(_) => (), - Err(error) => match error { - RclrsError::RclError { code, msg } => match code { - RclReturnCode::WaitSetEmpty => (), - _ => return Err(RclrsError::RclError { code, msg }), - }, - _ => return Err(error), - }, - } - let mut ready_entities = ReadyEntities { - subscriptions: Vec::new(), - clients: Vec::new(), - guard_conditions: Vec::new(), - services: Vec::new(), - }; - for (i, subscription) in self.subscriptions.iter().enumerate() { - // SAFETY: The `subscriptions` entry is an array of pointers, and this dereferencing is - // equivalent to - // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 - let wait_set_entry = unsafe { *self.handle.rcl_wait_set.subscriptions.add(i) }; - if !wait_set_entry.is_null() { - ready_entities - .subscriptions - .push(Arc::clone(&subscription.waitable)); - } - } - - for (i, client) in self.clients.iter().enumerate() { - // SAFETY: The `clients` entry is an array of pointers, and this dereferencing is - // equivalent to - // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 - let wait_set_entry = unsafe { *self.handle.rcl_wait_set.clients.add(i) }; - if !wait_set_entry.is_null() { - ready_entities.clients.push(Arc::clone(&client.waitable)); - } - } - - for (i, guard_condition) in self.guard_conditions.iter().enumerate() { - // SAFETY: The `clients` entry is an array of pointers, and this dereferencing is - // equivalent to - // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 - let wait_set_entry = unsafe { *self.handle.rcl_wait_set.guard_conditions.add(i) }; - if !wait_set_entry.is_null() { - ready_entities - .guard_conditions - .push(Arc::clone(&guard_condition.waitable)); - } - } - - for (i, service) in self.services.iter().enumerate() { - // SAFETY: The `services` entry is an array of pointers, and this dereferencing is - // equivalent to - // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 - let wait_set_entry = unsafe { *self.handle.rcl_wait_set.services.add(i) }; - if !wait_set_entry.is_null() { - ready_entities.services.push(Arc::clone(&service.waitable)); - } - } - Ok(ready_entities) - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn traits() { - use crate::test_helpers::*; - - assert_send::(); - assert_sync::(); - } - - #[test] - fn guard_condition_in_wait_set_readies() -> Result<(), RclrsError> { - let context = Context::new([])?; - - let guard_condition = Arc::new(GuardCondition::new(&context)); - - let mut wait_set = WaitSet::new(0, 1, 0, 0, 0, 0, &context)?; - wait_set.add_guard_condition(Arc::clone(&guard_condition))?; - guard_condition.trigger()?; - - let readies = wait_set.wait(Some(std::time::Duration::from_millis(10)))?; - assert!(readies.guard_conditions.contains(&guard_condition)); - - Ok(()) - } -} diff --git a/rclrs/src/wait/exclusivity_guard.rs b/rclrs/src/wait/exclusivity_guard.rs deleted file mode 100644 index 7c18ba66d..000000000 --- a/rclrs/src/wait/exclusivity_guard.rs +++ /dev/null @@ -1,58 +0,0 @@ -use std::sync::{ - atomic::{AtomicBool, Ordering}, - Arc, -}; - -use crate::RclrsError; - -/// A helper struct for tracking whether the waitable is currently in a wait set. -/// -/// When this struct is constructed, which happens when adding an entity to the wait set, -/// it checks that the atomic boolean is false and sets it to true. -/// When it is dropped, which happens when it is removed from the wait set, -/// or the wait set itself is dropped, it sets the atomic bool to false. -pub(super) struct ExclusivityGuard { - in_use_by_wait_set: Arc, - pub(super) waitable: T, -} - -impl Drop for ExclusivityGuard { - fn drop(&mut self) { - self.in_use_by_wait_set.store(false, Ordering::Relaxed) - } -} - -impl ExclusivityGuard { - pub fn new(waitable: T, in_use_by_wait_set: Arc) -> Result { - if in_use_by_wait_set - .compare_exchange(false, true, Ordering::Relaxed, Ordering::Relaxed) - .is_err() - { - return Err(RclrsError::AlreadyAddedToWaitSet); - } - Ok(Self { - in_use_by_wait_set, - waitable, - }) - } -} - -#[cfg(test)] -mod tests { - use std::sync::{ - atomic::{AtomicBool, Ordering}, - Arc, - }; - - use super::*; - - #[test] - fn test_exclusivity_guard() { - let atomic = Arc::new(AtomicBool::new(false)); - let eg = ExclusivityGuard::new((), Arc::clone(&atomic)).unwrap(); - assert!(ExclusivityGuard::new((), Arc::clone(&atomic)).is_err()); - drop(eg); - assert!(!atomic.load(Ordering::Relaxed)); - assert!(ExclusivityGuard::new((), Arc::clone(&atomic)).is_ok()); - } -} diff --git a/rclrs/src/wait/guard_condition.rs b/rclrs/src/wait/guard_condition.rs deleted file mode 100644 index 92a6acd00..000000000 --- a/rclrs/src/wait/guard_condition.rs +++ /dev/null @@ -1,209 +0,0 @@ -use std::sync::{atomic::AtomicBool, Arc, Mutex}; - -use crate::{rcl_bindings::*, Context, ContextHandle, RclrsError, ToResult}; - -/// A waitable entity used for waking up a wait set manually. -/// -/// If a wait set that is currently waiting on events should be interrupted from a separate thread, this can be done -/// by adding an `Arc` to the wait set, and calling `trigger()` on the same `GuardCondition` while -/// the wait set is waiting. -/// -/// The guard condition may be reused multiple times, but like other waitable entities, can not be used in -/// multiple wait sets concurrently. -/// -/// # Example -/// ``` -/// # use rclrs::{Context, GuardCondition, WaitSet, RclrsError}; -/// # use std::sync::{Arc, atomic::Ordering}; -/// -/// let context = Context::new([])?; -/// -/// let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false)); -/// let atomic_bool_for_closure = Arc::clone(&atomic_bool); -/// -/// let gc = Arc::new(GuardCondition::new_with_callback( -/// &context, -/// move || { -/// atomic_bool_for_closure.store(true, Ordering::Relaxed); -/// }, -/// )); -/// -/// let mut ws = WaitSet::new(0, 1, 0, 0, 0, 0, &context)?; -/// ws.add_guard_condition(Arc::clone(&gc))?; -/// -/// // Trigger the guard condition, firing the callback and waking the wait set being waited on, if any. -/// gc.trigger()?; -/// -/// // The provided callback has now been called. -/// assert_eq!(atomic_bool.load(Ordering::Relaxed), true); -/// -/// // The wait call will now immediately return. -/// ws.wait(Some(std::time::Duration::from_millis(10)))?; -/// -/// # Ok::<(), RclrsError>(()) -/// ``` -pub struct GuardCondition { - /// The rcl_guard_condition_t that this struct encapsulates. - pub(crate) handle: GuardConditionHandle, - /// An optional callback to call when this guard condition is triggered. - callback: Option>, - /// A flag to indicate if this guard condition has already been assigned to a wait set. - pub(crate) in_use_by_wait_set: Arc, -} - -/// Manage the lifecycle of an `rcl_guard_condition_t`, including managing its dependency -/// on `rcl_context_t` by ensuring that this dependency is [dropped after][1] the -/// `rcl_guard_condition_t`. -/// -/// [1]: -pub(crate) struct GuardConditionHandle { - pub(crate) rcl_guard_condition: Mutex, - /// Keep the context alive for the whole lifecycle of the guard condition - #[allow(dead_code)] - pub(crate) context_handle: Arc, -} - -impl Drop for GuardCondition { - fn drop(&mut self) { - unsafe { - // SAFETY: No precondition for this function (besides passing in a valid guard condition) - rcl_guard_condition_fini(&mut *self.handle.rcl_guard_condition.lock().unwrap()); - } - } -} - -impl PartialEq for GuardCondition { - fn eq(&self, other: &Self) -> bool { - // Because GuardCondition controls the creation of the rcl_guard_condition, each unique GuardCondition should have a unique - // rcl_guard_condition. Thus comparing equality of this member should be enough. - std::ptr::eq( - &self.handle.rcl_guard_condition.lock().unwrap().impl_, - &other.handle.rcl_guard_condition.lock().unwrap().impl_, - ) - } -} - -impl Eq for GuardCondition {} - -// SAFETY: rcl_guard_condition is the only member that doesn't implement Send, and it is designed to be accessed from other threads -unsafe impl Send for rcl_guard_condition_t {} - -impl GuardCondition { - /// Creates a new guard condition with no callback. - pub fn new(context: &Context) -> Self { - Self::new_with_context_handle(Arc::clone(&context.handle), None) - } - - /// Creates a new guard condition with a callback. - pub fn new_with_callback(context: &Context, callback: F) -> Self - where - F: Fn() + Send + Sync + 'static, - { - Self::new_with_context_handle( - Arc::clone(&context.handle), - Some(Box::new(callback) as Box), - ) - } - - /// Creates a new guard condition by providing the rcl_context_t and an optional callback. - /// Note this function enables calling `Node::create_guard_condition`[1] without providing the Context separately - /// - /// [1]: Node::create_guard_condition - pub(crate) fn new_with_context_handle( - context_handle: Arc, - callback: Option>, - ) -> Self { - let rcl_guard_condition = { - // SAFETY: Getting a zero initialized value is always safe - let mut guard_condition = unsafe { rcl_get_zero_initialized_guard_condition() }; - let mut rcl_context = context_handle.rcl_context.lock().unwrap(); - unsafe { - // SAFETY: The context must be valid, and the guard condition must be zero-initialized - rcl_guard_condition_init( - &mut guard_condition, - &mut *rcl_context, - rcl_guard_condition_get_default_options(), - ); - } - - Mutex::new(guard_condition) - }; - - Self { - handle: GuardConditionHandle { - rcl_guard_condition, - context_handle, - }, - callback, - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), - } - } - - /// Triggers this guard condition, activating the wait set, and calling the optionally assigned callback. - pub fn trigger(&self) -> Result<(), RclrsError> { - unsafe { - // SAFETY: The rcl_guard_condition_t is valid. - rcl_trigger_guard_condition(&mut *self.handle.rcl_guard_condition.lock().unwrap()) - .ok()?; - } - if let Some(callback) = &self.callback { - callback(); - } - Ok(()) - } -} - -#[cfg(test)] -mod tests { - use std::sync::atomic::Ordering; - - use super::*; - use crate::WaitSet; - - #[test] - fn test_guard_condition() -> Result<(), RclrsError> { - let context = Context::new([])?; - - let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false)); - let atomic_bool_for_closure = Arc::clone(&atomic_bool); - - let guard_condition = GuardCondition::new_with_callback(&context, move || { - atomic_bool_for_closure.store(true, Ordering::Relaxed); - }); - - guard_condition.trigger()?; - - assert!(atomic_bool.load(Ordering::Relaxed)); - - Ok(()) - } - - #[test] - fn test_guard_condition_wait() -> Result<(), RclrsError> { - let context = Context::new([])?; - - let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false)); - let atomic_bool_for_closure = Arc::clone(&atomic_bool); - - let guard_condition = Arc::new(GuardCondition::new_with_callback(&context, move || { - atomic_bool_for_closure.store(true, Ordering::Relaxed); - })); - - let mut wait_set = WaitSet::new(0, 1, 0, 0, 0, 0, &context)?; - wait_set.add_guard_condition(Arc::clone(&guard_condition))?; - guard_condition.trigger()?; - - assert!(atomic_bool.load(Ordering::Relaxed)); - wait_set.wait(Some(std::time::Duration::from_millis(10)))?; - - Ok(()) - } - - #[test] - fn traits() { - use crate::test_helpers::*; - - assert_send::(); - assert_sync::(); - } -} diff --git a/rclrs/src/wait_set.rs b/rclrs/src/wait_set.rs new file mode 100644 index 000000000..5729aa98c --- /dev/null +++ b/rclrs/src/wait_set.rs @@ -0,0 +1,288 @@ +// Copyright 2020 DCS Corporation, All Rights Reserved. + +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at + +// http://www.apache.org/licenses/LICENSE-2.0 + +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// DISTRIBUTION A. Approved for public release; distribution unlimited. +// OPSEC #4584. + +use std::{collections::HashMap, sync::Arc, time::Duration, vec::Vec}; + +use crate::{ + error::{to_rclrs_result, RclReturnCode, RclrsError, ToResult}, + rcl_bindings::*, + Context, ContextHandle, +}; + +mod guard_condition; +pub use guard_condition::*; + +mod rcl_primitive; +pub use rcl_primitive::*; + +mod waitable; +pub use waitable::*; + +mod wait_set_runner; +pub use wait_set_runner::*; + +/// A struct for waiting on subscriptions and other waitable entities to become ready. +pub struct WaitSet { + primitives: HashMap>, + handle: WaitSetHandle, +} + +// SAFETY: While the rcl_wait_set_t does have some interior mutability (because it has +// members of non-const pointer type), this interior mutability is hidden/not used by +// the WaitSet type. Therefore, sharing &WaitSet between threads does not risk data races. +unsafe impl Sync for WaitSet {} + +impl WaitSet { + /// Creates a new empty wait set. + pub fn new(context: &Context) -> Result { + let count = WaitableCount::new(); + let rcl_wait_set = + unsafe { count.initialize(&mut context.handle.rcl_context.lock().unwrap())? }; + + let handle = WaitSetHandle { + rcl_wait_set, + context_handle: Arc::clone(&context.handle), + }; + + let mut wait_set = Self { + primitives: HashMap::new(), + handle, + }; + wait_set.register_rcl_primitives()?; + Ok(wait_set) + } + + /// Take all the items out of `entities` and move them into this wait set. + pub fn add(&mut self, entities: impl IntoIterator) -> Result<(), RclrsError> { + for entity in entities { + if entity.in_wait_set() { + return Err(RclrsError::AlreadyAddedToWaitSet); + } + let kind = entity.primitive.kind(); + self.primitives.entry(kind).or_default().push(entity); + } + self.resize_rcl_containers()?; + self.register_rcl_primitives()?; + Ok(()) + } + + /// Removes all entities from the wait set. + /// + /// This effectively resets the wait set to the state it was in after being created by + /// [`WaitSet::new`]. + pub fn clear(&mut self) { + self.primitives.clear(); + self.rcl_clear(); + } + + /// Blocks until the wait set is ready, or until the timeout has been exceeded. + /// + /// If the timeout is `None` then this function will block indefinitely until + /// something in the wait set is valid or it is interrupted. + /// + /// If the timeout is [`Duration::ZERO`][1] then this function will be non-blocking; checking what's + /// ready now, but not waiting if nothing is ready yet. + /// + /// If the timeout is greater than [`Duration::ZERO`][1] then this function will return after + /// that period of time has elapsed or the wait set becomes ready, which ever + /// comes first. + /// + /// Once one or more items in the wait set are ready, `f` will be triggered + /// for each ready item. + /// + /// This function does not change the entities registered in the wait set. + /// + /// # Errors + /// + /// - Passing a wait set with no wait-able items in it will return an error. + /// - The timeout must not be so large so as to overflow an `i64` with its nanosecond + /// representation, or an error will occur. + /// + /// This list is not comprehensive, since further errors may occur in the `rmw` or `rcl` layers. + /// + /// [1]: std::time::Duration::ZERO + pub fn wait( + &mut self, + timeout: Option, + mut f: impl FnMut(&mut dyn RclPrimitive) -> Result<(), RclrsError>, + ) -> Result<(), RclrsError> { + let timeout_ns = match timeout.map(|d| d.as_nanos()) { + None => -1, + Some(ns) if ns <= i64::MAX as u128 => ns as i64, + _ => { + return Err(RclrsError::RclError { + code: RclReturnCode::InvalidArgument, + msg: None, + }) + } + }; + // SAFETY: The comments in rcl mention "This function cannot operate on the same wait set + // in multiple threads, and the wait sets may not share content." + // We cannot currently guarantee that the wait sets may not share content, but it is + // mentioned in the doc comment for `add_subscription`. + // Also, the rcl_wait_set is obviously valid. + match unsafe { rcl_wait(&mut self.handle.rcl_wait_set, timeout_ns) }.ok() { + Ok(_) => (), + Err(error) => match error { + RclrsError::RclError { code, msg } => match code { + RclReturnCode::WaitSetEmpty => (), + _ => return Err(RclrsError::RclError { code, msg }), + }, + _ => return Err(error), + }, + } + + // Remove any waitables that are no longer being used + for waitable in self.primitives.values_mut() { + waitable.retain(|w| w.in_use()); + } + + // For the remaining entities, check if they were activated and then run + // the callback for those that were. + for waiter in self.primitives.values_mut().flat_map(|v| v) { + if waiter.is_ready(&self.handle.rcl_wait_set) { + f(&mut *waiter.primitive)?; + } + } + + // Each time we call rcl_wait, the rcl_wait_set_t handle will have some + // of its entities set to null, so we need to put them back in. We do + // not need to resize the rcl_wait_set_t because no new entities could + // have been added while we had the mutable borrow of the WaitSet. Some + // entities could have been removed, but that does not require a resizing. + + // Note that self.clear() will not change the allocated size of each rcl + // entity container, so we do not need to resize before re-registering + // the rcl entities. + self.rcl_clear(); + if let Err(err) = self.register_rcl_primitives() { + // TODO(@mxgrey): Log this error when logging is available + eprintln!("Error while registering rcl primitives: {err}"); + } + + Ok(()) + } + + /// Get a count of the different kinds of entities in the wait set. + pub fn count(&self) -> WaitableCount { + let mut c = WaitableCount::new(); + for (kind, collection) in &self.primitives { + c.add(*kind, collection.len()); + } + c + } + + fn resize_rcl_containers(&mut self) -> Result<(), RclrsError> { + let count = self.count(); + unsafe { + count.resize(&mut self.handle.rcl_wait_set)?; + } + Ok(()) + } + + /// Clear only the rcl_wait_set. This is done so that we can safely repopulate + /// it to perform another wait. This does not effect the entities that we + /// consider to still be in the wait set. + fn rcl_clear(&mut self) { + // This cannot fail – the rcl_wait_set_clear function only checks that the input handle is + // valid, which it always is in our case. Hence, only debug_assert instead of returning + // Result. + // SAFETY: No preconditions for this function (besides passing in a valid wait set). + let ret = unsafe { rcl_wait_set_clear(&mut self.handle.rcl_wait_set) }; + debug_assert_eq!(ret, 0); + } + + /// Registers all the waitable entities with the rcl wait set. + /// + /// # Errors + /// - If the number of subscriptions in the wait set is larger than the + /// allocated size [`WaitSetFull`][1] will be returned. If this happens + /// then there is a bug in rclrs. + /// + /// [1]: crate::RclReturnCode + fn register_rcl_primitives(&mut self) -> Result<(), RclrsError> { + for entity in self.primitives.values_mut().flat_map(|c| c) { + entity.add_to_wait_set(&mut self.handle.rcl_wait_set)?; + } + Ok(()) + } +} + +impl Drop for rcl_wait_set_t { + fn drop(&mut self) { + // SAFETY: No preconditions for this function (besides passing in a valid wait set). + let rc = unsafe { rcl_wait_set_fini(self) }; + if let Err(e) = to_rclrs_result(rc) { + panic!("Unable to release WaitSet. {:?}", e) + } + } +} + +// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread +// they are running in. Therefore, this type can be safely sent to another thread. +unsafe impl Send for rcl_wait_set_t {} + +/// Manage the lifecycle of an `rcl_wait_set_t`, including managing its dependency +/// on `rcl_context_t` by ensuring that this dependency is [dropped after][1] the +/// `rcl_wait_set_t`. +/// +/// [1]: +struct WaitSetHandle { + pub(crate) rcl_wait_set: rcl_wait_set_t, + // Used to ensure the context is alive while the wait set is alive. + #[allow(dead_code)] + context_handle: Arc, +} + +#[cfg(test)] +mod tests { + use crate::*; + use std::time::Duration; + + #[test] + fn traits() { + use crate::test_helpers::*; + + assert_send::(); + assert_sync::(); + } + + #[test] + fn guard_condition_in_wait_set_readies() -> Result<(), RclrsError> { + let mut executor = Context::default().create_basic_executor(); + + executor.commands().get_guard_condition().trigger().unwrap(); + + let start = std::time::Instant::now(); + // This should stop spinning right away because the guard condition was + // already triggered. + executor + .spin(SpinOptions::spin_once().timeout(Duration::from_secs(10))) + .unwrap(); + + // If it took more than a second to finish spinning then something is + // probably wrong. + // + // Note that this test could theoretically be flaky if it runs on a + // machine with very strange CPU scheduling behaviors. To have a test + // that is guaranteed to be stable we could write a custom executor for + // testing that will give us more introspection. + assert!(std::time::Instant::now() - start < Duration::from_secs(1)); + + Ok(()) + } +} diff --git a/rclrs/src/wait_set/guard_condition.rs b/rclrs/src/wait_set/guard_condition.rs new file mode 100644 index 000000000..729572354 --- /dev/null +++ b/rclrs/src/wait_set/guard_condition.rs @@ -0,0 +1,232 @@ +use std::{ + any::Any, + sync::{Arc, Mutex}, +}; + +use crate::{ + rcl_bindings::*, ContextHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, + ToResult, Waitable, WaitableLifecycle, +}; + +/// A waitable entity used for waking up a wait set manually. +/// +/// This is used internally to wake up the executor's wait set to check if its +/// spin conditions are still valid or to perform cleanup (releasing waitable +/// entries that have been dropped by the user). +/// +/// Users of rclrs have no reason to use this struct unless you are +/// implementing a custom executor. If you want to trigger a function to run on +/// the executor you should use [`ExecutorCommands::run`] or +/// [`ExecutorCommands::run_detached`]. +pub struct GuardCondition { + /// The rcl_guard_condition_t that this struct encapsulates. Holding onto + /// this keeps the rcl_guard_condition alive and allows us to trigger it. + handle: Arc, + /// This manages the lifecycle of this guard condition's waiter. Dropping + /// this will remove the guard condition from its wait set. + #[allow(unused)] + lifecycle: WaitableLifecycle, +} + +// SAFETY: rcl_guard_condition is the only member that doesn't implement Send, and it is designed to be accessed from other threads +unsafe impl Send for rcl_guard_condition_t {} +// SAFETY: We make sure internally to defend all invariants of the unowned +// pointer. +unsafe impl Send for InnerGuardConditionHandle {} + +impl GuardCondition { + /// Triggers this guard condition, activating the wait set, and calling the optionally assigned callback. + pub fn trigger(&self) -> Result<(), RclrsError> { + // SAFETY: The rcl_guard_condition_t is valid. + if let Some(handle) = self.handle.rcl_guard_condition.lock().unwrap().owned_mut() { + unsafe { + rcl_trigger_guard_condition(handle).ok()?; + } + } else { + return Err(RclrsError::UnownedGuardCondition); + } + Ok(()) + } + + /// Creates a new guard condition. This is only for internal use. Ordinary + /// users of rclrs do not need to access guard conditions. + pub(crate) fn new( + context: &Arc, + callback: Option>, + ) -> (Self, Waitable) { + let rcl_guard_condition = { + // SAFETY: Getting a zero initialized value is always safe + let mut guard_condition = unsafe { rcl_get_zero_initialized_guard_condition() }; + let mut rcl_context = context.rcl_context.lock().unwrap(); + unsafe { + // SAFETY: The context must be valid, and the guard condition must be zero-initialized + rcl_guard_condition_init( + &mut guard_condition, + &mut *rcl_context, + rcl_guard_condition_get_default_options(), + ); + } + + Mutex::new(InnerGuardConditionHandle::Owned(guard_condition)) + }; + + let handle = Arc::new(GuardConditionHandle { + rcl_guard_condition, + context_handle: Arc::clone(&context), + }); + + let (waitable, lifecycle) = Waitable::new( + Box::new(GuardConditionExecutable { + handle: Arc::clone(&handle), + callback, + }), + None, + ); + + (Self { handle, lifecycle }, waitable) + } + + /// SAFETY: The caller is responsible for ensuring that the pointer being + /// passed in is valid and non-null, and also that as long as `owner` is + /// held, the pointer will remain valid. + pub(crate) unsafe fn from_rcl( + context: &Arc, + rcl_guard_condition: *const rcl_guard_condition_t, + owner: Box, + callback: Option>, + ) -> (Self, Waitable) { + let rcl_guard_condition = Mutex::new(InnerGuardConditionHandle::Unowned { + handle: rcl_guard_condition, + owner, + }); + + let handle = Arc::new(GuardConditionHandle { + rcl_guard_condition, + context_handle: Arc::clone(&context), + }); + + let (waitable, lifecycle) = Waitable::new( + Box::new(GuardConditionExecutable { + handle: Arc::clone(&handle), + callback, + }), + None, + ); + + (Self { handle, lifecycle }, waitable) + } +} + +/// Manage the lifecycle of an `rcl_guard_condition_t`, including managing its dependency +/// on `rcl_context_t` by ensuring that this dependency is [dropped after][1] the +/// `rcl_guard_condition_t`. +/// +/// [1]: +struct GuardConditionHandle { + rcl_guard_condition: Mutex, + /// Keep the context alive for the whole lifecycle of the guard condition + #[allow(dead_code)] + context_handle: Arc, +} + +/// We need to wrap the underlying condition variable with this because some +/// condition variables are owned at the rclrs layer while others were obtained +/// from rcl and either have static lifetimes or lifetimes that depend on +/// something else. +pub enum InnerGuardConditionHandle { + /// This variant means the guard condition was created and owned by rclrs. + /// Its memory is managed by us. + Owned(rcl_guard_condition_t), + /// This variant means the guard condition was created and owned by rcl. + /// The owner object represents something that the lifecycle of the guard + /// condition depends on, such as the rcl_node that created it. + Unowned { + /// This is the unowned guard condition pointer. We must not deallocate + /// it. + handle: *const rcl_guard_condition_t, + /// This somehow holds a shared reference to the owner of the guard + /// condition. We need to hold onto this to ensure the guard condition + /// remains valid. + owner: Box, + }, +} + +impl InnerGuardConditionHandle { + /// Get the handle if it is owned by rclrs + pub fn owned(&self) -> Option<&rcl_guard_condition_t> { + match self { + Self::Owned(handle) => Some(handle), + _ => None, + } + } + + /// Get the handle if it is owned by rclrs + pub fn owned_mut(&mut self) -> Option<&mut rcl_guard_condition_t> { + match self { + Self::Owned(handle) => Some(handle), + _ => None, + } + } + + /// Apply a function to the handle + pub fn use_handle(&self, f: impl FnOnce(&rcl_guard_condition_t) -> Out) -> Out { + match self { + Self::Owned(handle) => f(handle), + Self::Unowned { handle, .. } => f(unsafe { + // SAFETY: The enum ensures that the pointer remains valid + handle.as_ref().unwrap() + }), + } + } +} + +impl Drop for GuardConditionHandle { + fn drop(&mut self) { + if let InnerGuardConditionHandle::Owned(rcl_guard_condition) = + &mut *self.rcl_guard_condition.lock().unwrap() + { + unsafe { + // SAFETY: No precondition for this function (besides passing in a valid guard condition) + rcl_guard_condition_fini(rcl_guard_condition); + } + } + } +} + +struct GuardConditionExecutable { + handle: Arc, + /// The callback that will be triggered when execute is called. Not all + /// guard conditions need to have a callback associated, so this could be + /// [`None`]. + callback: Option>, +} + +impl RclPrimitive for GuardConditionExecutable { + fn execute(&mut self) -> Result<(), RclrsError> { + if let Some(callback) = &mut self.callback { + callback(); + } + Ok(()) + } + + fn kind(&self) -> RclPrimitiveKind { + RclPrimitiveKind::GuardCondition + } + + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::GuardCondition(self.handle.rcl_guard_condition.lock().unwrap()) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn traits() { + use crate::test_helpers::*; + + assert_send::(); + assert_sync::(); + } +} diff --git a/rclrs/src/wait_set/rcl_primitive.rs b/rclrs/src/wait_set/rcl_primitive.rs new file mode 100644 index 000000000..78c237b7d --- /dev/null +++ b/rclrs/src/wait_set/rcl_primitive.rs @@ -0,0 +1,48 @@ +use std::sync::MutexGuard; + +use crate::{rcl_bindings::*, InnerGuardConditionHandle, RclrsError}; + +/// This provides the public API for executing a waitable item. +pub trait RclPrimitive: Send + Sync { + /// Trigger this primitive to run. + fn execute(&mut self) -> Result<(), RclrsError>; + + /// Indicate what kind of primitive this is. + fn kind(&self) -> RclPrimitiveKind; + + /// Provide the handle for this primitive + fn handle(&self) -> RclPrimitiveHandle; +} + +/// Enum to describe the kind of an executable. +#[derive(Clone, Copy, Hash, PartialEq, Eq, PartialOrd, Ord)] +pub enum RclPrimitiveKind { + /// Subscription + Subscription, + /// Guard Condition + GuardCondition, + /// Timer + Timer, + /// Client + Client, + /// Service + Service, + /// Event + Event, +} + +/// Used by the wait set to obtain the handle of a primitive. +pub enum RclPrimitiveHandle<'a> { + /// Handle for a subscription + Subscription(MutexGuard<'a, rcl_subscription_t>), + /// Handle for a guard condition + GuardCondition(MutexGuard<'a, InnerGuardConditionHandle>), + /// Handle for a timer + Timer(MutexGuard<'a, rcl_timer_t>), + /// Handle for a client + Client(MutexGuard<'a, rcl_client_t>), + /// Handle for a service + Service(MutexGuard<'a, rcl_service_t>), + /// Handle for an event + Event(MutexGuard<'a, rcl_event_t>), +} diff --git a/rclrs/src/wait_set/wait_set_runner.rs b/rclrs/src/wait_set/wait_set_runner.rs new file mode 100644 index 000000000..f7ce8c71a --- /dev/null +++ b/rclrs/src/wait_set/wait_set_runner.rs @@ -0,0 +1,120 @@ +use futures::channel::{ + mpsc::{unbounded, UnboundedReceiver, UnboundedSender}, + oneshot::channel, +}; + +use std::{ + sync::atomic::Ordering, + time::{Duration, Instant}, +}; + +use crate::{Context, Promise, RclrsError, SpinConditions, WaitSet, Waitable}; + +/// This is a utility class that executors can use to easily run and manage +/// their wait set. +pub struct WaitSetRunner { + wait_set: WaitSet, + waitable_sender: UnboundedSender, + waitable_receiver: UnboundedReceiver, +} + +impl WaitSetRunner { + /// Create a new WaitSetRunner. + pub fn new(context: &Context) -> Self { + let (waitable_sender, waitable_receiver) = unbounded(); + Self { + wait_set: WaitSet::new(context) + // SAFETY: This only gets called from Context which ensures that + // everything is valid when creating a wait set. + .expect("Unable to create wait set for basic executor"), + waitable_sender, + waitable_receiver, + } + } + + /// Get the sender that allows users to send new [`Waitables`] to this + /// `WaitSetRunner`. + pub fn sender(&self) -> UnboundedSender { + self.waitable_sender.clone() + } + + /// Spawn a thread to run the wait set. You will receive a Promise that will + /// be resolved once the wait set stops spinning. + /// + /// Note that if the user gives a [`SpinOptions::until_promise_resolved`], + /// the best practice is for your executor runtime to swap that out with a + /// new promise which ensures that the [`SpinConditions::guard_condition`] + /// will be triggered after the user-provided promise is resolved. + pub fn run(mut self, conditions: SpinConditions) -> Promise<(Self, Result<(), RclrsError>)> { + let (sender, promise) = channel(); + std::thread::spawn(move || { + let result = self.run_blocking(conditions); + // TODO(@mxgrey): Log any error here when logging becomes available + sender.send((self, result)).ok(); + }); + + promise + } + + /// Run the wait set on the current thread. This will block the execution of + /// the current thread until the wait set is finished waiting. + /// + /// Note that if the user gives a [`SpinOptions::until_promise_resolved`], + /// the best practice is for your executor runtime to swap that out with a + /// new promise which ensures that the [`SpinConditions::guard_condition`] + /// will be triggered after the user-provided promise is resolved. + pub fn run_blocking(&mut self, mut conditions: SpinConditions) -> Result<(), RclrsError> { + let mut first_spin = true; + let t_stop_spinning = conditions.options.timeout.map(|dt| Instant::now() + dt); + loop { + // TODO(@mxgrey): SmallVec would be better suited here if we are + // okay with adding that as a dependency. + let mut new_waitables = Vec::new(); + while let Ok(Some(new_waitable)) = self.waitable_receiver.try_next() { + new_waitables.push(new_waitable); + } + if !new_waitables.is_empty() { + // TODO(@mxgrey): Log any error here when logging becomes available + self.wait_set.add(new_waitables).ok(); + } + + if conditions.options.only_next_available_work && !first_spin { + // We've already completed a spin and were asked to only do one, + // so break here + return Ok(()); + } + first_spin = false; + + if let Some(promise) = &mut conditions.options.until_promise_resolved { + let r = promise.try_recv(); + if r.is_ok_and(|r| r.is_some()) || r.is_err() { + // The promise has been resolved, so we should stop spinning. + return Ok(()); + } + } + + if conditions.halt_spinning.load(Ordering::Acquire) { + // The user has manually asked for the spinning to stop + return Ok(()); + } + + if !conditions.context.ok() { + // The ROS context has switched to being invalid, so we should + // stop spinning. + return Ok(()); + } + + let timeout = t_stop_spinning.map(|t| { + let timeout = t - Instant::now(); + if timeout < Duration::ZERO { + Duration::ZERO + } else { + timeout + } + }); + + self.wait_set + .wait(timeout, |executable| executable.execute())?; + } + } +} diff --git a/rclrs/src/wait_set/waitable.rs b/rclrs/src/wait_set/waitable.rs new file mode 100644 index 000000000..a931846a0 --- /dev/null +++ b/rclrs/src/wait_set/waitable.rs @@ -0,0 +1,201 @@ +use std::sync::{ + atomic::{AtomicBool, Ordering}, + Arc, +}; + +use crate::{ + error::ToResult, rcl_bindings::*, GuardCondition, RclPrimitive, RclPrimitiveHandle, + RclPrimitiveKind, RclrsError, +}; + +/// This struct manages the presence of an rcl primitive inside the wait set. +/// It will keep track of where the primitive is within the wait set as well as +/// automatically remove the primitive from the wait set once it isn't being +/// used anymore. +#[must_use = "If you do not give the Waiter to a WaitSet then it will never be useful"] +pub struct Waitable { + pub(super) primitive: Box, + in_use: Arc, + index_in_wait_set: Option, +} + +impl Waitable { + /// Create a new waitable. + pub fn new( + primitive: Box, + guard_condition: Option>, + ) -> (Self, WaitableLifecycle) { + let in_use = Arc::new(AtomicBool::new(true)); + let waiter = Self { + primitive, + in_use: Arc::clone(&in_use), + index_in_wait_set: None, + }; + + let lifecycle = WaitableLifecycle { + in_use, + guard_condition, + }; + (waiter, lifecycle) + } + + pub(super) fn in_wait_set(&self) -> bool { + self.index_in_wait_set.is_some() + } + + pub(super) fn in_use(&self) -> bool { + self.in_use.load(Ordering::Relaxed) + } + + pub(super) fn is_ready(&self, wait_set: &rcl_wait_set_t) -> bool { + self.index_in_wait_set.is_some_and(|index| { + let ptr_is_null = unsafe { + // SAFETY: Each field in the wait set is an array of points. + // The dereferencing that we do is equivalent to obtaining the + // element of the array at the index-th position. + match self.primitive.kind() { + RclPrimitiveKind::Subscription => wait_set.subscriptions.add(index).is_null(), + RclPrimitiveKind::GuardCondition => { + wait_set.guard_conditions.add(index).is_null() + } + RclPrimitiveKind::Service => wait_set.services.add(index).is_null(), + RclPrimitiveKind::Client => wait_set.clients.add(index).is_null(), + RclPrimitiveKind::Timer => wait_set.timers.add(index).is_null(), + RclPrimitiveKind::Event => wait_set.events.add(index).is_null(), + } + }; + !ptr_is_null + }) + } + + pub(super) fn add_to_wait_set( + &mut self, + wait_set: &mut rcl_wait_set_t, + ) -> Result<(), RclrsError> { + let mut index = 0; + unsafe { + // SAFETY: The Executable is responsible for maintaining the lifecycle + // of the handle, so it is guaranteed to be valid here. + match self.primitive.handle() { + RclPrimitiveHandle::Subscription(handle) => { + rcl_wait_set_add_subscription(wait_set, &*handle, &mut index) + } + RclPrimitiveHandle::GuardCondition(handle) => handle.use_handle(|handle| { + rcl_wait_set_add_guard_condition(wait_set, &*handle, &mut index) + }), + RclPrimitiveHandle::Service(handle) => { + rcl_wait_set_add_service(wait_set, &*handle, &mut index) + } + RclPrimitiveHandle::Client(handle) => { + rcl_wait_set_add_client(wait_set, &*handle, &mut index) + } + RclPrimitiveHandle::Timer(handle) => { + rcl_wait_set_add_timer(wait_set, &*handle, &mut index) + } + RclPrimitiveHandle::Event(handle) => { + rcl_wait_set_add_event(wait_set, &*handle, &mut index) + } + } + } + .ok()?; + + self.index_in_wait_set = Some(index); + Ok(()) + } +} + +/// This is used internally to track whether an rcl primitive is still being +/// used. When this gets dropped, the rcl primitive will automatically be +/// removed from the wait set. +#[must_use = "If you do not hold onto the WaiterLifecycle, then its Waiter will be immediately dropped"] +pub struct WaitableLifecycle { + in_use: Arc, + guard_condition: Option>, +} + +impl Drop for WaitableLifecycle { + fn drop(&mut self) { + self.in_use.store(false, Ordering::Release); + if let Some(guard_condition) = &self.guard_condition { + guard_condition.trigger().ok(); + } + } +} + +/// Count the number of rcl primitives in the wait set. +#[derive(Default, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] +pub struct WaitableCount { + /// How many subscriptions are present + pub subscriptions: usize, + /// How many guard conditions are present + pub guard_conditions: usize, + /// How many timers are present + pub timers: usize, + /// How many clients are present + pub clients: usize, + /// How many services are present + pub services: usize, + /// How many events are present + pub events: usize, +} + +impl WaitableCount { + /// Begin a new count with everything starting out at zero. + pub fn new() -> Self { + Self::default() + } + + pub(super) fn add(&mut self, kind: RclPrimitiveKind, count: usize) { + match kind { + RclPrimitiveKind::Subscription => self.subscriptions += count, + RclPrimitiveKind::GuardCondition => self.guard_conditions += count, + RclPrimitiveKind::Timer => self.timers += count, + RclPrimitiveKind::Client => self.clients += count, + RclPrimitiveKind::Service => self.services += count, + RclPrimitiveKind::Event => self.events += count, + } + } + + pub(super) unsafe fn initialize( + &self, + rcl_context: &mut rcl_context_s, + ) -> Result { + unsafe { + // SAFETY: Getting a zero-initialized value is always safe + let mut rcl_wait_set = rcl_get_zero_initialized_wait_set(); + // SAFETY: We're passing in a zero-initialized wait set and a valid context. + // There are no other preconditions. + rcl_wait_set_init( + &mut rcl_wait_set, + self.subscriptions, + self.guard_conditions, + self.timers, + self.clients, + self.services, + self.events, + &mut *rcl_context, + rcutils_get_default_allocator(), + ) + .ok()?; + Ok(rcl_wait_set) + } + } + + pub(super) unsafe fn resize( + &self, + rcl_wait_set: &mut rcl_wait_set_t, + ) -> Result<(), RclrsError> { + unsafe { + rcl_wait_set_resize( + rcl_wait_set, + self.subscriptions, + self.guard_conditions, + self.timers, + self.clients, + self.services, + self.events, + ) + } + .ok() + } +}