diff --git a/examples/logging_demo/Cargo.toml b/examples/logging_demo/Cargo.toml new file mode 100644 index 000000000..778981c51 --- /dev/null +++ b/examples/logging_demo/Cargo.toml @@ -0,0 +1,8 @@ +[package] +name = "logging_demo" +version = "0.1.0" +edition = "2021" + +[dependencies] +rclrs = "0.4" +example_interfaces = "*" diff --git a/examples/logging_demo/package.xml b/examples/logging_demo/package.xml new file mode 100644 index 000000000..0cfdd3520 --- /dev/null +++ b/examples/logging_demo/package.xml @@ -0,0 +1,21 @@ + + + + examples_logging_demo + Esteve Fernandez + + Jacob Hassold + 0.4.1 + Package containing an example of how to use a worker in rclrs. + Apache License 2.0 + + rclrs + rosidl_runtime_rs + example_interfaces + + + ament_cargo + + diff --git a/examples/logging_demo/src/main.rs b/examples/logging_demo/src/main.rs new file mode 100644 index 000000000..789a93e00 --- /dev/null +++ b/examples/logging_demo/src/main.rs @@ -0,0 +1,39 @@ +use rclrs::*; +use std::time::Duration; + +fn main() -> Result<(), RclrsError> { + let mut executor = Context::default_from_env()?.create_basic_executor(); + let node = executor.create_node("logging_demo")?; + + let _subscription = node.clone().create_subscription( + "logging_demo", + move |msg: example_interfaces::msg::String| { + let data = msg.data; + + // You can apply modifiers such as .once() to node.logger() + // to dictate how the logging behaves. + log!(node.logger().once(), "First message: {data}",); + + log!(node.logger().skip_first(), "Subsequent message: {data}",); + + // You can chain multiple modifiers together. + log_warn!( + node.logger().skip_first().throttle(Duration::from_secs(5)), + "Throttled message: {data}", + ); + }, + )?; + + // Any &str can be used as the logger name and have + // logging modifiers applied to it. + log_info!( + "notice".once(), + "Ready to begin logging example_interfaces/msg/String messages published to 'logging_demo'.", + ); + log_warn!( + "help", + "Try running\n \ + $ ros2 topic pub logging_demo example_interfaces/msg/String \"data: message\"", + ); + executor.spin(SpinOptions::default()).first_error() +} diff --git a/examples/minimal_client_service/src/minimal_client.rs b/examples/minimal_client_service/src/minimal_client.rs index a8651b4a5..da287a6a1 100644 --- a/examples/minimal_client_service/src/minimal_client.rs +++ b/examples/minimal_client_service/src/minimal_client.rs @@ -1,4 +1,5 @@ use anyhow::{Error, Result}; +use example_interfaces::srv::*; use rclrs::*; fn main() -> Result<(), Error> { @@ -6,31 +7,25 @@ fn main() -> Result<(), Error> { let node = executor.create_node("minimal_client")?; - let client = node.create_client::("add_two_ints")?; + let client = node.create_client::("add_two_ints")?; - let request = example_interfaces::srv::AddTwoInts_Request { a: 41, b: 1 }; + let promise = executor.commands().run(async move { + println!("Waiting for service..."); + client.notify_on_service_ready().await.unwrap(); - println!("Starting client"); + let request = AddTwoInts_Request { a: 41, b: 1 }; - while !client.service_is_ready()? { - std::thread::sleep(std::time::Duration::from_millis(10)); - } + println!("Waiting for response"); + let response: AddTwoInts_Response = client.call(&request).unwrap().await.unwrap(); - client.async_send_request_with_callback( - &request, - move |response: example_interfaces::srv::AddTwoInts_Response| { - println!( - "Result of {} + {} is: {}", - request.a, request.b, response.sum - ); - }, - )?; + println!( + "Result of {} + {} is: {}", + request.a, request.b, response.sum, + ); + }); - std::thread::sleep(std::time::Duration::from_millis(500)); - - println!("Waiting for response"); executor - .spin(SpinOptions::default()) - .first_error() - .map_err(|err| err.into()) + .spin(SpinOptions::new().until_promise_resolved(promise)) + .first_error()?; + Ok(()) } diff --git a/examples/minimal_client_service/src/minimal_client_async.rs b/examples/minimal_client_service/src/minimal_client_async.rs index c31f2e26e..7feb462ca 100644 --- a/examples/minimal_client_service/src/minimal_client_async.rs +++ b/examples/minimal_client_service/src/minimal_client_async.rs @@ -1,4 +1,5 @@ use anyhow::{Error, Result}; +use example_interfaces::srv::*; use rclrs::*; #[tokio::main] @@ -7,7 +8,7 @@ async fn main() -> Result<(), Error> { let node = executor.create_node("minimal_client")?; - let client = node.create_client::("add_two_ints")?; + let client = node.create_client::("add_two_ints")?; println!("Starting client"); @@ -15,20 +16,20 @@ async fn main() -> Result<(), Error> { std::thread::sleep(std::time::Duration::from_millis(10)); } - let request = example_interfaces::srv::AddTwoInts_Request { a: 41, b: 1 }; + let request = AddTwoInts_Request { a: 41, b: 1 }; - let future = client.call_async(&request); + let promise = client + .call_then(&request, move |response: 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 || executor.spin(SpinOptions::default())); - - 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)) + .first_error()?; Ok(()) } diff --git a/examples/minimal_client_service/src/minimal_service.rs b/examples/minimal_client_service/src/minimal_service.rs index 0fe681dbf..8142012dd 100644 --- a/examples/minimal_client_service/src/minimal_service.rs +++ b/examples/minimal_client_service/src/minimal_service.rs @@ -1,12 +1,15 @@ use anyhow::{Error, Result}; +use example_interfaces::srv::*; use rclrs::*; -fn handle_service( - _request_header: &rclrs::rmw_request_id_t, - request: example_interfaces::srv::AddTwoInts_Request, -) -> example_interfaces::srv::AddTwoInts_Response { - println!("request: {} + {}", request.a, request.b); - example_interfaces::srv::AddTwoInts_Response { +fn handle_service(request: AddTwoInts_Request, info: ServiceInfo) -> AddTwoInts_Response { + let timestamp = info + .received_timestamp + .map(|t| format!(" at [{t:?}]")) + .unwrap_or(String::new()); + + println!("request{timestamp}: {} + {}", request.a, request.b); + AddTwoInts_Response { sum: request.a + request.b, } } @@ -16,12 +19,9 @@ fn main() -> Result<(), Error> { let node = executor.create_node("minimal_service")?; - let _server = node - .create_service::("add_two_ints", handle_service)?; + let _server = node.create_service::("add_two_ints", handle_service)?; println!("Starting server"); - executor - .spin(SpinOptions::default()) - .first_error() - .map_err(|err| err.into()) + executor.spin(SpinOptions::default()).first_error()?; + Ok(()) } diff --git a/examples/minimal_pub_sub/Cargo.toml b/examples/minimal_pub_sub/Cargo.toml index 209359698..ab7849541 100644 --- a/examples/minimal_pub_sub/Cargo.toml +++ b/examples/minimal_pub_sub/Cargo.toml @@ -29,7 +29,7 @@ path = "src/zero_copy_publisher.rs" anyhow = {version = "1", features = ["backtrace"]} rclrs = "0.4" rosidl_runtime_rs = "0.4" -std_msgs = "*" +example_interfaces = "*" # This specific version is compatible with Rust 1.75 backtrace = "=0.3.74" diff --git a/examples/minimal_pub_sub/package.xml b/examples/minimal_pub_sub/package.xml index da3e76041..3494e36bb 100644 --- a/examples/minimal_pub_sub/package.xml +++ b/examples/minimal_pub_sub/package.xml @@ -14,11 +14,11 @@ rclrs rosidl_runtime_rs - std_msgs + example_interfaces rclrs rosidl_runtime_rs - std_msgs + example_interfaces ament_cargo diff --git a/examples/minimal_pub_sub/src/minimal_publisher.rs b/examples/minimal_pub_sub/src/minimal_publisher.rs index f6f5d54da..2eaf409e2 100644 --- a/examples/minimal_pub_sub/src/minimal_publisher.rs +++ b/examples/minimal_pub_sub/src/minimal_publisher.rs @@ -7,9 +7,9 @@ fn main() -> Result<(), Error> { let node = executor.create_node("minimal_publisher")?; - let publisher = node.create_publisher::("topic")?; + let publisher = node.create_publisher::("topic")?; - let mut message = std_msgs::msg::String::default(); + let mut message = example_interfaces::msg::String::default(); let mut publish_count: u32 = 1; diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index a29b23e5f..7750c6062 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -7,19 +7,16 @@ fn main() -> Result<(), Error> { let node = executor.create_node("minimal_subscriber")?; - let mut num_messages: usize = 0; - - let _subscription = node.create_subscription::( + let worker = node.create_worker::(0); + let _subscription = worker.create_subscription::( "topic", - move |msg: std_msgs::msg::String| { - num_messages += 1; - println!("I heard: '{}'", msg.data); - println!("(Got {} messages so far)", num_messages); + move |num_messages: &mut usize, msg: example_interfaces::msg::String| { + *num_messages += 1; + println!("#{} | I heard: '{}'", *num_messages, msg.data); }, )?; - executor - .spin(SpinOptions::default()) - .first_error() - .map_err(|err| err.into()) + println!("Waiting for messages..."); + executor.spin(SpinOptions::default()).first_error()?; + Ok(()) } diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index 968d14d1f..b34947687 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -1,47 +1,41 @@ use rclrs::*; -use std::sync::{ - atomic::{AtomicU32, Ordering}, - Arc, Mutex, -}; +use std::sync::Arc; use anyhow::{Error, Result}; -struct MinimalSubscriber { - num_messages: AtomicU32, +struct MinimalSubscriberNode { + #[allow(unused)] + subscription: WorkerSubscription, +} + +struct SubscriptionData { node: Node, - subscription: Mutex>>, + num_messages: usize, } -impl MinimalSubscriber { - pub fn new(executor: &Executor, name: &str, topic: &str) -> Result, RclrsError> { +impl MinimalSubscriberNode { + pub fn new(executor: &Executor, name: &str, topic: &str) -> Result { let node = executor.create_node(name)?; - let minimal_subscriber = Arc::new(MinimalSubscriber { - num_messages: 0.into(), - node, - subscription: None.into(), + + let worker = node.create_worker::(SubscriptionData { + node: Arc::clone(&node), + num_messages: 0, }); - let minimal_subscriber_aux = Arc::clone(&minimal_subscriber); - let subscription = minimal_subscriber - .node - .create_subscription::( - topic, - move |msg: std_msgs::msg::String| { - minimal_subscriber_aux.callback(msg); - }, - )?; - *minimal_subscriber.subscription.lock().unwrap() = Some(subscription); - Ok(minimal_subscriber) - } + let subscription = worker.create_subscription( + topic, + |data: &mut SubscriptionData, msg: example_interfaces::msg::String| { + data.num_messages += 1; + println!("[{}] I heard: '{}'", data.node.name(), msg.data); + println!( + "[{}] (Got {} messages so far)", + data.node.name(), + data.num_messages, + ); + }, + )?; - fn callback(&self, msg: std_msgs::msg::String) { - self.num_messages.fetch_add(1, Ordering::SeqCst); - println!("[{}] I heard: '{}'", self.node.name(), msg.data); - println!( - "[{}] (Got {} messages so far)", - self.node.name(), - self.num_messages.load(Ordering::SeqCst) - ); + Ok(MinimalSubscriberNode { subscription }) } } @@ -50,14 +44,16 @@ fn main() -> Result<(), Error> { let publisher_node = executor.create_node("minimal_publisher")?; let _subscriber_node_one = - MinimalSubscriber::new(&executor, "minimal_subscriber_one", "topic")?; + MinimalSubscriberNode::new(&executor, "minimal_subscriber_one", "topic")?; let _subscriber_node_two = - MinimalSubscriber::new(&executor, "minimal_subscriber_two", "topic")?; + MinimalSubscriberNode::new(&executor, "minimal_subscriber_two", "topic")?; - let publisher = publisher_node.create_publisher::("topic")?; + let publisher = publisher_node.create_publisher::("topic")?; - std::thread::spawn(move || -> Result<(), RclrsError> { - let mut message = std_msgs::msg::String::default(); + // TODO(@mxgrey): Replace this with a timer once we have the Timer feature + // merged in. + std::thread::spawn(move || -> Result<(), rclrs::RclrsError> { + let mut message = example_interfaces::msg::String::default(); let mut publish_count: u32 = 1; loop { message.data = format!("Hello, world! {}", publish_count); @@ -68,8 +64,6 @@ fn main() -> Result<(), Error> { } }); - executor - .spin(SpinOptions::default()) - .first_error() - .map_err(|err| err.into()) + executor.spin(rclrs::SpinOptions::default()).first_error()?; + Ok(()) } diff --git a/examples/minimal_pub_sub/src/zero_copy_publisher.rs b/examples/minimal_pub_sub/src/zero_copy_publisher.rs index 0de7dcb5b..68d3700e2 100644 --- a/examples/minimal_pub_sub/src/zero_copy_publisher.rs +++ b/examples/minimal_pub_sub/src/zero_copy_publisher.rs @@ -7,7 +7,7 @@ fn main() -> Result<(), Error> { let node = executor.create_node("minimal_publisher")?; - let publisher = node.create_publisher::("topic")?; + 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 b3fa72e79..ce6c5a83f 100644 --- a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -6,19 +6,17 @@ fn main() -> Result<(), Error> { let node = executor.create_node("minimal_subscriber")?; - let mut num_messages: usize = 0; - - let _subscription = node.create_subscription::( + let worker = node.create_worker::(0); + let _subscription = worker.create_subscription::( "topic", - move |msg: rclrs::ReadOnlyLoanedMessage<'_, std_msgs::msg::UInt32>| { - num_messages += 1; + move |num_messages: &mut usize, + msg: ReadOnlyLoanedMessage| { + *num_messages += 1; println!("I heard: '{}'", msg.data); - println!("(Got {} messages so far)", num_messages); + println!("(Got {} messages so far)", *num_messages); }, )?; - executor - .spin(SpinOptions::default()) - .first_error() - .map_err(|err| err.into()) + executor.spin(SpinOptions::default()).first_error()?; + Ok(()) } diff --git a/examples/parameter_demo/Cargo.toml b/examples/parameter_demo/Cargo.toml new file mode 100644 index 000000000..4f90061a9 --- /dev/null +++ b/examples/parameter_demo/Cargo.toml @@ -0,0 +1,8 @@ +[package] +name = "parameter_demo" +version = "0.1.0" +edition = "2021" + +[dependencies] +rclrs = "0.4" +example_interfaces = "*" diff --git a/examples/parameter_demo/package.xml b/examples/parameter_demo/package.xml new file mode 100644 index 000000000..e2443c803 --- /dev/null +++ b/examples/parameter_demo/package.xml @@ -0,0 +1,21 @@ + + + + examples_parameter_demo + Esteve Fernandez + + Jacob Hassold + 0.4.1 + Package containing an example of how to use a worker in rclrs. + Apache License 2.0 + + rclrs + rosidl_runtime_rs + example_interfaces + + + ament_cargo + + diff --git a/examples/parameter_demo/src/main.rs b/examples/parameter_demo/src/main.rs new file mode 100644 index 000000000..7b9daa629 --- /dev/null +++ b/examples/parameter_demo/src/main.rs @@ -0,0 +1,26 @@ +use rclrs::*; +use std::sync::Arc; + +fn main() -> Result<(), RclrsError> { + let mut executor = Context::default_from_env()?.create_basic_executor(); + let node = executor.create_node("parameter_demo")?; + + let greeting: MandatoryParameter> = node + .declare_parameter("greeting") + .default("Hello".into()) + .mandatory()?; + + let _subscription = + node.create_subscription("greet", move |msg: example_interfaces::msg::String| { + println!("{}, {}", greeting.get(), msg.data); + })?; + + println!( + "Ready to provide a greeting. \ + \n\nTo see a greeting, try running\n \ + $ ros2 topic pub greet example_interfaces/msg/String \"data: Alice\"\ + \n\nTo change the kind of greeting, try running\n \ + $ ros2 param set parameter_demo greeting \"Guten tag\"\n" + ); + executor.spin(SpinOptions::default()).first_error() +} diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index 2428cbb12..ff211f0ac 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -2,11 +2,11 @@ use rclrs::*; use std::{thread, time::Duration}; use std_msgs::msg::String as StringMsg; -struct SimplePublisher { +struct SimplePublisherNode { publisher: Publisher, } -impl SimplePublisher { +impl SimplePublisherNode { fn new(executor: &Executor) -> Result { let node = executor.create_node("simple_publisher").unwrap(); let publisher = node.create_publisher("publish_hello").unwrap(); @@ -24,11 +24,14 @@ impl SimplePublisher { fn main() -> Result<(), RclrsError> { let mut executor = Context::default_from_env().unwrap().create_basic_executor(); - let publisher = SimplePublisher::new(&executor).unwrap(); + let node = SimplePublisherNode::new(&executor).unwrap(); let mut count: i32 = 0; + + // TODO(@mxgrey): Replace this with a timer once the Timer feature + // is merged. thread::spawn(move || loop { thread::sleep(Duration::from_millis(1000)); - count = publisher.publish_data(count).unwrap(); + count = node.publish_data(count).unwrap(); }); executor.spin(SpinOptions::default()).first_error() } diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index e9dc9a15a..3ebbb4d89 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -1,43 +1,46 @@ use rclrs::*; -use std::{ - sync::{Arc, Mutex}, - thread, - time::Duration, -}; +use std::{thread, time::Duration}; use std_msgs::msg::String as StringMsg; pub struct SimpleSubscriptionNode { - _subscriber: Subscription, - data: Arc>>, + #[allow(unused)] + subscriber: WorkerSubscription>, + worker: Worker>, } impl SimpleSubscriptionNode { 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 - .create_subscription::("publish_hello", move |msg: StringMsg| { - *data_mut.lock().unwrap() = Some(msg); - }) + let worker = node.create_worker(None); + + let subscriber = worker + .create_subscription::( + "publish_hello", + move |data: &mut Option, msg: StringMsg| { + *data = Some(msg.data); + }, + ) .unwrap(); - Ok(Self { _subscriber, data }) - } - fn data_callback(&self) -> Result<(), RclrsError> { - if let Some(data) = self.data.lock().unwrap().as_ref() { - println!("{}", data.data); - } else { - println!("No message available yet."); - } - Ok(()) + + Ok(Self { subscriber, worker }) } } fn main() -> Result<(), RclrsError> { let mut executor = Context::default_from_env().unwrap().create_basic_executor(); - let subscription = SimpleSubscriptionNode::new(&executor).unwrap(); + let node = SimpleSubscriptionNode::new(&executor).unwrap(); + + // TODO(@mxgrey): Replace this thread with a timer when the Timer feature + // gets merged. thread::spawn(move || loop { - thread::sleep(Duration::from_millis(1000)); - subscription.data_callback().unwrap() + thread::sleep(Duration::from_secs(1)); + let _ = node.worker.run(|data: &mut Option| { + if let Some(data) = data { + println!("{data}"); + } else { + println!("No message available yet."); + } + }); }); + executor.spin(SpinOptions::default()).first_error() } diff --git a/examples/worker_demo/Cargo.toml b/examples/worker_demo/Cargo.toml new file mode 100644 index 000000000..7fd78c74b --- /dev/null +++ b/examples/worker_demo/Cargo.toml @@ -0,0 +1,8 @@ +[package] +name = "worker_demo" +version = "0.1.0" +edition = "2021" + +[dependencies] +rclrs = "0.4" +example_interfaces = "*" diff --git a/examples/worker_demo/package.xml b/examples/worker_demo/package.xml new file mode 100644 index 000000000..f5624c76b --- /dev/null +++ b/examples/worker_demo/package.xml @@ -0,0 +1,21 @@ + + + + examples_worker_demo + Esteve Fernandez + + Jacob Hassold + 0.4.1 + Package containing an example of how to use a worker in rclrs. + Apache License 2.0 + + rclrs + rosidl_runtime_rs + example_interfaces + + + ament_cargo + + diff --git a/examples/worker_demo/src/main.rs b/examples/worker_demo/src/main.rs new file mode 100644 index 000000000..253a95fce --- /dev/null +++ b/examples/worker_demo/src/main.rs @@ -0,0 +1,50 @@ +use rclrs::*; +use std::sync::Arc; + +fn main() -> Result<(), RclrsError> { + let mut executor = Context::default_from_env()?.create_basic_executor(); + let node = executor.create_node("worker_demo")?; + + let publisher = node.create_publisher("output_topic")?; + let worker = node.create_worker(String::new()); + + let _subscription = worker.create_subscription( + "input_topic", + move |data: &mut String, msg: example_interfaces::msg::String| { + *data = msg.data; + }, + )?; + + // // Use this timer-based implementation when timers are available instead + // // of using std::thread::spawn. + // let _timer = worker.create_timer_repeating( + // Duration::from_secs(1), + // move |data: &mut String| { + // let msg = example_interfaces::msg::String { + // data: data.clone() + // }; + + // publisher.publish(msg).ok(); + // } + // )?; + + std::thread::spawn(move || loop { + std::thread::sleep(std::time::Duration::from_secs(1)); + let publisher = Arc::clone(&publisher); + let _ = worker.run(move |data: &mut String| { + let msg = example_interfaces::msg::String { data: data.clone() }; + publisher.publish(msg).unwrap(); + }); + }); + + println!( + "Beginning repeater... \n >> \ + Publish a std_msg::msg::String to \"input_topic\" and we will periodically republish it to \"output_topic\".\n\n\ + To see this in action run the following commands in two different terminals:\n \ + $ ros2 topic echo output_topic\n \ + $ ros2 topic pub input_topic std_msgs/msg/String \"{{data: Hello}}\"" + ); + executor.spin(SpinOptions::default()); + + Ok(()) +} diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index 09f59cb62..a32f12fbf 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 } @@ -38,8 +41,10 @@ serde-big-array = { version = "0.5.1", optional = true } tempfile = "3.3.0" # Needed for publisher and subscriber tests test_msgs = {version = "*"} +# Used in doctests +example_interfaces = { 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/package.xml b/rclrs/package.xml index 4c3754f48..3ba8dddda 100644 --- a/rclrs/package.xml +++ b/rclrs/package.xml @@ -19,8 +19,9 @@ builtin_interfaces rcl_interfaces rosgraph_msgs - + test_msgs + example_interfaces ament_cargo diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index cf80b043a..971d4f88f 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -1,67 +1,26 @@ use std::{ - boxed::Box, + any::Any, collections::HashMap, - ffi::CString, - sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, + ffi::{CStr, CString}, + sync::{Arc, Mutex, MutexGuard}, }; -use futures::channel::oneshot; use rosidl_runtime_rs::Message; use crate::{ - error::{RclReturnCode, ToResult}, - rcl_bindings::*, - IntoPrimitiveOptions, MessageCow, Node, NodeHandle, QoSProfile, RclrsError, - ENTITY_LIFECYCLE_MUTEX, + error::ToResult, log_fatal, 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, -} - -impl ClientHandle { - pub(crate) fn lock(&self) -> MutexGuard { - self.rcl_client.lock().unwrap() - } -} +mod client_async_callback; +pub use client_async_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); - } - } -} - -/// Trait to be implemented by concrete Client structs. -/// -/// 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>; -} +mod client_callback; +pub use client_callback::*; -type RequestValue = Box; - -type RequestId = i64; +mod client_output; +pub use client_output::*; /// Main class responsible for sending requests to a ROS service. /// @@ -70,7 +29,7 @@ type RequestId = i64; /// Receiving responses requires the node's executor to [spin][2]. /// /// [1]: crate::NodeState::create_client -/// [2]: crate::spin +/// [2]: crate::Executor::spin pub type Client = Arc>; /// The inner state of a [`Client`]. @@ -87,24 +46,263 @@ pub struct ClientState where T: rosidl_runtime_rs::Service, { - pub(crate) handle: Arc, - requests: Mutex>>, - futures: Arc>>>, - /// Ensure the parent node remains alive as long as the subscription is held. - /// This implementation will change in the future. + handle: Arc, + board: Arc>>, #[allow(unused)] - node: Node, + lifecycle: WaitableLifecycle, } 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()?; + + self.board + .lock() + .map_err(|_| RclrsError::PoisonedMutex)? + .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. + /// + /// # Client Callbacks + /// + /// Three callback signatures are supported: + /// - [`FnOnce`] ( `Response` ) + /// - [`FnOnce`] ( `Response`, [`RequestId`][1] ) + /// - [`FnOnce`] ( `Response`, [`ServiceInfo`] ) + /// + /// [1]: crate::RequestId + /// + /// Note that all of these are [`FnOnce`] which grants the greatest amount + /// of freedom for what kind of operations you can perform within the + /// callback. Just remember that this also means the callbacks are strictly + /// one-time-use. + 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. + /// + /// # Async Client Callbacks + /// + /// Three callback signatures are supported: + /// - [`FnOnce`] ( `Response` ) -> impl [`Future`][1] + /// - [`FnOnce`] ( `Response`, [`RequestId`][2] ) -> impl [`Future`][1] + /// - [`FnOnce`] ( `Response`, [`ServiceInfo`] ) -> impl [`Future`][1] + /// + /// [1]: std::future::Future + /// [2]: crate::RequestId + /// + /// Since this method is to help implement async behaviors, the callback that + /// you pass to it must return a [`Future`][1]. There are two ways to create + /// a `Future` in Rust: + /// + /// ## 1. `async fn` + /// + /// Define an `async fn` whose arguments are compatible with one of the above + /// signatures and which returns a `()` (a.k.a. nothing). + /// ``` + /// # use rclrs::*; + /// # let node = Context::default() + /// # .create_basic_executor() + /// # .create_node("test_node")?; + /// use test_msgs::srv::{Empty, Empty_Request, Empty_Response}; + /// + /// async fn print_hello(_response: Empty_Response) { + /// print!("Hello!"); + /// } + /// + /// let client = node.create_client::("my_service")?; + /// let request = Empty_Request::default(); + /// let promise = client.call_then_async(&request, print_hello)?; + /// # Ok::<(), RclrsError>(()) + /// ``` + /// + /// ## 2. Function that returns an `async { ... }` + /// + /// You can pass in a callback that returns an `async` block. `async` blocks + /// have an important advantage over `async fn`: You can use `async move { ... }` + /// to capture data into the async block. This allows you to embed some state + /// data into your callback. + /// + /// You can do this with either a regular `fn` or with a closure. + /// + /// ### `fn` + /// + /// ``` + /// # use rclrs::*; + /// # use std::future::Future; + /// # let node = Context::default() + /// # .create_basic_executor() + /// # .create_node("test_node")?; + /// use test_msgs::srv::{Empty, Empty_Request, Empty_Response}; + /// + /// fn print_greeting(_response: Empty_Response) -> impl Future { + /// let greeting = "Hello!"; + /// async move { + /// print!("Hello!"); + /// } + /// } + /// + /// let client = node.create_client::("my_service")?; + /// let request = Empty_Request::default(); + /// let promise = client.call_then_async( + /// &request, + /// print_greeting)?; + /// # Ok::<(), RclrsError>(()) + /// ``` + /// + /// ### Closure + /// + /// A closure will allow you to capture data into the callback from the + /// surrounding context. While the syntax for this is more complicated, it + /// is also the most powerful option. + /// + /// ``` + /// # use rclrs::*; + /// # let node = Context::default() + /// # .create_basic_executor() + /// # .create_node("test_node")?; + /// use test_msgs::srv::{Empty, Empty_Request, Empty_Response}; + /// + /// let greeting = "Hello!"; + /// let client = node.create_client::("my_service")?; + /// let request = Empty_Request::default(); + /// let promise = client.call_then_async( + /// &request, + /// move |response: Empty_Response| { + /// async move { + /// print!("{greeting}"); + /// } + /// })?; + /// # Ok::<(), RclrsError>(()) + /// ``` + 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(_) => { + log_fatal!( + "rclrs.client.call_then_async", + "Request promise has been dropped by the executor", + ); + } + } + }); + + 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(move || client.service_is_ready().is_ok_and(|r| r)) + } + + /// Get the name of the service that this client intends to call. + pub fn service_name(&self) -> String { + unsafe { + let char_ptr = rcl_client_get_service_name(&*self.handle.lock() as *const _); + debug_assert!(!char_ptr.is_null()); + CStr::from_ptr(char_ptr).to_string_lossy().into_owned() + } + } + /// Creates a new client. - pub(crate) fn new<'a>( - node: &Node, + pub(crate) fn create<'a>( options: impl Into>, - ) -> Result + node: &Node, + ) -> 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 @@ -126,7 +324,7 @@ where 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: @@ -148,162 +346,35 @@ where } } + let commands = node.commands().async_worker_commands(); let handle = Arc::new(ClientHandle { rcl_client: Mutex::new(rcl_client), - node_handle: Arc::clone(&node.handle), - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + node: Arc::clone(&node), }); - Ok(Self { - handle, - requests: Mutex::new(HashMap::new()), - futures: Arc::new(Mutex::new( - HashMap::>::new(), - )), - node: Arc::clone(node), - }) - } - - /// 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, - ) - } - .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()) - } + let board = Arc::new(Mutex::new(ClientRequestBoard::new())); - /// 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)) - } + 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); - /// 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(); - - 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) + Ok(Arc::new(Self { + handle, + board, + lifecycle, + })) } } /// `ClientOptions` are used by [`Node::create_client`][1] to initialize a /// [`Client`] for a service. /// -/// [1]: crate::Node::create_client +/// [1]: crate::NodeState::create_client #[derive(Debug, Clone)] #[non_exhaustive] pub struct ClientOptions<'a> { @@ -332,38 +403,167 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From for ClientOptions<'a> { } } -impl ClientBase for ClientState +struct ClientExecutable +where + T: rosidl_runtime_rs::Service, +{ + handle: Arc, + board: Arc>>, +} + +impl RclPrimitive for ClientExecutable where T: rosidl_runtime_rs::Service, { - fn handle(&self) -> &ClientHandle { - &self.handle + unsafe fn execute(&mut self, _: &mut dyn Any) -> Result<(), RclrsError> { + self.board.lock().unwrap().execute(&self.handle) } - 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 handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::Client(self.handle.lock()) + } + + fn kind(&self) -> RclPrimitiveKind { + RclPrimitiveKind::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 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(&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 => { + log_fatal!( + "rclrs.client.execute", + "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..27af830ba --- /dev/null +++ b/rclrs/src/client/client_async_callback.rs @@ -0,0 +1,60 @@ +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. +/// +/// Three callback signatures are supported: +/// - [`FnOnce`] ( `Response` ) -> impl [`Future`] +/// - [`FnOnce`] ( `Response`, [`RequestId`] ) -> impl [`Future`] +/// - [`FnOnce`] ( `Response`, [`ServiceInfo`] ) -> impl [`Future`] +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..b56e315ee --- /dev/null +++ b/rclrs/src/client/client_callback.rs @@ -0,0 +1,49 @@ +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. +/// +/// Three callback signatures are supported: +/// - [`FnOnce`] ( `Response` ) +/// - [`FnOnce`] ( `Response`, [`RequestId`] ) +/// - [`FnOnce`] ( `Response`, [`ServiceInfo`] ) +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/clock.rs b/rclrs/src/clock.rs index f7c085e14..992cd4b44 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -66,21 +66,26 @@ impl Clock { } fn make(kind: ClockType) -> Self { - let mut rcl_clock; + let rcl_clock; unsafe { // SAFETY: Getting a default value is always safe. - rcl_clock = Self::init_generic_clock(); + let allocator = rcutils_get_default_allocator(); + rcl_clock = Arc::new(Mutex::new(rcl_clock_t { + type_: rcl_clock_type_t::RCL_CLOCK_UNINITIALIZED, + jump_callbacks: std::ptr::null_mut(), + num_jump_callbacks: 0, + get_now: None, + data: std::ptr::null_mut::(), + allocator, + })); let mut allocator = rcutils_get_default_allocator(); // Function will return Err(_) only if there isn't enough memory to allocate a clock // object. - rcl_clock_init(kind.into(), &mut rcl_clock, &mut allocator) + rcl_clock_init(kind.into(), &mut *rcl_clock.lock().unwrap(), &mut allocator) .ok() .unwrap(); } - Self { - kind, - rcl_clock: Arc::new(Mutex::new(rcl_clock)), - } + Self { kind, rcl_clock } } /// Returns the clock's `ClockType`. @@ -101,22 +106,6 @@ impl Clock { clock: Arc::downgrade(&self.rcl_clock), } } - - /// Helper function to privately initialize a default clock, with the same behavior as - /// `rcl_init_generic_clock`. By defining a private function instead of implementing - /// `Default`, we avoid exposing a public API to create an invalid clock. - // SAFETY: Getting a default value is always safe. - unsafe fn init_generic_clock() -> rcl_clock_t { - let allocator = rcutils_get_default_allocator(); - rcl_clock_t { - type_: rcl_clock_type_t::RCL_CLOCK_UNINITIALIZED, - jump_callbacks: std::ptr::null_mut::(), - num_jump_callbacks: 0, - get_now: None, - data: std::ptr::null_mut::(), - allocator, - } - } } impl Drop for ClockSource { diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 2e70b8f04..d9a2b475d 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -6,7 +6,7 @@ use std::{ vec::Vec, }; -use crate::{rcl_bindings::*, LoggingLifecycle, RclrsError, ToResult}; +use crate::{rcl_bindings::*, 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 @@ -60,10 +60,19 @@ unsafe impl Send for rcl_context_t {} /// (as well as the terminal). TODO: This behaviour should be configurable using an /// "auto logging initialise" flag as per rclcpp and rclpy. /// +#[derive(Clone)] pub struct Context { pub(crate) handle: Arc, } +impl std::fmt::Debug for Context { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("Context") + .field("handle", &self.handle.rcl_context.lock()) + .finish() + } +} + /// This struct manages the lifetime and access to the `rcl_context_t`. It will also /// account for the lifetimes of any dependencies, if we need to add /// dependencies in the future (currently there are none). It is not strictly @@ -190,6 +199,14 @@ impl Context { Self::new(std::env::args(), InitOptions::default()) } + /// 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]. diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index 527a4d3a6..b177aaafa 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -4,7 +4,7 @@ use std::{ fmt::{self, Display}, }; -use crate::rcl_bindings::*; +use crate::{rcl_bindings::*, DeclarationError}; /// The main error type. #[derive(Debug, PartialEq, Eq)] @@ -32,6 +32,25 @@ pub enum RclrsError { }, /// It was attempted to add a waitable to a wait set twice. AlreadyAddedToWaitSet, + /// A negative duration was obtained from rcl which should have been positive. + /// + /// The value represents nanoseconds. + NegativeDuration(i64), + /// The guard condition that you tried to trigger is not owned by the + /// [`GuardCondition`][crate::GuardCondition] instance. + UnownedGuardCondition, + /// The payload given to a primitive that belongs to a worker was the wrong + /// type. + InvalidPayload { + /// The payload type expected by the primitive + expected: std::any::TypeId, + /// The payload type given by the worker + received: std::any::TypeId, + }, + /// An error happened while declaring a parameter. + ParameterDeclarationError(crate::DeclarationError), + /// A mutex used internally has been [poisoned][std::sync::PoisonError]. + PoisonedMutex, } impl RclrsError { @@ -75,6 +94,30 @@ impl Display for RclrsError { "Could not add entity to wait set because it was already added to a wait set" ) } + RclrsError::NegativeDuration(duration) => { + write!( + f, + "A duration was negative when it should not have been: {duration:?}" + ) + } + RclrsError::UnownedGuardCondition => { + write!( + f, + "Could not trigger guard condition because it is not owned by rclrs" + ) + } + RclrsError::InvalidPayload { expected, received } => { + write!( + f, + "Received invalid payload: expected {expected:?}, received {received:?}", + ) + } + RclrsError::ParameterDeclarationError(err) => { + write!(f, "An error occurred while declaring a parameter: {err}",) + } + RclrsError::PoisonedMutex => { + write!(f, "A mutex used internally has been poisoned") + } } } } @@ -106,7 +149,14 @@ 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::NegativeDuration(_) => None, + RclrsError::UnownedGuardCondition => None, + RclrsError::InvalidPayload { .. } => None, + RclrsError::ParameterDeclarationError(_) => None, + RclrsError::PoisonedMutex => None, } } } @@ -208,6 +258,12 @@ pub enum RclReturnCode { LifecycleStateNotRegistered = 3001, } +impl From for RclrsError { + fn from(value: DeclarationError) -> Self { + RclrsError::ParameterDeclarationError(value) + } +} + impl TryFrom for RclReturnCode { type Error = i32; diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index f38eec5bf..24ac5ea50 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,27 +1,46 @@ +mod basic_executor; +pub use self::basic_executor::*; + use crate::{ - rcl_bindings::rcl_context_is_valid, Context, ContextHandle, IntoNodeOptions, Node, NodeState, - RclrsError, WaitSet, + Context, ContextHandle, GuardCondition, IntoNodeOptions, Node, RclrsError, Waitable, + WeakActivityListener, +}; +pub use futures::channel::oneshot::Receiver as Promise; +use futures::{ + channel::oneshot, + future::{select, BoxFuture, Either}, }; use std::{ - sync::{Arc, Mutex, Weak}, + any::Any, + future::Future, + sync::{ + atomic::{AtomicBool, Ordering}, + Arc, + }, time::Duration, }; -/// Single-threaded executor implementation. +/// An executor that can be used to create nodes and run their callbacks. pub struct Executor { context: Arc, - nodes_mtx: Mutex>>, + commands: Arc, + runtime: Box, } impl Executor { + /// Access the commands interface for this executor. Use the returned + /// [`ExecutorCommands`] to create [nodes][Node]. + pub fn commands(&self) -> &Arc { + &self.commands + } + /// 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.context)?; - self.nodes_mtx.lock().unwrap().push(Arc::downgrade(&node)); + let node = options.build(&self.commands)?; Ok(node) } @@ -32,69 +51,335 @@ impl Executor { /// certain conditions are met. Use `SpinOptions::default()` to allow the /// Executor to keep spinning indefinitely. pub fn spin(&mut self, options: SpinOptions) -> Vec { - loop { - if self.nodes_mtx.lock().unwrap().is_empty() { - // Nothing to spin for, so just quit here - return Vec::new(); - } + let conditions = self.make_spin_conditions(options); + let result = self.runtime.spin(conditions); + self.commands.halt_spinning.store(false, Ordering::Release); + result + } - if let Err(err) = self.spin_once(options.timeout) { - return vec![err]; - } + /// 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, Vec) { + let conditions = self.make_spin_conditions(options); + let Self { + context, + commands, + runtime, + } = self; - if options.only_next_available_work { - // We were only suppposed to spin once, so quit here - return Vec::new(); - } + let (runtime, result) = runtime.spin_async(conditions).await; + commands.halt_spinning.store(false, Ordering::Release); + + ( + Self { + context, + commands, + runtime, + }, + result, + ) + } - std::thread::yield_now(); + /// 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 executor_channel = runtime.channel(); + let async_worker_commands = ExecutorCommands::impl_create_worker_commands( + &Context { + handle: Arc::clone(&context), + }, + &*executor_channel, + Box::new(()), + ); + + let commands = Arc::new(ExecutorCommands { + context: Context { + handle: Arc::clone(&context), + }, + executor_channel, + halt_spinning: Arc::new(AtomicBool::new(false)), + async_worker_commands, + }); + + Self { + context, + commands, + runtime: Box::new(runtime), + } + } + + 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), + }, } } +} + +/// 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, + executor_channel: Arc, + async_worker_commands: Arc, + halt_spinning: 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); + self.executor_channel.wake_all_wait_sets(); + } + + /// 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. - 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()?; - } + /// 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.async_worker_commands + .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(); + })); - for ready_client in ready_entities.clients { - ready_client.execute()?; - } + receiver + } - for ready_service in ready_entities.services { - ready_service.execute()?; + /// 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.async_worker_commands + .channel + .add_async_task(Box::pin(async move { + sender.send(f.await).ok(); + })); + receiver + } + + /// Pass in a promise to get a second promise that will notify when the main + /// promise is fulfilled. This second promise can be passed into + /// [`SpinOptions::until_promise_resolved`]. + pub fn create_notice(&self, promise: Promise) -> (Promise, Promise<()>) + where + Out: 'static + Send, + { + let (main_sender, main_receiver) = oneshot::channel(); + let notice_receiver = self.run(async move { + if let Ok(out) = promise.await { + main_sender.send(out).ok(); } - } + }); - // Clear out any nodes that have been dropped. - self.nodes_mtx - .lock() - .unwrap() - .retain(|weak_node| weak_node.strong_count() > 0); + (main_receiver, notice_receiver) + } - Ok(()) + /// Get the context that the executor is associated with. + pub fn context(&self) -> &Context { + &self.context } - /// Used by [`Context`] to create the `Executor`. Users cannot call this - /// function. - pub(crate) fn new(context: Arc) -> Self { - Self { - context, - nodes_mtx: Mutex::new(Vec::new()), - } + pub(crate) fn add_to_wait_set(&self, waitable: Waitable) { + self.async_worker_commands.channel.add_to_waitset(waitable); + } + + #[cfg(test)] + pub(crate) fn wake_all_wait_sets(&self) { + self.executor_channel.wake_all_wait_sets(); } + + pub(crate) fn async_worker_commands(&self) -> &Arc { + &self.async_worker_commands + } + + pub(crate) fn create_worker_commands( + &self, + payload: Box, + ) -> Arc { + Self::impl_create_worker_commands(&self.context, &*self.executor_channel, payload) + } + + /// We separate out this impl function so that we can create the async worker + /// before the [`ExecutorCommands`] is finished being constructed. + fn impl_create_worker_commands( + context: &Context, + executor_channel: &dyn ExecutorChannel, + payload: Box, + ) -> Arc { + let (guard_condition, waitable) = GuardCondition::new(&context.handle, None); + + let worker_channel = executor_channel.create_worker(ExecutorWorkerOptions { + context: context.clone(), + payload, + guard_condition: Arc::clone(&guard_condition), + }); + + worker_channel.add_to_waitset(waitable); + + Arc::new(WorkerCommands { + channel: worker_channel, + wakeup_wait_set: guard_condition, + }) + } +} + +/// This is used internally to transmit commands to a specific worker in the +/// executor. It is not accessible to downstream users because it does not +/// retain information about the worker's payload type. +/// +/// Downstream users of rclrs should instead use [`Worker`][crate::Worker]. +pub(crate) struct WorkerCommands { + channel: Arc, + wakeup_wait_set: Arc, +} + +impl WorkerCommands { + pub(crate) fn add_to_wait_set(&self, waitable: Waitable) { + self.channel.add_to_waitset(waitable); + } + + pub(crate) fn run_async(&self, f: F) + where + F: 'static + Future + Send, + { + self.channel.add_async_task(Box::pin(f)); + } + + pub(crate) fn run_on_payload(&self, task: PayloadTask) { + self.channel.send_payload_task(task); + let _ = self.wakeup_wait_set.trigger(); + } + + pub(crate) fn add_activity_listener(&self, listener: WeakActivityListener) { + self.channel.add_activity_listener(listener); + } + + /// 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 + } +} + +/// Channel to send commands related to a specific worker. +pub trait WorkerChannel: 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); + + /// Send a one-time task for the worker to run with its payload. + fn send_payload_task(&self, f: PayloadTask); + + /// Send something to listen to worker activity. + fn add_activity_listener(&self, listener: WeakActivityListener); +} + +/// Encapsulates a task that can operate on the payload of a worker +pub type PayloadTask = Box; + +/// This is constructed by [`ExecutorCommands`] and passed to the [`ExecutorRuntime`] +/// to create a new worker. Downstream users of rclrs should not be using this class +/// unless you are implementing your own [`ExecutorRuntime`]. +pub struct ExecutorWorkerOptions { + /// The context that the executor is associated with + pub context: Context, + /// The payload that the worker provides to different primitives. + pub payload: Box, + /// The guard condition that should wake up the wait set for the worker. + pub guard_condition: Arc, +} + +/// This trait defines the interface for passing new items into an executor to +/// run. +pub trait ExecutorChannel: Send + Sync { + /// Create a new channel specific to a worker whose payload must be + /// initialized with the given function. + fn create_worker(&self, options: ExecutorWorkerOptions) -> Arc; + + /// Wake all the wait sets that are being managed by this executor. This is + /// used to make sure they respond to [`ExecutorCommands::halt_spinning`]. + fn wake_all_wait_sets(&self); +} + +/// 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) -> Arc; + + /// Tell the runtime to spin while blocking any further execution until the + /// spinning is complete. + fn spin(&mut self, conditions: SpinConditions) -> Vec; + + /// 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, Vec)>; } /// A bundle of optional conditions that a user may want to impose on how long @@ -110,6 +395,11 @@ pub struct SpinOptions { /// 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, @@ -129,11 +419,59 @@ impl SpinOptions { } } + /// Stop spinning once this promise is resolved. + pub fn until_promise_resolved(mut self, promise: Promise<()>) -> Self { + self.until_promise_resolved = Some(promise); + self + } + /// Stop spinning once this durtion of time is reached. pub fn timeout(mut self, timeout: Duration) -> Self { self.timeout = Some(timeout); self } + + /// Clone the items inside of [`SpinOptions`] that are able to be cloned. + /// This will exclude: + /// - [`until_promise_resolved`][Self::until_promise_resolved] + pub fn clone_partial(&self) -> SpinOptions { + SpinOptions { + only_next_available_work: self.only_next_available_work, + timeout: self.timeout, + until_promise_resolved: None, + } + } +} + +/// 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, +} + +impl SpinConditions { + /// Clone the items inside of [`SpinConditions`] that are able to be cloned. + /// This will exclude: + /// - [`until_promise_resolved`][SpinOptions::until_promise_resolved] + pub fn clone_partial(&self) -> SpinConditions { + SpinConditions { + options: self.options.clone_partial(), + halt_spinning: Arc::clone(&self.halt_spinning), + context: self.context.clone(), + } + } } /// This trait allows [`Context`] to create a basic executor. @@ -144,6 +482,7 @@ pub trait CreateBasicExecutor { impl CreateBasicExecutor for Context { fn create_basic_executor(&self) -> Executor { - Executor::new(Arc::clone(&self.handle)) + let runtime = BasicExecutorRuntime::new(); + self.create_executor(runtime) } } diff --git a/rclrs/src/executor/basic_executor.rs b/rclrs/src/executor/basic_executor.rs new file mode 100644 index 000000000..1ad9476b1 --- /dev/null +++ b/rclrs/src/executor/basic_executor.rs @@ -0,0 +1,452 @@ +use futures::{ + channel::{ + mpsc::{unbounded, UnboundedReceiver, UnboundedSender}, + oneshot, + }, + future::{select, select_all, BoxFuture, Either}, + stream::StreamFuture, + task::{waker_ref, ArcWake}, + StreamExt, +}; +use std::{ + sync::{ + atomic::{AtomicBool, Ordering}, + mpsc::{channel, Receiver, Sender}, + Arc, Mutex, Weak, + }, + task::Context as TaskContext, + time::Instant, +}; + +use crate::{ + log_debug, log_fatal, log_warn, ExecutorChannel, ExecutorRuntime, ExecutorWorkerOptions, + GuardCondition, PayloadTask, RclrsError, SpinConditions, WaitSetRunConditions, WaitSetRunner, + Waitable, WeakActivityListener, WorkerChannel, +}; + +static FAILED_TO_SEND_WORKER: &'static str = + "Failed to send the new runner. This should never happen. \ + Please report this to the rclrs maintainers with a minimal reproducible example."; + +/// The implementation of this runtime is based off of the async Rust reference book: +/// +/// +/// 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 or under a feature gate and refer to it in this documentation. +pub struct BasicExecutorRuntime { + ready_queue: Receiver>, + task_sender: TaskSender, + wait_set_runners: Vec, + all_guard_conditions: AllGuardConditions, + new_worker_receiver: Option>>, + new_worker_sender: UnboundedSender, +} + +#[derive(Clone, Default)] +struct AllGuardConditions { + inner: Arc>>>, +} + +impl AllGuardConditions { + fn trigger(&self) { + self.inner.lock().unwrap().retain(|guard_condition| { + if let Some(guard_condition) = guard_condition.upgrade() { + if let Err(err) = guard_condition.trigger() { + log_fatal!( + "rclrs.executor.basic_executor", + "Failed to trigger a guard condition. This should never happen. \ + Please report this to the rclrs maintainers with a minimal reproducible example. \ + Error: {err}", + ); + } + true + } else { + false + } + }); + } + + fn push(&self, guard_condition: Weak) { + let mut inner = self.inner.lock().unwrap(); + if inner + .iter() + .find(|other| guard_condition.ptr_eq(other)) + .is_some() + { + // This guard condition is already known + return; + } + + inner.push(guard_condition); + } +} + +impl ExecutorRuntime for BasicExecutorRuntime { + fn spin(&mut self, conditions: SpinConditions) -> Vec { + let conditions = self.process_spin_conditions(conditions); + + let new_workers = self.new_worker_receiver.take().expect( + "Basic executor was missing its new_worker_receiver at the start 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." + ); + let all_guard_conditions = self.all_guard_conditions.clone(); + + // 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 (worker_result_sender, worker_result_receiver) = channel(); + + // Use this atomic bool to recognize when we should stop spinning. + let workers_finished = Arc::new(AtomicBool::new(false)); + + for runner in self.wait_set_runners.drain(..) { + if let Err(err) = self.new_worker_sender.unbounded_send(runner) { + log_fatal!( + "rclrs.executor.basic_executor", + "{FAILED_TO_SEND_WORKER} Error: {err}", + ); + } + } + + // Use this to terminate the spinning once the wait set is finished. + let workers_finished_clone = Arc::clone(&workers_finished); + self.task_sender.add_async_task(Box::pin(async move { + let workers = manage_workers(new_workers, all_guard_conditions, conditions).await; + + if let Err(err) = worker_result_sender.send(workers) { + log_fatal!( + "rclrs.executor.basic_executor", + "Failed to send a runner result. This should never happen. \ + Please report this to the rclrs maintainers with a minimal \ + reproducible example. Error: {err}", + ); + } + workers_finished_clone.store(true, Ordering::Release); + })); + + while let Ok(task) = self.next_task(&workers_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 (runners, new_worker_receiver, errors) = worker_result_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_runners = runners; + self.new_worker_receiver = Some(new_worker_receiver); + + errors + } + + fn spin_async( + mut self: Box, + conditions: SpinConditions, + ) -> BoxFuture<'static, (Box, Vec)> { + 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) -> Arc { + Arc::new(BasicExecutorChannel { + task_sender: self.task_sender.clone(), + new_worker_sender: self.new_worker_sender.clone(), + all_guard_conditions: self.all_guard_conditions.clone(), + }) + } +} + +impl BasicExecutorRuntime { + pub(crate) fn new() -> Self { + let (task_sender, ready_queue) = channel(); + let (new_worker_sender, new_worker_receiver) = unbounded(); + + Self { + ready_queue, + task_sender: TaskSender { task_sender }, + wait_set_runners: Vec::new(), + all_guard_conditions: AllGuardConditions::default(), + new_worker_receiver: Some(new_worker_receiver.into_future()), + new_worker_sender, + } + } + + fn process_spin_conditions(&self, mut conditions: SpinConditions) -> WaitSetRunConditions { + if let Some(promise) = conditions.options.until_promise_resolved.take() { + let halt_spinning = Arc::clone(&conditions.halt_spinning); + let all_guard_conditions = self.all_guard_conditions.clone(); + self.task_sender.add_async_task(Box::pin(async move { + if let Err(err) = promise.await { + log_warn!( + "rclrs.executor.basic_executor", + "Sender for SpinOptions::until_promise_resolved was \ + dropped, so the Promise will never be fulfilled. \ + Spinning will stop now. Error message: {err}" + ); + } + + // Ordering is very important here. halt_spinning must be set + // before we lock and trigger the guard conditions. This ensures + // that when the wait sets wake up, the halt_spinning value is + // already set to true. Ordering::Release is also important for + // that purpose. + // + // When a new worker is added, the guard conditions will be locked + // and the new guard condition will be added before checking the + // value of halt_spinning. That's the opposite order of using + // these variables. This opposite usage prevents a race condition + // where the new wait set will start running after we've already + // triggered all the known guard conditions. + halt_spinning.store(true, Ordering::Release); + all_guard_conditions.trigger(); + })); + } + + WaitSetRunConditions { + only_next_available_work: conditions.options.only_next_available_work, + stop_time: conditions.options.timeout.map(|t| Instant::now() + t), + context: conditions.context, + halt_spinning: conditions.halt_spinning, + } + } + + 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, + all_guard_conditions: AllGuardConditions, + new_worker_sender: UnboundedSender, +} + +impl ExecutorChannel for BasicExecutorChannel { + fn create_worker(&self, options: ExecutorWorkerOptions) -> Arc { + let runner = WaitSetRunner::new(options); + let waitable_sender = runner.waitable_sender(); + let payload_task_sender = runner.payload_task_sender(); + let activity_listeners = runner.activity_listeners(); + + if let Err(err) = self.new_worker_sender.unbounded_send(runner) { + log_fatal!( + "rclrs.executor.basic_executor", + "{FAILED_TO_SEND_WORKER} Error: {err}", + ); + } + + Arc::new(BasicWorkerChannel { + waitable_sender, + task_sender: self.task_sender.clone(), + payload_task_sender, + activity_listeners, + }) + } + + fn wake_all_wait_sets(&self) { + self.all_guard_conditions.trigger(); + } +} + +struct BasicWorkerChannel { + task_sender: TaskSender, + waitable_sender: UnboundedSender, + payload_task_sender: UnboundedSender, + activity_listeners: Arc>>, +} + +impl WorkerChannel for BasicWorkerChannel { + fn add_to_waitset(&self, new_entity: Waitable) { + if let Err(err) = self.waitable_sender.unbounded_send(new_entity) { + // This is a debug log because it is normal for this to happen while + // an executor is winding down. + log_debug!( + "rclrs.basic_executor.add_to_waitset", + "Failed to add an item to the waitset: {err}", + ); + } + } + + fn add_async_task(&self, f: BoxFuture<'static, ()>) { + self.task_sender.add_async_task(f); + } + + fn send_payload_task(&self, f: PayloadTask) { + if let Err(err) = self.payload_task_sender.unbounded_send(f) { + // This is a debug log because it is normal for this to happen while + // an executor is winding down. + log_debug!( + "rclrs.BasicWorkerChannel", + "Failed to send a payload task: {err}", + ); + } + } + + fn add_activity_listener(&self, listener: WeakActivityListener) { + self.activity_listeners.lock().unwrap().push(listener); + } +} + +#[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(), + }); + + if let Err(_) = self.task_sender.send(task) { + // This is a debug log because it is normal for this to happen while + // an executor is winding down. + log_debug!( + "rclrs.TaskSender.add_async_task", + "Failed to send a task. This indicates the Worker has shut down.", + ); + } + } +} + +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(); + } +} + +async fn manage_workers( + mut new_workers: StreamFuture>, + all_guard_conditions: AllGuardConditions, + conditions: WaitSetRunConditions, +) -> ( + Vec, + StreamFuture>, + Vec, +) { + let mut active_runners: Vec)>> = + Vec::new(); + let mut finished_runners: Vec = Vec::new(); + let mut errors: Vec = Vec::new(); + + let add_runner = |new_runner: Option, + active_runners: &mut Vec<_>, + finished_runners: &mut Vec<_>| { + if let Some(runner) = new_runner { + all_guard_conditions.push(Arc::downgrade(runner.guard_condition())); + if conditions.halt_spinning.load(Ordering::Acquire) { + finished_runners.push(runner); + } else { + active_runners.push(runner.run(conditions.clone())); + } + } + }; + + // We expect to start with at least one worker + let (initial_worker, new_worker_receiver) = new_workers.await; + new_workers = new_worker_receiver.into_future(); + add_runner(initial_worker, &mut active_runners, &mut finished_runners); + + while !active_runners.is_empty() { + let next_event = select(select_all(active_runners), new_workers); + + match next_event.await { + Either::Left(((finished_worker, _, remaining_workers), new_worker_stream)) => { + match finished_worker { + Ok((runner, result)) => { + finished_runners.push(runner); + if let Err(err) = result { + errors.push(err); + } + } + Err(_) => { + log_fatal!( + "rclrs.basic_executor", + "WaitSetRunner unexpectedly dropped. This should never happen. \ + Please report this to the rclrs maintainers with a minimal \ + reproducible example.", + ); + } + } + + active_runners = remaining_workers; + new_workers = new_worker_stream; + } + Either::Right(((new_worker, new_worker_receiver), remaining_workers)) => { + active_runners = remaining_workers.into_inner(); + add_runner(new_worker, &mut active_runners, &mut finished_runners); + new_workers = new_worker_receiver.into_future(); + } + } + } + + (finished_runners, new_workers, errors) +} diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 73d478191..366e499b8 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -1,9 +1,178 @@ #![warn(missing_docs)] //! Rust client library for ROS 2. //! -//! For getting started, see the [README][1]. +//! Since this library depends on the ROS ecosystem, see the [README][1] for +//! setup instructions. //! //! [1]: https://github.com/ros2-rust/ros2_rust/blob/main/README.md +//! +//! This client library is made to be familiar for ROS users who are used to +//! the conventional client libraries `rcl`, `rclcpp`, and `rclpy`, while taking +//! full advantage of the unique strengths of the Rust programming language. +//! +//! The library provides structs that will be familiar to ROS users: +//! - [`Context`] +//! - [`Executor`] +//! - [`Node`] +//! - [`Subscription`] +//! - [`Publisher`] +//! - [`Service`] +//! - [`Client`] +//! +//! It also provides some unique utilities to help leverage Rust language features, +//! such as `async` programming: +//! - [`Worker`] +//! - [`ExecutorCommands`] +//! +//! # Basic Usage +//! +//! To build a typical ROS application, create a [`Context`], followed by an +//! [`Executor`], and then a [`Node`]. Create whatever primitives you need, and +//! then tell the [`Executor`] to spin: +//! +//! ```no_run +//! use rclrs::*; +//! +//! let context = Context::default_from_env()?; +//! let mut executor = context.create_basic_executor(); +//! let node = executor.create_node("example_node")?; +//! +//! let subscription = node.create_subscription( +//! "topic_name", +//! |msg: example_interfaces::msg::String| { +//! println!("Received message: {}", msg.data); +//! } +//! )?; +//! +//! executor.spin(SpinOptions::default()).first_error()?; +//! # Ok::<(), RclrsError>(()) +//! ``` +//! +//! If your callback needs to interact with some state data, consider using a +//! [`Worker`], especially if that state data needs to be shared with other +//! callbacks: +//! +//! ```no_run +//! # use rclrs::*; +//! # +//! # let context = Context::default_from_env()?; +//! # let mut executor = context.create_basic_executor(); +//! # let node = executor.create_node("example_node")?; +//! # +//! // This worker will manage the data for us. +//! // The worker's data is called its payload. +//! let worker = node.create_worker::>(None); +//! +//! // We use the worker to create a subscription. +//! // This subscription's callback can borrow the worker's +//! // payload with its first function argument. +//! let subscription = worker.create_subscription( +//! "topic_name", +//! |data: &mut Option, msg: example_interfaces::msg::String| { +//! // Print out the previous message, if one exists. +//! if let Some(previous) = data { +//! println!("Previous message: {}", *previous) +//! } +//! +//! // Save the latest message, to be printed out the +//! // next time this callback is triggered. +//! *data = Some(msg.data); +//! } +//! )?; +//! +//! # executor.spin(SpinOptions::default()).first_error()?; +//! # Ok::<(), RclrsError>(()) +//! ``` +//! +//! # Parameters +//! +//! `rclrs` provides an ergonomic way to declare and use node parameters. A +//! parameter can be declared as [mandatory][crate::MandatoryParameter], +//! [optional][crate::OptionalParameter], or [read-only][crate::ReadOnlyParameter]. +//! The API of each reflects their respective constraints. +//! - Mandatory and read-only parameters always have a value that you can [get][MandatoryParameter::get] +//! - Optional parameters will return an [`Option`] when you [get][OptionalParameter::get] from them. +//! - Read-only parameters do not allow you to modify them after they have been declared. +//! +//! The following is a simple example of using a mandatory parameter: +//! ```no_run +//! use rclrs::*; +//! use std::sync::Arc; +//! +//! let mut executor = Context::default_from_env()?.create_basic_executor(); +//! let node = executor.create_node("parameter_demo")?; +//! +//! let greeting: MandatoryParameter> = node +//! .declare_parameter("greeting") +//! .default("Hello".into()) +//! .mandatory()?; +//! +//! let _subscription = node.create_subscription( +//! "greet", +//! move |msg: example_interfaces::msg::String| { +//! println!("{}, {}", greeting.get(), msg.data); +//! } +//! )?; +//! +//! executor.spin(SpinOptions::default()).first_error()?; +//! # Ok::<(), RclrsError>(()) +//! ``` +//! +//! # Logging +//! +//! `rclrs` provides the same logging utilites as `rclcpp` and `rclpy` with an +//! ergonomic Rust API. [`ToLogParams`] can be used to dictate how logging is +//! performed. +//! +//! ```no_run +//! use rclrs::*; +//! use std::time::Duration; +//! +//! let mut executor = Context::default_from_env()?.create_basic_executor(); +//! let node = executor.create_node("logging_demo")?; +//! +//! let _subscription = node.clone().create_subscription( +//! "logging_demo", +//! move |msg: example_interfaces::msg::String| { +//! let data = msg.data; +//! +//! // You can apply modifiers such as .once() to node.logger() +//! // to dictate how the logging behaves. +//! log!( +//! node.logger().once(), +//! "First message: {data}", +//! ); +//! +//! log!( +//! node.logger().skip_first(), +//! "Subsequent message: {data}", +//! ); +//! +//! // You can chain multiple modifiers together. +//! log_warn!( +//! node +//! .logger() +//! .skip_first() +//! .throttle(Duration::from_secs(5)), +//! "Throttled message: {data}", +//! ); +//! } +//! )?; +//! +//! // Any &str can be used as the logger name and have +//! // logging modifiers applied to it. +//! log_info!( +//! "notice".once(), +//! "Ready to begin logging example_interfaces/msg/String messages published to 'logging_demo'.", +//! ); +//! log_warn!( +//! "help", +//! "Try running\n \ +//! $ ros2 topic pub logging_demo example_interfaces/msg/String \"data: message\"", +//! ); +//! executor.spin(SpinOptions::default()).first_error()?; +//! # Ok::<(), RclrsError>(()) +//! ``` mod arguments; mod client; @@ -21,7 +190,8 @@ mod subscription; mod time; mod time_source; mod vendor; -mod wait; +mod wait_set; +mod worker; #[cfg(test)] mod test_helpers; @@ -47,4 +217,5 @@ pub use service::*; pub use subscription::*; pub use time::*; use time_source::*; -pub use wait::*; +pub use wait_set::*; +pub use worker::*; diff --git a/rclrs/src/logging.rs b/rclrs/src/logging.rs index cbc30114f..0d46d33cb 100644 --- a/rclrs/src/logging.rs +++ b/rclrs/src/logging.rs @@ -201,7 +201,7 @@ macro_rules! log { macro_rules! log_debug { ($to_log_params: expr, $($args:tt)*) => {{ let log_params = $crate::ToLogParams::to_log_params($to_log_params); - $crate::log!(log_params.debug(), $($args)*); + $crate::log!($crate::ToLogParams::debug(log_params), $($args)*); }} } @@ -210,7 +210,7 @@ macro_rules! log_debug { macro_rules! log_info { ($to_log_params: expr, $($args:tt)*) => {{ let log_params = $crate::ToLogParams::to_log_params($to_log_params); - $crate::log!(log_params.info(), $($args)*); + $crate::log!($crate::ToLogParams::info(log_params), $($args)*); }} } @@ -219,7 +219,7 @@ macro_rules! log_info { macro_rules! log_warn { ($to_log_params: expr, $($args:tt)*) => {{ let log_params = $crate::ToLogParams::to_log_params($to_log_params); - $crate::log!(log_params.warn(), $($args)*); + $crate::log!($crate::ToLogParams::warn(log_params), $($args)*); }} } @@ -228,7 +228,7 @@ macro_rules! log_warn { macro_rules! log_error { ($to_log_params: expr, $($args:tt)*) => {{ let log_params = $crate::ToLogParams::to_log_params($to_log_params); - $crate::log!(log_params.error(), $($args)*); + $crate::log!($crate::ToLogParams::error(log_params), $($args)*); }} } @@ -237,7 +237,7 @@ macro_rules! log_error { macro_rules! log_fatal { ($to_log_params: expr, $($args:tt)*) => {{ let log_params = $crate::ToLogParams::to_log_params($to_log_params); - $crate::log!(log_params.fatal(), $($args)*); + $crate::log!($crate::ToLogParams::fatal(log_params), $($args)*); }} } diff --git a/rclrs/src/logging/logger.rs b/rclrs/src/logging/logger.rs index 30770919c..c6193ad10 100644 --- a/rclrs/src/logging/logger.rs +++ b/rclrs/src/logging/logger.rs @@ -20,7 +20,7 @@ use crate::{ /// then you can choose to simply pass in `&node` instead of `node.logger()`. /// /// [1]: crate::log -/// [2]: crate::Node::logger +/// [2]: crate::NodeState::logger #[derive(Debug, Clone, PartialEq, Eq, PartialOrd, Ord)] pub struct Logger { name: Arc, diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index b2b734dee..dd01d0605 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,36 +1,44 @@ -mod graph; -pub use graph::*; - 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; use crate::{ - rcl_bindings::*, Client, ClientBase, ClientOptions, ClientState, Clock, ContextHandle, - GuardCondition, LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant, - Parameters, Publisher, PublisherOptions, PublisherState, RclrsError, Service, ServiceBase, - ServiceOptions, ServiceState, Subscription, SubscriptionBase, SubscriptionCallback, - SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands, + IntoAsyncServiceCallback, IntoAsyncSubscriptionCallback, IntoNodeServiceCallback, + IntoNodeSubscriptionCallback, LogParams, Logger, ParameterBuilder, ParameterInterface, + ParameterVariant, Parameters, Promise, Publisher, PublisherOptions, PublisherState, RclrsError, + Service, ServiceOptions, ServiceState, Subscription, SubscriptionOptions, SubscriptionState, + TimeSource, ToLogParams, Worker, WorkerOptions, WorkerState, 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. +/// A processing unit that can communicate with other nodes. See the API of +/// [`NodeState`] to find out what methods you can call on a [`Node`]. /// /// Nodes are a core concept in ROS 2. Refer to the official ["Understanding ROS 2 nodes"][1] /// tutorial for an introduction. @@ -40,7 +48,7 @@ unsafe impl Send for rcl_node_t {} /// 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 +/// Use [`Executor::create_node`][5] 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 @@ -63,34 +71,32 @@ unsafe impl Send for rcl_node_t {} /// /// ## Rules for valid names /// The rules for valid node names and node namespaces are explained in -/// [`NodeOptions::new()`][5] and [`NodeOptions::namespace()`][6]. +/// [`NodeOptions::new()`][3] and [`NodeOptions::namespace()`][4]. /// /// [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]: Node::namespace -/// [4]: Node::name -/// [5]: crate::NodeOptions::new -/// [6]: crate::NodeOptions::namespace -/// [7]: crate::Executor::create_node +/// [3]: crate::NodeOptions::new +/// [4]: crate::NodeOptions::namespace +/// [5]: 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 +/// This is public so that you can choose to put it inside a [`Weak`][1] 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`] /// is provided as convenience alias for that. /// /// The public API of the [`Node`] type is implemented via `NodeState`. +/// +/// [1]: std::sync::Weak pub struct NodeState { - pub(crate) clients_mtx: Mutex>>, - pub(crate) guard_conditions_mtx: Mutex>>, - pub(crate) services_mtx: Mutex>>, - pub(crate) subscriptions_mtx: Mutex>>, time_source: TimeSource, parameter: ParameterInterface, logger: Logger, - pub(crate) handle: Arc, + commands: Arc, + graph_change_action: UnboundedSender, + handle: Arc, } /// This struct manages the lifetime of an `rcl_node_t`, and accounts for its @@ -229,13 +235,74 @@ impl NodeState { 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) } + /// Create a new [`Worker`] for this Node. + /// + /// Workers carry a data "payload" that they can lend out to callbacks that + /// are created using the worker. This makes it easy to share data between + /// callbacks. + /// + /// In some cases the payload type can be inferred by Rust: + /// ``` + /// # use rclrs::*; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node("my_node").unwrap(); + /// + /// let worker = node.create_worker(String::new()); + /// + /// let subscription = worker.create_subscription( + /// "input_topic", + /// move |data: &mut String, msg: example_interfaces::msg::String| { + /// *data = msg.data; + /// } + /// )?; + /// # Ok::<(), RclrsError>(()) + /// ``` + /// + /// If the compiler complains about not knowing the payload type, you can + /// specify it explicitly in two ways: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let worker: Worker = node.create_worker(String::new()); + /// ``` + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let worker = node.create_worker::(String::new()); + /// ``` + /// + /// The data given to the worker can be any custom data type: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// + /// #[derive(Default)] + /// struct MyNodeData { + /// addition_client: Option>, + /// result_publisher: Option>, + /// } + /// + /// let worker = node.create_worker(MyNodeData::default()); + /// ``` + /// + /// In the above example, `addition_client` and `result_publisher` can be + /// created later inside a subscription or service callback using the [`Node`]. + pub fn create_worker<'a, Payload>( + self: &Arc, + options: impl Into>, + ) -> Worker + where + Payload: 'static + Send + Sync, + { + let options = options.into(); + let commands = self + .commands + .create_worker_commands(Box::new(options.payload)); + WorkerState::create(Arc::clone(self), commands) } /// Creates a [`Client`]. @@ -280,50 +347,11 @@ impl NodeState { where T: rosidl_runtime_rs::Service, { - let client = Arc::new(ClientState::::new(self, options)?); - { self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); - Ok(client) + ClientState::::create(options, self) } - /// 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 its executor is [spinning][2]), - /// the guard condition can be used to interrupt the wait. - /// - /// [1]: crate::GuardCondition - /// [2]: crate::Executor::spin - 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 - } - - /// Creates a [`GuardCondition`][1] with a callback. + /// Creates a [`Publisher`]. /// - /// A weak pointer to the `GuardCondition` is stored within this node. - /// When this node is added to a wait set (e.g. when its executor is [spinning][2]), - /// the guard condition can be used to interrupt the wait. - /// - /// [1]: crate::GuardCondition - /// [2]: crate::Executor::spin - pub fn create_guard_condition_with_callback(&mut self, callback: F) -> Arc - where - F: Fn() + Send + Sync + 'static, - { - 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 - } - /// Pass in only the topic name for the `options` argument to use all default publisher options: /// ``` /// # use rclrs::*; @@ -363,11 +391,25 @@ impl NodeState { where T: Message, { - let publisher = Arc::new(PublisherState::::new(Arc::clone(&self.handle), options)?); - Ok(publisher) + PublisherState::::create(options, Arc::clone(&self.handle)) } - /// Creates a [`Service`][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. This + /// will depend on what kind of executor is running. + /// + /// If you want to ensure that calls to this service can only run + /// one-at-a-time then consider creating a [`Worker`] and using that to + /// [create a service][worker-service]. + /// + /// [worker-service]: WorkerState::create_service + /// + /// # Service Options /// /// Pass in only the service name for the `options` argument to use all default service options: /// ``` @@ -376,7 +418,7 @@ impl NodeState { /// # let node = executor.create_node("my_node").unwrap(); /// let service = node.create_service::( /// "my_service", - /// |_info, _request| { + /// |_request: test_msgs::srv::Empty_Request| { /// println!("Received request!"); /// test_msgs::srv::Empty_Response::default() /// }, @@ -394,7 +436,7 @@ impl NodeState { /// "my_service" /// .keep_all() /// .transient_local(), - /// |_info, _request| { + /// |_request: test_msgs::srv::Empty_Request| { /// println!("Received request!"); /// test_msgs::srv::Empty_Response::default() /// }, @@ -409,23 +451,211 @@ impl NodeState { /// /// [1]: crate::Service /// [2]: crate::QoSReliabilityPolicy::Reliable - pub fn create_service<'a, T, F>( - self: &Arc, + /// + /// # Service Callbacks + /// + /// Three callback signatures are supported: + /// - [`Fn`] ( `Request` ) -> `Response` + /// - [`Fn`] ( `Request`, [`RequestId`][3] ) -> `Response` + /// - [`Fn`] ( `Request`, [`ServiceInfo`][4] ) -> `Response` + /// + /// [3]: crate::RequestId + /// [4]: crate::ServiceInfo + /// + /// Any internal state captured into the callback that needs to be mutated + /// will need to be wrapped in [`Mutex`] to ensure it is synchronized across + /// multiple simultaneous runs of the callback. For example: + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// use std::sync::Mutex; + /// use example_interfaces::srv::*; + /// + /// let counter = Mutex::new(0usize); + /// let service = node.create_service::( + /// "trigger_counter", + /// move |_request: Trigger_Request| { + /// let mut counter = counter.lock().unwrap(); + /// *counter += 1; + /// println!("Triggered {} times", *counter); + /// Trigger_Response { + /// success: true, + /// message: "no problems here".to_string(), + /// } + /// } + /// )?; + /// # Ok::<(), RclrsError>(()) + /// ``` + /// To share the internal state outside of the callback you will need to + /// wrap it in [`Arc`] or `Arc>` and then clone the [`Arc`] before + /// capturing it in the closure: + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// use std::sync::{Arc, Mutex}; + /// use example_interfaces::srv::*; + /// + /// let counter = Arc::new(Mutex::new(0usize)); + /// + /// let counter_in_service = Arc::clone(&counter); + /// let service = node.create_service::( + /// "trigger_counter", + /// move |_request: Trigger_Request| { + /// let mut counter = counter_in_service.lock().unwrap(); + /// *counter += 1; + /// println!("Triggered {} times", *counter); + /// Trigger_Response { + /// success: true, + /// message: "no problems here".to_string(), + /// } + /// } + /// )?; + /// + /// // TODO(@mxgrey): Replace this with a timer when timers become available + /// std::thread::spawn(move || { + /// loop { + /// std::thread::sleep(std::time::Duration::from_secs(1)); + /// println!("Last count of triggers: {}", counter.lock().unwrap()); + /// } + /// }); + /// # Ok::<(), RclrsError>(()) + /// ``` + /// + /// In general, when you need to manage some state within a blocking service, + /// it may be a good idea to create a service using a [`WorkerState`] instead of + /// creating one directly from the node. The [`WorkerState`] can carry your state + /// as a payload so you don't have to worry about locking and sharing. + /// + /// The advantage of creating a service directly from the [`NodeState`] is you + /// can create async services using [`NodeState::create_async_service`]. + pub fn create_service<'a, T, Args>( + &self, options: impl Into>, - callback: F, + callback: impl IntoNodeServiceCallback, ) -> Result, RclrsError> where T: rosidl_runtime_rs::Service, - F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send, { - let service = Arc::new(ServiceState::::new(self, options, callback)?); - { self.services_mtx.lock().unwrap() } - .push(Arc::downgrade(&service) as Weak); - Ok(service) + ServiceState::::create( + options, + callback.into_node_service_callback(), + &self.handle, + self.commands.async_worker_commands(), + ) } - /// Creates a [`Subscription`]. + /// Creates a [`Service`] with an async callback. + /// + /// # 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. /// + /// # Service Options + /// + /// See [`create_service`][NodeState::create_service] for examples of setting + /// the service options. + /// + /// # Async Service Callbacks + /// + /// Three callback signatures are supported: + /// - [`FnMut`] ( `Request` ) -> impl [`Future`][5] + /// - [`FnMut`] ( `Request`, [`RequestId`][3] ) -> impl [`Future`][5] + /// - [`FnMut`] ( `Request`, [`ServiceInfo`][4] ) -> impl [`Future`][5] + /// + /// [3]: crate::RequestId + /// [4]: crate::ServiceInfo + /// [5]: std::future::Future + /// + /// In this case the closure can be [`FnMut`], allowing internal state to + /// be mutable, but it should be noted that the function is expected to + /// immediately return a [`Future`][5], so in many cases any internal state + /// that needs to be mutated will still need to be wrapped in [`Arc`] and + /// [`Mutex`] to ensure it is synchronized across multiple runs of the + /// `Future` that the callback produces. + /// + /// However unlike the blocking callbacks that can be provided to + /// [`NodeState::create_service`], callbacks for async services can take + /// advantage of `.await`. This allows you to capture [`Client`]s or + /// [`Worker`]s into the closure, run tasks on them, and await the outcome. + /// This allows one async service to share state data across multiple workers. + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node")?; + /// use std::sync::Arc; + /// use example_interfaces::srv::*; + /// + /// let worker_a = node.create_worker(0_i64); + /// let worker_b = node.create_worker(0_i64); + /// + /// let service = node.create_async_service::( + /// "add", + /// move |request: AddTwoInts_Request| { + /// // Clone the workers so they can be captured into the async block + /// let worker_a = Arc::clone(&worker_a); + /// let worker_b = Arc::clone(&worker_b); + /// async move { + /// // Save the requested values into each worker + /// let a = request.a; + /// let _ = worker_a.run(move |last_a: &mut i64| { + /// *last_a = a; + /// }).await; + /// + /// let b = request.b; + /// let _ = worker_b.run(move |last_b: &mut i64| { + /// *last_b = b; + /// }).await; + /// + /// // Awaiting above ensures that each number from the + /// // request is saved in its respective worker before + /// // we give back a response. + /// AddTwoInts_Response { sum: a + b } + /// } + /// } + /// )?; + /// # Ok::<(), RclrsError>(()) + /// ``` + pub fn create_async_service<'a, T, Args>( + &self, + options: impl Into>, + callback: impl IntoAsyncServiceCallback, + ) -> Result, RclrsError> + where + T: rosidl_runtime_rs::Service, + { + ServiceState::::create( + options, + callback.into_async_service_callback(), + &self.handle, + self.commands.async_worker_commands(), + ) + } + + /// 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>`. In cases where you need to + /// manage a changing state for the subscription, consider using + /// [`WorkerState::create_subscription`] instead of this one. + /// + /// # Subscription Options /// /// Pass in only the topic name for the `options` argument to use all default subscription options: /// ``` @@ -465,59 +695,214 @@ impl NodeState { /// ); /// ``` /// + /// # Subscription Callbacks + /// + /// Subscription callbacks support six signatures: + /// - [`Fn`] ( `Message` ) + /// - [`Fn`] ( `Message`, [`MessageInfo`][1] ) + /// - [`Fn`] ( [`Box`]<`Message`> ) + /// - [`Fn`] ( [`Box`]<`Message`>, [`MessageInfo`][1] ) + /// - [`Fn`] ( [`ReadOnlyLoanedMessage`][2]<`Message`> ) + /// - [`Fn`] ( [`ReadOnlyLoanedMessage`][2]<`Message`>, [`MessageInfo`][1] ) + /// + /// [1]: crate::MessageInfo + /// [2]: crate::ReadOnlyLoanedMessage + /// + /// All function signatures use [`Fn`] since the callback may be run + /// multiple times simultaneously across different threads depending on the + /// executor runtime that is being used. Because of this, any internal state + /// captured into the callback that needs to be mutated will need to be + /// wrapped in [`Mutex`] to ensure it is synchronized across multiple + /// simultaneous runs of the callback. For example: + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// use std::sync::Mutex; + /// + /// let num_messages = Mutex::new(0usize); + /// let subscription = node.create_subscription( + /// "topic", + /// move |msg: example_interfaces::msg::String| { + /// let mut num = num_messages.lock().unwrap(); + /// *num += 1; + /// println!("#{} | I heard: '{}'", *num, msg.data); + /// }, + /// )?; + /// # Ok::<(), RclrsError>(()) + /// ``` + /// + /// To share the internal state outside of the callback you will need to + /// wrap it in [`Arc`] (or `Arc>` for mutability) and then clone + /// the [`Arc`] before capturing it in the closure: + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// use std::sync::{Arc, Mutex}; + /// + /// let data = Arc::new(Mutex::new(String::new())); + /// + /// let data_in_subscription = Arc::clone(&data); + /// let subscription = node.create_subscription( + /// "topic", + /// move |msg: example_interfaces::msg::String| { + /// let mut data = data_in_subscription.lock().unwrap(); + /// *data = msg.data; + /// }, + /// )?; + /// + /// // TODO(@mxgrey): Replace this with a timer when timers become available + /// std::thread::spawn(move || { + /// loop { + /// std::thread::sleep(std::time::Duration::from_secs(1)); + /// println!("Last message received: {}", data.lock().unwrap()); + /// } + /// }); + /// # Ok::<(), RclrsError>(()) + /// ``` + /// + /// You can change the subscription at any time by calling + /// [`SubscriptionState::set_callback`] or + /// [`SubscriptionState::set_async_callback`]. Even if the subscription is + /// initially created with a regular callback, it can be changed to an async + /// callback at any time. pub fn create_subscription<'a, T, Args>( - self: &Arc, + &self, options: impl Into>, - callback: impl SubscriptionCallback, + callback: impl IntoNodeSubscriptionCallback, ) -> Result, RclrsError> where T: Message, { - let subscription = Arc::new(SubscriptionState::::new(self, options, 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() - } - - pub(crate) fn live_guard_conditions(&self) -> Vec> { - { self.guard_conditions_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .collect() + SubscriptionState::::create( + options, + callback.into_node_subscription_callback(), + &self.handle, + self.commands.async_worker_commands(), + ) } - 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. + /// + /// The key advantage of an async subscription is that the callback can use + /// `.await`, and other callbacks will be able to run concurrently without + /// being blocked by this callback. You can also pass in an `async fn` as + /// the callback, but in most cases you will probably need to use a closure + /// that returns an `async { ... }` block so that you can capture some state + /// into the closure. + /// + /// If you don't need async language features for your callback, then + /// consider using [`NodeState::create_subscription`] or + /// [`WorkerState::create_subscription`]. + /// + /// # Subscription Options + /// + /// See [`create_subscription`][NodeState::create_subscription] for examples + /// of setting the subscription options. + /// + /// # Async Subscription Callbacks + /// + /// Async subscription callbacks support six signatures: + /// - [`FnMut`] ( `Message` ) -> impl [`Future`][1] + /// - [`FnMut`] ( `Message`, [`MessageInfo`][2] ) -> impl [`Future`][1] + /// - [`FnMut`] ( [`Box`]<`Message`> ) -> impl [`Future`][1] + /// - [`FnMut`] ( [`Box`]<`Message`>, [`MessageInfo`][2] ) -> impl [`Future`][1] + /// - [`FnMut`] ( [`ReadOnlyLoanedMessage`][3]<`Message`> ) -> impl [`Future`][1] + /// - [`FnMut`] ( [`ReadOnlyLoanedMessage`][3]<`Message`>, [`MessageInfo`][2] ) -> impl [`Future`][1] + /// + /// [1]: std::future::Future + /// [2]: crate::MessageInfo + /// [3]: crate::ReadOnlyLoanedMessage + /// + /// In this case the closure can be [`FnMut`], allowing internal state to be + /// mutable, but it should be noted that the function is expected to + /// immediately return a [`Future`][1], so in many cases any internal state + /// that needs to be mutable will still need to be wrapped in [`Arc`] and + /// [`Mutex`] to ensure it is synchronized across mutliple runs of the + /// `Future` that the callback produces. + /// + /// However unlike the blocking callbacks that can be provided to + /// [`NodeState::create_subscription`], callbacks for async subscriptions + /// can take advantage of `.await`. This allows you to capture [`Client`]s + /// or [`Worker`]s into the closure, run tasks on them, and await the + /// outcome. This allows one async subscription to share state data across + /// multiple workers. + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// use std::sync::Arc; + /// + /// let count_worker = node.create_worker(0_usize); + /// let data_worker = node.create_worker(String::new()); + /// + /// let service = node.create_async_subscription::( + /// "topic", + /// move |msg: example_interfaces::msg::String| { + /// // Clone the workers so they can be captured into the async block + /// let count_worker = Arc::clone(&count_worker); + /// let data_worker = Arc::clone(&data_worker); + /// async move { + /// // Update the message count + /// let current_count = count_worker.run(move |count: &mut usize| { + /// *count += 1; + /// *count + /// }).await.unwrap(); + /// + /// // Change the data in the data_worker and get back the data + /// // that was previously put in there. + /// let previous = data_worker.run(move |data: &mut String| { + /// std::mem::replace(data, msg.data) + /// }).await.unwrap(); + /// + /// println!("Current count is {current_count}, data was previously {previous}"); + /// } + /// } + /// )?; + /// # Ok::<(), RclrsError>(()) + /// ``` + /// + /// You can change the subscription at any time by calling + /// [`SubscriptionState::set_callback`] or + /// [`SubscriptionState::set_async_callback`]. Even if the subscription is + /// initially created with an async callback, it can be changed to a regular + /// callback at any time. + pub fn create_async_subscription<'a, T, Args>( + &self, + options: impl Into>, + callback: impl IntoAsyncSubscriptionCallback, + ) -> Result, RclrsError> + where + T: Message, + { + SubscriptionState::::create( + options, + callback.into_async_subscription_callback(), + &self.handle, + self.commands.async_worker_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 or by - /// passing custom [`NodeOptions`] into [`Context::new`][2] or [`Context::from_env`][3]. + /// 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::Context::new - /// [3]: crate::Context::from_env + /// [2]: crate::InitOptions + /// [3]: crate::Context::new + /// [4]: crate::Context::from_env /// /// # Example /// ``` @@ -587,10 +972,87 @@ impl NodeState { } } + /// 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. + /// + /// 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 + /// + /// 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. + /// + /// + 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 NodeState { @@ -616,14 +1078,18 @@ 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 crate::{test_helpers::*, *}; #[test] fn traits() { - assert_send::(); - assert_sync::(); + assert_send::(); + assert_sync::(); } #[test] diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index ff0dc0985..69e97e432 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -489,8 +489,8 @@ mod tests { let node = executor.create_node(node_name).unwrap(); let check_rosout = |topics: HashMap>| { - // rosout shows up in humble, even if the graph is empty - #[cfg(ros_distro = "humble")] + // rosout shows up in humble and iron, even if the graph is empty + #[cfg(any(ros_distro = "humble"))] { assert_eq!(topics.len(), 1); assert_eq!( 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/node_options.rs b/rclrs/src/node/node_options.rs index 4744a4812..4b82df4e6 100644 --- a/rclrs/src/node/node_options.rs +++ b/rclrs/src/node/node_options.rs @@ -4,8 +4,12 @@ use std::{ sync::{atomic::AtomicBool, Arc, Mutex}, }; +use futures::channel::mpsc::unbounded; + use crate::{ - rcl_bindings::*, ClockType, ContextHandle, Logger, Node, NodeHandle, NodeState, + 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, }; @@ -286,7 +290,7 @@ impl<'a> NodeOptions<'a> { /// /// Only used internally. Downstream users should call /// [`Executor::create_node`]. - pub(crate) fn build(self, context: &Arc) -> Result { + 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(), @@ -297,12 +301,12 @@ impl<'a> NodeOptions<'a> { s: self.namespace.to_owned(), })?; let rcl_node_options = self.create_rcl_node_options()?; - let rcl_context = &mut *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(context), + context_handle: Arc::clone(&commands.context().handle), initialized: AtomicBool::new(false), }); @@ -356,16 +360,43 @@ impl<'a> NodeOptions<'a> { } }; + // --- 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 { - clients_mtx: Mutex::default(), - guard_conditions_mtx: Mutex::default(), - services_mtx: Mutex::default(), - subscriptions_mtx: Mutex::default(), 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); diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index 3a9f026da..fe9fa0918 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -639,7 +639,7 @@ impl std::fmt::Display for ParameterValueError { impl std::error::Error for ParameterValueError {} /// Error that can be generated when doing operations on parameters. -#[derive(Debug)] +#[derive(Debug, PartialEq, Eq)] pub enum DeclarationError { /// Parameter was already declared and a new declaration was attempted. AlreadyDeclared, diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index ad22f2c75..5d1c25216 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -9,7 +9,7 @@ use rosidl_runtime_rs::Sequence; use super::ParameterMap; use crate::{ parameter::{DeclaredValue, ParameterKind, ParameterStorage}, - rmw_request_id_t, IntoPrimitiveOptions, Node, QoSProfile, RclrsError, Service, + IntoPrimitiveOptions, Node, QoSProfile, RclrsError, Service, }; // The variables only exist to keep a strong reference to the services and are technically unused. @@ -248,7 +248,7 @@ impl ParameterService { let map = parameter_map.clone(); let describe_parameters_service = node.create_service( (fqn.clone() + "/describe_parameters").qos(QoSProfile::parameter_services_default()), - move |_req_id: &rmw_request_id_t, req: DescribeParameters_Request| { + move |req: DescribeParameters_Request| { let map = map.lock().unwrap(); describe_parameters(req, &map) }, @@ -256,7 +256,7 @@ impl ParameterService { let map = parameter_map.clone(); let get_parameter_types_service = node.create_service( (fqn.clone() + "/get_parameter_types").qos(QoSProfile::parameter_services_default()), - move |_req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { + move |req: GetParameterTypes_Request| { let map = map.lock().unwrap(); get_parameter_types(req, &map) }, @@ -264,7 +264,7 @@ impl ParameterService { let map = parameter_map.clone(); let get_parameters_service = node.create_service( (fqn.clone() + "/get_parameters").qos(QoSProfile::parameter_services_default()), - move |_req_id: &rmw_request_id_t, req: GetParameters_Request| { + move |req: GetParameters_Request| { let map = map.lock().unwrap(); get_parameters(req, &map) }, @@ -272,7 +272,7 @@ impl ParameterService { let map = parameter_map.clone(); let list_parameters_service = node.create_service( (fqn.clone() + "/list_parameters").qos(QoSProfile::parameter_services_default()), - move |_req_id: &rmw_request_id_t, req: ListParameters_Request| { + move |req: ListParameters_Request| { let map = map.lock().unwrap(); list_parameters(req, &map) }, @@ -280,7 +280,7 @@ impl ParameterService { let map = parameter_map.clone(); let set_parameters_service = node.create_service( (fqn.clone() + "/set_parameters").qos(QoSProfile::parameter_services_default()), - move |_req_id: &rmw_request_id_t, req: SetParameters_Request| { + move |req: SetParameters_Request| { let mut map = map.lock().unwrap(); set_parameters(req, &mut map) }, @@ -288,7 +288,7 @@ impl ParameterService { let set_parameters_atomically_service = node.create_service( (fqn.clone() + "/set_parameters_atomically") .qos(QoSProfile::parameter_services_default()), - move |_req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { + move |req: SetParametersAtomically_Request| { let mut map = parameter_map.lock().unwrap(); set_parameters_atomically(req, &mut map) }, @@ -317,7 +317,10 @@ mod tests { }; use rosidl_runtime_rs::{seq, Sequence}; use std::{ - sync::{Arc, RwLock}, + sync::{ + atomic::{AtomicBool, Ordering}, + Arc, + }, time::Duration, }; @@ -329,21 +332,6 @@ mod tests { dynamic_param: MandatoryParameter, } - async fn try_until_timeout(mut f: F) -> Result<(), ()> - where - F: FnMut() -> bool, - { - 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(ns: &str) -> (Executor, TestNode, Node) { let executor = Context::default().create_basic_executor(); let node = executor @@ -397,560 +385,542 @@ mod tests { #[test] fn test_parameter_services_names_and_types() -> Result<(), RclrsError> { - let (_, node, _client) = construct_test_nodes("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)), + ) + .first_error()?; - let names_and_types = node.node.get_service_names_and_types()?; - let types = names_and_types - .get("/names_types/node/describe_parameters") - .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/DescribeParameters".to_string())); - let types = names_and_types - .get("/names_types/node/get_parameters") - .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/GetParameters".to_string())); - let types = names_and_types - .get("/names_types/node/set_parameters") - .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/SetParameters".to_string())); - let types = names_and_types - .get("/names_types/node/set_parameters_atomically") - .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/SetParametersAtomically".to_string())); - let types = names_and_types - .get("/names_types/node/list_parameters") - .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/ListParameters".to_string())); - let types = names_and_types - .get("/names_types/node/get_parameter_types") - .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/GetParameterTypes".to_string())); Ok(()) } - #[tokio::test] - async fn test_list_parameters_service() -> Result<(), RclrsError> { - let (mut executor, _test, client) = construct_test_nodes("list"); - let list_client = client.create_client::("/list/node/list_parameters")?; - - try_until_timeout(|| list_client.service_is_ready().unwrap()) - .await + #[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)), + ) + .first_error()?; + + // 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(); - let done = Arc::new(RwLock::new(false)); + executor + .spin( + SpinOptions::default() + .until_promise_resolved(promise) + .timeout(Duration::from_secs(5)), + ) + .first_error()?; + 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(); - let inner_done = done.clone(); - let rclrs_spin = tokio::task::spawn(async move { - try_until_timeout(move || { - executor - .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) - .timeout_ok() - .first_error() - .unwrap(); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .first_error()?; + 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(); - *inner_done.read().unwrap() + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .first_error()?; + 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); }) - .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; - 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)) + .first_error()?; + assert!(callback_ran.load(Ordering::Acquire)); Ok(()) } - #[tokio::test] - async fn test_get_set_parameters_service() -> Result<(), RclrsError> { - let (mut executor, test, client) = construct_test_nodes("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 + #[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")?; - 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 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 done = Arc::new(RwLock::new(false)); + let clients_ready = client_node + .notify_on_graph_change_with_period(Duration::from_millis(1), clients_ready_condition); - let inner_done = done.clone(); - let rclrs_spin = tokio::task::spawn(async move { - try_until_timeout(move || { - executor - .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) - .timeout_ok() - .first_error() - .unwrap(); + executor + .spin(SpinOptions::default().until_promise_resolved(clients_ready)) + .first_error()?; - *inner_done.read().unwrap() + // 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); }) - .await .unwrap(); - }); - let _hold_node = test.node.clone(); - let _hold_client = client.clone(); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .first_error()?; + 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); + }) + .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)) + .first_error()?; + 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() + ], + }; + + // 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(); + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .first_error()?; + 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], + }; + + // 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); + }) + .unwrap(); + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .first_error()?; + 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!(test.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!(!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); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - // Set the node to use undeclared parameters and try to set one - test.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!( - test.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)) + .first_error()?; + assert!(callback_ran.load(Ordering::Acquire)); Ok(()) } - #[tokio::test] - async fn test_describe_get_types_parameters_service() -> Result<(), RclrsError> { - let (mut executor, _test, client) = construct_test_nodes("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 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 done = Arc::new(RwLock::new(false)); + 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)) + .first_error()?; + + // 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); + }) + .unwrap(); - let inner_done = done.clone(); - let rclrs_spin = tokio::task::spawn(async move { - try_until_timeout(move || { - executor - .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) - .timeout_ok() - .first_error() - .unwrap(); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .first_error()?; + 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(); - *inner_done.read().unwrap() + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .first_error()?; + 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); }) - .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(); + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .first_error()?; + assert!(callback_ran.load(Ordering::Acquire)); Ok(()) } diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 7deb0111a..29a7e1579 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -54,10 +54,10 @@ impl Drop for PublisherHandle { /// The underlying RMW will decide on the concrete delivery mechanism (network stack, shared /// memory, or intraprocess). /// -/// Sending messages does not require the node's executor to [spin][1]. +/// Sending messages does not require the node's executor to [spin][2]. /// /// [1]: crate::NodeState::create_publisher -/// [2]: crate::spin +/// [2]: crate::Executor::spin pub type Publisher = Arc>; /// The inner state of a [`Publisher`]. @@ -95,10 +95,10 @@ where /// Creates a new `Publisher`. /// /// Node and namespace changes are always applied _before_ topic remapping. - pub(crate) fn new<'a>( - node_handle: Arc, + pub(crate) fn create<'a>( options: impl Into>, - ) -> Result + node_handle: Arc, + ) -> Result, RclrsError> where T: Message, { @@ -137,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. @@ -267,10 +267,10 @@ where } } -/// `PublisherOptions` are used by [`Node::create_publisher`][1] to initialize +/// `PublisherOptions` are used by [`NodeState::create_publisher`][1] to initialize /// a [`Publisher`]. /// -/// [1]: crate::Node::create_publisher +/// [1]: crate::NodeState::create_publisher #[derive(Debug, Clone)] #[non_exhaustive] pub struct PublisherOptions<'a> { diff --git a/rclrs/src/rcl_bindings.rs b/rclrs/src/rcl_bindings.rs index 90f434009..dbfb5d5b0 100644 --- a/rclrs/src/rcl_bindings.rs +++ b/rclrs/src/rcl_bindings.rs @@ -89,6 +89,10 @@ cfg_if::cfg_if! { #[derive(Debug)] pub struct rcl_wait_set_t; + #[repr(C)] + #[derive(Debug)] + pub struct rcl_timer_t; + #[repr(C)] #[derive(Debug)] pub struct rcutils_string_array_t; diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index 120b2aa69..aaa07abcf 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -1,68 +1,43 @@ use std::{ + any::Any, boxed::Box, - ffi::CString, - sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, + ffi::{CStr, CString}, + sync::{Arc, Mutex, MutexGuard}, }; -use rosidl_runtime_rs::Message; +use rosidl_runtime_rs::{Message, Service as IdlService}; use crate::{ - error::{RclReturnCode, ToResult}, - rcl_bindings::*, - IntoPrimitiveOptions, MessageCow, Node, NodeHandle, QoSProfile, RclrsError, - ENTITY_LIFECYCLE_MUTEX, + error::ToResult, rcl_bindings::*, IntoPrimitiveOptions, MessageCow, Node, NodeHandle, + QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, Waitable, + WaitableLifecycle, WorkScope, Worker, WorkerCommands, 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 node_service_callback; +pub use node_service_callback::*; -impl ServiceHandle { - pub(crate) fn lock(&self) -> MutexGuard { - self.rcl_service.lock().unwrap() - } -} +mod into_async_service_callback; +pub use into_async_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 into_node_service_callback; +pub use into_node_service_callback::*; -/// Trait to be implemented by concrete Service structs. -/// -/// 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>; -} +mod into_worker_service_callback; +pub use into_worker_service_callback::*; -type ServiceCallback = - Box Response + 'static + Send>; +mod service_info; +pub use service_info::*; + +mod worker_service_callback; +pub use worker_service_callback::*; /// Provide a service that can respond to requests sent by ROS service clients. /// -/// Create a service using [`Node::create_service`][1]. +/// Create a service using [`Node::create_service`][1] +/// or [`Node::create_async_service`][2]. /// /// ROS only supports having one service provider for any given fully-qualified /// service name. "Fully-qualified" means the namespace is also taken into account @@ -70,11 +45,19 @@ type ServiceCallback = /// 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][2]. +/// Responding to requests requires the node's executor to [spin][3]. /// /// [1]: crate::NodeState::create_service -/// [2]: crate::spin -pub type Service = Arc>; +/// [2]: crate::NodeState::create_async_service +/// [3]: crate::Executor::spin +pub type Service = Arc>; + +/// Provide a [`Service`] that runs on a [`Worker`]. +/// +/// Create a worker service using [`WorkerState::create_service`][1]. +/// +/// [1]: crate::WorkerState::create_service +pub type WorkerService = Arc>>; /// The inner state of a [`Service`]. /// @@ -86,36 +69,43 @@ pub type Service = Arc>; /// The public API of the [`Service`] type is implemented via `ServiceState`. /// /// [1]: std::sync::Weak -pub struct ServiceState +pub struct ServiceState where - T: rosidl_runtime_rs::Service, + T: IdlService, + Scope: WorkScope, { - pub(crate) handle: Arc, - /// The callback function that runs when a request was received. - pub callback: Mutex>, - /// Ensure the parent node remains alive as long as the subscription is held. - /// This implementation will change in the future. + /// 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)] - node: Node, + lifecycle: WaitableLifecycle, } -impl ServiceState +impl ServiceState where - T: rosidl_runtime_rs::Service, + T: IdlService, + Scope: WorkScope + 'static, { - /// Creates a new service. - pub(crate) fn new<'a, F>( - node: &Node, + /// 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 { + self.handle.service_name() + } + + /// Used by [`Node`][crate::Node] to create a new service + pub(crate) fn create<'a>( options: impl Into>, - 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, - { + 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() @@ -130,7 +120,7 @@ where service_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(); unsafe { // SAFETY: @@ -153,58 +143,68 @@ where let handle = Arc::new(ServiceHandle { rcl_service: Mutex::new(rcl_service), - node_handle: Arc::clone(&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, - node: Arc::clone(node), - callback: Mutex::new(Box::new(callback)), - }) + callback, + lifecycle, + }); + commands.add_to_wait_set(waitable); + + Ok(service) } +} - /// Fetches a new request. +impl ServiceState { + /// Set the callback of this service, replacing the callback that was + /// previously set. /// - /// When there is no new message, this will return a - /// [`ServiceTakeFailed`][1]. + /// This can be used even if the service previously used an async callback. /// - /// [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 _, - ) - } - .ok()?; - Ok((T::Request::from_rmw_message(request_out), request_id_out)) + /// This can only be called when the `Scope` of the [`ServiceState`] is [`Node`]. + /// If the `Scope` is [`Worker`] then use [`Self::set_worker_callback`] instead. + pub fn set_callback(&self, callback: impl IntoNodeServiceCallback) { + let callback = callback.into_node_service_callback(); + *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. + /// + /// This can only be called when the `Scope` of the [`ServiceState`] is [`Node`]. + /// If the `Scope` is [`Worker`] then use [`Self::set_worker_callback`] instead. + pub fn set_async_callback(&self, callback: impl IntoAsyncServiceCallback) { + let callback = callback.into_async_service_callback(); + *self.callback.lock().unwrap() = callback; + } +} + +impl ServiceState> { + /// Set the callback of this service, replacing the callback that was + /// previously set. + /// + /// This can only be called when the `Scope` of the [`ServiceState`] is [`Worker`]. + /// If the `Scope` is [`Node`] then use [`Self::set_callback`] or + /// [`Self::set_async_callback`] instead. + pub fn set_worker_callback( + &self, + callback: impl IntoWorkerServiceCallback, + ) { + let callback = callback.into_worker_service_callback(); + *self.callback.lock().unwrap() = callback; } } @@ -238,35 +238,134 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From for ServiceOptions<'a> { } } -impl ServiceBase for ServiceState +struct ServiceExecutable { + handle: Arc, + callback: Arc>>, + commands: Arc, +} + +impl RclPrimitive for ServiceExecutable where - T: rosidl_runtime_rs::Service, + T: IdlService, + Scope: WorkScope, { - fn handle(&self) -> &ServiceHandle { - &self.handle + unsafe fn execute(&mut self, payload: &mut dyn Any) -> Result<(), RclrsError> { + self.callback + .lock() + .unwrap() + .execute(&self.handle, payload, &self.commands) } - 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 { + fn lock(&self) -> MutexGuard { + self.rcl_service.lock().unwrap() + } + + 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.lock()); + CStr::from_ptr(raw_service_pointer) + } + .to_string_lossy() + .into_owned() + } + + /// 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(&self) -> 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 = &*self.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( + &self, + ) -> 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 = &*self.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( + self: &Arc, + request_id: &mut rmw_request_id_t, + response: T::Response, + ) -> Result<(), RclrsError> { + let rmw_message = ::into_rmw_message(response.into_cow()); + let handle = &*self.lock(); unsafe { // SAFETY: The response type is guaranteed to match the service type by the type system. rcl_send_response( handle, - &mut req_id, + request_id, rmw_message.as_ref() as *const ::RmwMsg as *mut _, ) } @@ -274,6 +373,19 @@ where } } +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); + } + } +} + #[cfg(test)] mod tests { use super::*; @@ -299,14 +411,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..6a4cc6198 --- /dev/null +++ b/rclrs/src/service/any_service_callback.rs @@ -0,0 +1,51 @@ +use rosidl_runtime_rs::Service; + +use crate::{ + NodeServiceCallback, RclrsError, ServiceHandle, WorkerCommands, WorkerServiceCallback, +}; + +use std::{any::Any, sync::Arc}; + +/// An enum capturing the various possible function signatures for service callbacks. +pub enum AnyServiceCallback +where + T: Service, + Payload: 'static + Send, +{ + /// A callback in the Node scope + Node(NodeServiceCallback), + /// A callback in the worker scope + Worker(WorkerServiceCallback), +} + +impl AnyServiceCallback +where + T: Service, + Payload: 'static + Send, +{ + pub(super) fn execute( + &mut self, + handle: &Arc, + payload: &mut dyn Any, + commands: &Arc, + ) -> Result<(), RclrsError> { + match self { + Self::Node(node) => node.execute(Arc::clone(&handle), commands), + Self::Worker(worker) => worker.execute(handle, payload), + } + } +} + +impl From> for AnyServiceCallback { + fn from(value: NodeServiceCallback) -> Self { + AnyServiceCallback::Node(value) + } +} + +impl From> + for AnyServiceCallback +{ + fn from(value: WorkerServiceCallback) -> Self { + AnyServiceCallback::Worker(value) + } +} diff --git a/rclrs/src/service/into_async_service_callback.rs b/rclrs/src/service/into_async_service_callback.rs new file mode 100644 index 000000000..0c2663d1d --- /dev/null +++ b/rclrs/src/service/into_async_service_callback.rs @@ -0,0 +1,60 @@ +use rosidl_runtime_rs::Service; + +use super::{AnyServiceCallback, NodeServiceCallback, RequestId, ServiceInfo}; + +use std::future::Future; + +/// A trait for async callbacks of services. +/// +/// Three callback signatures are supported: +/// - [`FnMut`] ( `Request` ) -> impl [`Future`] +/// - [`FnMut`] ( `Request`, [`RequestId`] ) -> impl [`Future`] +/// - [`FnMut`] ( `Request`, [`ServiceInfo`] ) -> impl [`Future`] +pub trait IntoAsyncServiceCallback: Send + 'static +where + T: Service, +{ + /// Converts the callback into an enum. + /// + /// User code never needs to call this function. + fn into_async_service_callback(self) -> AnyServiceCallback; +} + +impl IntoAsyncServiceCallback for Func +where + T: Service, + Func: FnMut(T::Request) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_async_service_callback(mut self) -> AnyServiceCallback { + NodeServiceCallback::OnlyRequest(Box::new(move |request| Box::pin(self(request)))).into() + } +} + +impl IntoAsyncServiceCallback for Func +where + T: Service, + Func: FnMut(T::Request, RequestId) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_async_service_callback(mut self) -> AnyServiceCallback { + NodeServiceCallback::WithId(Box::new(move |request, request_id| { + Box::pin(self(request, request_id)) + })) + .into() + } +} + +impl IntoAsyncServiceCallback for Func +where + T: Service, + Func: FnMut(T::Request, ServiceInfo) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_async_service_callback(mut self) -> AnyServiceCallback { + NodeServiceCallback::WithInfo(Box::new(move |request, service_info| { + Box::pin(self(request, service_info)) + })) + .into() + } +} diff --git a/rclrs/src/service/into_node_service_callback.rs b/rclrs/src/service/into_node_service_callback.rs new file mode 100644 index 000000000..dd7507909 --- /dev/null +++ b/rclrs/src/service/into_node_service_callback.rs @@ -0,0 +1,68 @@ +use rosidl_runtime_rs::Service; + +use crate::{AnyServiceCallback, NodeServiceCallback, RequestId, ServiceInfo}; + +use std::sync::Arc; + +/// A trait to deduce regular callbacks of services. +/// +/// Users of rclrs never need to use this trait directly. +/// +/// Three callback signatures are supported: +/// - [`Fn`] ( `Request` ) -> `Response` +/// - [`Fn`] ( `Request`, [`RequestId`] ) -> `Response` +/// - [`Fn`] ( `Request`, [`ServiceInfo`] ) -> `Response` +pub trait IntoNodeServiceCallback: Send + 'static +where + T: Service, +{ + /// Converts the callback into an enum. + /// + /// User code never needs to call this function. + fn into_node_service_callback(self) -> AnyServiceCallback; +} + +impl IntoNodeServiceCallback for Func +where + T: Service, + Func: Fn(T::Request) -> T::Response + Send + Sync + 'static, +{ + fn into_node_service_callback(self) -> AnyServiceCallback { + let func = Arc::new(self); + NodeServiceCallback::OnlyRequest(Box::new(move |request| { + let f = Arc::clone(&func); + Box::pin(async move { f(request) }) + })) + .into() + } +} + +impl IntoNodeServiceCallback for Func +where + T: Service, + Func: Fn(T::Request, RequestId) -> T::Response + Send + Sync + 'static, +{ + fn into_node_service_callback(self) -> AnyServiceCallback { + let func = Arc::new(self); + NodeServiceCallback::WithId(Box::new(move |request, request_id| { + let f = Arc::clone(&func); + Box::pin(async move { f(request, request_id) }) + })) + .into() + } +} + +impl IntoNodeServiceCallback for Func +where + T: Service, + Func: Fn(T::Request, ServiceInfo) -> T::Response + Send + Sync + 'static, +{ + fn into_node_service_callback(self) -> AnyServiceCallback { + let func = Arc::new(self); + NodeServiceCallback::WithInfo(Box::new(move |request, service_info| { + let f = Arc::clone(&func); + Box::pin(async move { f(request, service_info) }) + })) + .into() + } +} diff --git a/rclrs/src/service/into_worker_service_callback.rs b/rclrs/src/service/into_worker_service_callback.rs new file mode 100644 index 000000000..86a34d240 --- /dev/null +++ b/rclrs/src/service/into_worker_service_callback.rs @@ -0,0 +1,94 @@ +use rosidl_runtime_rs::Service; + +use crate::{AnyServiceCallback, RequestId, ServiceInfo, Worker, WorkerServiceCallback}; + +/// A trait used to deduce callbacks for services that run on a worker. +/// +/// Users of rclrs never need to use this trait directly. +/// +/// Worker service callbacks support six signatures: +/// - [`FnMut`] ( `Request` ) -> `Response` +/// - [`FnMut`] ( `Request`, [`RequestId`] ) -> `Response` +/// - [`FnMut`] ( `Request`, [`ServiceInfo`] ) -> `Response` +/// - [`FnMut`] ( `&mut Payload`, `Request` ) -> `Response` +/// - [`FnMut`] ( `&mut Payload`, `Request`, [`RequestId`] ) -> `Response` +/// - [`FnMut`] ( `&mut Payload`, `Request`, [`ServiceInfo`] ) -> `Response` +pub trait IntoWorkerServiceCallback: Send + 'static +where + T: Service, + Payload: 'static + Send, +{ + /// Converts the callback into an enum + /// + /// User code never needs to call this function. + fn into_worker_service_callback(self) -> AnyServiceCallback; +} + +impl IntoWorkerServiceCallback for Func +where + T: Service, + Payload: 'static + Send, + Func: FnMut(T::Request) -> T::Response + Send + 'static, +{ + fn into_worker_service_callback(mut self) -> AnyServiceCallback { + let f = Box::new(move |_: &mut Payload, request| self(request)); + WorkerServiceCallback::OnlyRequest(f).into() + } +} + +impl IntoWorkerServiceCallback> for Func +where + T: Service, + Payload: 'static + Send, + Func: FnMut(&mut Payload, T::Request) -> T::Response + Send + 'static, +{ + fn into_worker_service_callback(self) -> AnyServiceCallback { + WorkerServiceCallback::OnlyRequest(Box::new(self)).into() + } +} + +impl IntoWorkerServiceCallback for Func +where + T: Service, + Payload: 'static + Send, + Func: FnMut(T::Request, RequestId) -> T::Response + Send + 'static, +{ + fn into_worker_service_callback(mut self) -> AnyServiceCallback { + let f = Box::new(move |_: &mut Payload, request, request_id| self(request, request_id)); + WorkerServiceCallback::WithId(f).into() + } +} + +impl IntoWorkerServiceCallback, RequestId)> for Func +where + T: Service, + Payload: 'static + Send, + Func: FnMut(&mut Payload, T::Request, RequestId) -> T::Response + Send + 'static, +{ + fn into_worker_service_callback(self) -> AnyServiceCallback { + WorkerServiceCallback::WithId(Box::new(self)).into() + } +} + +impl IntoWorkerServiceCallback for Func +where + T: Service, + Payload: 'static + Send, + Func: FnMut(T::Request, ServiceInfo) -> T::Response + Send + 'static, +{ + fn into_worker_service_callback(mut self) -> AnyServiceCallback { + let f = Box::new(move |_: &mut Payload, request, info| self(request, info)); + WorkerServiceCallback::WithInfo(f).into() + } +} + +impl IntoWorkerServiceCallback, ServiceInfo)> for Func +where + T: Service, + Payload: 'static + Send, + Func: FnMut(&mut Payload, T::Request, ServiceInfo) -> T::Response + Send + 'static, +{ + fn into_worker_service_callback(self) -> AnyServiceCallback { + WorkerServiceCallback::WithInfo(Box::new(self)).into() + } +} diff --git a/rclrs/src/service/node_service_callback.rs b/rclrs/src/service/node_service_callback.rs new file mode 100644 index 000000000..6713396d3 --- /dev/null +++ b/rclrs/src/service/node_service_callback.rs @@ -0,0 +1,88 @@ +use rosidl_runtime_rs::Service; + +use crate::{ + log_error, rcl_bindings::rmw_request_id_t, RclrsError, RclrsErrorFilter, RequestId, + ServiceHandle, ServiceInfo, WorkerCommands, +}; + +use futures::future::BoxFuture; + +use std::sync::Arc; + +/// An enum capturing the various possible function signatures for service callbacks. +pub enum NodeServiceCallback +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 NodeServiceCallback { + pub(super) fn execute( + &mut self, + handle: Arc, + commands: &Arc, + ) -> Result<(), RclrsError> { + let evaluate = || { + match self { + NodeServiceCallback::OnlyRequest(cb) => { + let (msg, mut rmw_request_id) = handle.take_request::()?; + let response = cb(msg); + commands.run_async(async move { + if let Err(err) = + handle.send_response::(&mut rmw_request_id, response.await) + { + log_service_send_error(&*handle, rmw_request_id, err); + } + }); + } + NodeServiceCallback::WithId(cb) => { + let (msg, mut rmw_request_id) = handle.take_request::()?; + let request_id = RequestId::from_rmw_request_id(&rmw_request_id); + let response = cb(msg, request_id); + commands.run_async(async move { + if let Err(err) = + handle.send_response::(&mut rmw_request_id, response.await) + { + log_service_send_error(&*handle, rmw_request_id, err); + } + }); + } + NodeServiceCallback::WithInfo(cb) => { + let (msg, rmw_service_info) = handle.take_request_with_info::()?; + let mut rmw_request_id = rmw_service_info.rmw_request_id(); + let service_info = ServiceInfo::from_rmw_service_info(&rmw_service_info); + let response = cb(msg, service_info); + commands.run_async(async move { + if let Err(err) = + handle.send_response::(&mut rmw_request_id, response.await) + { + log_service_send_error(&*handle, rmw_request_id, err); + } + }); + } + } + + Ok(()) + }; + + evaluate().take_failed_ok() + } +} + +fn log_service_send_error( + handle: &ServiceHandle, + rmw_request_id: rmw_request_id_t, + err: RclrsError, +) { + let service_name = handle.service_name(); + log_error!( + &service_name, + "Error while sending service response for {rmw_request_id:?}: {err}", + ); +} diff --git a/rclrs/src/service/service_info.rs b/rclrs/src/service/service_info.rs new file mode 100644 index 000000000..462fb1ab9 --- /dev/null +++ b/rclrs/src/service/service_info.rs @@ -0,0 +1,84 @@ +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. + #[cfg(ros_distro = "humble")] + pub writer_guid: [i8; 16usize], + + /// A globally unique identifier for the writer of the request. + #[cfg(not(ros_distro = "humble"))] + pub writer_guid: [u8; 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, + } + } +} + +impl rmw_service_info_t { + pub(super) fn rmw_request_id(&self) -> rmw_request_id_t { + rmw_request_id_t { + writer_guid: self.request_id.writer_guid, + sequence_number: self.request_id.sequence_number, + } + } +} diff --git a/rclrs/src/service/worker_service_callback.rs b/rclrs/src/service/worker_service_callback.rs new file mode 100644 index 000000000..fd935b7e5 --- /dev/null +++ b/rclrs/src/service/worker_service_callback.rs @@ -0,0 +1,70 @@ +use rosidl_runtime_rs::Service; + +use crate::{RclrsError, RclrsErrorFilter, RequestId, ServiceHandle, ServiceInfo}; + +use std::{any::Any, sync::Arc}; + +/// An enum capturing the various possible function signatures for service +/// callbacks that can be used by a [`Worker`][crate::Worker]. +/// +/// The correct enum variant is deduced by the [`IntoWorkerServiceCallback`][1] trait. +/// +/// [1]: crate::IntoWorkerServiceCallback +pub enum WorkerServiceCallback +where + T: Service, + Payload: 'static + Send, +{ + /// A callback that only takes in the request value + OnlyRequest(Box T::Response + Send>), + /// A callback that takes in the request value and the ID of the request + WithId(Box T::Response + Send>), + /// A callback that takes in the request value and all available + WithInfo(Box T::Response + Send>), +} + +impl WorkerServiceCallback +where + T: Service, + Payload: 'static + Send, +{ + pub(super) fn execute( + &mut self, + handle: &Arc, + any_payload: &mut dyn Any, + ) -> Result<(), RclrsError> { + let Some(payload) = any_payload.downcast_mut::() else { + return Err(RclrsError::InvalidPayload { + expected: std::any::TypeId::of::(), + received: (*any_payload).type_id(), + }); + }; + + let mut evaluate = || { + match self { + WorkerServiceCallback::OnlyRequest(cb) => { + let (msg, mut rmw_request_id) = handle.take_request::()?; + let response = cb(payload, msg); + handle.send_response::(&mut rmw_request_id, response)?; + } + WorkerServiceCallback::WithId(cb) => { + let (msg, mut rmw_request_id) = handle.take_request::()?; + let request_id = RequestId::from_rmw_request_id(&rmw_request_id); + let response = cb(payload, msg, request_id); + handle.send_response::(&mut rmw_request_id, response)?; + } + WorkerServiceCallback::WithInfo(cb) => { + let (msg, rmw_service_info) = handle.take_request_with_info::()?; + let mut rmw_request_id = rmw_service_info.rmw_request_id(); + let service_info = ServiceInfo::from_rmw_service_info(&rmw_service_info); + let response = cb(payload, msg, service_info); + handle.send_response::(&mut rmw_request_id, response)?; + } + } + + Ok(()) + }; + + evaluate().take_failed_ok() + } +} diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 70afe320e..e57c542f6 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -1,83 +1,66 @@ use std::{ + any::Any, 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::*, - IntoPrimitiveOptions, Node, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + error::ToResult, qos::QoSProfile, rcl_bindings::*, IntoPrimitiveOptions, Node, NodeHandle, + RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, Waitable, WaitableLifecycle, + WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, }; -mod callback; -mod message_info; -mod readonly_loaned_message; -pub use callback::*; -pub use message_info::*; -pub use readonly_loaned_message::*; +mod any_subscription_callback; +pub use any_subscription_callback::*; -// 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 node_subscription_callback; +pub use node_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 into_async_subscription_callback; +pub use into_async_subscription_callback::*; -impl SubscriptionHandle { - pub(crate) fn lock(&self) -> MutexGuard { - self.rcl_subscription.lock().unwrap() - } -} +mod into_node_subscription_callback; +pub use into_node_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 into_worker_subscription_callback; +pub use into_worker_subscription_callback::*; -/// 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 message_info; +pub use message_info::*; + +mod readonly_loaned_message; +pub use readonly_loaned_message::*; + +mod worker_subscription_callback; +pub use worker_subscription_callback::*; /// Struct for receiving messages of type `T`. /// -/// Create a subscription using [`Node::create_subscription`][1]. +/// Create a subscription using [`NodeState::create_subscription()`][1] +/// or [`NodeState::create_async_subscription`][2]. /// /// 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 the node's executor to [spin][2]. +/// Receiving messages requires the node's executor to [spin][3]. /// /// When a subscription is created, it may take some time to get "matched" with a corresponding /// publisher. /// /// [1]: crate::NodeState::create_subscription -/// [2]: crate::spin -pub type Subscription = Arc>; +/// [2]: crate::NodeState::create_async_subscription +/// [3]: crate::Executor::spin +pub type Subscription = Arc>; + +/// A [`Subscription`] that runs on a [`Worker`]. +/// +/// Create a worker subscription using [`WorkerState::create_subscription`][1]. +/// +/// [1]: crate::WorkerState::create_subscription +pub type WorkerSubscription = Arc>>; /// The inner state of a [`Subscription`]. /// @@ -89,36 +72,54 @@ pub type Subscription = Arc>; /// The public API of the [`Subscription`] type is implemented via `SubscriptionState`. /// /// [1]: std::sync::Weak -pub struct SubscriptionState +pub struct SubscriptionState where T: Message, + Scope: WorkScope, { - pub(crate) handle: Arc, - /// The callback function that runs when a message was received. - pub callback: Mutex>, - /// Ensure the parent node remains alive as long as the subscription is held. - /// This implementation will change in the future. + /// 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)] - node: Node, - message: PhantomData, + lifecycle: WaitableLifecycle, } -impl SubscriptionState +impl SubscriptionState where T: Message, + Scope: WorkScope, { - /// Creates a new subscription. - pub(crate) fn new<'a, Args>( - node: &Node, + /// 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() + } + + /// Used by [`Node`][crate::Node] to create a new subscription. + pub(crate) fn create<'a>( options: impl Into>, - 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, - { + 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 = @@ -133,7 +134,7 @@ where rcl_subscription_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(); unsafe { // SAFETY: @@ -155,32 +156,145 @@ where let handle = Arc::new(SubscriptionHandle { rcl_subscription: Mutex::new(rcl_subscription), - node_handle: Arc::clone(&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()), - node: Arc::clone(node), - message: PhantomData, - }) + callback, + lifecycle, + })) } +} - /// Returns the topic name of the subscription. +impl SubscriptionState { + /// Set the callback of this subscription, replacing the callback that was + /// previously set. /// - /// 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() + /// This can be used even if the subscription previously used an async callback. + /// + /// This can only be called when the `Scope` of the [`SubscriptionState`] is [`Node`]. + /// If the `Scope` is [`Worker`] then use [`Self::set_worker_callback`] instead. + pub fn set_callback(&self, callback: impl IntoNodeSubscriptionCallback) { + let callback = callback.into_node_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. + /// + /// This can only be called when the `Scope` of the [`SubscriptionState`] is [`Node`]. + /// If the `Scope` is [`Worker`] then use [`Self::set_worker_callback`] instead. + pub fn set_async_callback(&self, callback: impl IntoAsyncSubscriptionCallback) { + let callback = callback.into_async_subscription_callback(); + *self.callback.lock().unwrap() = callback; + } +} + +impl SubscriptionState> { + /// Set the callback of this subscription, replacing the callback that was + /// previously set. + /// + /// This can only be called when the `Scope` of the [`SubscriptionState`] is [`Worker`]. + /// If the `Scope` is [`Node`] then use [`Self::set_callback`] or + /// [`Self::set_async_callback`] instead. + pub fn set_worker_callback( + &self, + callback: impl IntoWorkerSubscriptionCallback, + ) { + let callback = callback.into_worker_subscription_callback(); + *self.callback.lock().unwrap() = callback; + } +} + +/// `SubscriptionOptions` are used by [`Node::create_subscription`][1] to initialize +/// a [`Subscription`]. +/// +/// [1]: crate::NodeState::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(), } } +} + +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_to(&mut options.qos); + options + } +} + +struct SubscriptionExecutable { + handle: Arc, + callback: Arc>>, + commands: Arc, +} + +impl RclPrimitive for SubscriptionExecutable +where + T: Message, +{ + unsafe fn execute(&mut self, payload: &mut dyn Any) -> Result<(), RclrsError> { + self.callback + .lock() + .unwrap() + .execute(&self.handle, payload, &self.commands) + } + + fn kind(&self) -> RclPrimitiveKind { + RclPrimitiveKind::Subscription + } + + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::Subscription(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_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() + } /// Fetches a new message. /// @@ -204,18 +318,18 @@ where // | rmw_take | // +-------------+ // ``` - pub fn take(&self) -> Result<(T, MessageInfo), RclrsError> { + fn take(&self) -> Result<(T, MessageInfo), RclrsError> { let mut rmw_message = ::RmwMsg::default(); - let message_info = self.take_inner(&mut rmw_message)?; + let message_info = Self::take_inner::(self, &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. - pub fn take_boxed(&self) -> Result<(Box, MessageInfo), RclrsError> { + fn take_boxed(&self) -> Result<(Box, MessageInfo), RclrsError> { let mut rmw_message = Box::<::RmwMsg>::default(); - let message_info = self.take_inner(&mut *rmw_message)?; + let message_info = Self::take_inner::(self, &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)); @@ -223,12 +337,12 @@ where } // Inner function, to be used by both regular and boxed versions. - fn take_inner( + 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(); + let rcl_subscription = &mut *self.lock(); unsafe { // SAFETY: The first two pointers are valid/initialized, and do not need to be valid // beyond the function call. @@ -249,19 +363,21 @@ where /// When there is no new message, this will return a /// [`SubscriptionTakeFailed`][1]. /// - /// This is the counterpart to [`PublisherState::borrow_loaned_message()`][2]. See its documentation + /// This is the counterpart to [`Publisher::borrow_loaned_message()`][2]. See its documentation /// for more information. /// /// [1]: crate::RclrsError - /// [2]: crate::PublisherState::borrow_loaned_message - pub fn take_loaned(&self) -> Result<(ReadOnlyLoanedMessage<'_, T>, MessageInfo), RclrsError> { + /// [2]: crate::Publisher::borrow_loaned_message + fn take_loaned( + self: &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( - &*self.handle.lock(), + &*self.lock(), &mut msg_ptr, &mut message_info, std::ptr::null_mut(), @@ -270,7 +386,7 @@ where } let read_only_loaned_msg = ReadOnlyLoanedMessage { msg_ptr: msg_ptr as *const T::RmwMsg, - subscription: self, + handle: Arc::clone(self), }; Ok(( read_only_loaned_msg, @@ -279,89 +395,15 @@ where } } -/// `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(), - } - } -} - -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_to(&mut options.qos); - options - } -} - -impl SubscriptionBase for SubscriptionState -where - T: Message, -{ - fn handle(&self) -> &SubscriptionHandle { - &self.handle - } - - 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); } } } @@ -374,8 +416,8 @@ mod tests { #[test] fn traits() { - assert_send::>(); - assert_sync::>(); + assert_send::>(); + assert_sync::>(); } #[test] @@ -445,7 +487,7 @@ mod tests { #[test] fn test_node_subscription_raii() { use crate::*; - use std::sync::atomic::Ordering; + use std::sync::atomic::{AtomicBool, Ordering}; let mut executor = Context::default().create_basic_executor(); diff --git a/rclrs/src/subscription/any_subscription_callback.rs b/rclrs/src/subscription/any_subscription_callback.rs new file mode 100644 index 000000000..c3f7227e0 --- /dev/null +++ b/rclrs/src/subscription/any_subscription_callback.rs @@ -0,0 +1,51 @@ +use rosidl_runtime_rs::Message; + +use crate::{ + subscription::SubscriptionHandle, NodeSubscriptionCallback, RclrsError, WorkerCommands, + WorkerSubscriptionCallback, +}; + +use std::{any::Any, sync::Arc}; + +/// An enum capturing the various possible function signatures for subscription callbacks. +/// +/// The correct enum variant is deduced by the [`IntoNodeSubscriptionCallback`][1], +/// [`IntoAsyncSubscriptionCallback`][2], or [`IntoWorkerSubscriptionCallback`][3] trait. +/// +/// [1]: crate::IntoNodeSubscriptionCallback +/// [2]: crate::IntoAsyncSubscriptionCallback +/// [3]: crate::IntoWorkerSubscriptionCallback +pub enum AnySubscriptionCallback { + /// A callback in the Node scope + Node(NodeSubscriptionCallback), + /// A callback in the worker scope + Worker(WorkerSubscriptionCallback), +} + +impl AnySubscriptionCallback { + pub(super) fn execute( + &mut self, + handle: &Arc, + payload: &mut dyn Any, + commands: &WorkerCommands, + ) -> Result<(), RclrsError> { + match self { + Self::Node(node) => node.execute(handle, commands), + Self::Worker(worker) => worker.execute(handle, payload), + } + } +} + +impl From> for AnySubscriptionCallback { + fn from(value: NodeSubscriptionCallback) -> Self { + AnySubscriptionCallback::Node(value) + } +} + +impl From> + for AnySubscriptionCallback +{ + fn from(value: WorkerSubscriptionCallback) -> Self { + AnySubscriptionCallback::Worker(value) + } +} 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/into_async_subscription_callback.rs b/rclrs/src/subscription/into_async_subscription_callback.rs new file mode 100644 index 000000000..f513a7fee --- /dev/null +++ b/rclrs/src/subscription/into_async_subscription_callback.rs @@ -0,0 +1,190 @@ +use rosidl_runtime_rs::Message; + +use super::{AnySubscriptionCallback, MessageInfo, NodeSubscriptionCallback}; +use crate::ReadOnlyLoanedMessage; + +use std::future::Future; + +/// A trait for async callbacks of subscriptions. +/// +/// Async subscription callbacks support six signatures: +/// - [`FnMut`] ( `Message` ) -> impl [`Future`] +/// - [`FnMut`] ( `Message`, [`MessageInfo`] ) -> impl [`Future`] +/// - [`FnMut`] ( [`Box`]<`Message`> ) -> impl [`Future`] +/// - [`FnMut`] ( [`Box`]<`Message`>, [`MessageInfo`] ) -> impl [`Future`] +/// - [`FnMut`] ( [`ReadOnlyLoanedMessage`]<`Message`> ) -> impl [`Future`] +/// - [`FnMut`] ( [`ReadOnlyLoanedMessage`]<`Message`>, [`MessageInfo`] ) -> impl [`Future`] +pub trait IntoAsyncSubscriptionCallback: Send + 'static +where + T: Message, +{ + /// Converts the callback into an enum. + /// + /// User code never needs to call this function. + fn into_async_subscription_callback(self) -> AnySubscriptionCallback; +} + +impl IntoAsyncSubscriptionCallback for Func +where + T: Message, + Func: FnMut(T) -> Out + Send + 'static, + Out: Future + Send + 'static, +{ + fn into_async_subscription_callback(mut self) -> AnySubscriptionCallback { + NodeSubscriptionCallback::Regular(Box::new(move |message| Box::pin(self(message)))).into() + } +} + +impl IntoAsyncSubscriptionCallback for Func +where + T: Message, + Func: FnMut(T, MessageInfo) -> Out + Send + 'static, + Out: Future + Send + 'static, +{ + fn into_async_subscription_callback(mut self) -> AnySubscriptionCallback { + NodeSubscriptionCallback::RegularWithMessageInfo(Box::new(move |message, info| { + Box::pin(self(message, info)) + })) + .into() + } +} + +impl IntoAsyncSubscriptionCallback,)> for Func +where + T: Message, + Func: FnMut(Box) -> Out + Send + 'static, + Out: Future + Send + 'static, +{ + fn into_async_subscription_callback(mut self) -> AnySubscriptionCallback { + NodeSubscriptionCallback::Boxed(Box::new(move |message| Box::pin(self(message)))).into() + } +} + +impl IntoAsyncSubscriptionCallback, MessageInfo)> for Func +where + T: Message, + Func: FnMut(Box, MessageInfo) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_async_subscription_callback(mut self) -> AnySubscriptionCallback { + NodeSubscriptionCallback::BoxedWithMessageInfo(Box::new(move |message, info| { + Box::pin(self(message, info)) + })) + .into() + } +} + +impl IntoAsyncSubscriptionCallback,)> for Func +where + T: Message, + Func: FnMut(ReadOnlyLoanedMessage) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_async_subscription_callback(mut self) -> AnySubscriptionCallback { + NodeSubscriptionCallback::Loaned(Box::new(move |message| Box::pin(self(message)))).into() + } +} + +impl IntoAsyncSubscriptionCallback, MessageInfo)> for Func +where + T: Message, + Func: FnMut(ReadOnlyLoanedMessage, MessageInfo) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_async_subscription_callback(mut self) -> AnySubscriptionCallback { + NodeSubscriptionCallback::LoanedWithMessageInfo(Box::new(move |message, info| { + Box::pin(self(message, info)) + })) + .into() + } +} + +#[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_async_subscription_callback(), + AnySubscriptionCallback::Node(NodeSubscriptionCallback::::Regular(_)), + )); + let cb = |_msg: TestMessage, _info: MessageInfo| async {}; + assert!(matches!( + cb.into_async_subscription_callback(), + AnySubscriptionCallback::Node( + NodeSubscriptionCallback::::RegularWithMessageInfo(_) + ), + )); + let cb = |_msg: Box| async {}; + assert!(matches!( + cb.into_async_subscription_callback(), + AnySubscriptionCallback::Node(NodeSubscriptionCallback::::Boxed(_)), + )); + let cb = |_msg: Box, _info: MessageInfo| async {}; + assert!(matches!( + cb.into_async_subscription_callback(), + AnySubscriptionCallback::Node( + NodeSubscriptionCallback::::BoxedWithMessageInfo(_) + ), + )); + let cb = |_msg: ReadOnlyLoanedMessage| async {}; + assert!(matches!( + cb.into_async_subscription_callback(), + AnySubscriptionCallback::Node(NodeSubscriptionCallback::::Loaned(_)), + )); + let cb = |_msg: ReadOnlyLoanedMessage, _info: MessageInfo| async {}; + assert!(matches!( + cb.into_async_subscription_callback(), + AnySubscriptionCallback::Node( + NodeSubscriptionCallback::::LoanedWithMessageInfo(_) + ), + )); + + assert!(matches!( + test_regular.into_async_subscription_callback(), + AnySubscriptionCallback::Node(NodeSubscriptionCallback::::Regular(_)), + )); + assert!(matches!( + test_regular_with_info.into_async_subscription_callback(), + AnySubscriptionCallback::Node( + NodeSubscriptionCallback::::RegularWithMessageInfo(_) + ), + )); + assert!(matches!( + test_boxed.into_async_subscription_callback(), + AnySubscriptionCallback::Node(NodeSubscriptionCallback::::Boxed(_)), + )); + assert!(matches!( + test_boxed_with_info.into_async_subscription_callback(), + AnySubscriptionCallback::Node( + NodeSubscriptionCallback::::BoxedWithMessageInfo(_) + ), + )); + assert!(matches!( + test_loaned.into_async_subscription_callback(), + AnySubscriptionCallback::Node(NodeSubscriptionCallback::::Loaned(_)), + )); + assert!(matches!( + test_loaned_with_info.into_async_subscription_callback(), + AnySubscriptionCallback::Node( + NodeSubscriptionCallback::::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/into_node_subscription_callback.rs b/rclrs/src/subscription/into_node_subscription_callback.rs new file mode 100644 index 000000000..a55c86f52 --- /dev/null +++ b/rclrs/src/subscription/into_node_subscription_callback.rs @@ -0,0 +1,218 @@ +use rosidl_runtime_rs::Message; + +use crate::{ + AnySubscriptionCallback, MessageInfo, NodeSubscriptionCallback, ReadOnlyLoanedMessage, +}; + +use std::sync::Arc; + +/// A trait for regular callbacks of subscriptions. +/// +/// Subscription callbacks support six signatures: +/// - [`Fn`] ( `Message` ) +/// - [`Fn`] ( `Message`, [`MessageInfo`] ) +/// - [`Fn`] ( [`Box`]<`Message`> ) +/// - [`Fn`] ( [`Box`]<`Message`>, [`MessageInfo`] ) +/// - [`Fn`] ( [`ReadOnlyLoanedMessage`]<`Message`> ) +/// - [`Fn`] ( [`ReadOnlyLoanedMessage`]<`Message`>, [`MessageInfo`] ) +pub trait IntoNodeSubscriptionCallback: Send + 'static +where + T: Message, +{ + /// Converts the callback into an enum. + /// + /// User code never needs to call this function. + fn into_node_subscription_callback(self) -> AnySubscriptionCallback; +} + +impl IntoNodeSubscriptionCallback for Func +where + T: Message, + Func: Fn(T) + Send + Sync + 'static, +{ + fn into_node_subscription_callback(self) -> AnySubscriptionCallback { + let func = Arc::new(self); + NodeSubscriptionCallback::Regular(Box::new(move |message| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message); + }) + })) + .into() + } +} + +impl IntoNodeSubscriptionCallback for Func +where + T: Message, + Func: Fn(T, MessageInfo) + Send + Sync + 'static, +{ + fn into_node_subscription_callback(self) -> AnySubscriptionCallback { + let func = Arc::new(self); + NodeSubscriptionCallback::RegularWithMessageInfo(Box::new(move |message, info| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message, info); + }) + })) + .into() + } +} + +impl IntoNodeSubscriptionCallback,)> for Func +where + T: Message, + Func: Fn(Box) + Send + Sync + 'static, +{ + fn into_node_subscription_callback(self) -> AnySubscriptionCallback { + let func = Arc::new(self); + NodeSubscriptionCallback::Boxed(Box::new(move |message| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message); + }) + })) + .into() + } +} + +impl IntoNodeSubscriptionCallback, MessageInfo)> for Func +where + T: Message, + Func: Fn(Box, MessageInfo) + Send + Sync + 'static, +{ + fn into_node_subscription_callback(self) -> AnySubscriptionCallback { + let func = Arc::new(self); + NodeSubscriptionCallback::BoxedWithMessageInfo(Box::new(move |message, info| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message, info); + }) + })) + .into() + } +} + +impl IntoNodeSubscriptionCallback,)> for Func +where + T: Message, + Func: Fn(ReadOnlyLoanedMessage) + Send + Sync + 'static, +{ + fn into_node_subscription_callback(self) -> AnySubscriptionCallback { + let func = Arc::new(self); + NodeSubscriptionCallback::Loaned(Box::new(move |message| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message); + }) + })) + .into() + } +} + +impl IntoNodeSubscriptionCallback, MessageInfo)> for Func +where + T: Message, + Func: Fn(ReadOnlyLoanedMessage, MessageInfo) + Send + Sync + 'static, +{ + fn into_node_subscription_callback(self) -> AnySubscriptionCallback { + let func = Arc::new(self); + NodeSubscriptionCallback::LoanedWithMessageInfo(Box::new(move |message, info| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message, info); + }) + })) + .into() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + type TestMessage = test_msgs::msg::BoundedSequences; + + #[test] + fn callback_conversion() { + let cb = |_msg: TestMessage| {}; + assert!(matches!( + cb.into_node_subscription_callback(), + AnySubscriptionCallback::Node(NodeSubscriptionCallback::::Regular(_)), + )); + let cb = |_msg: TestMessage, _info: MessageInfo| {}; + assert!(matches!( + cb.into_node_subscription_callback(), + AnySubscriptionCallback::Node( + NodeSubscriptionCallback::::RegularWithMessageInfo(_) + ) + )); + let cb = |_msg: Box| {}; + assert!(matches!( + cb.into_node_subscription_callback(), + AnySubscriptionCallback::Node(NodeSubscriptionCallback::::Boxed(_)), + )); + let cb = |_msg: Box, _info: MessageInfo| {}; + assert!(matches!( + cb.into_node_subscription_callback(), + AnySubscriptionCallback::Node( + NodeSubscriptionCallback::::BoxedWithMessageInfo(_) + ), + )); + let cb = |_msg: ReadOnlyLoanedMessage| {}; + assert!(matches!( + cb.into_node_subscription_callback(), + AnySubscriptionCallback::Node(NodeSubscriptionCallback::::Loaned(_)), + )); + let cb = |_msg: ReadOnlyLoanedMessage, _info: MessageInfo| {}; + assert!(matches!( + cb.into_node_subscription_callback(), + AnySubscriptionCallback::Node( + NodeSubscriptionCallback::::LoanedWithMessageInfo(_) + ), + )); + + assert!(matches!( + test_regular.into_node_subscription_callback(), + AnySubscriptionCallback::Node(NodeSubscriptionCallback::::Regular(_)), + )); + assert!(matches!( + test_regular_with_info.into_node_subscription_callback(), + AnySubscriptionCallback::Node( + NodeSubscriptionCallback::::RegularWithMessageInfo(_) + ), + )); + assert!(matches!( + test_boxed.into_node_subscription_callback(), + AnySubscriptionCallback::Node(NodeSubscriptionCallback::::Boxed(_)), + )); + assert!(matches!( + test_boxed_with_info.into_node_subscription_callback(), + AnySubscriptionCallback::Node( + NodeSubscriptionCallback::::BoxedWithMessageInfo(_) + ), + )); + assert!(matches!( + test_loaned.into_node_subscription_callback(), + AnySubscriptionCallback::Node(NodeSubscriptionCallback::::Loaned(_)), + )); + assert!(matches!( + test_loaned_with_info.into_node_subscription_callback(), + AnySubscriptionCallback::Node( + NodeSubscriptionCallback::::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/subscription/into_worker_subscription_callback.rs b/rclrs/src/subscription/into_worker_subscription_callback.rs new file mode 100644 index 000000000..a3fd7a1a4 --- /dev/null +++ b/rclrs/src/subscription/into_worker_subscription_callback.rs @@ -0,0 +1,176 @@ +use rosidl_runtime_rs::Message; + +use crate::{ + AnySubscriptionCallback, MessageInfo, ReadOnlyLoanedMessage, WorkerSubscriptionCallback, +}; + +/// A trait for callbacks of subscriptions that run on a worker. +/// +/// Worker Subscription callbacks support twelve signatures: +/// - [`FnMut`] ( `Message` ) +/// - [`FnMut`] ( `Message`, [`MessageInfo`] ) +/// - [`FnMut`] ( `&mut Payload`, `Message` ) +/// - [`FnMut`] ( `&mut Payload`, `Message`, [`MessageInfo`] ) +/// - [`FnMut`] ( [`Box`]<`Message`> ) +/// - [`FnMut`] ( [`Box`]<`Message`>, [`MessageInfo`] ) +/// - [`FnMut`] ( `&mut Payload`, [`Box`]<`Message`> ) +/// - [`FnMut`] ( `&mut Payload`, [`Box`]<`Message`>, [`MessageInfo`] ) +/// - [`FnMut`] ( [`ReadOnlyLoanedMessage`]<`Message`> ) +/// - [`FnMut`] ( [`ReadOnlyLoanedMessage`]<`Message`>, [`MessageInfo`] ) +/// - [`FnMut`] ( `&mut Payload`, [`ReadOnlyLoanedMessage`]<`Message`> ) +/// - [`FnMut`] ( `&mut Payload`, [`ReadOnlyLoanedMessage`]<`Message`>, [`MessageInfo`] ) +pub trait IntoWorkerSubscriptionCallback: Send + 'static +where + T: Message, + Payload: 'static, +{ + /// Converts a worker subscription callback into an enum. + /// + /// User code never needs to call this function. + fn into_worker_subscription_callback(self) -> AnySubscriptionCallback; +} + +impl IntoWorkerSubscriptionCallback for Func +where + T: Message, + Payload: 'static, + Func: FnMut(T) + Send + 'static, +{ + fn into_worker_subscription_callback(mut self) -> AnySubscriptionCallback { + let f = Box::new(move |_: &mut Payload, message| self(message)); + WorkerSubscriptionCallback::Regular(f).into() + } +} + +impl IntoWorkerSubscriptionCallback for Func +where + T: Message, + Payload: 'static, + Func: FnMut(&mut Payload, T) + Send + 'static, +{ + fn into_worker_subscription_callback(self) -> AnySubscriptionCallback { + WorkerSubscriptionCallback::Regular(Box::new(self)).into() + } +} + +impl IntoWorkerSubscriptionCallback for Func +where + T: Message, + Payload: 'static, + Func: FnMut(T, MessageInfo) + Send + 'static, +{ + fn into_worker_subscription_callback(mut self) -> AnySubscriptionCallback { + let f = Box::new(move |_: &mut Payload, message, info| self(message, info)); + WorkerSubscriptionCallback::RegularWithMessageInfo(f).into() + } +} + +impl IntoWorkerSubscriptionCallback + for Func +where + T: Message, + Payload: 'static, + Func: FnMut(&mut Payload, T, MessageInfo) + Send + 'static, +{ + fn into_worker_subscription_callback(self) -> AnySubscriptionCallback { + WorkerSubscriptionCallback::RegularWithMessageInfo(Box::new(self)).into() + } +} + +impl IntoWorkerSubscriptionCallback,)> for Func +where + T: Message, + Payload: 'static, + Func: FnMut(Box) + Send + 'static, +{ + fn into_worker_subscription_callback(mut self) -> AnySubscriptionCallback { + let f = Box::new(move |_: &mut Payload, message| self(message)); + WorkerSubscriptionCallback::Boxed(f).into() + } +} + +impl IntoWorkerSubscriptionCallback)> for Func +where + T: Message, + Payload: 'static, + Func: FnMut(&mut Payload, Box) + Send + 'static, +{ + fn into_worker_subscription_callback(self) -> AnySubscriptionCallback { + WorkerSubscriptionCallback::Boxed(Box::new(self)).into() + } +} + +impl IntoWorkerSubscriptionCallback, MessageInfo)> for Func +where + T: Message, + Payload: 'static, + Func: FnMut(Box, MessageInfo) + Send + 'static, +{ + fn into_worker_subscription_callback(mut self) -> AnySubscriptionCallback { + let f = Box::new(move |_: &mut Payload, message, info| self(message, info)); + WorkerSubscriptionCallback::BoxedWithMessageInfo(f).into() + } +} + +impl IntoWorkerSubscriptionCallback, MessageInfo)> + for Func +where + T: Message, + Payload: 'static, + Func: FnMut(&mut Payload, Box, MessageInfo) + Send + 'static, +{ + fn into_worker_subscription_callback(self) -> AnySubscriptionCallback { + WorkerSubscriptionCallback::BoxedWithMessageInfo(Box::new(self)).into() + } +} + +impl IntoWorkerSubscriptionCallback,)> + for Func +where + T: Message, + Payload: 'static, + Func: FnMut(ReadOnlyLoanedMessage) + Send + 'static, +{ + fn into_worker_subscription_callback(mut self) -> AnySubscriptionCallback { + let f = Box::new(move |_: &mut Payload, message| self(message)); + WorkerSubscriptionCallback::Loaned(f).into() + } +} + +impl + IntoWorkerSubscriptionCallback)> for Func +where + T: Message, + Payload: 'static, + Func: FnMut(&mut Payload, ReadOnlyLoanedMessage) + Send + 'static, +{ + fn into_worker_subscription_callback(self) -> AnySubscriptionCallback { + WorkerSubscriptionCallback::Loaned(Box::new(self)).into() + } +} + +impl + IntoWorkerSubscriptionCallback, MessageInfo)> for Func +where + T: Message, + Payload: 'static, + Func: FnMut(ReadOnlyLoanedMessage, MessageInfo) + Send + 'static, +{ + fn into_worker_subscription_callback(mut self) -> AnySubscriptionCallback { + let f = Box::new(move |_: &mut Payload, message, info| self(message, info)); + WorkerSubscriptionCallback::LoanedWithMessageInfo(f).into() + } +} + +impl + IntoWorkerSubscriptionCallback, MessageInfo)> + for Func +where + T: Message, + Payload: 'static, + Func: FnMut(&mut Payload, ReadOnlyLoanedMessage, MessageInfo) + Send + 'static, +{ + fn into_worker_subscription_callback(self) -> AnySubscriptionCallback { + WorkerSubscriptionCallback::LoanedWithMessageInfo(Box::new(self)).into() + } +} 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/node_subscription_callback.rs b/rclrs/src/subscription/node_subscription_callback.rs new file mode 100644 index 000000000..0b9b6347c --- /dev/null +++ b/rclrs/src/subscription/node_subscription_callback.rs @@ -0,0 +1,75 @@ +use rosidl_runtime_rs::Message; + +use super::{MessageInfo, SubscriptionHandle}; +use crate::{RclrsError, RclrsErrorFilter, ReadOnlyLoanedMessage, WorkerCommands}; + +use futures::future::BoxFuture; + +use std::sync::Arc; + +/// An enum capturing the various possible function signatures for subscription callbacks +/// that can be used with the async worker. +/// +/// The correct enum variant is deduced by the [`IntoNodeSubscriptionCallback`][1] or +/// [`IntoAsyncSubscriptionCallback`][2] trait. +/// +/// [1]: crate::IntoNodeSubscriptionCallback +/// [2]: crate::IntoAsyncSubscriptionCallback +pub enum NodeSubscriptionCallback { + /// 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) -> BoxFuture<'static, ()> + Send>), + /// A callback with the loaned message and the message info as arguments. + #[allow(clippy::type_complexity)] + LoanedWithMessageInfo( + Box, MessageInfo) -> BoxFuture<'static, ()> + Send>, + ), +} + +impl NodeSubscriptionCallback { + pub(super) fn execute( + &mut self, + handle: &Arc, + commands: &WorkerCommands, + ) -> Result<(), RclrsError> { + let mut evaluate = || { + match self { + NodeSubscriptionCallback::Regular(cb) => { + let (msg, _) = handle.take::()?; + commands.run_async(cb(msg)); + } + NodeSubscriptionCallback::RegularWithMessageInfo(cb) => { + let (msg, msg_info) = handle.take::()?; + commands.run_async(cb(msg, msg_info)); + } + NodeSubscriptionCallback::Boxed(cb) => { + let (msg, _) = handle.take_boxed::()?; + commands.run_async(cb(msg)); + } + NodeSubscriptionCallback::BoxedWithMessageInfo(cb) => { + let (msg, msg_info) = handle.take_boxed::()?; + commands.run_async(cb(msg, msg_info)); + } + NodeSubscriptionCallback::Loaned(cb) => { + let (msg, _) = handle.take_loaned::()?; + commands.run_async(cb(msg)); + } + NodeSubscriptionCallback::LoanedWithMessageInfo(cb) => { + let (msg, msg_info) = handle.take_loaned::()?; + commands.run_async(cb(msg, msg_info)); + } + } + Ok(()) + }; + + evaluate().take_failed_ok() + } +} diff --git a/rclrs/src/subscription/readonly_loaned_message.rs b/rclrs/src/subscription/readonly_loaned_message.rs index 3c7212ccc..67ac3fd69 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::*, SubscriptionState, ToResult}; +use crate::{rcl_bindings::*, subscription::SubscriptionHandle, ToResult}; /// A message that is owned by the middleware, loaned out for reading. /// @@ -10,19 +10,19 @@ use crate::{rcl_bindings::*, SubscriptionState, ToResult}; /// message, it's the same as `&T`, and otherwise it's the corresponding RMW-native /// message. /// -/// This type is returned by [`SubscriptionState::take_loaned()`] and may be used in -/// subscription callbacks. +/// This type may be used in subscription callbacks to receive a message. The +/// loan is returned by dropping the `ReadOnlyLoanedMessage`. /// -/// The loan is returned by dropping the `ReadOnlyLoanedMessage`. -pub struct ReadOnlyLoanedMessage<'a, T> +/// [1]: crate::SubscriptionState::take_loaned +pub struct ReadOnlyLoanedMessage where T: Message, { pub(super) msg_ptr: *const T::RmwMsg, - pub(super) subscription: &'a SubscriptionState, + 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/worker_subscription_callback.rs b/rclrs/src/subscription/worker_subscription_callback.rs new file mode 100644 index 000000000..58cfc6180 --- /dev/null +++ b/rclrs/src/subscription/worker_subscription_callback.rs @@ -0,0 +1,78 @@ +use rosidl_runtime_rs::Message; + +use crate::{ + subscription::SubscriptionHandle, MessageInfo, RclrsError, RclrsErrorFilter, + ReadOnlyLoanedMessage, +}; + +use std::{any::Any, sync::Arc}; + +/// An enum capturing the various possible function signatures for subscription +/// callbacks that can be used by a [`Worker`][crate::Worker]. +/// +/// The correct enum variant is deduced by the [`IntoWorkerSubscriptionCallback`][1] trait. +/// +/// [1]: crate::IntoWorkerSubscriptionCallback +pub enum WorkerSubscriptionCallback { + /// A callback that only takes the payload and the message as arguments. + Regular(Box), + /// A callback with the payload, message, and the message info as arguments. + RegularWithMessageInfo(Box), + /// A callback with only the payload and boxed message as arguments. + Boxed(Box) + Send>), + /// A callback with the payload, boxed message, and the message info as arguments. + BoxedWithMessageInfo(Box, MessageInfo) + Send>), + /// A callback with only the payload and loaned message as arguments. + Loaned(Box) + Send>), + /// A callback with the payload, loaned message, and the message info as arguments. + LoanedWithMessageInfo( + Box, MessageInfo) + Send>, + ), +} + +impl WorkerSubscriptionCallback { + pub(super) fn execute( + &mut self, + handle: &Arc, + any_payload: &mut dyn Any, + ) -> Result<(), RclrsError> { + let Some(payload) = any_payload.downcast_mut::() else { + return Err(RclrsError::InvalidPayload { + expected: std::any::TypeId::of::(), + received: (*any_payload).type_id(), + }); + }; + + let mut evalute = || { + match self { + WorkerSubscriptionCallback::Regular(cb) => { + let (msg, _) = handle.take::()?; + cb(payload, msg); + } + WorkerSubscriptionCallback::RegularWithMessageInfo(cb) => { + let (msg, msg_info) = handle.take::()?; + cb(payload, msg, msg_info); + } + WorkerSubscriptionCallback::Boxed(cb) => { + let (msg, _) = handle.take_boxed::()?; + cb(payload, msg); + } + WorkerSubscriptionCallback::BoxedWithMessageInfo(cb) => { + let (msg, msg_info) = handle.take_boxed::()?; + cb(payload, msg, msg_info); + } + WorkerSubscriptionCallback::Loaned(cb) => { + let (msg, _) = handle.take_loaned::()?; + cb(payload, msg); + } + WorkerSubscriptionCallback::LoanedWithMessageInfo(cb) => { + let (msg, msg_info) = handle.take_loaned::()?; + cb(payload, msg, msg_info); + } + } + Ok(()) + }; + + evalute().take_failed_ok() + } +} diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs deleted file mode 100644 index c0e0c659d..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::default(); - - 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 d9d8b62d9..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::default(); -/// -/// 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::default(); - - 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::default(); - - 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..a7951f00a --- /dev/null +++ b/rclrs/src/wait_set.rs @@ -0,0 +1,280 @@ +use std::{collections::HashMap, sync::Arc, time::Duration, vec::Vec}; + +use crate::{ + error::{to_rclrs_result, RclReturnCode, RclrsError, ToResult}, + log_error, + 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() { + log_error!( + "rclrs.WaitSet.wait", + "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(); + + // After spinning has started, wait a moment and then wake up the wait sets. + // TODO(@mxgrey): When we have timers, change this to use a one-shot timer instead. + let commands = executor.commands().clone(); + std::thread::spawn(move || { + std::thread::sleep(Duration::from_millis(1)); + commands.wake_all_wait_sets(); + }); + + 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))) + .first_error()?; + + // 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..8860e23ac --- /dev/null +++ b/rclrs/src/wait_set/guard_condition.rs @@ -0,0 +1,235 @@ +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`][1] or +/// [`ExecutorCommands::query`][2]. +/// +/// [1]: crate::ExecutorCommands::run +/// [2]: crate::ExecutorCommands::query +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>, + ) -> (Arc, 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, + ); + + (Arc::new(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 { + unsafe fn execute(&mut self, _: &mut dyn Any) -> 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..205926c62 --- /dev/null +++ b/rclrs/src/wait_set/rcl_primitive.rs @@ -0,0 +1,63 @@ +use std::{any::Any, 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. + /// + /// * `payload` - The shared payload expected by the primitive. For primitives + /// sent through the [`ExecutorChannel`][1], this must be `&mut ()`. For + /// primitives sent through a [`WorkerChannel`][2] by a [`Worker`][3] this must be + /// the same type as the `Worker`'s generic argument. + /// + /// SAFETY: Make sure the type of the payload always matches what the primitive + /// expects to receive. For now we will return an error if there is a mismatch. + /// In the future we may use `std::Any::downcast_mut_unchecked` once it + /// stabilizes, which would give undefined behavior in a mismatch, making it + /// a serious safety concern. + /// + /// [1]: crate::ExecutorChannel + /// [2]: crate::WorkerChannel + /// [3]: crate::Worker + unsafe fn execute(&mut self, payload: &mut dyn Any) -> 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..b60f07050 --- /dev/null +++ b/rclrs/src/wait_set/wait_set_runner.rs @@ -0,0 +1,253 @@ +use futures::channel::{ + mpsc::{unbounded, UnboundedReceiver, UnboundedSender}, + oneshot::channel, +}; + +use std::{ + any::Any, + sync::{ + atomic::{AtomicBool, Ordering}, + Arc, Mutex, + }, + time::{Duration, Instant}, +}; + +use crate::{ + log_debug, log_fatal, ActivityListenerCallback, Context, ExecutorWorkerOptions, GuardCondition, + PayloadTask, Promise, RclReturnCode, RclrsError, WaitSet, Waitable, WeakActivityListener, +}; + +/// 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, + task_sender: UnboundedSender, + task_receiver: UnboundedReceiver, + activity_listeners: Arc>>, + guard_condition: Arc, + payload: Box, +} + +/// These are the conditions used by the [`WaitSetRunner`] to determine when it +/// needs to halt. +#[derive(Clone, Debug)] +pub struct WaitSetRunConditions { + /// 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, + /// Stop spinning once this instant in time is reached. + pub stop_time: Option, + /// 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, + /// Halt trigger that gets set by [`ExecutorCommands`][1]. + /// + /// [1]: crate::ExecutorCommands + pub halt_spinning: Arc, +} + +impl WaitSetRunner { + /// Create a new WaitSetRunner. + pub fn new(worker_options: ExecutorWorkerOptions) -> Self { + let (waitable_sender, waitable_receiver) = unbounded(); + let (task_sender, task_receiver) = unbounded(); + Self { + wait_set: WaitSet::new(&worker_options.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, + task_sender, + task_receiver, + activity_listeners: Arc::default(), + guard_condition: worker_options.guard_condition, + payload: worker_options.payload, + } + } + + /// Get the sender that allows users to send new [`Waitable`]s to this + /// `WaitSetRunner`. + pub fn waitable_sender(&self) -> UnboundedSender { + self.waitable_sender.clone() + } + + /// Get the sender that allows users to send new [`PayloadTask`]s to this + /// `WaitSetRunner`. + pub fn payload_task_sender(&self) -> UnboundedSender { + self.task_sender.clone() + } + + /// Get the group of senders that will be triggered each time the wait set + /// is woken up. This is used + pub fn activity_listeners(&self) -> Arc>> { + Arc::clone(&self.activity_listeners) + } + + /// Get the guard condition associated with the wait set of this runner. + pub fn guard_condition(&self) -> &Arc { + &self.guard_condition + } + + /// 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`][1], + /// the best practice is for your executor runtime to swap that out with a + /// new promise which ensures that the [`ExecutorWorkerOptions::guard_condition`] + /// will be triggered after the user-provided promise is resolved. + /// + /// [1]: crate::SpinOptions::until_promise_resolved + pub fn run( + mut self, + conditions: WaitSetRunConditions, + ) -> Promise<(Self, Result<(), RclrsError>)> { + let (sender, promise) = channel(); + std::thread::spawn(move || { + let result = self.run_blocking(conditions); + if let Err(_) = sender.send((self, result)) { + // This is a debug log because this is a normal thing to occur + // when an executor is winding down. + log_debug!( + "rclrs.wait_set_runner.run", + "Unable to return the wait set runner from an async run" + ); + } + }); + + 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`][1], + /// the best practice is for your executor runtime to swap that out with a + /// new promise which ensures that the [`ExecutorWorkerOptions::guard_condition`] + /// will be triggered after the user-provided promise is resolved. + /// + /// [1]: crate::SpinOptions::until_promise_resolved + pub fn run_blocking(&mut self, conditions: WaitSetRunConditions) -> Result<(), RclrsError> { + let mut first_spin = true; + let mut listeners = Vec::new(); + 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() { + if let Err(err) = self.wait_set.add(new_waitables) { + log_fatal!( + "rclrs.wait_set_runner.run_blocking", + "Failed to add an item to the wait set: {err}", + ); + } + } + + while let Ok(Some(task)) = self.task_receiver.try_next() { + task(&mut *self.payload); + } + + if conditions.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 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 = conditions.stop_time.map(|t| { + let timeout = t - Instant::now(); + if timeout < Duration::ZERO { + Duration::ZERO + } else { + timeout + } + }); + + let mut at_least_one = false; + self.wait_set.wait(timeout, |executable| { + at_least_one = true; + // SAFETY: The user of WaitSetRunner is responsible for ensuring + // the runner has the same payload type as the executables that + // are given to it. + unsafe { executable.execute(&mut *self.payload) } + })?; + + if at_least_one { + // We drain all listeners from activity_listeners to ensure that we + // don't get a deadlock from double-locking the activity_listeners + // mutex while executing one of the listeners. If the listener has + // access to the Worker then it could attempt to add another + // listener while we have the vector locked, which would cause a + // deadlock. + listeners.extend( + self.activity_listeners + .lock() + .unwrap() + .drain(..) + .filter_map(|x| x.upgrade()), + ); + + for arc_listener in &listeners { + // We pull the callback out of its mutex entirely and release + // the lock on the mutex before executing the callback. Otherwise + // if the callback triggers its own WorkerActivity to change the + // callback then we would get a deadlock from double-locking the + // mutex. + let listener = { arc_listener.lock().unwrap().take() }; + if let Some(mut listener) = listener { + match &mut listener { + ActivityListenerCallback::Listen(listen) => { + listen(&mut *self.payload); + } + ActivityListenerCallback::Inert => { + // Do nothing + } + } + + // We replace instead of assigning in case the callback + // inserted its own + arc_listener.lock().unwrap().replace(listener); + } + } + + self.activity_listeners + .lock() + .unwrap() + .extend(listeners.drain(..).map(|x| Arc::downgrade(&x))); + } + + if let Some(stop_time) = conditions.stop_time { + if stop_time <= Instant::now() { + // If we have exceeded the stop time, then quit spinning. + // self.wait_set.wait will not always return Err after a + // timeout because it's possible for a primitive to produce + // new worker faster than this loop spins. + return Err(RclrsError::RclError { + code: RclReturnCode::Timeout, + msg: None, + }); + } + } + } + } +} diff --git a/rclrs/src/wait_set/waitable.rs b/rclrs/src/wait_set/waitable.rs new file mode 100644 index 000000000..d510d1f93 --- /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() + } +} diff --git a/rclrs/src/worker.rs b/rclrs/src/worker.rs new file mode 100644 index 000000000..f89da20fc --- /dev/null +++ b/rclrs/src/worker.rs @@ -0,0 +1,567 @@ +use crate::{ + log_fatal, IntoWorkerServiceCallback, IntoWorkerSubscriptionCallback, Node, Promise, + RclrsError, ServiceOptions, ServiceState, SubscriptionOptions, SubscriptionState, + WorkerCommands, WorkerService, WorkerSubscription, +}; +use futures::channel::oneshot; +use rosidl_runtime_rs::{Message, Service as IdlService}; +use std::{ + any::Any, + sync::{Arc, Mutex, Weak}, +}; + +/// A worker that carries a payload and synchronizes callbacks for subscriptions +/// and services. Workers share much in common with "callback groups" from rclcpp, +/// with the addition of holding a data payload to share between the callbacks. +/// +/// The payload is any data type of your choosing. Each callback associated with +/// this worker will receive a mutable borrow (`&mut Payload`) of the payload, +/// allowing them to share the payload data for both viewing and modifying. The +/// worker only contains exactly one instance of the payload that is shared +/// across all callbacks. +/// +/// You can also run ad hoc tasks on the worker to view or modify the payload +/// from callbacks that are not associated with this worker. +/// +/// The API for the worker is provided through [`WorkerState`]. +pub type Worker = Arc>; + +/// The inner state of a [`Worker`]. +/// +/// 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 [`Worker`] in a non-owning way. It is +/// generally recommended to manage the `WorkerState` inside of an [`Arc`], +/// and [`Worker`] is provided as a convenience alias for that. +/// +/// The public API of the [`Worker`] type is implemented via `WorkerState`. +/// +/// [1]: std::sync::Weak +pub struct WorkerState { + /// The node that this worker is associated with + node: Node, + /// The commands to communicate with the runtime of the worker + commands: Arc, + _ignore: std::marker::PhantomData, +} + +impl WorkerState { + /// Run a task on this worker. This allows you to view and modify the payload + /// of the worker. You will receive a [`Promise`] which you can use to view + /// what happened inside the callback. + pub fn run(&self, f: F) -> Promise + where + F: FnOnce(&mut Payload) -> Out + 'static + Send, + Out: 'static + Send, + { + let (sender, receiver) = oneshot::channel(); + self.commands.run_on_payload(Box::new(move |any_payload| { + let Some(payload) = any_payload.downcast_mut::() else { + log_fatal!( + "rclrs.worker", + "{}", + Self::conversion_failure_message(any_payload) + ); + return; + }; + + let out = f(payload); + sender.send(out).ok(); + })); + receiver + } + + /// Same as [`Self::run`] but you will additionally receive a notification + /// promise that will be triggered after the output is available. The + /// notification is suitable to be passed into [`until_promise_resolved`][1]. + /// + /// [1]: crate::SpinOptions::until_promise_resolved + pub fn run_with_notice(&self, f: F) -> (Promise, Promise<()>) + where + F: FnOnce(&mut Payload) -> Out + 'static + Send, + Out: 'static + Send, + { + let (notice_sender, notice_receiver) = oneshot::channel(); + let (sender, receiver) = oneshot::channel(); + self.commands.run_on_payload(Box::new(move |any_payload| { + let Some(payload) = any_payload.downcast_mut::() else { + log_fatal!( + "rclrs.worker", + "{}", + Self::conversion_failure_message(any_payload) + ); + return; + }; + + let out = f(payload); + sender.send(out).ok(); + notice_sender.send(()).ok(); + })); + (receiver, notice_receiver) + } + + /// Listen to activity that happens with this worker's primitives. The + /// listening will continue until the [`ActivityListener`] is dropped. + pub fn listen(&self, mut f: F) -> ActivityListener + where + F: FnMut(&mut Payload) + 'static + Send + Sync, + Payload: Sync, + { + let f = Box::new(move |any_payload: &mut dyn Any| { + let Some(payload) = any_payload.downcast_mut::() else { + log_fatal!( + "rclrs.worker", + "{}", + Self::conversion_failure_message(any_payload) + ); + return; + }; + + f(payload); + }); + + let listener = ActivityListener::new(ActivityListenerCallback::Listen(f)); + self.commands + .add_activity_listener(Arc::downgrade(&listener.callback)); + listener + } + + /// The callback that you provide will be triggered each time the worker has + /// some activity until it returns [`Some`] or until the [`Promise`] is + /// dropped. + /// + /// As long as the [`Promise`] is alive and the callback returns [`None`], + /// the listener will keep running. + pub fn listen_until(&self, mut f: F) -> Promise + where + F: FnMut(&mut Payload) -> Option + 'static + Send + Sync, + Out: 'static + Send + Sync, + Payload: Sync, + { + let (sender, receiver) = oneshot::channel(); + let mut captured_sender = Some(sender); + let listener = ActivityListener::new(ActivityListenerCallback::Inert); + + // We capture the listener so that its lifecycle is tied to the success + // of the callback. + let mut _captured_listener = Some(listener.clone()); + listener.set_callback(move |payload| { + if let Some(sender) = captured_sender.take() { + if sender.is_canceled() { + _captured_listener = None; + return; + } + + if let Some(out) = f(payload) { + sender.send(out).ok(); + _captured_listener = None; + } else { + captured_sender = Some(sender); + } + } else { + _captured_listener = None; + } + }); + + self.commands + .add_activity_listener(Arc::downgrade(&listener.callback)); + + receiver + } + + /// Creates a [`WorkerSubscription`]. + /// + /// Unlike subscriptions created from a [`Node`], the callbacks for these + /// subscriptions can have an additional argument to get a mutable borrow of + /// the [`Worker`]'s payload. This allows the subscription callback to operate + /// on state data that gets shared with other callbacks. + /// + /// # Behavior + /// + /// While the callback of this subscription is running, no other callbacks + /// associated with the [`Worker`] will be able to run. This is necessary to + /// guarantee that the mutable borrow of the payload is safe. + /// + /// Since the callback of this subscription may block other callbacks from + /// being able to run, it is strongly recommended to ensure that the + /// callback returns quickly. If the callback needs to trigger long-running + /// behavior then you can consider using `std::thread::spawn`, or for async + /// behaviors you can capture an [`ExecutorCommands`][1] in your callback + /// and use [`ExecutorCommands::run`][2] to issue a task for the executor + /// to run in its async task pool. + /// + /// [1]: crate::ExecutorCommands + /// [2]: crate::ExecutorCommands::run + /// + /// # Subscription Options + /// + /// See [`NodeState::create_subscription`][3] for examples of setting the + /// subscription options. + /// + /// [3]: crate::NodeState::create_subscription + /// + /// # Worker Subscription Callbacks + /// + /// Worker Subscription callbacks support twelve signatures: + /// - [`FnMut`] ( `Message` ) + /// - [`FnMut`] ( `Message`, [`MessageInfo`][4] ) + /// - [`FnMut`] ( `&mut Payload`, `Message` ) + /// - [`FnMut`] ( `&mut Payload`, `Message`, [`MessageInfo`][4] ) + /// - [`FnMut`] ( [`Box`]<`Message`> ) + /// - [`FnMut`] ( [`Box`]<`Message`>, [`MessageInfo`][4] ) + /// - [`FnMut`] ( `&mut Payload`, [`Box`]<`Message`> ) + /// - [`FnMut`] ( `&mut Payload`, [`Box`]<`Message`>, [`MessageInfo`][4] ) + /// - [`FnMut`] ( [`ReadOnlyLoanedMessage`][5]<`Message`> ) + /// - [`FnMut`] ( [`ReadOnlyLoanedMessage`][5]<`Message`>, [`MessageInfo`][4] ) + /// - [`FnMut`] ( `&mut Payload`, [`ReadOnlyLoanedMessage`][5]<`Message`> ) + /// - [`FnMut`] ( `&mut Payload`, [`ReadOnlyLoanedMessage`][5]<`Message`>, [`MessageInfo`][4] ) + /// + /// [4]: crate::MessageInfo + /// [5]: crate::ReadOnlyLoanedMessage + /// + /// Note that these signatures all use [`FnMut`] which means, unlike node + /// subscriptions, the callback can have mutable internal state without + /// needing to use [`Mutex`]. This is possible because the [`Worker`] + /// ensures the callback cannot run multiple times simultaneously. + /// + /// Additionally your callback can get mutable access to the worker's + /// payload by setting the first argument of the callback to `&mut Payload`. + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// // The worker's payload is data that we want to share with other callbacks. + /// let worker = node.create_worker::>(None); + /// + /// // This variable will be the mutable internal state of the subscription + /// // callback. + /// let mut count = 0_usize; + /// + /// let subscription = worker.create_subscription::( + /// "topic", + /// move |data: &mut Option, msg: example_interfaces::msg::String| { + /// count += 1; + /// println!("#{count} | I heard: '{}'", msg.data); + /// + /// *data = Some(msg.data); + /// }, + /// )?; + /// # Ok::<(), RclrsError>(()) + /// ``` + pub fn create_subscription<'a, T, Args>( + &self, + options: impl Into>, + callback: impl IntoWorkerSubscriptionCallback, + ) -> Result, RclrsError> + where + T: Message, + { + SubscriptionState::>::create( + options, + callback.into_worker_subscription_callback(), + self.node.handle(), + &self.commands, + ) + } + + /// Creates a [`WorkerService`]. + /// + /// Unlike services created from a [`Node`], the callbacks for these services + /// can have an additional argument to get a mutable borrow of the [`Worker`]'s + /// payload. This allows the service callback to operate on state data that + /// gets shared with other callbacks. + /// + /// # Behavior + /// + /// While the callback of this service is running, no other callbacks + /// associated with the [`Worker`] will be able to run. This is necessary to + /// guarantee that the mutable borrow of the payload is safe. + /// + /// Since the callback of this service may block other callbacks from being + /// able to run, it is strongly recommended to ensure that the callback + /// returns quickly. If the callback needs to trigger some long-running + /// behavior before returning, then you should probably create the service + /// using [`NodeState::create_async_service`][1] since the service callback + /// of a worker is required to return the response of the service as its + /// output value. + /// + /// To access the payload of a worker from an async service, see examples in + /// the documentation of [`NodeState::create_async_service`][1]. + /// + /// [1]: crate::NodeState::create_async_service + /// + /// # Service Options + /// + /// See the documentation of [`NodeState::create_service`][2] for examples + /// of setting the service options. + /// + /// [2]: crate::NodeState::create_service + /// + /// # Worker Service Callbacks + /// + /// Worker service callbacks support six signatures: + /// - [`FnMut`] ( `Request` ) -> `Response` + /// - [`FnMut`] ( `Request`, [`RequestId`][3] ) -> `Response` + /// - [`FnMut`] ( `Request`, [`ServiceInfo`][4] ) -> `Response` + /// - [`FnMut`] ( `&mut Payload`, `Request` ) -> `Response` + /// - [`FnMut`] ( `&mut Payload`, `Request`, [`RequestId`][3] ) -> `Response` + /// - [`FnMut`] ( `&mut Payload`, `Request`, [`ServiceInfo`][4] ) -> `Response` + /// + /// [3]: crate::RequestId + /// [4]: crate::ServiceInfo + /// + /// Note that these signatures all use [`FnMut`] which means, unlike node + /// services, the callback can have mutable internal state without needing + /// to use [`Mutex`]. This is possible because the [`Worker`] ensures the + /// callback cannot run multiple times simultaneously. + /// + /// Additionally your callback can get mutable access to the worker's + /// payload by setting the first argument of the callback to `&mut Payload`. + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// use example_interfaces::srv::*; + /// + /// /// Store the operands of the service request for later reference + /// #[derive(Default)] + /// struct Operands { + /// a: i64, + /// b: i64, + /// } + /// + /// // The worker's payload is data that we want to share with other callbacks. + /// let worker = node.create_worker::(Operands::default()); + /// + /// // This variable will be the mutable internal state of the service + /// // callback. + /// let mut count = 0_usize; + /// + /// let service = worker.create_service::( + /// "add", + /// move |payload: &mut Operands, request: AddTwoInts_Request| { + /// count += 1; + /// let AddTwoInts_Request { a, b } = request; + /// let sum = a + b; + /// println!("#{count} | {a} + {b} = {sum}"); + /// + /// *payload = Operands { a, b }; + /// + /// AddTwoInts_Response { sum } + /// } + /// )?; + /// # Ok::<(), RclrsError>(()) + /// ``` + pub fn create_service<'a, T, Args>( + &self, + options: impl Into>, + callback: impl IntoWorkerServiceCallback, + ) -> Result, RclrsError> + where + T: IdlService, + { + ServiceState::>::create( + options, + callback.into_worker_service_callback(), + self.node.handle(), + &self.commands, + ) + } + + /// Used by [`Node`][crate::Node] to create a `WorkerState`. Users should + /// call [`Node::create_worker`][crate::NodeState::create_worker] instead of + /// this. + pub(crate) fn create(node: Node, commands: Arc) -> Arc { + Arc::new(Self { + node, + commands, + _ignore: Default::default(), + }) + } + + fn conversion_failure_message(any_payload: &mut dyn Any) -> String { + format!( + "Received invalid payload from worker. Expected: {:?}, received: {:?}. \ + This should never happen. Please report this to the maintainers of rclrs \ + with a minimal reproducible example.", + std::any::TypeId::of::(), + any_payload, + ) + } +} + +/// Options used while creating a new [`Worker`]. +pub struct WorkerOptions { + /// The value of the initial payload for the [`Worker`]. + pub payload: Payload, +} + +impl WorkerOptions { + /// Create a new `WorkerOptions`. + pub fn new(payload: Payload) -> Self { + Self { payload } + } +} + +impl From for WorkerOptions { + fn from(value: T) -> Self { + WorkerOptions::new(value) + } +} + +/// `ActivityListener` is used to watch the activity of the primitives of a [`Worker`]. +/// +/// Each listener will be triggered each time a primitive is active on the `Worker`. +/// Listener callbacks will not be triggered when something other than a primitive +/// modifies the payload, which can happen through [interior mutability][1] or by +/// one of the other listeners, so this is not a sure-fire way to track changes in +/// the payload. +/// +/// Note that the listener callback will be triggered if any primitive has run +/// since the payload *may* have been modified. We do not track changes on the +/// payload, so it is possible that its data was not changed by the primitive at +/// all, but the listener will still be triggered. +/// +/// [1]: https://doc.rust-lang.org/book/ch15-05-interior-mutability.html +pub struct ActivityListener { + callback: Arc>>, + _ignore: std::marker::PhantomData, +} + +impl Clone for ActivityListener { + fn clone(&self) -> Self { + Self { + callback: Arc::clone(&self.callback), + _ignore: Default::default(), + } + } +} + +impl ActivityListener { + /// Change the callback for this listener. + pub fn set_callback(&self, mut f: F) + where + F: FnMut(&mut Payload) + 'static + Send + Sync, + { + let f = Box::new(move |any_payload: &mut dyn Any| { + let Some(payload) = any_payload.downcast_mut::() else { + let msg = WorkerState::::conversion_failure_message(any_payload); + log_fatal!("rclrs.worker", "{msg}"); + return; + }; + + f(payload); + }); + + *self.callback.lock().unwrap() = Some(ActivityListenerCallback::Listen(f)); + } + + /// Change the listener to be inert but remain alive. + pub fn set_inert(&self) { + *self.callback.lock().unwrap() = Some(ActivityListenerCallback::Inert); + } + + fn new(callback: ActivityListenerCallback) -> Self { + let callback = Arc::new(Mutex::new(Some(callback))); + Self { + callback, + _ignore: Default::default(), + } + } +} + +/// This type is used by executor runtimes to keep track of listeners. +pub type WeakActivityListener = Weak>>; + +/// Enum for the different types of callbacks that a listener may have +pub enum ActivityListenerCallback { + /// The listener is listening + Listen(Box), + /// The listener is inert + Inert, +} + +/// This is used to determine what kind of payload a callback can accept, as +/// well as what kind of callbacks can be used with it. Users should not implement +/// this trait. +pub trait WorkScope: 'static + Send + Sync { + /// What kind of payload should the worker hold for this scope. + type Payload: 'static + Send; +} + +impl WorkScope for Node { + type Payload = (); +} + +impl WorkScope for Worker { + type Payload = Payload; +} + +#[cfg(test)] +mod tests { + use crate::*; + use std::time::Duration; + use test_msgs::{ + msg::Empty as EmptyMsg, + srv::{Empty as EmptySrv, Empty_Request, Empty_Response}, + }; + + #[derive(Default, Clone, Copy, Debug)] + struct TestPayload { + subscription_count: usize, + service_count: usize, + } + + #[test] + fn test_worker() { + let mut executor = Context::default().create_basic_executor(); + let node = executor.create_node("test_worker_node").unwrap(); + let worker = node.create_worker(TestPayload::default()); + let _count_sub = worker.create_subscription( + "test_worker_topic", + |payload: &mut TestPayload, _msg: EmptyMsg| { + payload.subscription_count += 1; + }, + ); + + let _count_srv = worker.create_service::( + "test_worker_service", + |payload: &mut TestPayload, _req: Empty_Request| { + payload.service_count += 1; + Empty_Response::default() + }, + ); + + let promise = worker.listen_until(move |payload| { + if payload.service_count > 0 && payload.subscription_count > 0 { + Some(*payload) + } else { + None + } + }); + + let publisher = node.create_publisher("test_worker_topic").unwrap(); + publisher.publish(EmptyMsg::default()).unwrap(); + + let client = node + .create_client::("test_worker_service") + .unwrap(); + let _: Promise = client.call(Empty_Request::default()).unwrap(); + + let (mut promise, notice) = executor.commands().create_notice(promise); + + executor + .spin( + SpinOptions::new() + .until_promise_resolved(notice) + .timeout(Duration::from_millis(500)), + ) + .first_error() + .unwrap(); + + let payload = promise.try_recv().unwrap().unwrap(); + assert_eq!(payload.subscription_count, 1); + assert_eq!(payload.service_count, 1); + } +}