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/timer_demo/Cargo.toml b/examples/timer_demo/Cargo.toml new file mode 100644 index 000000000..23b9f1fa1 --- /dev/null +++ b/examples/timer_demo/Cargo.toml @@ -0,0 +1,8 @@ +[package] +name = "timer_demo" +version = "0.1.0" +edition = "2021" + +[dependencies] +rclrs = "0.4" +example_interfaces = "*" diff --git a/examples/timer_demo/package.xml b/examples/timer_demo/package.xml new file mode 100644 index 000000000..684cea8fa --- /dev/null +++ b/examples/timer_demo/package.xml @@ -0,0 +1,21 @@ + + + + examples_timer_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/timer_demo/src/main.rs b/examples/timer_demo/src/main.rs new file mode 100644 index 000000000..608a82f10 --- /dev/null +++ b/examples/timer_demo/src/main.rs @@ -0,0 +1,22 @@ +/// Creates a SimpleTimerNode, initializes a node and the timer with a callback +/// that prints the timer callback execution iteration. The callback is executed +/// thanks to the spin, which is in charge of executing the timer's events among +/// other entities' events. +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("timer_demo")?; + let worker = node.create_worker::(0); + let timer_period = Duration::from_secs(1); + let _timer = worker.create_timer_repeating(timer_period, move |count: &mut usize| { + *count += 1; + println!( + "Drinking 🧉 for the {}th time every {:?}.", + *count, timer_period, + ); + })?; + + 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..bb3a97615 --- /dev/null +++ b/examples/worker_demo/src/main.rs @@ -0,0 +1,35 @@ +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("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; + }, + )?; + + 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(); + })?; + + 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..8dd543455 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -66,21 +66,31 @@ 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 `rcl_clock_t`. + pub(crate) fn get_rcl_clock(&self) -> &Arc> { + &self.rcl_clock } /// Returns the clock's `ClockType`. @@ -101,22 +111,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..f5584fa27 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; @@ -20,8 +189,10 @@ mod service; mod subscription; mod time; mod time_source; +mod timer; mod vendor; -mod wait; +mod wait_set; +mod worker; #[cfg(test)] mod test_helpers; @@ -47,4 +218,6 @@ pub use service::*; pub use subscription::*; pub use time::*; use time_source::*; -pub use wait::*; +pub use timer::*; +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..1a0c67224 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,36 +1,46 @@ -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::*, AnyTimerCallback, Client, ClientOptions, ClientState, Clock, ContextHandle, + ExecutorCommands, IntoAsyncServiceCallback, IntoAsyncSubscriptionCallback, + IntoNodeServiceCallback, IntoNodeSubscriptionCallback, IntoNodeTimerOneshotCallback, + IntoNodeTimerRepeatingCallback, IntoTimerOptions, LogParams, Logger, ParameterBuilder, + ParameterInterface, ParameterVariant, Parameters, Promise, Publisher, PublisherOptions, + PublisherState, RclrsError, Service, ServiceOptions, ServiceState, Subscription, + SubscriptionOptions, SubscriptionState, TimeSource, Timer, TimerState, 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 +50,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 +73,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 +237,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 +349,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) - } - - /// 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 + ClientState::::create(options, self) } - /// Creates a [`GuardCondition`][1] with a 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. + /// Creates a [`Publisher`]. /// - /// [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 +393,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 +420,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 +438,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 +453,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 +697,278 @@ 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) + SubscriptionState::::create( + options, + callback.into_node_subscription_callback(), + &self.handle, + self.commands.async_worker_commands(), + ) } - /// 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() + /// 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(), + ) + } + + /// Create a [`Timer`] with a repeating callback. + /// + /// See also: + /// * [`Self::create_timer_oneshot`] + /// * [`Self::create_timer_inert`] + pub fn create_timer_repeating<'a, Args>( + &self, + options: impl IntoTimerOptions<'a>, + callback: impl IntoNodeTimerRepeatingCallback, + ) -> Result { + self.create_timer(options, callback.into_node_timer_repeating_callback()) } - pub(crate) fn live_clients(&self) -> Vec> { - { self.clients_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .collect() + /// Create a [`Timer`] whose callback will be triggered once after the period + /// of the timer has elapsed. After that you will need to use + /// [`Timer::set_repeating`] or [`Timer::set_oneshot`] or else nothing will happen + /// the following times that the `Timer` elapses. + /// + /// See also: + /// * [`Self::create_timer_repeating`] + /// * [`Self::create_time_inert`] + pub fn create_timer_oneshot<'a, Args>( + &self, + options: impl IntoTimerOptions<'a>, + callback: impl IntoNodeTimerOneshotCallback, + ) -> Result { + self.create_timer(options, callback.into_node_timer_oneshot_callback()) } - pub(crate) fn live_guard_conditions(&self) -> Vec> { - { self.guard_conditions_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .collect() + /// Create a [`Timer`] without a callback. Nothing will happen when this + /// `Timer` elapses until you use [`Timer::set_callback`] or a related method. + /// + /// See also: + /// * [`Self::create_timer_repeating`] + /// * [`Self::create_timer_oneshot`] + pub fn create_timer_inert<'a>( + &self, + options: impl IntoTimerOptions<'a>, + ) -> Result { + self.create_timer(options, AnyTimerCallback::Inert) } - pub(crate) fn live_services(&self) -> Vec> { - { self.services_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .collect() + /// Used internally to create a [`Timer`]. + /// + /// Downstream users should instead use: + /// * [`Self::create_timer_repeating`] + /// * [`Self::create_timer_oneshot`] + /// * [`Self::create_timer_inert`] + fn create_timer<'a>( + &self, + options: impl IntoTimerOptions<'a>, + callback: AnyTimerCallback, + ) -> Result { + let options = options.into_timer_options(); + let clock = options.clock.as_clock(self); + TimerState::create( + options.period, + clock, + callback, + self.commands.async_worker_commands(), + &self.handle.context_handle, + ) } /// 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 +1038,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 +1144,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/timer.rs b/rclrs/src/timer.rs new file mode 100644 index 000000000..934060fd1 --- /dev/null +++ b/rclrs/src/timer.rs @@ -0,0 +1,777 @@ +use crate::{ + clock::Clock, context::ContextHandle, error::RclrsError, log_error, rcl_bindings::*, Node, + RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, ToLogParams, ToResult, Waitable, + WaitableLifecycle, WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, +}; +// TODO: fix me when the callback type is properly defined. +// use std::fmt::Debug; +use std::{ + any::Any, + sync::{Arc, Mutex, Weak}, + time::Duration, +}; + +mod any_timer_callback; +pub use any_timer_callback::*; + +mod timer_options; +pub use timer_options::*; + +mod into_node_timer_callback; +pub use into_node_timer_callback::*; + +mod into_worker_timer_callback; +pub use into_worker_timer_callback::*; + +/// Struct for executing periodic events. +/// +/// The executor needs to be [spinning][1] for a timer's callback to be triggered. +/// +/// Timers can be created by a [`Node`] using one of these methods: +/// - [`NodeState::create_timer_repeating`] +/// - [`NodeState::create_timer_oneshot`] +/// - [`NodeState::create_timer_inert`] +/// +/// Timers can also be created by a [`Worker`], in which case they can access the worker's payload: +/// - [`Worker::create_timer_repeating`] +/// - [`Worker::create_timer_oneshot`] +/// - [`Worker::create_timer_inert`] +/// +/// The API of timers is given by [`TimerState`]. +/// +/// [1]: crate::Executor::spin +pub type Timer = Arc>; + +/// A [`Timer`] that runs on a [`Worker`]. +/// +/// Create a worker timer using [`Worker::create_timer_repeating`], +/// [`Worker::create_timer_oneshot`], or [`Worker::create_timer_inert`]. +pub type WorkerTimer = Arc>>; + +/// The inner state of a [`Timer`]. +/// +/// This is public so that you can choose to create a [`Weak`] reference to it +/// if you want to be able to refer to a [`Timer`] in a non-owning way. It is +/// generally recommended to manage the `TimerState` inside of an [`Arc`], and +/// [`Timer`] is provided as a convenience alias for that. +/// +/// The public API of [`Timer`] is implemented via `TimerState`. +/// +/// Timers that run inside of a [`Worker`] are represented by [`WorkerTimer`]. +pub struct TimerState { + pub(crate) handle: Arc, + /// The callback function that runs when the timer is due. + callback: Mutex>>, + /// What was the last time lapse between calls to this timer + last_elapse: Mutex, + /// We use Mutex> here because we need to construct the TimerState object + /// before we can get the lifecycle handle. + #[allow(unused)] + lifecycle: Mutex>, + _ignore: std::marker::PhantomData, +} + +impl TimerState { + /// Gets the period of the timer + pub fn get_timer_period(&self) -> Result { + let mut timer_period_ns = 0; + unsafe { + let rcl_timer = self.handle.rcl_timer.lock().unwrap(); + rcl_timer_get_period(&*rcl_timer, &mut timer_period_ns) + } + .ok()?; + + rcl_duration(timer_period_ns) + } + + /// Cancels the timer, stopping the execution of the callback + pub fn cancel(&self) -> Result<(), RclrsError> { + let mut rcl_timer = self.handle.rcl_timer.lock().unwrap(); + let cancel_result = unsafe { rcl_timer_cancel(&mut *rcl_timer) }.ok()?; + Ok(cancel_result) + } + + /// Checks whether the timer is canceled or not + pub fn is_canceled(&self) -> Result { + let mut is_canceled = false; + unsafe { + let rcl_timer = self.handle.rcl_timer.lock().unwrap(); + rcl_timer_is_canceled(&*rcl_timer, &mut is_canceled) + } + .ok()?; + Ok(is_canceled) + } + + /// Get the last time lapse between calls to the timer. + /// + /// This is different from [`Self::time_since_last_call`] because it remains + /// constant between calls to the Timer. + /// + /// It keeps track of the what the value of [`Self::time_since_last_call`] + /// was immediately before the most recent call to the callback. This will + /// be [`Duration::ZERO`] if the `Timer` has never been triggered. + pub fn last_elapse(&self) -> Duration { + *self.last_elapse.lock().unwrap() + } + + /// Retrieves the time since the last call to the callback + pub fn time_since_last_call(&self) -> Result { + let mut time_value_ns: i64 = 0; + unsafe { + let rcl_timer = self.handle.rcl_timer.lock().unwrap(); + rcl_timer_get_time_since_last_call(&*rcl_timer, &mut time_value_ns) + } + .ok()?; + + rcl_duration(time_value_ns) + } + + /// Retrieves the time until the next call of the callback + pub fn time_until_next_call(&self) -> Result { + let mut time_value_ns: i64 = 0; + unsafe { + let rcl_timer = self.handle.rcl_timer.lock().unwrap(); + rcl_timer_get_time_until_next_call(&*rcl_timer, &mut time_value_ns) + } + .ok()?; + + rcl_duration(time_value_ns) + } + + /// Resets the timer. + pub fn reset(&self) -> Result<(), RclrsError> { + let mut rcl_timer = self.handle.rcl_timer.lock().unwrap(); + unsafe { rcl_timer_reset(&mut *rcl_timer) }.ok() + } + + /// Checks if the timer is ready (not canceled) + pub fn is_ready(&self) -> Result { + let is_ready = unsafe { + let mut is_ready: bool = false; + let rcl_timer = self.handle.rcl_timer.lock().unwrap(); + rcl_timer_is_ready(&*rcl_timer, &mut is_ready).ok()?; + is_ready + }; + + Ok(is_ready) + } + + /// Get the clock that this timer runs on. + pub fn clock(&self) -> &Clock { + &self.handle.clock + } + + /// Set a new callback for the timer. This will return whatever callback + /// was already present unless you are calling the function from inside of + /// the timer's callback, in which case you will receive [`None`]. + /// + /// See also: + /// * [`Self::set_repeating`] + /// * [`Self::set_oneshot`] + /// * [`Self::set_inert`]. + pub fn set_callback( + &self, + callback: AnyTimerCallback, + ) -> Option> { + self.callback.lock().unwrap().replace(callback) + } + + /// Remove the callback from the timer. + /// + /// This does not cancel the timer; it will continue to wake up and be + /// triggered at its regular period. However, nothing will happen when the + /// timer is triggered until you give a new callback to the timer. + /// + /// You can give the timer a new callback at any time by calling: + /// * [`Self::set_repeating`] + /// * [`Self::set_oneshot`] + pub fn set_inert(&self) -> Option> { + self.set_callback(AnyTimerCallback::Inert) + } + + /// Creates a new timer. Users should call one of [`Node::create_timer`], + /// [`Node::create_timer_repeating`], [`Node::create_timer_oneshot`], or + /// [`Node::create_timer_inert`]. + pub(crate) fn create<'a>( + period: Duration, + clock: Clock, + callback: AnyTimerCallback, + commands: &Arc, + context: &ContextHandle, + ) -> Result, RclrsError> { + let period = period.as_nanos() as i64; + + // Callbacks will be handled at the rclrs layer. + let rcl_timer_callback: rcl_timer_callback_t = None; + + let rcl_timer = Arc::new(Mutex::new( + // SAFETY: Zero-initializing a timer is always safe + unsafe { rcl_get_zero_initialized_timer() }, + )); + + unsafe { + let mut rcl_clock = clock.get_rcl_clock().lock().unwrap(); + let mut rcl_context = context.rcl_context.lock().unwrap(); + + // SAFETY: Getting a default value is always safe. + let allocator = rcutils_get_default_allocator(); + + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: We lock the lifecycle mutex since rcl_timer_init is not + // thread-safe. + rcl_timer_init( + &mut *rcl_timer.lock().unwrap(), + &mut *rcl_clock, + &mut *rcl_context, + period, + rcl_timer_callback, + allocator, + ) + } + .ok()?; + + let timer = Arc::new(TimerState { + handle: Arc::new(TimerHandle { rcl_timer, clock }), + callback: Mutex::new(Some(callback)), + last_elapse: Mutex::new(Duration::ZERO), + lifecycle: Mutex::default(), + _ignore: Default::default(), + }); + + let (waitable, lifecycle) = Waitable::new( + Box::new(TimerExecutable:: { + timer: Arc::downgrade(&timer), + handle: Arc::clone(&timer.handle), + }), + Some(Arc::clone(commands.get_guard_condition())), + ); + + *timer.lifecycle.lock().unwrap() = Some(lifecycle); + + commands.add_to_wait_set(waitable); + + Ok(timer) + } + + /// Force the timer to be called, even if it is not ready to be triggered yet. + /// We could consider making this public, but the behavior may confuse users. + fn call(self: &Arc, any_payload: &mut dyn Any) -> Result<(), RclrsError> { + // Keep track of the time elapsed since the last call. We need to run + // this before we trigger rcl_call. + let last_elapse = self.time_since_last_call().unwrap_or(Duration::ZERO); + *self.last_elapse.lock().unwrap() = last_elapse; + + if let Err(err) = self.rcl_call() { + log_error!("timer", "Unable to call timer: {err:?}",); + } + + let Some(callback) = self.callback.lock().unwrap().take() else { + log_error!( + "timer".once(), + "Timer is missing its callback information. This should not \ + be possible, please report it to the maintainers of rclrs.", + ); + return Ok(()); + }; + + let Some(payload) = any_payload.downcast_mut::() else { + return Err(RclrsError::InvalidPayload { + expected: std::any::TypeId::of::(), + received: (*any_payload).type_id(), + }); + }; + + match callback { + AnyTimerCallback::Repeating(mut callback) => { + callback(payload, self); + self.restore_callback(AnyTimerCallback::Repeating(callback).into()); + } + AnyTimerCallback::OneShot(callback) => { + callback(payload, self); + self.restore_callback(AnyTimerCallback::Inert); + } + AnyTimerCallback::Inert => { + self.restore_callback(AnyTimerCallback::Inert); + } + } + + Ok(()) + } + + /// Updates the state of the rcl_timer to know that it has been called. This + /// should only be called by [`Self::call`]. + /// + /// The callback held by the rcl_timer is null because we store the callback + /// in the [`Timer`] struct. This means there are no side-effects to this + /// except to keep track of when the timer has been called. + fn rcl_call(&self) -> Result<(), RclrsError> { + let mut rcl_timer = self.handle.rcl_timer.lock().unwrap(); + unsafe { rcl_timer_call(&mut *rcl_timer) }.ok() + } + + /// Used by [`Timer::execute`] to restore the state of the callback if and + /// only if the user has not already set a new callback. + fn restore_callback(&self, callback: AnyTimerCallback) { + let mut self_callback = self.callback.lock().unwrap(); + if self_callback.is_none() { + *self_callback = Some(callback); + } + } +} + +impl TimerState { + /// Set a repeating callback for this timer. + /// + /// See also: + /// * [`Self::set_oneshot`] + /// * [`Self::set_inert`] + pub fn set_repeating( + &self, + f: impl IntoNodeTimerRepeatingCallback, + ) -> Option> { + self.set_callback(f.into_node_timer_repeating_callback()) + } + + /// Set a one-shot callback for the timer. + /// + /// The next time the timer is triggered, the callback will be set to + /// [`AnyTimerCallback::Inert`] after this callback is triggered. To keep the + /// timer useful, you can reset the Timer callback at any time, including + /// inside the one-shot callback itself. + /// + /// See also: + /// * [`Self::set_repeating`] + /// * [`Self::set_inert`] + pub fn set_oneshot( + &self, + f: impl IntoNodeTimerOneshotCallback, + ) -> Option> { + self.set_callback(f.into_node_timer_oneshot_callback()) + } +} + +impl TimerState> { + /// Set a repeating callback for this worker timer. + /// + /// See also: + /// * [`Self::set_worker_oneshot`] + /// * [`Self::set_inert`] + pub fn set_worker_repeating( + &self, + f: impl IntoWorkerTimerRepeatingCallback, Args>, + ) -> Option>> { + self.set_callback(f.into_worker_timer_repeating_callback()) + } + + /// Set a one-shot callback for the worker timer. + /// + /// The next time the timer is triggered, the callback will be set to + /// [`AnyTimerCallback::Inert`] after this callback is triggered. To keep the + /// timer useful, you can reset the Timer callback at any time, including + /// inside the one-shot callback itself. + /// + /// See also: + /// * [`Self::set_worker_repeating`] + /// * [`Self::set_inert`] + pub fn set_worker_oneshot( + &self, + f: impl IntoWorkerTimerOneshotCallback, Args>, + ) -> Option>> { + self.set_callback(f.into_worker_timer_oneshot_callback()) + } +} + +struct TimerExecutable { + timer: Weak>, + handle: Arc, +} + +impl RclPrimitive for TimerExecutable { + unsafe fn execute(&mut self, payload: &mut dyn Any) -> Result<(), RclrsError> { + if let Some(timer) = self.timer.upgrade() { + if timer.is_ready()? { + timer.call(payload)?; + } + } + + Ok(()) + } + + fn kind(&self) -> RclPrimitiveKind { + RclPrimitiveKind::Timer + } + + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::Timer(self.handle.rcl_timer.lock().unwrap()) + } +} + +impl PartialEq for TimerState { + fn eq(&self, other: &Self) -> bool { + Arc::ptr_eq(&self.handle.rcl_timer, &other.handle.rcl_timer) + } +} + +fn rcl_duration(duration_value_ns: i64) -> Result { + if duration_value_ns < 0 { + Err(RclrsError::NegativeDuration(duration_value_ns)) + } else { + Ok(Duration::from_nanos(duration_value_ns as u64)) + } +} + +/// Manage the lifecycle of an `rcl_timer_t`, including managing its dependency +/// on `rcl_clock_t` by ensuring that this dependency are [dropped after][1] +/// the `rcl_timer_t`. +/// +/// [1]: +pub(crate) struct TimerHandle { + pub(crate) rcl_timer: Arc>, + clock: Clock, +} + +/// 'Drop' trait implementation to be able to release the resources +impl Drop for TimerHandle { + fn drop(&mut self) { + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The lifecycle mutex is locked and the clock for the timer + // must still be valid because TimerHandle keeps it alive. + unsafe { rcl_timer_fini(&mut *self.rcl_timer.lock().unwrap()) }; + } +} + +// 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_timer_t {} + +#[cfg(test)] +mod tests { + use super::TimerExecutable; + use crate::*; + use std::{ + sync::{ + atomic::{AtomicBool, Ordering}, + Arc, + }, + thread, + time::Duration, + }; + + #[test] + fn traits() { + use crate::test_helpers::*; + + assert_send::>(); + assert_sync::>(); + } + + #[test] + fn test_new_with_system_clock() { + let executor = Context::default().create_basic_executor(); + let result = TimerState::::create( + Duration::from_millis(1), + Clock::system(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + assert!(result.is_ok()); + } + + #[test] + fn test_new_with_steady_clock() { + let executor = Context::default().create_basic_executor(); + let result = TimerState::::create( + Duration::from_millis(1), + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + assert!(result.is_ok()); + } + + #[test] + fn test_new_with_source_clock() { + let (clock, source) = Clock::with_source(); + // No manual time set, it should default to 0 + assert_eq!(clock.now().nsec, 0); + let set_time = 1234i64; + source.set_ros_time_override(set_time); + + // ROS time is set, should return the value that was set + assert_eq!(clock.now().nsec, set_time); + + let executor = Context::default().create_basic_executor(); + let result = TimerState::::create( + Duration::from_millis(1), + clock, + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + assert!(result.is_ok()); + } + + #[test] + fn test_get_period() { + let period = Duration::from_millis(1); + + let executor = Context::default().create_basic_executor(); + + let result = TimerState::::create( + period, + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + + let timer = result.unwrap(); + let timer_period = timer.get_timer_period().unwrap(); + assert_eq!(timer_period, period); + } + + #[test] + fn test_cancel() { + let executor = Context::default().create_basic_executor(); + + let result = TimerState::::create( + Duration::from_millis(1), + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + + let timer = result.unwrap(); + assert!(!timer.is_canceled().unwrap()); + timer.cancel().unwrap(); + assert!(timer.is_canceled().unwrap()); + } + + #[test] + fn test_time_since_last_call_before_first_event() { + let executor = Context::default().create_basic_executor(); + + let result = TimerState::::create( + Duration::from_millis(2), + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + let timer = result.unwrap(); + + let sleep_period = Duration::from_millis(1); + thread::sleep(sleep_period); + + let time_since_last_call = timer.time_since_last_call().unwrap(); + assert!( + time_since_last_call >= sleep_period, + "time_since_last_call: {:?} vs sleep period: {:?}", + time_since_last_call, + sleep_period, + ); + } + + #[test] + fn test_time_until_next_call_before_first_event() { + let executor = Context::default().create_basic_executor(); + let period = Duration::from_millis(2); + + let result = TimerState::::create( + period, + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ); + let timer = result.unwrap(); + + let time_until_next_call = timer.time_until_next_call().unwrap(); + assert!( + time_until_next_call <= period, + "time_until_next_call: {:?} vs period: {:?}", + time_until_next_call, + period, + ); + } + + #[test] + fn test_reset() { + let executor = Context::default().create_basic_executor(); + let period = Duration::from_millis(2); + let timer = TimerState::::create( + period, + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ) + .unwrap(); + + // The unwrap will panic if the remaining time is negative + timer.time_until_next_call().unwrap(); + + // Sleep until we're past the timer period + thread::sleep(Duration::from_millis(3)); + + // Now the time until next call should give an error + assert!(matches!( + timer.time_until_next_call(), + Err(RclrsError::NegativeDuration(_)) + )); + + // Reset the timer so its interval begins again + assert!(timer.reset().is_ok()); + + // The unwrap will panic if the remaining time is negative + timer.time_until_next_call().unwrap(); + } + + #[test] + fn test_call() { + let executor = Context::default().create_basic_executor(); + let timer = TimerState::::create( + Duration::from_millis(1), + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ) + .unwrap(); + + // The unwrap will panic if the remaining time is negative + timer.time_until_next_call().unwrap(); + + // Sleep until we're past the timer period + thread::sleep(Duration::from_micros(1500)); + + // Now the time until the next call should give an error + assert!(matches!( + timer.time_until_next_call(), + Err(RclrsError::NegativeDuration(_)) + )); + + // The unwrap will panic if anything went wrong with the call + timer.call(&mut ()).unwrap(); + + // The unwrap will panic if the remaining time is negative + timer.time_until_next_call().unwrap(); + } + + #[test] + fn test_is_ready() { + let executor = Context::default().create_basic_executor(); + let timer = TimerState::::create( + Duration::from_millis(1), + Clock::steady(), + (|| {}).into_node_timer_repeating_callback(), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ) + .unwrap(); + + assert!(!timer.is_ready().unwrap()); + + // Sleep until the period has elapsed + thread::sleep(Duration::from_micros(1100)); + + assert!(timer.is_ready().unwrap()); + } + + #[test] + fn test_callback() { + let clock = Clock::steady(); + let initial_time = clock.now(); + + let executor = Context::default().create_basic_executor(); + let executed = Arc::new(AtomicBool::new(false)); + + let timer = TimerState::::create( + Duration::from_millis(1), + clock, + create_timer_callback_for_testing(initial_time, Arc::clone(&executed)), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ) + .unwrap(); + + timer.call(&mut ()).unwrap(); + assert!(executed.load(Ordering::Acquire)); + } + + #[test] + fn test_execute_when_is_not_ready() { + let clock = Clock::steady(); + let initial_time = clock.now(); + + let executor = Context::default().create_basic_executor(); + let executed = Arc::new(AtomicBool::new(false)); + + let timer = TimerState::::create( + Duration::from_millis(1), + clock, + create_timer_callback_for_testing(initial_time, Arc::clone(&executed)), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ) + .unwrap(); + + let mut executable = TimerExecutable { + timer: Arc::downgrade(&timer), + handle: Arc::clone(&timer.handle), + }; + + // SAFETY: Node timers expect a payload of () + unsafe { + executable.execute(&mut ()).unwrap(); + } + assert!(!executed.load(Ordering::Acquire)); + } + + #[test] + fn test_execute_when_is_ready() { + let clock = Clock::steady(); + let initial_time = clock.now(); + + let executor = Context::default().create_basic_executor(); + let executed = Arc::new(AtomicBool::new(false)); + + let timer = TimerState::::create( + Duration::from_millis(1), + clock, + create_timer_callback_for_testing(initial_time, Arc::clone(&executed)), + executor.commands().async_worker_commands(), + &executor.commands().context().handle, + ) + .unwrap(); + + let mut executable = TimerExecutable { + timer: Arc::downgrade(&timer), + handle: Arc::clone(&timer.handle), + }; + + thread::sleep(Duration::from_millis(2)); + + // SAFETY: Node timers expect a payload of () + unsafe { + executable.execute(&mut ()).unwrap(); + } + assert!(executed.load(Ordering::Acquire)); + } + + fn create_timer_callback_for_testing( + initial_time: Time, + executed: Arc, + ) -> AnyTimerCallback { + (move |t: Time| { + assert!(t + .compare_with(&initial_time, |t, initial| t >= initial) + .unwrap()); + executed.store(true, Ordering::Release); + }) + .into_node_timer_oneshot_callback() + } +} diff --git a/rclrs/src/timer/any_timer_callback.rs b/rclrs/src/timer/any_timer_callback.rs new file mode 100644 index 000000000..3c4493d72 --- /dev/null +++ b/rclrs/src/timer/any_timer_callback.rs @@ -0,0 +1,15 @@ +use crate::{TimerState, WorkScope}; +use std::sync::Arc; + +/// A callback that can be triggered when a timer elapses. +pub enum AnyTimerCallback { + /// This callback will be triggered repeatedly, each time the period of the + /// timer elapses. + Repeating(Box>) + Send>), + /// This callback will be triggered exactly once, the first time the period + /// of the timer elapses. + OneShot(Box>) + Send>), + /// Do nothing when the timer elapses. This can be replaced later so that + /// the timer does something. + Inert, +} diff --git a/rclrs/src/timer/into_node_timer_callback.rs b/rclrs/src/timer/into_node_timer_callback.rs new file mode 100644 index 000000000..5ef3772fb --- /dev/null +++ b/rclrs/src/timer/into_node_timer_callback.rs @@ -0,0 +1,67 @@ +use crate::{AnyTimerCallback, Node, Time, Timer}; + +/// This trait is used to create timer callbacks for repeating timers in a Node. +pub trait IntoNodeTimerRepeatingCallback: 'static + Send { + /// Convert a suitable object into a repeating timer callback for the node scope + fn into_node_timer_repeating_callback(self) -> AnyTimerCallback; +} + +impl IntoNodeTimerRepeatingCallback<()> for Func +where + Func: FnMut() + 'static + Send, +{ + fn into_node_timer_repeating_callback(mut self) -> AnyTimerCallback { + AnyTimerCallback::Repeating(Box::new(move |_, _| self())).into() + } +} + +impl IntoNodeTimerRepeatingCallback for Func +where + Func: FnMut(&Timer) + 'static + Send, +{ + fn into_node_timer_repeating_callback(mut self) -> AnyTimerCallback { + AnyTimerCallback::Repeating(Box::new(move |_, t| self(t))).into() + } +} + +impl IntoNodeTimerRepeatingCallback