diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index 3e2bf53b2..f8cc6431d 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -1,4 +1,4 @@ -use std::{convert::TryInto, env, sync::Arc}; +use std::convert::TryInto; use anyhow::{Error, Result}; use rosidl_runtime_rs::{seq, BoundedSequence, Message, Sequence}; @@ -138,38 +138,32 @@ fn demonstrate_sequences() { fn demonstrate_pubsub() -> Result<(), Error> { println!("================== Interoperability demo =================="); // Demonstrate interoperability between idiomatic and RMW-native message types - let context = rclrs::Context::new(env::args())?; - let node = rclrs::create_node(&context, "message_demo")?; + let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); + let node = executor.create_node("message_demo")?; - let idiomatic_publisher = node.create_publisher::( - "topic", - rclrs::QOS_PROFILE_DEFAULT, - )?; - let direct_publisher = node.create_publisher::( - "topic", - rclrs::QOS_PROFILE_DEFAULT, - )?; + let idiomatic_publisher = + node.create_publisher::("topic")?; + let direct_publisher = + node.create_publisher::("topic")?; let _idiomatic_subscription = node .create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, move |_msg: rclrs_example_msgs::msg::VariousTypes| println!("Got idiomatic message!"), )?; let _direct_subscription = node .create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, move |_msg: rclrs_example_msgs::msg::rmw::VariousTypes| { println!("Got RMW-native message!") }, )?; println!("Sending idiomatic message."); idiomatic_publisher.publish(rclrs_example_msgs::msg::VariousTypes::default())?; - rclrs::spin_once(Arc::clone(&node), None)?; + executor.spin(rclrs::SpinOptions::spin_once())?; println!("Sending RMW-native message."); direct_publisher.publish(rclrs_example_msgs::msg::rmw::VariousTypes::default())?; - rclrs::spin_once(Arc::clone(&node), None)?; + executor.spin(rclrs::SpinOptions::spin_once())?; Ok(()) } diff --git a/examples/minimal_client_service/src/minimal_client.rs b/examples/minimal_client_service/src/minimal_client.rs index 915541d54..d1b35c0d9 100644 --- a/examples/minimal_client_service/src/minimal_client.rs +++ b/examples/minimal_client_service/src/minimal_client.rs @@ -1,34 +1,32 @@ -use std::env; - use anyhow::{Error, Result}; +use rclrs::{Context, SpinOptions, Promise}; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let mut executor = Context::default_from_env()?.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_client")?; + let node = executor.create_node("minimal_client")?; let client = node.create_client::("add_two_ints")?; - let request = example_interfaces::srv::AddTwoInts_Request { a: 41, b: 1 }; - println!("Starting client"); while !client.service_is_ready()? { std::thread::sleep(std::time::Duration::from_millis(10)); } - client.async_send_request_with_callback( - &request, - move |response: example_interfaces::srv::AddTwoInts_Response| { - println!( - "Result of {} + {} is: {}", - request.a, request.b, response.sum - ); - }, - )?; + let request = example_interfaces::srv::AddTwoInts_Request { a: 41, b: 1 }; + + let response: Promise = client.call(&request).unwrap(); - std::thread::sleep(std::time::Duration::from_millis(500)); + let promise = executor.commands().run(async move { + let response = response.await.unwrap(); + println!( + "Result of {} + {} is: {}", + request.a, request.b, response.sum, + ); + }); println!("Waiting for response"); - rclrs::spin(node).map_err(|err| err.into()) + executor.spin(SpinOptions::new().until_promise_resolved(promise))?; + Ok(()) } diff --git a/examples/minimal_client_service/src/minimal_client_async.rs b/examples/minimal_client_service/src/minimal_client_async.rs index 0eeb87f4d..b4bca1372 100644 --- a/examples/minimal_client_service/src/minimal_client_async.rs +++ b/examples/minimal_client_service/src/minimal_client_async.rs @@ -1,12 +1,10 @@ -use std::env; - use anyhow::{Error, Result}; +use rclrs::{Context, SpinOptions}; -#[tokio::main] -async fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; +fn main() -> Result<(), Error> { + let mut executor = Context::default_from_env()?.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_client")?; + let node = executor.create_node("minimal_client")?; let client = node.create_client::("add_two_ints")?; @@ -18,18 +16,17 @@ async fn main() -> Result<(), Error> { let request = example_interfaces::srv::AddTwoInts_Request { a: 41, b: 1 }; - let future = client.call_async(&request); + let promise = client.call_then( + &request, + move |response: example_interfaces::srv::AddTwoInts_Response| { + println!( + "Result of {} + {} is: {}", + request.a, request.b, response.sum, + ); + } + ).unwrap(); println!("Waiting for response"); - - let rclrs_spin = tokio::task::spawn_blocking(move || rclrs::spin(node)); - - let response = future.await?; - println!( - "Result of {} + {} is: {}", - request.a, request.b, response.sum - ); - - rclrs_spin.await.ok(); + executor.spin(SpinOptions::new().until_promise_resolved(promise))?; Ok(()) } diff --git a/examples/minimal_client_service/src/minimal_service.rs b/examples/minimal_client_service/src/minimal_service.rs index b4149c817..f249940bf 100644 --- a/examples/minimal_client_service/src/minimal_service.rs +++ b/examples/minimal_client_service/src/minimal_service.rs @@ -1,25 +1,30 @@ -use std::env; - use anyhow::{Error, Result}; +use rclrs::{Context, ServiceInfo, SpinOptions}; fn handle_service( - _request_header: &rclrs::rmw_request_id_t, request: example_interfaces::srv::AddTwoInts_Request, + info: ServiceInfo, ) -> example_interfaces::srv::AddTwoInts_Response { - println!("request: {} + {}", request.a, request.b); + let timestamp = info + .received_timestamp + .map(|t| format!(" at [{t:?}]")) + .unwrap_or(String::new()); + + println!("request{timestamp}: {} + {}", request.a, request.b); example_interfaces::srv::AddTwoInts_Response { sum: request.a + request.b, } } fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let mut executor = Context::default_from_env()?.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_service")?; + let node = executor.create_node("minimal_service")?; let _server = node .create_service::("add_two_ints", handle_service)?; println!("Starting server"); - rclrs::spin(node).map_err(|err| err.into()) + executor.spin(SpinOptions::default())?; + Ok(()) } diff --git a/examples/minimal_pub_sub/src/minimal_publisher.rs b/examples/minimal_pub_sub/src/minimal_publisher.rs index 720086917..be88b0f5a 100644 --- a/examples/minimal_pub_sub/src/minimal_publisher.rs +++ b/examples/minimal_pub_sub/src/minimal_publisher.rs @@ -1,14 +1,12 @@ -use std::env; - use anyhow::{Error, Result}; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let context = rclrs::Context::default_from_env()?; + let executor = context.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_publisher")?; + let node = executor.create_node("minimal_publisher")?; - let publisher = - node.create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + let publisher = node.create_publisher::("topic")?; let mut message = std_msgs::msg::String::default(); diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index ebc5fc194..59fa37b1e 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -1,23 +1,25 @@ -use std::env; - use anyhow::{Error, Result}; +use std::sync::Mutex; +use rclrs::{Context, SpinOptions}; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; - - let node = rclrs::create_node(&context, "minimal_subscriber")?; + let context = Context::default_from_env()?; + let mut executor = context.create_basic_executor(); - let mut num_messages: usize = 0; + let node = executor.create_node("minimal_subscriber")?; + let num_messages = Mutex::new(0usize); let _subscription = node.create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, move |msg: std_msgs::msg::String| { - num_messages += 1; + let mut num = num_messages.lock().unwrap(); + *num += 1; println!("I heard: '{}'", msg.data); - println!("(Got {} messages so far)", num_messages); + println!("(Got {} messages so far)", num); }, )?; - rclrs::spin(node).map_err(|err| err.into()) + println!("Waiting for messages..."); + executor.spin(SpinOptions::default())?; + Ok(()) } diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index fb03574a2..46bd9780c 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -1,23 +1,23 @@ -use std::{ - env, - sync::{ - atomic::{AtomicU32, Ordering}, - Arc, Mutex, - }, +use std::sync::{ + atomic::{AtomicU32, Ordering}, + Arc, Mutex, }; use anyhow::{Error, Result}; struct MinimalSubscriber { num_messages: AtomicU32, - node: Arc, - subscription: Mutex>>>, + node: rclrs::Node, + subscription: Mutex>>, } impl MinimalSubscriber { - pub fn new(name: &str, topic: &str) -> Result, rclrs::RclrsError> { - let context = rclrs::Context::new(env::args())?; - let node = rclrs::create_node(&context, name)?; + pub fn new( + executor: &rclrs::Executor, + name: &str, + topic: &str, + ) -> Result, rclrs::RclrsError> { + let node = executor.create_node(name)?; let minimal_subscriber = Arc::new(MinimalSubscriber { num_messages: 0.into(), node, @@ -29,7 +29,6 @@ impl MinimalSubscriber { .node .create_subscription::( topic, - rclrs::QOS_PROFILE_DEFAULT, move |msg: std_msgs::msg::String| { minimal_subscriber_aux.callback(msg); }, @@ -50,14 +49,15 @@ impl MinimalSubscriber { } fn main() -> Result<(), Error> { - let publisher_context = rclrs::Context::new(env::args())?; - let publisher_node = rclrs::create_node(&publisher_context, "minimal_publisher")?; + let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); + let publisher_node = executor.create_node("minimal_publisher")?; - let subscriber_node_one = MinimalSubscriber::new("minimal_subscriber_one", "topic")?; - let subscriber_node_two = MinimalSubscriber::new("minimal_subscriber_two", "topic")?; + let _subscriber_node_one = + MinimalSubscriber::new(&executor, "minimal_subscriber_one", "topic")?; + let _subscriber_node_two = + MinimalSubscriber::new(&executor, "minimal_subscriber_two", "topic")?; - let publisher = publisher_node - .create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + let publisher = publisher_node.create_publisher::("topic")?; std::thread::spawn(move || -> Result<(), rclrs::RclrsError> { let mut message = std_msgs::msg::String::default(); @@ -71,11 +71,7 @@ fn main() -> Result<(), Error> { } }); - let executor = rclrs::SingleThreadedExecutor::new(); - - executor.add_node(&publisher_node)?; - executor.add_node(&subscriber_node_one.node)?; - executor.add_node(&subscriber_node_two.node)?; - - executor.spin().map_err(|err| err.into()) + executor + .spin(rclrs::SpinOptions::default()) + .map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/zero_copy_publisher.rs b/examples/minimal_pub_sub/src/zero_copy_publisher.rs index 5e73b5de7..d495f90bb 100644 --- a/examples/minimal_pub_sub/src/zero_copy_publisher.rs +++ b/examples/minimal_pub_sub/src/zero_copy_publisher.rs @@ -1,14 +1,12 @@ -use std::env; - use anyhow::{Error, Result}; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let context = rclrs::Context::default_from_env()?; + let executor = context.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_publisher")?; + let node = executor.create_node("minimal_publisher")?; - let publisher = - node.create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + let publisher = node.create_publisher::("topic")?; let mut publish_count: u32 = 1; diff --git a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs index 9551dba0e..4769e9f12 100644 --- a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -1,23 +1,24 @@ -use std::env; - use anyhow::{Error, Result}; +use std::sync::Mutex; +use rclrs::ReadOnlyLoanedMessage; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_subscriber")?; + let node = executor.create_node("minimal_subscriber")?; - let mut num_messages: usize = 0; + let num_messages = Mutex::new(0usize); let _subscription = node.create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, - move |msg: rclrs::ReadOnlyLoanedMessage<'_, std_msgs::msg::UInt32>| { - num_messages += 1; + move |msg: ReadOnlyLoanedMessage| { + let mut num = num_messages.lock().unwrap(); + *num += 1; println!("I heard: '{}'", msg.data); - println!("(Got {} messages so far)", num_messages); + println!("(Got {} messages so far)", *num); }, )?; - rclrs::spin(node).map_err(|err| err.into()) + executor.spin(rclrs::SpinOptions::default())?; + Ok(()) } diff --git a/examples/rclrs_timer_demo/Cargo.toml b/examples/rclrs_timer_demo/Cargo.toml new file mode 100644 index 000000000..772d635be --- /dev/null +++ b/examples/rclrs_timer_demo/Cargo.toml @@ -0,0 +1,12 @@ +[package] +name = "rclrs_timer_demo" +version = "0.1.0" +edition = "2021" + +[[bin]] +name="rclrs_timer_demo" +path="src/rclrs_timer_demo.rs" + + +[dependencies] +rclrs = "*" diff --git a/examples/rclrs_timer_demo/package.xml b/examples/rclrs_timer_demo/package.xml new file mode 100644 index 000000000..64e673704 --- /dev/null +++ b/examples/rclrs_timer_demo/package.xml @@ -0,0 +1,13 @@ + + rclrs_timer_demo + 0.1.0 + Shows how to implement a timer within a Node using rclrs. + user + TODO: License declaration. + + rclrs + + + ament_cargo + + diff --git a/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs b/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs new file mode 100644 index 000000000..76c8734b7 --- /dev/null +++ b/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs @@ -0,0 +1,46 @@ +/// 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::{create_node, Context, Node, RclrsError, Timer}; +use std::{ + env, + sync::Arc, + time::Duration, +}; + +/// Contains both the node and timer. +struct SimpleTimerNode { + node: Arc, + #[allow(unused)] + timer: Arc, +} + +impl SimpleTimerNode { + /// Creates a node and a timer with a callback. + /// + /// The callback will simply print to stdout: + /// "Drinking 🧉 for the xth time every p nanoseconds." + /// where x is the iteration callback counter and p is the period of the timer. + fn new(context: &Context, timer_period: Duration) -> Result { + let node = create_node(context, "simple_timer_node")?; + let mut x = 0; + let timer = node.create_timer_repeating( + timer_period, + move || { + x += 1; + println!( + "Drinking 🧉 for the {x}th time every {:?}.", + timer_period, + ); + }, + )?; + Ok(Self { node, timer }) + } +} + +fn main() -> Result<(), RclrsError> { + let context = Context::new(env::args()).unwrap(); + let simple_timer_node = Arc::new(SimpleTimerNode::new(&context, Duration::from_secs(1)).unwrap()); + rclrs::spin(simple_timer_node.node.clone()) +} diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index 98d0e0f74..8b5b467a7 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -1,36 +1,36 @@ -use rclrs::{create_node, Context, Node, Publisher, RclrsError, QOS_PROFILE_DEFAULT}; -use std::{sync::Arc, thread, time::Duration}; +use rclrs::{Context, Executor, Publisher, RclrsError, SpinOptions}; +use std::{thread, time::Duration}; use std_msgs::msg::String as StringMsg; + struct SimplePublisherNode { - node: Arc, - _publisher: Arc>, + publisher: Publisher, } + impl SimplePublisherNode { - fn new(context: &Context) -> Result { - let node = create_node(context, "simple_publisher").unwrap(); - let _publisher = node - .create_publisher("publish_hello", QOS_PROFILE_DEFAULT) + fn new(executor: &Executor) -> Result { + let node = executor.create_node("simple_publisher").unwrap(); + let publisher = node + .create_publisher("publish_hello") .unwrap(); - Ok(Self { node, _publisher }) + Ok(Self { publisher }) } fn publish_data(&self, increment: i32) -> Result { let msg: StringMsg = StringMsg { data: format!("Hello World {}", increment), }; - self._publisher.publish(msg).unwrap(); + self.publisher.publish(msg).unwrap(); Ok(increment + 1_i32) } } fn main() -> Result<(), RclrsError> { - let context = Context::new(std::env::args()).unwrap(); - let publisher = Arc::new(SimplePublisherNode::new(&context).unwrap()); - let publisher_other_thread = Arc::clone(&publisher); + let mut executor = Context::default_from_env().unwrap().create_basic_executor(); + let node = SimplePublisherNode::new(&executor).unwrap(); let mut count: i32 = 0; thread::spawn(move || loop { thread::sleep(Duration::from_millis(1000)); - count = publisher_other_thread.publish_data(count).unwrap(); + count = node.publish_data(count).unwrap(); }); - rclrs::spin(publisher.node.clone()) + executor.spin(SpinOptions::default()) } diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index a0d02bb4c..5e11f5fda 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -1,35 +1,31 @@ -use rclrs::{create_node, Context, Node, RclrsError, Subscription, QOS_PROFILE_DEFAULT}; +use rclrs::{Context, Executor, RclrsError, SpinOptions, Subscription}; use std::{ - env, sync::{Arc, Mutex}, thread, time::Duration, }; use std_msgs::msg::String as StringMsg; + pub struct SimpleSubscriptionNode { - node: Arc, - _subscriber: Arc>, + #[allow(unused)] + subscriber: Subscription, data: Arc>>, } + impl SimpleSubscriptionNode { - fn new(context: &Context) -> Result { - let node = create_node(context, "simple_subscription").unwrap(); + fn new(executor: &Executor) -> Result { + let node = executor.create_node("simple_subscription").unwrap(); let data: Arc>> = Arc::new(Mutex::new(None)); let data_mut: Arc>> = Arc::clone(&data); - let _subscriber = node + let subscriber = node .create_subscription::( "publish_hello", - QOS_PROFILE_DEFAULT, move |msg: StringMsg| { *data_mut.lock().unwrap() = Some(msg); }, ) .unwrap(); - Ok(Self { - node, - _subscriber, - data, - }) + Ok(Self { subscriber, data }) } fn data_callback(&self) -> Result<(), RclrsError> { if let Some(data) = self.data.lock().unwrap().as_ref() { @@ -41,12 +37,11 @@ impl SimpleSubscriptionNode { } } fn main() -> Result<(), RclrsError> { - let context = Context::new(env::args()).unwrap(); - let subscription = Arc::new(SimpleSubscriptionNode::new(&context).unwrap()); - let subscription_other_thread = Arc::clone(&subscription); + let mut executor = Context::default_from_env().unwrap().create_basic_executor(); + let node = SimpleSubscriptionNode::new(&executor).unwrap(); thread::spawn(move || loop { thread::sleep(Duration::from_millis(1000)); - subscription_other_thread.data_callback().unwrap() + node.data_callback().unwrap() }); - rclrs::spin(subscription.node.clone()) + executor.spin(SpinOptions::default()) } diff --git a/examples/worker_demo/Cargo.toml b/examples/worker_demo/Cargo.toml new file mode 100644 index 000000000..ff956a3ec --- /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" +std_msgs = "*" diff --git a/examples/worker_demo/src/main.rs b/examples/worker_demo/src/main.rs new file mode 100644 index 000000000..0979006bb --- /dev/null +++ b/examples/worker_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("worker_demo")?; + + let publisher = node.create_publisher("output_topic")?; + let worker = node.create_worker(String::new()); + + let _timer = worker.create_timer_repeating( + Duration::from_secs(1), + move |data: &mut String| { + let msg = std_msgs::msg::String { + data: data.clone() + }; + + publisher.publish(msg).ok(); + } + )?; + + let _subscription = worker.create_subscription( + "input_topic", + move |data: &mut String, msg: std_msgs::msg::String| { + *data = msg.data; + } + )?; + + 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 fe17cc990..a47e90a5d 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -23,6 +23,9 @@ cfg-if = "1.0.0" # Needed for clients futures = "0.3" +# Needed for the runtime-agnostic timeout feature +async-std = "1.13" + # Needed for dynamic messages libloading = { version = "0.8", optional = true } @@ -39,7 +42,7 @@ tempfile = "3.3.0" # Needed for publisher and subscriber tests test_msgs = {version = "*"} # Needed for parameter service tests -tokio = { version = "*", features = ["rt", "time", "macros"] } +tokio = { version = "1", features = ["rt", "time", "macros"] } [build-dependencies] # Needed for FFI diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index b308f1de2..1fa5c5e37 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -1,108 +1,215 @@ use std::{ - boxed::Box, + any::Any, collections::HashMap, ffi::CString, - sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, + sync::{Arc, Mutex, MutexGuard}, }; -use futures::channel::oneshot; use rosidl_runtime_rs::Message; use crate::{ - error::{RclReturnCode, ToResult}, - rcl_bindings::*, - MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + error::ToResult, rcl_bindings::*, IntoPrimitiveOptions, MessageCow, Node, Promise, QoSProfile, + RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclReturnCode, RclrsError, ServiceInfo, + Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, }; -// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread -// they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for rcl_client_t {} - -/// Manage the lifecycle of an `rcl_client_t`, including managing its dependencies -/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are -/// [dropped after][1] the `rcl_client_t`. -/// -/// [1]: -pub struct ClientHandle { - rcl_client: Mutex, - node_handle: Arc, - pub(crate) in_use_by_wait_set: Arc, -} +mod client_async_callback; +pub use client_async_callback::*; -impl ClientHandle { - pub(crate) fn lock(&self) -> MutexGuard { - self.rcl_client.lock().unwrap() - } -} +mod client_callback; +pub use client_callback::*; -impl Drop for ClientHandle { - fn drop(&mut self) { - let rcl_client = self.rcl_client.get_mut().unwrap(); - let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); - let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); - // SAFETY: The entity lifecycle mutex is locked to protect against the risk of - // global variables in the rmw implementation being unsafely modified during cleanup. - unsafe { - rcl_client_fini(rcl_client, &mut *rcl_node); - } - } -} +mod client_output; +pub use client_output::*; -/// Trait to be implemented by concrete Client structs. +/// Main class responsible for sending requests to a ROS service. /// -/// See [`Client`] for an example. -pub trait ClientBase: Send + Sync { - /// Internal function to get a reference to the `rcl` handle. - fn handle(&self) -> &ClientHandle; - /// Tries to take a new response and run the callback or future with it. - fn execute(&self) -> Result<(), RclrsError>; -} - -type RequestValue = Box; - -type RequestId = i64; +/// Create a client using [`Node::create_client`][1]. +/// +/// Receiving responses requires the node's executor to [spin][2]. +/// +/// [1]: crate::NodeState::create_client +/// [2]: crate::spin +pub type Client = Arc>; -/// Main class responsible for sending requests to a ROS service. +/// The inner state of a [`Client`]. /// -/// The only available way to instantiate clients is via [`Node::create_client`][1], this is to -/// ensure that [`Node`][2]s can track all the clients that have been created. +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Client`] in a non-owning way. It is +/// generally recommended to manage the `ClientState` inside of an [`Arc`], +/// and [`Client`] is provided as a convenience alias for that. /// -/// [1]: crate::Node::create_client -/// [2]: crate::Node -pub struct Client +/// The public API of the [`Client`] type is implemented via `ClientState`. +/// +/// [1]: std::sync::Weak +pub struct ClientState where T: rosidl_runtime_rs::Service, { - pub(crate) handle: Arc, - requests: Mutex>>, - futures: Arc>>>, + handle: Arc, + board: Arc>>, + #[allow(unused)] + lifecycle: WaitableLifecycle, } -impl Client +impl ClientState where T: rosidl_runtime_rs::Service, { + /// Send out a request for this service client. + /// + /// If the call to rcl succeeds, you will receive a [`Promise`] of the + /// service response. You can choose what kind of metadata you receive. The + /// promise can provide any of the following: + /// - `Response` + /// - `(Response, `[`RequestId`][1]`)` + /// - `(Response, `[`ServiceInfo`][2]`)` + /// + /// Dropping the [`Promise`] that this returns will not cancel the request. + /// Once this function is called, the service provider will receive the + /// request and respond to it no matter what. + /// + /// [1]: crate::RequestId + /// [2]: crate::ServiceInfo + pub fn call<'a, Req, Out>(&self, request: Req) -> Result, RclrsError> + where + Req: MessageCow<'a, T::Request>, + Out: ClientOutput, + { + let (sender, promise) = Out::create_channel(); + let rmw_message = T::Request::into_rmw_message(request.into_cow()); + let mut sequence_number = -1; + unsafe { + // SAFETY: The client handle ensures the rcl_client is valid and + // our generic system ensures it has the correct type. + rcl_send_request( + &*self.handle.lock() as *const _, + rmw_message.as_ref() as *const ::RmwMsg as *mut _, + &mut sequence_number, + ) + } + .ok()?; + + // TODO(@mxgrey): Log errors here when logging becomes available. + self.board + .lock() + .unwrap() + .new_request(sequence_number, sender); + + Ok(promise) + } + + /// Call this service and then handle its response with a regular callback. + /// + /// You do not need to retain the [`Promise`] that this returns, even if the + /// compiler warns you that you need to. You can use the [`Promise`] to know + /// when the response is finished being processed, but otherwise you can + /// safely discard it. + // + // TODO(@mxgrey): Add documentation to show what callback signatures are supported + pub fn call_then<'a, Req, Args>( + &self, + request: Req, + callback: impl ClientCallback, + ) -> Result, RclrsError> + where + Req: MessageCow<'a, T::Request>, + { + let callback = move |response, info| async { + callback.run_client_callback(response, info); + }; + self.call_then_async(request, callback) + } + + /// Call this service and then handle its response with an async callback. + /// + /// You do not need to retain the [`Promise`] that this returns, even if the + /// compiler warns you that you need to. You can use the [`Promise`] to know + /// when the response is finished being processed, but otherwise you can + /// safely discard it. + // + // TODO(@mxgrey): Add documentation to show what callback signatures are supported + pub fn call_then_async<'a, Req, Args>( + &self, + request: Req, + callback: impl ClientAsyncCallback, + ) -> Result, RclrsError> + where + Req: MessageCow<'a, T::Request>, + { + let response: Promise<(T::Response, ServiceInfo)> = self.call(request)?; + let promise = self.handle.node.commands().run(async move { + match response.await { + Ok((response, info)) => { + callback.run_client_async_callback(response, info).await; + } + Err(_) => { + // TODO(@mxgrey): Log this error when logging becomes available + } + } + }); + + Ok(promise) + } + + /// Check if a service server is available. + /// + /// Will return true if there is a service server available, false if unavailable. + /// + /// Consider using [`Self::notify_on_service_ready`] if you want to wait + /// until a service for this client is ready. + pub fn service_is_ready(&self) -> Result { + let mut is_ready = false; + let client = &mut *self.handle.rcl_client.lock().unwrap(); + let node = &mut *self.handle.node.handle().rcl_node.lock().unwrap(); + + unsafe { + // SAFETY both node and client are guaranteed to be valid here + // client is guaranteed to have been generated with node + rcl_service_server_is_available(node as *const _, client as *const _, &mut is_ready) + } + .ok()?; + Ok(is_ready) + } + + /// Get a promise that will be fulfilled when a service is ready for this + /// client. You can `.await` the promise in an async function or use it for + /// `until_promise_resolved` in [`SpinOptions`][crate::SpinOptions]. + pub fn notify_on_service_ready(self: &Arc) -> Promise<()> { + let client = Arc::clone(self); + self.handle.node.notify_on_graph_change( + // TODO(@mxgrey): Log any errors here once logging is available + move || client.service_is_ready().is_ok_and(|r| r), + ) + } + /// Creates a new client. - pub(crate) fn new(node_handle: Arc, topic: &str) -> Result + pub(crate) fn create<'a>( + options: impl Into>, + 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 T: rosidl_runtime_rs::Service, { + let ClientOptions { service_name, qos } = options.into(); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_client = unsafe { rcl_get_zero_initialized_client() }; let type_support = ::get_type_support() as *const rosidl_service_type_support_t; - let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { - err, - s: topic.into(), - })?; + let topic_c_string = + CString::new(service_name).map_err(|err| RclrsError::StringContainsNul { + err, + s: service_name.into(), + })?; // SAFETY: No preconditions for this function. - let client_options = unsafe { rcl_client_get_default_options() }; + let mut client_options = unsafe { rcl_client_get_default_options() }; + client_options.qos = qos.into(); { - let rcl_node = node_handle.rcl_node.lock().unwrap(); + let rcl_node = node.handle().rcl_node.lock().unwrap(); let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); // SAFETY: @@ -124,189 +231,222 @@ where } } + let commands = node.commands().async_worker_commands(); let handle = Arc::new(ClientHandle { rcl_client: Mutex::new(rcl_client), - node_handle, - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + node: Arc::clone(&node), }); - Ok(Self { + let board = Arc::new(Mutex::new(ClientRequestBoard::new())); + + let (waitable, lifecycle) = Waitable::new( + Box::new(ClientExecutable { + handle: Arc::clone(&handle), + board: Arc::clone(&board), + }), + Some(Arc::clone(&commands.get_guard_condition())), + ); + commands.add_to_wait_set(waitable); + + Ok(Arc::new(Self { handle, - requests: Mutex::new(HashMap::new()), - futures: Arc::new(Mutex::new( - HashMap::>::new(), - )), - }) + board, + lifecycle, + })) } +} - /// Sends a request with a callback to be called with the response. - /// - /// The [`MessageCow`] trait is implemented by any - /// [`Message`] as well as any reference to a `Message`. - /// - /// The reason for allowing owned messages is that publishing owned messages can be more - /// efficient in the case of idiomatic messages[^note]. - /// - /// [^note]: See the [`Message`] trait for an explanation of "idiomatic". - /// - /// Hence, when a message will not be needed anymore after publishing, pass it by value. - /// When a message will be needed again after publishing, pass it by reference, instead of cloning and passing by value. - pub fn async_send_request_with_callback<'a, M: MessageCow<'a, T::Request>, F>( - &self, - message: M, - callback: F, - ) -> Result<(), RclrsError> - where - F: FnOnce(T::Response) + 'static + Send, - { - let rmw_message = T::Request::into_rmw_message(message.into_cow()); - let mut sequence_number = -1; - unsafe { - // SAFETY: The request type is guaranteed to match the client type by the type system. - rcl_send_request( - &*self.handle.lock() as *const _, - rmw_message.as_ref() as *const ::RmwMsg as *mut _, - &mut sequence_number, - ) +/// `ClientOptions` are used by [`Node::create_client`][1] to initialize a +/// [`Client`] for a service. +/// +/// [1]: crate::Node::create_client +#[derive(Debug, Clone)] +#[non_exhaustive] +pub struct ClientOptions<'a> { + /// The name of the service that this client will send requests to + pub service_name: &'a str, + /// The quality of the service profile for this client + pub qos: QoSProfile, +} + +impl<'a> ClientOptions<'a> { + /// Initialize a new [`ClientOptions`] with default settings. + pub fn new(service_name: &'a str) -> Self { + Self { + service_name, + qos: QoSProfile::services_default(), } - .ok()?; - let requests = &mut *self.requests.lock().unwrap(); - requests.insert(sequence_number, Box::new(callback)); - Ok(()) } +} - /// Sends a request and returns the response as a `Future`. - /// - /// The [`MessageCow`] trait is implemented by any - /// [`Message`] as well as any reference to a `Message`. - /// - /// The reason for allowing owned messages is that publishing owned messages can be more - /// efficient in the case of idiomatic messages[^note]. - /// - /// [^note]: See the [`Message`] trait for an explanation of "idiomatic". - /// - /// Hence, when a message will not be needed anymore after publishing, pass it by value. - /// When a message will be needed again after publishing, pass it by reference, instead of cloning and passing by value. - pub async fn call_async<'a, R: MessageCow<'a, T::Request>>( - &self, - request: R, - ) -> Result - where - T: rosidl_runtime_rs::Service, - { - let rmw_message = T::Request::into_rmw_message(request.into_cow()); - let mut sequence_number = -1; - unsafe { - // SAFETY: The request type is guaranteed to match the client type by the type system. - rcl_send_request( - &*self.handle.lock() as *const _, - rmw_message.as_ref() as *const ::RmwMsg as *mut _, - &mut sequence_number, - ) - } - .ok()?; - let (tx, rx) = oneshot::channel::(); - self.futures.lock().unwrap().insert(sequence_number, tx); - // It is safe to call unwrap() here since the `Canceled` error will only happen when the - // `Sender` is dropped - // https://docs.rs/futures/latest/futures/channel/oneshot/struct.Canceled.html - Ok(rx.await.unwrap()) +impl<'a, T: IntoPrimitiveOptions<'a>> From for ClientOptions<'a> { + fn from(value: T) -> Self { + let primitive = value.into_primitive_options(); + let mut options = Self::new(primitive.name); + primitive.apply(&mut options.qos); + options } +} - /// Fetches a new response. - /// - /// When there is no new message, this will return a - /// [`ClientTakeFailed`][1]. - /// - /// [1]: crate::RclrsError - // - // ```text - // +----------------------+ - // | rclrs::take_response | - // +----------+-----------+ - // | - // | - // +----------v-----------+ - // | rcl_take_response | - // +----------+-----------+ - // | - // | - // +----------v----------+ - // | rmw_take | - // +---------------------+ - // ``` - pub fn take_response(&self) -> Result<(T::Response, rmw_request_id_t), RclrsError> { - let mut request_id_out = rmw_request_id_t { - writer_guid: [0; 16], - sequence_number: 0, - }; - type RmwMsg = - <::Response as rosidl_runtime_rs::Message>::RmwMsg; - let mut response_out = RmwMsg::::default(); - let handle = &*self.handle.lock(); - unsafe { - // SAFETY: The three pointers are valid/initialized - rcl_take_response( - handle, - &mut request_id_out, - &mut response_out as *mut RmwMsg as *mut _, - ) - } - .ok()?; - Ok((T::Response::from_rmw_message(response_out), request_id_out)) +struct ClientExecutable +where + T: rosidl_runtime_rs::Service, +{ + handle: Arc, + board: Arc>>, +} + +impl RclPrimitive for ClientExecutable +where + T: rosidl_runtime_rs::Service, +{ + unsafe fn execute(&mut self, _: &mut dyn Any) -> Result<(), RclrsError> { + self.board.lock().unwrap().execute(&self.handle) } - /// Check if a service server is available. - /// - /// Will return true if there is a service server available, false if unavailable. - /// - pub fn service_is_ready(&self) -> Result { - let mut is_ready = false; - let client = &mut *self.handle.rcl_client.lock().unwrap(); - let node = &mut *self.handle.node_handle.rcl_node.lock().unwrap(); + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::Client(self.handle.lock()) + } - unsafe { - // SAFETY both node and client are guaranteed to be valid here - // client is guaranteed to have been generated with node - rcl_service_server_is_available(node as *const _, client as *const _, &mut is_ready) - } - .ok()?; - Ok(is_ready) + fn kind(&self) -> RclPrimitiveKind { + RclPrimitiveKind::Client } } -impl ClientBase for Client +type SequenceNumber = i64; + +/// This is used internally to monitor the state of active requests, as well as +/// responses that have arrived without a known request. +struct ClientRequestBoard +where + T: rosidl_runtime_rs::Service, +{ + // This stores all active requests that have not received a response yet + active_requests: HashMap>, + // This holds responses that came in when no active request matched the + // sequence number. This could happen if take_response is triggered before + // the new_request for the same sequence number. That is extremely unlikely + // to ever happen but is theoretically possible on systems that may exhibit + // very strange CPU scheduling patterns, so we should account for it. + loose_responses: HashMap, +} + +impl ClientRequestBoard where T: rosidl_runtime_rs::Service, { - fn handle(&self) -> &ClientHandle { - &self.handle + fn new() -> Self { + Self { + active_requests: Default::default(), + loose_responses: Default::default(), + } + } + + fn new_request( + &mut self, + sequence_number: SequenceNumber, + sender: AnyClientOutputSender, + ) { + if let Some((response, info)) = self.loose_responses.remove(&sequence_number) { + // Weirdly the response for this request already arrived, so we'll + // send it off immediately. + sender.send_response(response, info); + } else { + self.active_requests.insert(sequence_number, sender); + } } - fn execute(&self) -> Result<(), RclrsError> { - let (res, req_id) = match self.take_response() { - Ok((res, req_id)) => (res, req_id), - Err(RclrsError::RclError { - code: RclReturnCode::ClientTakeFailed, - .. - }) => { - // Spurious wakeup – this may happen even when a waitset indicated that this - // client was ready, so it shouldn't be an error. - return Ok(()); + fn execute(&mut self, handle: &Arc) -> Result<(), RclrsError> { + match self.take_response(handle) { + Ok((response, info)) => { + let seq = info.request_id.sequence_number; + if let Some(sender) = self.active_requests.remove(&seq) { + // The active request is available, so send this response off + sender.send_response(response, info); + } else { + // Weirdly there isn't an active request for this, so save + // it in the loose responses map. + self.loose_responses.insert(seq, (response, info)); + } + } + Err(err) => { + match err { + RclrsError::RclError { + code: RclReturnCode::ClientTakeFailed, + .. + } => { + // This is okay, it means a spurious wakeup happened + } + err => { + // TODO(@mxgrey): Log the error here once logging is available + eprintln!("Error while taking a response for a client: {err}"); + } + } } - Err(e) => return Err(e), - }; - let requests = &mut *self.requests.lock().unwrap(); - let futures = &mut *self.futures.lock().unwrap(); - if let Some(callback) = requests.remove(&req_id.sequence_number) { - callback(res); - } else if let Some(future) = futures.remove(&req_id.sequence_number) { - let _ = future.send(res); } Ok(()) } + + fn take_response( + &self, + handle: &Arc, + ) -> Result<(T::Response, rmw_service_info_t), RclrsError> { + let mut service_info_out = ServiceInfo::zero_initialized_rmw(); + let mut response_out = ::RmwMsg::default(); + let handle = &*handle.lock(); + unsafe { + // SAFETY: The three pointers are all kept valid by the handle + rcl_take_response_with_info( + handle, + &mut service_info_out, + &mut response_out as *mut ::RmwMsg as *mut _, + ) + } + .ok() + .map(|_| { + ( + T::Response::from_rmw_message(response_out), + service_info_out, + ) + }) + } +} + +/// Manage the lifecycle of an `rcl_client_t`, including managing its dependencies +/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are +/// [dropped after][1] the `rcl_client_t`. +/// +/// [1]: +struct ClientHandle { + rcl_client: Mutex, + /// We store the whole node here because we use some of its user-facing API + /// in some of the Client methods. + node: Node, +} + +impl ClientHandle { + fn lock(&self) -> MutexGuard { + self.rcl_client.lock().unwrap() + } } +impl Drop for ClientHandle { + fn drop(&mut self) { + let rcl_client = self.rcl_client.get_mut().unwrap(); + let mut rcl_node = self.node.handle().rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of + // global variables in the rmw implementation being unsafely modified during cleanup. + unsafe { + rcl_client_fini(rcl_client, &mut *rcl_node); + } + } +} + +// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread +// they are running in. Therefore, this type can be safely sent to another thread. +unsafe impl Send for rcl_client_t {} + #[cfg(test)] mod tests { use super::*; diff --git a/rclrs/src/client/client_async_callback.rs b/rclrs/src/client/client_async_callback.rs new file mode 100644 index 000000000..537ae1bbf --- /dev/null +++ b/rclrs/src/client/client_async_callback.rs @@ -0,0 +1,57 @@ +use rosidl_runtime_rs::Service; + +use std::future::Future; + +use crate::{RequestId, ServiceInfo}; + +/// A trait to deduce async callbacks of service clients. +/// +/// Users of rclrs never need to use this trait directly. +// +// TODO(@mxgrey): Add a description of what callback signatures are supported +pub trait ClientAsyncCallback: Send + 'static +where + T: Service, +{ + /// This represents the type of task (Future) that will be produced by the callback + type Task: Future + Send; + + /// Trigger the callback to run + fn run_client_async_callback(self, response: T::Response, info: ServiceInfo) -> Self::Task; +} + +impl ClientAsyncCallback for Func +where + T: Service, + Func: FnOnce(T::Response) -> Fut + Send + 'static, + Fut: Future + Send, +{ + type Task = Fut; + fn run_client_async_callback(self, response: T::Response, _info: ServiceInfo) -> Fut { + self(response) + } +} + +impl ClientAsyncCallback for Func +where + T: Service, + Func: FnOnce(T::Response, RequestId) -> Fut + Send + 'static, + Fut: Future + Send, +{ + type Task = Fut; + fn run_client_async_callback(self, response: T::Response, info: ServiceInfo) -> Fut { + self(response, info.request_id) + } +} + +impl ClientAsyncCallback for Func +where + T: Service, + Func: FnOnce(T::Response, ServiceInfo) -> Fut + Send + 'static, + Fut: Future + Send, +{ + type Task = Fut; + fn run_client_async_callback(self, response: T::Response, info: ServiceInfo) -> Fut { + self(response, info) + } +} diff --git a/rclrs/src/client/client_callback.rs b/rclrs/src/client/client_callback.rs new file mode 100644 index 000000000..d05a83de3 --- /dev/null +++ b/rclrs/src/client/client_callback.rs @@ -0,0 +1,46 @@ +use rosidl_runtime_rs::Service; + +use crate::{RequestId, ServiceInfo}; + +/// A trait to deduce regular callbacks of service clients. +/// +/// Users of rclrs never need to use this trait directly. +// +// TODO(@mxgrey): Add a description of what callback signatures are supported +pub trait ClientCallback: Send + 'static +where + T: Service, +{ + /// Trigger the callback to run + fn run_client_callback(self, response: T::Response, info: ServiceInfo); +} + +impl ClientCallback for Func +where + T: Service, + Func: FnOnce(T::Response) + Send + 'static, +{ + fn run_client_callback(self, response: T::Response, _info: ServiceInfo) { + self(response) + } +} + +impl ClientCallback for Func +where + T: Service, + Func: FnOnce(T::Response, RequestId) + Send + 'static, +{ + fn run_client_callback(self, response: T::Response, info: ServiceInfo) { + self(response, info.request_id) + } +} + +impl ClientCallback for Func +where + T: Service, + Func: FnOnce(T::Response, ServiceInfo) + Send + 'static, +{ + fn run_client_callback(self, response: T::Response, info: ServiceInfo) { + self(response, info) + } +} diff --git a/rclrs/src/client/client_output.rs b/rclrs/src/client/client_output.rs new file mode 100644 index 000000000..f0c2b2314 --- /dev/null +++ b/rclrs/src/client/client_output.rs @@ -0,0 +1,66 @@ +use rosidl_runtime_rs::Message; + +use futures::channel::oneshot::{channel, Sender}; + +use crate::{rcl_bindings::rmw_service_info_t, Promise, RequestId, ServiceInfo}; + +/// This trait allows us to deduce how much information a user wants to receive +/// from a client call. A user can choose to receive only the response from the +/// service or may include the [`RequestId`] or [`ServiceInfo`] metadata. +/// +/// Users never need to use this trait directly. +pub trait ClientOutput: Sized { + /// Create the appropriate type of channel to send the information that the + /// user asked for. + fn create_channel() -> (AnyClientOutputSender, Promise); +} + +impl ClientOutput for Response { + fn create_channel() -> (AnyClientOutputSender, Promise) { + let (sender, receiver) = channel(); + (AnyClientOutputSender::ResponseOnly(sender), receiver) + } +} + +impl ClientOutput for (Response, RequestId) { + fn create_channel() -> (AnyClientOutputSender, Promise) { + let (sender, receiver) = channel(); + (AnyClientOutputSender::WithId(sender), receiver) + } +} + +impl ClientOutput for (Response, ServiceInfo) { + fn create_channel() -> (AnyClientOutputSender, Promise) { + let (sender, receiver) = channel(); + (AnyClientOutputSender::WithServiceInfo(sender), receiver) + } +} + +/// Can send any kind of response for a client call. +pub enum AnyClientOutputSender { + /// The user only asked for the response. + ResponseOnly(Sender), + /// The user also asked for the RequestId + WithId(Sender<(Response, RequestId)>), + /// The user also asked for the ServiceInfo + WithServiceInfo(Sender<(Response, ServiceInfo)>), +} + +impl AnyClientOutputSender { + pub(super) fn send_response(self, response: Response, service_info: rmw_service_info_t) { + match self { + Self::ResponseOnly(sender) => { + let _ = sender.send(response); + } + Self::WithId(sender) => { + let _ = sender.send(( + response, + RequestId::from_rmw_request_id(&service_info.request_id), + )); + } + Self::WithServiceInfo(sender) => { + let _ = sender.send((response, ServiceInfo::from_rmw_service_info(&service_info))); + } + } + } +} diff --git a/rclrs/src/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 524169bb2..0933817ed 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -6,7 +6,10 @@ use std::{ vec::Vec, }; -use crate::{rcl_bindings::*, LoggingLifecycle, RclrsError, ToResult}; +use crate::{ + rcl_bindings::*, BasicExecutorRuntime, Executor, ExecutorRuntime, LoggingLifecycle, RclrsError, + ToResult, +}; /// This is locked whenever initializing or dropping any middleware entity /// because we have found issues in RCL and some RMW implementations that @@ -60,10 +63,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 @@ -78,34 +90,40 @@ pub(crate) struct ContextHandle { logging: Arc, } +impl Default for Context { + fn default() -> Self { + // SAFETY: It should always be valid to instantiate a context with no + // arguments, no parameters, no options, etc. + Self::new([], InitOptions::default()).expect("Failed to instantiate a default context") + } +} + impl Context { /// Creates a new context. /// - /// Usually this would be called with `std::env::args()`, analogously to `rclcpp::init()`. - /// See also the official "Passing ROS arguments to nodes via the command-line" tutorial. + /// * `args` - A sequence of strings that resembles command line arguments + /// that users can pass into a ROS executable. See [the official tutorial][1] + /// to know what these arguments may look like. To simply pass in the arguments + /// that the user has provided from the command line, call [`Self::from_env`] + /// or [`Self::default_from_env`] instead. /// - /// Creating a context will fail if the args contain invalid ROS arguments. + /// * `options` - Additional options that your application can use to override + /// settings that would otherwise be determined by the environment. /// - /// # Example - /// ``` - /// # use rclrs::Context; - /// assert!(Context::new([]).is_ok()); - /// let invalid_remapping = ["--ros-args", "-r", ":=:*/]"].map(String::from); - /// assert!(Context::new(invalid_remapping).is_err()); - /// ``` - pub fn new(args: impl IntoIterator) -> Result { - Self::new_with_options(args, InitOptions::new()) - } - - /// Same as [`Context::new`] except you can additionally provide initialization options. + /// Creating a context will fail if `args` contains invalid ROS arguments. /// /// # Example /// ``` /// use rclrs::{Context, InitOptions}; - /// let context = Context::new_with_options([], InitOptions::new().with_domain_id(Some(5))).unwrap(); + /// let context = Context::new( + /// std::env::args(), + /// InitOptions::new().with_domain_id(Some(5)), + /// ).unwrap(); /// assert_eq!(context.domain_id(), 5); - /// ```` - pub fn new_with_options( + /// ``` + /// + /// [1]: https://docs.ros.org/en/rolling/How-To-Guides/Node-arguments.html + pub fn new( args: impl IntoIterator, options: InitOptions, ) -> Result { @@ -165,6 +183,35 @@ impl Context { }) } + /// Same as [`Self::new`] but [`std::env::args`] is automatically passed in + /// for `args`. + pub fn from_env(options: InitOptions) -> Result { + Self::new(std::env::args(), options) + } + + /// Same as [`Self::from_env`] but the default [`InitOptions`] is passed in + /// for `options`. + pub fn default_from_env() -> Result { + Self::new(std::env::args(), InitOptions::default()) + } + + /// Create an executor that uses the [basic executor runtime][1] that comes + /// built into rclrs. + /// + /// [1]: BasicExecutorRuntime + pub fn create_basic_executor(&self) -> Executor { + let runtime = BasicExecutorRuntime::new(); + self.create_executor(runtime) + } + + /// Create an [`Executor`] for this context. + pub fn create_executor(&self, runtime: E) -> Executor + where + E: 'static + ExecutorRuntime + Send, + { + Executor::new(Arc::clone(&self.handle), runtime) + } + /// Returns the ROS domain ID that the context is using. /// /// The domain ID controls which nodes can send messages to each other, see the [ROS 2 concept article][1]. @@ -265,14 +312,14 @@ mod tests { #[test] fn test_create_context() -> Result<(), RclrsError> { // If the context fails to be created, this will cause a panic - let _ = Context::new(vec![])?; + let _ = Context::new(vec![], InitOptions::default())?; Ok(()) } #[test] fn test_context_ok() -> Result<(), RclrsError> { // If the context fails to be created, this will cause a panic - let created_context = Context::new(vec![]).unwrap(); + let created_context = Context::new(vec![], InitOptions::default()).unwrap(); assert!(created_context.ok()); Ok(()) diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index 3eba2549f..5b20317d7 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -32,6 +32,46 @@ 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, + } +} + +impl RclrsError { + /// Returns true if the error was due to a timeout, otherwise returns false. + pub fn is_timeout(&self) -> bool { + matches!( + self, + RclrsError::RclError { + code: RclReturnCode::Timeout, + .. + } + ) + } + + /// Returns true if the error was because a subscription, service, or client + /// take failed, otherwise returns false. + pub fn is_take_failed(&self) -> bool { + matches!( + self, + RclrsError::RclError { + code: RclReturnCode::SubscriptionTakeFailed | RclReturnCode::ServiceTakeFailed | RclReturnCode::ClientTakeFailed, + .. + } + ) + } } impl Display for RclrsError { @@ -48,6 +88,24 @@ 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:?}", + ) + } } } } @@ -79,7 +137,12 @@ 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, } } } @@ -352,3 +415,82 @@ impl ToResult for rcl_ret_t { to_rclrs_result(*self) } } + +/// A helper trait to disregard timeouts as not an error. +pub trait RclrsErrorFilter { + /// Get the first error available, or Ok(()) if there are no errors. + fn first_error(self) -> Result<(), RclrsError>; + + /// If the result was a timeout error, change it to `Ok(())`. + fn timeout_ok(self) -> Self; + + /// If a subscription, service, or client take failed, change the result + /// to be `Ok(())`. + fn take_failed_ok(self) -> Self; + + /// Some error types just indicate an early termination but do not indicate + /// that anything in the system has misbehaved. This filters out anything + /// that is part of the normal operation of rcl. + fn ignore_non_errors(self) -> Self + where + Self: Sized, + { + self + .timeout_ok() + .take_failed_ok() + } +} + +impl RclrsErrorFilter for Result<(), RclrsError> { + fn first_error(self) -> Result<(), RclrsError> { + self + } + + fn timeout_ok(self) -> Result<(), RclrsError> { + match self { + Ok(()) => Ok(()), + Err(err) => { + if err.is_timeout() { + Ok(()) + } else { + Err(err) + } + } + } + } + + fn take_failed_ok(self) -> Result<(), RclrsError> { + match self { + Err(err) => { + if err.is_take_failed() { + // Spurious wakeup - this may happen even when a waitset indicated that + // work was ready, so we won't report it as an error + Ok(()) + } else { + Err(err) + } + } + other => other, + } + } +} + +impl RclrsErrorFilter for Vec { + fn first_error(mut self) -> Result<(), RclrsError> { + if self.is_empty() { + return Ok(()); + } + + Err(self.remove(0)) + } + + fn timeout_ok(mut self) -> Self { + self.retain(|err| !err.is_timeout()); + self + } + + fn take_failed_ok(mut self) -> Self { + self.retain(|err| !err.is_take_failed()); + self + } +} diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 37c43a68e..730e911ef 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,84 +1,466 @@ -use crate::{rcl_bindings::rcl_context_is_valid, Node, RclReturnCode, RclrsError, WaitSet}; +mod basic_executor; +pub use self::basic_executor::*; + +use crate::{WeakActivityListener, Context, ContextHandle, GuardCondition, IntoNodeOptions, Node, RclrsError, Waitable}; +pub use futures::channel::oneshot::Receiver as Promise; +use futures::{ + channel::oneshot, + future::{select, BoxFuture, Either}, +}; use std::{ - sync::{Arc, Mutex, Weak}, + any::Any, + future::Future, + sync::{ + atomic::{AtomicBool, Ordering}, + Arc, + }, time::Duration, }; -/// Single-threaded executor implementation. -pub struct SingleThreadedExecutor { - nodes_mtx: Mutex>>, +/// An executor that can be used to create nodes and run their callbacks. +pub struct Executor { + context: Arc, + commands: Arc, + runtime: Box, } -impl Default for SingleThreadedExecutor { - fn default() -> Self { - Self::new() +impl Executor { + /// Access the commands interface for this executor. Use the returned + /// [`ExecutorCommands`] to create [nodes][Node]. + pub fn commands(&self) -> &Arc { + &self.commands } -} -impl SingleThreadedExecutor { - /// Creates a new executor. - pub fn new() -> Self { - SingleThreadedExecutor { - nodes_mtx: Mutex::new(Vec::new()), + /// Create a [`Node`] that will run on this Executor. + pub fn create_node<'a>( + &'a self, + options: impl IntoNodeOptions<'a>, + ) -> Result { + let options = options.into_node_options(); + let node = options.build(&self.commands)?; + Ok(node) + } + + /// Spin the Executor. The current thread will be blocked until the Executor + /// stops spinning. + /// + /// [`SpinOptions`] can be used to automatically stop the spinning when + /// certain conditions are met. Use `SpinOptions::default()` to allow the + /// Executor to keep spinning indefinitely. + pub fn spin(&mut self, options: SpinOptions) -> Vec { + let conditions = self.make_spin_conditions(options); + let result = self.runtime.spin(conditions); + self.commands.halt_spinning.store(false, Ordering::Release); + result + } + + /// 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; + + let (runtime, result) = runtime.spin_async(conditions).await; + commands.halt_spinning.store(false, Ordering::Release); + + ( + Self { + context, + commands, + runtime, + }, + result, + ) + } + + /// Creates a new executor using the provided runtime. Users of rclrs should + /// use [`Context::create_executor`]. + pub(crate) fn new(context: Arc, runtime: E) -> Self + where + E: 'static + ExecutorRuntime + Send, + { + let 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), + }, } } +} - /// Add a node to the executor. - pub fn add_node(&self, node: &Arc) -> Result<(), RclrsError> { - { self.nodes_mtx.lock().unwrap() }.push(Arc::downgrade(node)); - Ok(()) +/// 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) } - /// Remove a node from the executor. - pub fn remove_node(&self, node: Arc) -> Result<(), RclrsError> { - { self.nodes_mtx.lock().unwrap() } - .retain(|n| !n.upgrade().map(|n| Arc::ptr_eq(&n, &node)).unwrap_or(false)); - Ok(()) + /// Tell the [`Executor`] to halt its spinning. + pub fn halt_spinning(&self) { + self.halt_spinning.store(true, Ordering::Release); + // TODO(@mxgrey): Log errors here when logging becomes available + self.executor_channel.wake_all_wait_sets(); } - /// Polls the nodes for new messages and executes the corresponding callbacks. + /// Run a task on the [`Executor`]. If the returned [`Promise`] is dropped + /// then the task will be dropped, which means it might not run to + /// completion. /// - /// This function additionally checks that the context is still valid. - pub fn spin_once(&self, timeout: Option) -> Result<(), RclrsError> { - for node in { self.nodes_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .filter(|node| unsafe { - rcl_context_is_valid(&*node.handle.context_handle.rcl_context.lock().unwrap()) - }) - { - let wait_set = WaitSet::new_for_node(&node)?; - let ready_entities = wait_set.wait(timeout)?; - - for ready_subscription in ready_entities.subscriptions { - ready_subscription.execute()?; - } + /// 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 + } + + /// 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 + } - for ready_service in ready_entities.services { - ready_service.execute()?; + /// 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(); } + }); + + (main_receiver, notice_receiver) + } + + /// Get the context that the executor is associated with. + pub fn context(&self) -> &Context { + &self.context + } + + pub(crate) fn add_to_wait_set(&self, waitable: Waitable) { + self.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); + } + + 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 +/// an executor spins for. +/// +/// By default the executor will be allowed to spin indefinitely. +#[non_exhaustive] +#[derive(Default)] +pub struct SpinOptions { + /// Only perform the next available work. This is similar to spin_once in + /// rclcpp and rclpy. + /// + /// To only process work that is immediately available without waiting at all, + /// set a timeout of zero. + pub only_next_available_work: bool, + /// The executor will stop spinning if the promise is resolved. The promise + /// does not need to be fulfilled (i.e. a value was sent), it could also be + /// cancelled (i.e. the Sender was dropped) and spinning will nevertheless + /// stop. + pub until_promise_resolved: Option>, + /// Stop waiting after this duration of time has passed. Use `Some(0)` to not + /// wait any amount of time. Use `None` to wait an infinite amount of time. + pub timeout: Option, +} + +impl SpinOptions { + /// Use default spin options. + pub fn new() -> Self { + Self::default() + } + + /// Behave like spin_once in rclcpp and rclpy. + pub fn spin_once() -> Self { + Self { + only_next_available_work: true, + ..Default::default() } + } - Ok(()) + /// Stop spinning once this promise is resolved. + pub fn until_promise_resolved(mut self, promise: Promise<()>) -> Self { + self.until_promise_resolved = Some(promise); + self } - /// Convenience function for calling [`SingleThreadedExecutor::spin_once`] in a loop. - pub fn spin(&self) -> Result<(), RclrsError> { - while !{ self.nodes_mtx.lock().unwrap() }.is_empty() { - match self.spin_once(None) { - Ok(_) - | Err(RclrsError::RclError { - code: RclReturnCode::Timeout, - .. - }) => std::thread::yield_now(), - error => return error, - } + /// 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, } + } +} - Ok(()) +/// 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(), + } } } diff --git a/rclrs/src/executor/basic_executor.rs b/rclrs/src/executor/basic_executor.rs new file mode 100644 index 000000000..888726aa3 --- /dev/null +++ b/rclrs/src/executor/basic_executor.rs @@ -0,0 +1,441 @@ +use futures::{ + channel::{mpsc::{UnboundedSender, UnboundedReceiver, unbounded}, oneshot}, + future::{BoxFuture, select, select_all, Either}, + task::{waker_ref, ArcWake}, + stream::StreamFuture, + StreamExt, +}; +use std::{ + sync::{ + atomic::{AtomicBool, Ordering}, + mpsc::{channel, Receiver, Sender}, + Arc, Mutex, Weak, + }, + task::Context as TaskContext, + time::Instant, +}; + +use crate::{ + WeakActivityListener, ExecutorChannel, ExecutorRuntime, SpinConditions, WorkerChannel, + RclrsError, WaitSetRunner, WaitSetRunConditions, Waitable, log_warn, log_fatal, ToLogParams, + GuardCondition, ExecutorWorkerOptions, PayloadTask, +}; + +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: +/// https://rust-lang.github.io/async-book/02_execution/04_executor.html +/// +/// This implements a single-threaded async executor. This means the execution of +/// all async tasks will be interlaced on a single thread. This is good for +/// minimizing context switching overhead and preventing the application from +/// consuming more CPU threads than it really needs. +/// +/// If you need high-throughput multi-threaded execution, then consider using +/// a different executor. +// +// TODO(@mxgrey): Implement a multi-threaded executor using tokio in a downstream +// crate and refer to it in this documentation. +pub struct BasicExecutorRuntime { + ready_queue: Receiver>, + task_sender: TaskSender, + 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 { + // TODO(@mxgrey): We should change this to a log when logging + // becomes available. + 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) { + // TODO(@mxgrey): Log errors here once logging becomes available. + self.waitable_sender.unbounded_send(new_entity).ok(); + } + + fn add_async_task(&self, f: BoxFuture<'static, ()>) { + self.task_sender.add_async_task(f); + } + + fn send_payload_task(&self, f: PayloadTask) { + self.payload_task_sender.unbounded_send(f).ok(); + } + + 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(), + }); + + // TODO(@mxgrey): Consider logging errors here once logging is available. + self.task_sender.send(task).ok(); + } +} + +struct Task { + /// This future is held inside an Option because we need to move it in and + /// out of this `Task` instance without destructuring the `Task` because the + /// [`ArcWake`] wakeup behavior relies on `Task` having a single instance + /// that is managed by an Arc. + /// + /// We wrap the Option in Mutex because we need to mutate the Option from a + /// shared borrow that comes from the Arc. + future: Mutex>>, + task_sender: Sender>, +} + +/// Implementing this trait gives us a very easy implementation of waking +/// behavior for Task on our BasicExecutorRuntime. +impl ArcWake for Task { + fn wake_by_ref(arc_self: &Arc) { + let cloned = Arc::clone(arc_self); + arc_self.task_sender.send(cloned).ok(); + } +} + +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 3a22c6da8..30b5cc2ec 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -20,8 +20,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; @@ -31,8 +33,6 @@ mod rcl_bindings; #[cfg(feature = "dyn_msg")] pub mod dynamic_message; -use std::{sync::Arc, time::Duration}; - pub use arguments::*; pub use client::*; pub use clock::*; @@ -49,67 +49,6 @@ pub use service::*; pub use subscription::*; pub use time::*; use time_source::*; -pub use wait::*; - -/// Polls the node for new messages and executes the corresponding callbacks. -/// -/// See [`WaitSet::wait`] for the meaning of the `timeout` parameter. -/// -/// This may under some circumstances return -/// [`SubscriptionTakeFailed`][1], [`ClientTakeFailed`][1], [`ServiceTakeFailed`][1] when the wait -/// set spuriously wakes up. -/// This can usually be ignored. -/// -/// [1]: crate::RclReturnCode -pub fn spin_once(node: Arc, timeout: Option) -> Result<(), RclrsError> { - let executor = SingleThreadedExecutor::new(); - executor.add_node(&node)?; - executor.spin_once(timeout) -} - -/// Convenience function for calling [`spin_once`] in a loop. -pub fn spin(node: Arc) -> Result<(), RclrsError> { - let executor = SingleThreadedExecutor::new(); - executor.add_node(&node)?; - executor.spin() -} - -/// Creates a new node in the empty namespace. -/// -/// Convenience function equivalent to [`Node::new`][1]. -/// Please see that function's documentation. -/// -/// [1]: crate::Node::new -/// -/// # Example -/// ``` -/// # use rclrs::{Context, RclrsError}; -/// let ctx = Context::new([])?; -/// let node = rclrs::create_node(&ctx, "my_node"); -/// assert!(node.is_ok()); -/// # Ok::<(), RclrsError>(()) -/// ``` -pub fn create_node(context: &Context, node_name: &str) -> Result, RclrsError> { - Node::new(context, node_name) -} - -/// Creates a [`NodeBuilder`]. -/// -/// Convenience function equivalent to [`NodeBuilder::new()`][1] and [`Node::builder()`][2]. -/// Please see that function's documentation. -/// -/// [1]: crate::NodeBuilder::new -/// [2]: crate::Node::builder -/// -/// # Example -/// ``` -/// # use rclrs::{Context, RclrsError}; -/// let context = Context::new([])?; -/// let node_builder = rclrs::create_node_builder(&context, "my_node"); -/// let node = node_builder.build()?; -/// assert_eq!(node.name(), "my_node"); -/// # Ok::<(), RclrsError>(()) -/// ``` -pub fn create_node_builder(context: &Context, node_name: &str) -> NodeBuilder { - Node::builder(context, node_name) -} +pub use timer::*; +pub use wait_set::*; +pub use worker::*; diff --git a/rclrs/src/logging.rs b/rclrs/src/logging.rs index 5143ae35c..5d2b2ac19 100644 --- a/rclrs/src/logging.rs +++ b/rclrs/src/logging.rs @@ -30,8 +30,8 @@ pub use logger::*; /// use std::time::Duration; /// use std::env; /// -/// let context = rclrs::Context::new(env::args()).unwrap(); -/// let node = rclrs::Node::new(&context, "test_node").unwrap(); +/// let executor = rclrs::Context::default().create_basic_executor(); +/// let node = executor.create_node("test_node").unwrap(); /// /// log!(node.debug(), "Simple debug message"); /// let some_variable = 43; @@ -473,7 +473,10 @@ macro_rules! function { #[cfg(test)] mod tests { use crate::{log_handler::*, test_helpers::*, *}; - use std::sync::Mutex; + use std::{ + sync::{Arc, Mutex}, + time::Duration, + }; #[test] fn test_logging_macros() -> Result<(), RclrsError> { diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index b51b59817..ef00701c7 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,36 +1,55 @@ -mod builder; +mod node_options; +pub use node_options::*; + +mod primitive_options; +pub use primitive_options::*; + mod graph; +pub use graph::*; + +mod node_graph_task; +use node_graph_task::*; + use std::{ cmp::PartialEq, ffi::CStr, fmt, os::raw::c_char, - sync::{atomic::AtomicBool, Arc, Mutex, Weak}, - vec::Vec, + sync::{atomic::AtomicBool, Arc, Mutex}, + time::Duration, +}; + +use futures::{ + channel::mpsc::{unbounded, UnboundedSender}, + StreamExt, }; +use async_std::future::timeout; + use rosidl_runtime_rs::Message; -pub use self::{builder::*, graph::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, LogParams, - Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, - QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, - SubscriptionCallback, TimeSource, ToLogParams, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands, + LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Promise, + Publisher, PublisherOptions, PublisherState, RclrsError, Service, IntoAsyncServiceCallback, + IntoNodeServiceCallback, ServiceOptions, ServiceState, Subscription, IntoAsyncSubscriptionCallback, + IntoNodeSubscriptionCallback, SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, + ENTITY_LIFECYCLE_MUTEX, IntoWorkerOptions, Worker, WorkerState, IntoTimerOptions, AnyTimerCallback, + Timer, IntoNodeTimerRepeatingCallback, IntoNodeTimerOneshotCallback, TimerState, }; -// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread -// they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for rcl_node_t {} - /// A processing unit that can communicate with other nodes. /// /// Nodes are a core concept in ROS 2. Refer to the official ["Understanding ROS 2 nodes"][1] /// tutorial for an introduction. /// -/// Ownership of the node is shared with all [`Publisher`]s and [`Subscription`]s created from it. -/// That means that even after the node itself is dropped, it will continue to exist and be -/// displayed by e.g. `ros2 topic` as long as its publishers and subscriptions are not dropped. +/// Ownership of the node is shared with all the primitives such as [`Publisher`]s and [`Subscription`]s +/// that are created from it. That means that even after the `Node` itself is dropped, it will continue +/// to exist and be displayed by e.g. `ros2 topic` as long as any one of its primitives is not dropped. +/// +/// # Creating +/// Use [`Executor::create_node`][7] to create a new node. Pass in [`NodeOptions`] to set all the different +/// options for node creation, or just pass in a string for the node's name if the default options are okay. /// /// # Naming /// A node has a *name* and a *namespace*. @@ -48,25 +67,38 @@ unsafe impl Send for rcl_node_t {} /// In that sense, the parameters to the node creation functions are only the _default_ namespace and /// name. /// See also the [official tutorial][1] on the command line arguments for ROS nodes, and the -/// [`Node::namespace()`] and [`Node::name()`] functions for examples. +/// [`Node::namespace()`][3] and [`Node::name()`][4] functions for examples. /// /// ## Rules for valid names /// The rules for valid node names and node namespaces are explained in -/// [`NodeBuilder::new()`][3] and [`NodeBuilder::namespace()`][4]. +/// [`NodeOptions::new()`][5] and [`NodeOptions::namespace()`][6]. /// /// [1]: https://docs.ros.org/en/rolling/Tutorials/Understanding-ROS2-Nodes.html /// [2]: https://docs.ros.org/en/rolling/How-To-Guides/Node-arguments.html -/// [3]: crate::NodeBuilder::new -/// [4]: crate::NodeBuilder::namespace -pub struct Node { - pub(crate) clients_mtx: Mutex>>, - pub(crate) guard_conditions_mtx: Mutex>>, - pub(crate) services_mtx: Mutex>>, - pub(crate) subscriptions_mtx: Mutex>>, +/// [3]: Node::namespace +/// [4]: Node::name +/// [5]: crate::NodeOptions::new +/// [6]: crate::NodeOptions::namespace +/// [7]: crate::Executor::create_node + +pub type Node = Arc; + +/// The inner state of a [`Node`]. +/// +/// This is public so that you can choose to put it inside a [`Weak`] if you +/// want to be able to refer to a [`Node`] in a non-owning way. It is generally +/// recommended to manage the [`NodeState`] inside of an [`Arc`], and [`Node`] +/// recommended to manage the `NodeState` inside of an [`Arc`], and [`Node`] +/// is provided as convenience alias for that. +/// +/// The public API of the [`Node`] type is implemented via `NodeState`. +pub struct NodeState { time_source: TimeSource, parameter: ParameterInterface, - pub(crate) handle: Arc, logger: Logger, + commands: Arc, + graph_change_action: UnboundedSender, + handle: Arc, } /// This struct manages the lifetime of an `rcl_node_t`, and accounts for its @@ -114,15 +146,15 @@ impl Drop for NodeHandle { } } -impl Eq for Node {} +impl Eq for NodeState {} -impl PartialEq for Node { +impl PartialEq for NodeState { fn eq(&self, other: &Self) -> bool { Arc::ptr_eq(&self.handle, &other.handle) } } -impl fmt::Debug for Node { +impl fmt::Debug for NodeState { fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { f.debug_struct("Node") .field("fully_qualified_name", &self.fully_qualified_name()) @@ -130,15 +162,7 @@ impl fmt::Debug for Node { } } -impl Node { - /// Creates a new node in the empty namespace. - /// - /// See [`NodeBuilder::new()`] for documentation. - #[allow(clippy::new_ret_no_self)] - pub fn new(context: &Context, node_name: &str) -> Result, RclrsError> { - Self::builder(context, node_name).build() - } - +impl NodeState { /// Returns the clock associated with this node. pub fn get_clock(&self) -> Clock { self.time_source.get_clock() @@ -151,15 +175,15 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError}; + /// # use rclrs::{Context, InitOptions, RclrsError}; /// // Without remapping - /// let context = Context::new([])?; - /// let node = rclrs::create_node(&context, "my_node")?; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node("my_node")?; /// assert_eq!(node.name(), "my_node"); /// // With remapping /// let remapping = ["--ros-args", "-r", "__node:=your_node"].map(String::from); - /// let context_r = Context::new(remapping)?; - /// let node_r = rclrs::create_node(&context_r, "my_node")?; + /// let executor_r = Context::new(remapping, InitOptions::default())?.create_basic_executor(); + /// let node_r = executor_r.create_node("my_node")?; /// assert_eq!(node_r.name(), "your_node"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -174,18 +198,18 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError}; + /// # use rclrs::{Context, InitOptions, RclrsError, IntoNodeOptions}; /// // Without remapping - /// let context = Context::new([])?; - /// let node = - /// rclrs::create_node_builder(&context, "my_node") - /// .namespace("/my/namespace") - /// .build()?; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node( + /// "my_node" + /// .namespace("/my/namespace") + /// )?; /// assert_eq!(node.namespace(), "/my/namespace"); /// // With remapping /// let remapping = ["--ros-args", "-r", "__ns:=/your_namespace"].map(String::from); - /// let context_r = Context::new(remapping)?; - /// let node_r = rclrs::create_node(&context_r, "my_node")?; + /// let executor_r = Context::new(remapping, InitOptions::default())?.create_basic_executor(); + /// let node_r = executor_r.create_node("my_node")?; /// assert_eq!(node_r.namespace(), "/your_namespace"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -196,16 +220,16 @@ impl Node { /// Returns the fully qualified name of the node. /// /// The fully qualified name of the node is the node namespace combined with the node name. - /// It is subject to the remappings shown in [`Node::name()`] and [`Node::namespace()`]. + /// It is subject to the remappings shown in [`NodeState::name()`] and [`NodeState::namespace()`]. /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError}; - /// let context = Context::new([])?; - /// let node = - /// rclrs::create_node_builder(&context, "my_node") - /// .namespace("/my/namespace") - /// .build()?; + /// # use rclrs::{Context, RclrsError, IntoNodeOptions}; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node( + /// "my_node" + /// .namespace("/my/namespace") + /// )?; /// assert_eq!(node.fully_qualified_name(), "/my/namespace/my_node"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -213,183 +237,411 @@ impl Node { self.call_string_getter(rcl_node_get_fully_qualified_name) } - // Helper for name(), namespace(), fully_qualified_name() - fn call_string_getter( - &self, - getter: unsafe extern "C" fn(*const rcl_node_t) -> *const c_char, - ) -> String { - let rcl_node = self.handle.rcl_node.lock().unwrap(); - unsafe { call_string_getter_with_rcl_node(&rcl_node, getter) } + /// Create a new [`Worker`] for this Node. + // + // TODO(@mxgrey): Write some usage examples. + pub fn create_worker<'a, Payload>( + self: &Arc, + options: impl IntoWorkerOptions, + ) -> Worker + where + Payload: 'static + Send + Sync, + { + let options = options.into_worker_options(); + let commands = self.commands.create_worker_commands(Box::new(options.payload)); + WorkerState::create(Arc::clone(self), commands) } /// Creates a [`Client`][1]. /// - /// [1]: crate::Client - // TODO: make client's lifetime depend on node's lifetime - pub fn create_client(&self, topic: &str) -> Result>, RclrsError> + /// Pass in only the service name for the `options` argument to use all default client options: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let client = node.create_client::( + /// "my_service" + /// ) + /// .unwrap(); + /// ``` + /// + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the + /// client options: + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let client = node.create_client::( + /// "my_service" + /// .keep_all() + /// .transient_local() + /// ) + /// .unwrap(); + /// ``` + /// + /// Any quality of service options that you explicitly specify will override + /// the default service options. Any that you do not explicitly specify will + /// remain the default service options. Note that clients are generally + /// expected to use [reliable][1], so it's best not to change the reliability + /// setting unless you know what you are doing. + /// + /// [1]: crate::QoSReliabilityPolicy::Reliable + pub fn create_client<'a, T>( + self: &Arc, + options: impl Into>, + ) -> Result, RclrsError> where T: rosidl_runtime_rs::Service, { - let client = Arc::new(Client::::new(Arc::clone(&self.handle), topic)?); - { self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); - Ok(client) + ClientState::::create(options, self) } - /// Creates a [`GuardCondition`][1] with no callback. + /// Creates a [`Publisher`][1]. + /// + /// Pass in only the topic name for the `options` argument to use all default publisher options: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let publisher = node.create_publisher::( + /// "my_topic" + /// ) + /// .unwrap(); + /// ``` + /// + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the + /// publisher options: + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let publisher = node.create_publisher::( + /// "my_topic" + /// .keep_last(100) + /// .transient_local() + /// ) + /// .unwrap(); /// - /// A weak pointer to the `GuardCondition` is stored within this node. - /// When this node is added to a wait set (e.g. when calling `spin_once`[2] - /// with this node as an argument), the guard condition can be used to - /// interrupt the wait. + /// let reliable_publisher = node.create_publisher::( + /// "my_topic" + /// .reliable() + /// ) + /// .unwrap(); + /// ``` /// - /// [1]: crate::GuardCondition - /// [2]: crate::spin_once - pub fn create_guard_condition(&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 + pub fn create_publisher<'a, T>( + &self, + options: impl Into>, + ) -> Result, RclrsError> + where + T: Message, + { + PublisherState::::create(options, Arc::clone(&self.handle)) } - /// Creates a [`GuardCondition`][1] with a callback. + /// Creates a [`Service`] with an ordinary callback. + /// + /// # Behavior + /// + /// Even though this takes in a blocking (non-async) function, the callback + /// may run in parallel with other callbacks. This callback may even run + /// multiple times simultaneously with different incoming requests. + /// + /// Any internal state that needs to be mutated will need to be wrapped in + /// [`Mutex`] to ensure it is synchronized across multiple simultaneous runs + /// of the callback. To share internal state outside of the callback you will + /// need to wrap it in [`Arc`] or `Arc>`. + /// + /// # Usage /// - /// A weak pointer to the `GuardCondition` is stored within this node. - /// When this node is added to a wait set (e.g. when calling `spin_once`[2] - /// with this node as an argument), the guard condition can be used to - /// interrupt the wait. + /// Pass in only the service name for the `options` argument to use all default service options: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let service = node.create_service::( + /// "my_service", + /// |_request: test_msgs::srv::Empty_Request| { + /// println!("Received request!"); + /// test_msgs::srv::Empty_Response::default() + /// }, + /// ); + /// ``` + /// + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the + /// service options: /// - /// [1]: crate::GuardCondition - /// [2]: crate::spin_once - pub fn create_guard_condition_with_callback(&mut self, callback: F) -> Arc + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let service = node.create_service::( + /// "my_service" + /// .keep_all() + /// .transient_local(), + /// |_request: test_msgs::srv::Empty_Request| { + /// println!("Received request!"); + /// test_msgs::srv::Empty_Response::default() + /// }, + /// ); + /// ``` + /// + /// Any quality of service options that you explicitly specify will override + /// the default service options. Any that you do not explicitly specify will + /// remain the default service options. Note that services are generally + /// expected to use [reliable][2], so it's best not to change the reliability + /// setting unless you know what you are doing. + /// + /// [1]: crate::Service + /// [2]: crate::QoSReliabilityPolicy::Reliable + // + // TODO(@mxgrey): Add examples showing each supported signature + pub fn create_service<'a, T, Args>( + &self, + options: impl Into>, + callback: impl IntoNodeServiceCallback, + ) -> Result, RclrsError> where - F: Fn() + Send + Sync + 'static, + T: rosidl_runtime_rs::Service, { - 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 + ServiceState::::create( + options, + callback.into_node_service_callback(), + &self.handle, + self.commands.async_worker_commands(), + ) } - /// Creates a [`Publisher`][1]. + /// 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. + /// + /// Any internal state that needs to be mutated will need to be wrapped in + /// [`Mutex`] to ensure it is synchronized across multiple runs of the + /// callback. To share internal state outside of the callback you will need + /// to wrap it in [`Arc`] (immutable) or `Arc>` (mutable). /// - /// [1]: crate::Publisher - // TODO: make publisher's lifetime depend on node's lifetime - pub fn create_publisher( + /// # Usage + /// + /// See [create_service][Node::create_service#Usage] for usage. + // + // TODO(@mxgrey): Add examples showing each supported signature + pub fn create_async_service<'a, T, Args>( &self, - topic: &str, - qos: QoSProfile, - ) -> Result>, RclrsError> + options: impl Into>, + callback: impl IntoAsyncServiceCallback, + ) -> Result, RclrsError> where - T: Message, + T: rosidl_runtime_rs::Service, { - let publisher = Arc::new(Publisher::::new(Arc::clone(&self.handle), topic, qos)?); - Ok(publisher) + ServiceState::::create( + options, + callback.into_async_service_callback(), + &self.handle, + self.commands.async_worker_commands(), + ) } - /// Creates a [`Service`][1]. + /// Creates a [`Subscription`] with an ordinary callback. /// - /// [1]: crate::Service - // TODO: make service's lifetime depend on node's lifetime - pub fn create_service( + /// # Behavior + /// + /// Even though this takes in a blocking (non-async) function, the callback + /// may run in parallel with other callbacks. This callback may even run + /// multiple times simultaneously with different incoming messages. This + /// parallelism will depend on the executor that is being used. + /// + /// Any internal state that needs to be mutated will need to be wrapped in + /// [`Mutex`] to ensure it is synchronized across multiple simultaneous runs + /// of the callback. To share internal state outside of the callback you will + /// need to wrap it in [`Arc`] or `Arc>`. + /// + /// # Usage + /// + /// Pass in only the topic name for the `options` argument to use all default subscription options: + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let subscription = node.create_subscription( + /// "my_topic", + /// |_msg: test_msgs::msg::Empty| { + /// println!("Received message!"); + /// }, + /// ); + /// ``` + /// + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the + /// subscription options: + /// + /// ``` + /// # use rclrs::*; + /// # let executor = Context::default().create_basic_executor(); + /// # let node = executor.create_node("my_node").unwrap(); + /// let subscription = node.create_subscription( + /// "my_topic" + /// .keep_last(100) + /// .transient_local(), + /// |_msg: test_msgs::msg::Empty| { + /// println!("Received message!"); + /// }, + /// ); + /// + /// let reliable_subscription = node.create_subscription( + /// "my_reliable_topic" + /// .reliable(), + /// |_msg: test_msgs::msg::Empty| { + /// println!("Received message!"); + /// }, + /// ); + /// ``` + /// + // + // TODO(@mxgrey): Add examples showing each supported callback signatures + pub fn create_subscription<'a, T, Args>( &self, - topic: &str, - callback: F, - ) -> Result>, RclrsError> + options: impl Into>, + callback: impl IntoNodeSubscriptionCallback, + ) -> Result, RclrsError> where - T: rosidl_runtime_rs::Service, - F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send, + T: Message, { - let service = Arc::new(Service::::new( - Arc::clone(&self.handle), - topic, - callback, - )?); - { self.services_mtx.lock().unwrap() } - .push(Arc::downgrade(&service) as Weak); - Ok(service) + SubscriptionState::::create( + options, + callback.into_node_subscription_callback(), + &self.handle, + self.commands.async_worker_commands(), + ) } - /// Creates a [`Subscription`][1]. + /// Creates a [`Subscription`] with an async callback. + /// + /// # Behavior + /// + /// This callback may run in parallel with other callbacks. It may even run + /// multiple times simultaneously with different incoming messages. This + /// parallelism will depend on the executor that is being used. When the + /// callback uses `.await`, it will not block anything else from running. + /// + /// Any internal state that needs to be mutated will need to be wrapped in + /// [`Mutex`] to ensure it is synchronized across multiple runs of the + /// callback. To share internal state outside of the callback you will need + /// to wrap it in [`Arc`] or `Arc>`. + /// + /// # Usage /// - /// [1]: crate::Subscription - // TODO: make subscription's lifetime depend on node's lifetime - pub fn create_subscription( + /// See [create_subscription][Node::create_subscription#Usage] for usage. + // + // TODO(@mxgrey): Add examples showing each supported signature + pub fn create_async_subscription<'a, T, Args>( &self, - topic: &str, - qos: QoSProfile, - callback: impl SubscriptionCallback, - ) -> Result>, RclrsError> + options: impl Into>, + callback: impl IntoAsyncSubscriptionCallback, + ) -> Result, RclrsError> where T: Message, { - let subscription = Arc::new(Subscription::::new( - Arc::clone(&self.handle), - topic, - qos, - callback, - )?); - { self.subscriptions_mtx.lock() } - .unwrap() - .push(Arc::downgrade(&subscription) as Weak); - Ok(subscription) + SubscriptionState::::create( + options, + callback.into_async_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() + /// 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. + /// It can be set through the `ROS_DOMAIN_ID` environment variable or by + /// passing custom [`InitOptions`][2] into [`Context::new`][3] or [`Context::from_env`][4]. /// /// [1]: https://docs.ros.org/en/rolling/Concepts/About-Domain-ID.html + /// [2]: crate::InitOptions + /// [3]: crate::Context::new + /// [4]: crate::Context::from_env /// /// # Example /// ``` /// # use rclrs::{Context, RclrsError}; /// // Set default ROS domain ID to 10 here /// std::env::set_var("ROS_DOMAIN_ID", "10"); - /// let context = Context::new([])?; - /// let node = rclrs::create_node(&context, "domain_id_node")?; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node("domain_id_node")?; /// let domain_id = node.domain_id(); /// assert_eq!(domain_id, 10); /// # Ok::<(), RclrsError>(()) /// ``` - // TODO: If node option is supported, - // add description about this function is for getting actual domain_id - // and about override of domain_id via node option pub fn domain_id(&self) -> usize { let rcl_node = self.handle.rcl_node.lock().unwrap(); let mut domain_id: usize = 0; @@ -410,8 +662,8 @@ impl Node { /// # Example /// ``` /// # use rclrs::{Context, ParameterRange, RclrsError}; - /// let context = Context::new([])?; - /// let node = rclrs::create_node(&context, "domain_id_node")?; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node("domain_id_node")?; /// // Set it to a range of 0-100, with a step of 2 /// let range = ParameterRange { /// lower: Some(0), @@ -447,32 +699,90 @@ impl Node { } } - /// Creates a [`NodeBuilder`][1] with the given name. + /// Same as [`Self::notify_on_graph_change_with_period`] but uses a + /// recommended default period of 100ms. + pub fn notify_on_graph_change( + &self, + condition: impl FnMut() -> bool + Send + 'static, + ) -> Promise<()> { + self.notify_on_graph_change_with_period(Duration::from_millis(100), condition) + } + + /// This function allows you to track when a specific graph change happens. /// - /// Convenience function equivalent to [`NodeBuilder::new()`][2]. + /// Provide a function that will be called each time a graph change occurs. + /// You will be given a [`Promise`] that will be fulfilled when the condition + /// returns true. The condition will be checked under these conditions: + /// - once immediately as this function is run + /// - each time rcl notifies us that a graph change has happened + /// - each time the period elapses /// - /// [1]: crate::NodeBuilder - /// [2]: crate::NodeBuilder::new + /// We specify a period because it is possible that race conditions at the + /// rcl layer could trigger a notification of a graph change before your + /// API calls will be able to observe it. /// - /// # Example - /// ``` - /// # use rclrs::{Context, Node, RclrsError}; - /// let context = Context::new([])?; - /// let node = Node::builder(&context, "my_node").build()?; - /// assert_eq!(node.name(), "my_node"); - /// # Ok::<(), RclrsError>(()) - /// ``` - pub fn builder(context: &Context, node_name: &str) -> NodeBuilder { - NodeBuilder::new(context, node_name) + /// + pub fn notify_on_graph_change_with_period( + &self, + period: Duration, + mut condition: impl FnMut() -> bool + Send + 'static, + ) -> Promise<()> { + let (listener, mut on_graph_change_receiver) = unbounded(); + let promise = self.commands.query(async move { + loop { + match timeout(period, on_graph_change_receiver.next()).await { + Ok(Some(_)) | Err(_) => { + // Either we received a notification that there was a + // graph change, or the timeout elapsed. Either way, we + // want to check the condition and break out of the loop + // if the condition is true. + if condition() { + return; + } + } + Ok(None) => { + // We've been notified that the graph change sender is + // closed which means we will never receive another + // graph change update. This only happens when a node + // is being torn down, so go ahead and exit this loop. + return; + } + } + } + }); + + self.graph_change_action + .unbounded_send(NodeGraphAction::NewGraphListener(listener)) + .ok(); + + promise + } + + /// Get the [`ExecutorCommands`] used by this Node. + pub fn commands(&self) -> &Arc { + &self.commands } /// Get the logger associated with this Node. pub fn logger(&self) -> &Logger { &self.logger } + + // Helper for name(), namespace(), fully_qualified_name() + fn call_string_getter( + &self, + getter: unsafe extern "C" fn(*const rcl_node_t) -> *const c_char, + ) -> String { + let rcl_node = self.handle.rcl_node.lock().unwrap(); + unsafe { call_string_getter_with_rcl_node(&rcl_node, getter) } + } + + pub(crate) fn handle(&self) -> &Arc { + &self.handle + } } -impl<'a> ToLogParams<'a> for &'a Node { +impl<'a> ToLogParams<'a> for &'a NodeState { fn to_log_params(self) -> LogParams<'a> { self.logger().to_log_params() } @@ -495,38 +805,41 @@ pub(crate) unsafe fn call_string_getter_with_rcl_node( cstr.to_string_lossy().into_owned() } +// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread +// they are running in. Therefore, this type can be safely sent to another thread. +unsafe impl Send for rcl_node_t {} + #[cfg(test)] mod tests { - use super::*; - use crate::test_helpers::*; + use crate::{test_helpers::*, *}; + + use std::{ + time::Duration, + sync::{Arc, atomic::{AtomicU64, Ordering}}, + }; #[test] fn traits() { - assert_send::(); - assert_sync::(); + assert_send::(); + assert_sync::(); } #[test] fn test_topic_names_and_types() -> Result<(), RclrsError> { - use crate::QOS_PROFILE_SYSTEM_DEFAULT; use test_msgs::msg; let graph = construct_test_graph("test_topics_graph")?; let _node_1_defaults_subscription = graph.node1.create_subscription::( "graph_test_topic_3", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::Defaults| {}, )?; - let _node_2_empty_subscription = graph.node2.create_subscription::( - "graph_test_topic_1", - QOS_PROFILE_SYSTEM_DEFAULT, - |_msg: msg::Empty| {}, - )?; + let _node_2_empty_subscription = graph + .node2 + .create_subscription::("graph_test_topic_1", |_msg: msg::Empty| {})?; let _node_2_basic_types_subscription = graph.node2.create_subscription::( "graph_test_topic_2", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::BasicTypes| {}, )?; @@ -551,6 +864,63 @@ mod tests { Ok(()) } + #[test] + fn test_create_timer() -> Result<(), RclrsError> { + let mut executor = Context::default().create_basic_executor(); + let node = executor.create_node("node_with_timer")?; + + let repeat_counter = Arc::new(AtomicU64::new(0)); + let repeat_counter_check = Arc::clone(&repeat_counter); + let _repeating_timer = node.create_timer_repeating( + Duration::from_millis(1), + move || { + repeat_counter.fetch_add(1, Ordering::AcqRel); + }, + )?; + + let oneshot_counter = Arc::new(AtomicU64::new(0)); + let oneshot_counter_check = Arc::clone(&oneshot_counter); + let _oneshot_timer = node.create_timer_oneshot( + Duration::from_millis(1) + .node_time(), + move || { + oneshot_counter.fetch_add(1, Ordering::AcqRel); + }, + )?; + + let oneshot_resetting_counter = Arc::new(AtomicU64::new(0)); + let oneshot_resetting_counter_check = Arc::clone(&oneshot_resetting_counter); + let _oneshot_resetting_timer = node.create_timer_oneshot( + Duration::from_millis(1), + move |timer: &Timer| { + recursive_oneshot(timer, oneshot_resetting_counter); + }, + ); + + executor.spin(SpinOptions::new().timeout(Duration::from_millis(10))); + + // We give a little leeway to the exact count since timers won't always + // be triggered perfectly. The important thing is that it was + // successfully called repeatedly. + assert!(repeat_counter_check.load(Ordering::Acquire) > 5); + assert!(oneshot_resetting_counter_check.load(Ordering::Acquire) > 5); + + // This should only have been triggered exactly once + assert_eq!(oneshot_counter_check.load(Ordering::Acquire), 1); + + Ok(()) + } + + fn recursive_oneshot( + timer: &Timer, + counter: Arc, + ) { + counter.fetch_add(1, Ordering::AcqRel); + timer.set_oneshot(move |timer: &Timer| { + recursive_oneshot(timer, counter); + }); + } + #[test] fn test_logger_name() -> Result<(), RclrsError> { // Use helper to create 2 nodes for us diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 639a38e38..948d55621 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -3,7 +3,7 @@ use std::{ ffi::{CStr, CString}, }; -use crate::{rcl_bindings::*, Node, RclrsError, ToResult}; +use crate::{rcl_bindings::*, NodeState, RclrsError, ToResult}; impl Drop for rmw_names_and_types_t { fn drop(&mut self) { @@ -57,7 +57,7 @@ pub struct TopicEndpointInfo { pub topic_type: String, } -impl Node { +impl NodeState { /// Returns a list of topic names and types for publishers associated with a node. pub fn get_publisher_names_and_types_by_node( &self, @@ -482,11 +482,11 @@ mod tests { .map(|value: usize| if value != 99 { 99 } else { 98 }) .unwrap_or(99); - let context = - Context::new_with_options([], InitOptions::new().with_domain_id(Some(domain_id))) - .unwrap(); + let executor = Context::new([], InitOptions::new().with_domain_id(Some(domain_id))) + .unwrap() + .create_basic_executor(); let node_name = "test_publisher_names_and_types"; - let node = Node::new(&context, node_name).unwrap(); + let node = executor.create_node(node_name).unwrap(); let check_rosout = |topics: HashMap>| { // rosout shows up in humble and iron, even if the graph is empty @@ -558,9 +558,9 @@ mod tests { #[test] fn test_node_names() { - let context = Context::new([]).unwrap(); + let executor = Context::default().create_basic_executor(); let node_name = "test_node_names"; - let node = Node::new(&context, node_name).unwrap(); + let node = executor.create_node(node_name).unwrap(); let names_and_namespaces = node.get_node_names().unwrap(); @@ -574,9 +574,9 @@ mod tests { #[test] fn test_node_names_with_enclaves() { - let context = Context::new([]).unwrap(); + let executor = Context::default().create_basic_executor(); let node_name = "test_node_names_with_enclaves"; - let node = Node::new(&context, node_name).unwrap(); + let node = executor.create_node(node_name).unwrap(); let names_and_namespaces = node.get_node_names_with_enclaves().unwrap(); diff --git a/rclrs/src/node/node_graph_task.rs b/rclrs/src/node/node_graph_task.rs new file mode 100644 index 000000000..523d2e491 --- /dev/null +++ b/rclrs/src/node/node_graph_task.rs @@ -0,0 +1,39 @@ +use futures::{ + channel::mpsc::{UnboundedReceiver, UnboundedSender}, + StreamExt, +}; + +use crate::GuardCondition; + +pub(super) enum NodeGraphAction { + NewGraphListener(UnboundedSender<()>), + GraphChange, +} + +// We take in the GuardCondition to ensure that its Waitable remains in the wait +// set for as long as this task is running. The task will be kept running as long +// as the Node that started it is alive. +pub(super) async fn node_graph_task( + mut receiver: UnboundedReceiver, + #[allow(unused)] guard_condition: GuardCondition, +) { + let mut listeners = Vec::new(); + while let Some(action) = receiver.next().await { + match action { + NodeGraphAction::NewGraphListener(listener) => { + if listener.unbounded_send(()).is_ok() { + // The listener might or might not still be relevant, so + // keep it until we see that the receiver is dropped. + listeners.push(listener); + } + } + NodeGraphAction::GraphChange => { + // We should let all listeners know that a graph event happened. + // If we see that the listener's receiver has dropped (i.e. + // unbounded_send returns an Err) then we remove it from the + // container. + listeners.retain(|listener| listener.unbounded_send(()).is_ok()); + } + } + } +} diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/node_options.rs similarity index 59% rename from rclrs/src/node/builder.rs rename to rclrs/src/node/node_options.rs index 1e7a9fc63..08f6bebbf 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/node_options.rs @@ -1,110 +1,26 @@ use std::{ + borrow::Borrow, ffi::{CStr, CString}, sync::{atomic::AtomicBool, Arc, Mutex}, }; +use futures::channel::mpsc::unbounded; + use crate::{ - rcl_bindings::*, ClockType, Context, ContextHandle, Logger, Node, NodeHandle, + node::node_graph_task::{node_graph_task, NodeGraphAction}, + rcl_bindings::*, + ClockType, ExecutorCommands, GuardCondition, Logger, Node, NodeHandle, NodeState, ParameterInterface, QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, }; -/// A builder for creating a [`Node`][1]. -/// -/// The builder pattern allows selectively setting some fields, and leaving all others at their default values. -/// This struct instance can be created via [`Node::builder()`][2]. -/// -/// The default values for optional fields are: -/// - `namespace: "/"` -/// - `use_global_arguments: true` -/// - `arguments: []` -/// - `enable_rosout: true` -/// - `start_parameter_services: true` -/// - `clock_type: ClockType::RosTime` -/// - `clock_qos: QOS_PROFILE_CLOCK` +/// This trait helps to build [`NodeOptions`] which can be passed into +/// [`Executor::create_node`][1]. /// -/// # Example -/// ``` -/// # use rclrs::{Context, NodeBuilder, Node, RclrsError}; -/// let context = Context::new([])?; -/// // Building a node in a single expression -/// let node = NodeBuilder::new(&context, "foo_node").namespace("/bar").build()?; -/// assert_eq!(node.name(), "foo_node"); -/// assert_eq!(node.namespace(), "/bar"); -/// // Building a node via Node::builder() -/// let node = Node::builder(&context, "bar_node").build()?; -/// assert_eq!(node.name(), "bar_node"); -/// // Building a node step-by-step -/// let mut builder = Node::builder(&context, "goose"); -/// builder = builder.namespace("/duck/duck"); -/// let node = builder.build()?; -/// assert_eq!(node.fully_qualified_name(), "/duck/duck/goose"); -/// # Ok::<(), RclrsError>(()) -/// ``` -/// -/// [1]: crate::Node -/// [2]: crate::Node::builder -pub struct NodeBuilder { - context: Arc, - name: String, - namespace: String, - use_global_arguments: bool, - arguments: Vec, - enable_rosout: bool, - start_parameter_services: bool, - clock_type: ClockType, - clock_qos: QoSProfile, -} - -impl NodeBuilder { - /// Creates a builder for a node with the given name. - /// - /// See the [`Node` docs][1] for general information on node names. - /// - /// # Rules for valid node names - /// - /// The rules for a valid node name are checked by the [`rmw_validate_node_name()`][2] - /// function. They are: - /// - Must contain only the `a-z`, `A-Z`, `0-9`, and `_` characters - /// - Must not be empty and not be longer than `RMW_NODE_NAME_MAX_NAME_LENGTH` - /// - Must not start with a number - /// - /// Note that node name validation is delayed until [`NodeBuilder::build()`][3]. - /// - /// # Example - /// ``` - /// # use rclrs::{Context, NodeBuilder, RclrsError, RclReturnCode}; - /// let context = Context::new([])?; - /// // This is a valid node name - /// assert!(NodeBuilder::new(&context, "my_node").build().is_ok()); - /// // This is another valid node name (although not a good one) - /// assert!(NodeBuilder::new(&context, "_______").build().is_ok()); - /// // This is an invalid node name - /// assert!(matches!( - /// NodeBuilder::new(&context, "röböt") - /// .build() - /// .unwrap_err(), - /// RclrsError::RclError { code: RclReturnCode::NodeInvalidName, .. } - /// )); - /// # Ok::<(), RclrsError>(()) - /// ``` - /// - /// [1]: crate::Node#naming - /// [2]: https://docs.ros2.org/latest/api/rmw/validate__node__name_8h.html#a5690a285aed9735f89ef11950b6e39e3 - /// [3]: NodeBuilder::build - pub fn new(context: &Context, name: &str) -> NodeBuilder { - NodeBuilder { - context: Arc::clone(&context.handle), - name: name.to_string(), - namespace: "/".to_string(), - use_global_arguments: true, - arguments: vec![], - enable_rosout: true, - start_parameter_services: true, - clock_type: ClockType::RosTime, - clock_qos: QOS_PROFILE_CLOCK, - } - } +/// [1]: crate::Executor::create_node +pub trait IntoNodeOptions<'a>: Sized { + /// Conver the object into [`NodeOptions`] with default settings. + fn into_node_options(self) -> NodeOptions<'a>; /// Sets the node namespace. /// @@ -123,29 +39,29 @@ impl NodeBuilder { /// - Must not contain two or more `/` characters in a row /// - Must not have a `/` character at the end, except if `/` is the full namespace /// - /// Note that namespace validation is delayed until [`NodeBuilder::build()`][4]. + /// Note that namespace validation is delayed until [`Executor::create_node`][4]. /// /// # Example /// ``` - /// # use rclrs::{Context, Node, RclrsError, RclReturnCode}; - /// let context = Context::new([])?; + /// # use rclrs::{Context, Node, IntoNodeOptions, RclrsError, RclReturnCode}; + /// let executor = Context::default().create_basic_executor(); /// // This is a valid namespace - /// let builder_ok_ns = Node::builder(&context, "my_node").namespace("/some/nested/namespace"); - /// assert!(builder_ok_ns.build().is_ok()); + /// let options_ok_ns = "my_node".namespace("/some/nested/namespace"); + /// assert!(executor.create_node(options_ok_ns).is_ok()); /// // This is an invalid namespace /// assert!(matches!( - /// Node::builder(&context, "my_node") + /// executor.create_node( + /// "my_node" /// .namespace("/10_percent_luck/20_percent_skill") - /// .build() - /// .unwrap_err(), + /// ).unwrap_err(), /// RclrsError::RclError { code: RclReturnCode::NodeInvalidNamespace, .. } /// )); /// // A missing forward slash at the beginning is automatically added /// assert_eq!( - /// Node::builder(&context, "my_node") + /// executor.create_node( + /// "my_node" /// .namespace("foo") - /// .build()? - /// .namespace(), + /// )?.namespace(), /// "/foo" /// ); /// # Ok::<(), RclrsError>(()) @@ -154,10 +70,11 @@ impl NodeBuilder { /// [1]: crate::Node#naming /// [2]: http://design.ros2.org/articles/topic_and_service_names.html /// [3]: https://docs.ros2.org/latest/api/rmw/validate__namespace_8h.html#a043f17d240cf13df01321b19a469ee49 - /// [4]: NodeBuilder::build - pub fn namespace(mut self, namespace: &str) -> Self { - self.namespace = namespace.to_string(); - self + /// [4]: crate::Executor::create_node + fn namespace(self, namespace: &'a str) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.namespace = namespace; + options } /// Enables or disables using global arguments. @@ -166,29 +83,30 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, NodeBuilder, RclrsError}; + /// # use rclrs::{Context, InitOptions, Node, IntoNodeOptions, RclrsError}; /// let context_args = ["--ros-args", "--remap", "__node:=your_node"] /// .map(String::from); - /// let context = Context::new(context_args)?; + /// let executor = Context::new(context_args, InitOptions::default())?.create_basic_executor(); /// // Ignore the global arguments: - /// let node_without_global_args = - /// rclrs::create_node_builder(&context, "my_node") - /// .use_global_arguments(false) - /// .build()?; + /// let node_without_global_args = executor.create_node( + /// "my_node" + /// .use_global_arguments(false) + /// )?; /// assert_eq!(node_without_global_args.name(), "my_node"); /// // Do not ignore the global arguments: - /// let node_with_global_args = - /// rclrs::create_node_builder(&context, "my_other_node") - /// .use_global_arguments(true) - /// .build()?; + /// let node_with_global_args = executor.create_node( + /// "my_other_node" + /// .use_global_arguments(true) + /// )?; /// assert_eq!(node_with_global_args.name(), "your_node"); /// # Ok::<(), RclrsError>(()) /// ``` /// /// [1]: crate::Context::new - pub fn use_global_arguments(mut self, enable: bool) -> Self { - self.use_global_arguments = enable; - self + fn use_global_arguments(self, enable: bool) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.use_global_arguments = enable; + options } /// Sets node-specific command line arguments. @@ -201,27 +119,31 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, NodeBuilder, RclrsError}; + /// # use rclrs::{Context, InitOptions, IntoNodeOptions, Node, RclrsError}; /// // Usually, this would change the name of "my_node" to "context_args_node": /// let context_args = ["--ros-args", "--remap", "my_node:__node:=context_args_node"] /// .map(String::from); - /// let context = Context::new(context_args)?; + /// let executor = Context::new(context_args, InitOptions::default())?.create_basic_executor(); /// // But the node arguments will change it to "node_args_node": /// let node_args = ["--ros-args", "--remap", "my_node:__node:=node_args_node"] /// .map(String::from); - /// let node = - /// rclrs::create_node_builder(&context, "my_node") - /// .arguments(node_args) - /// .build()?; + /// let node = executor.create_node( + /// "my_node" + /// .arguments(node_args) + /// )?; /// assert_eq!(node.name(), "node_args_node"); /// # Ok::<(), RclrsError>(()) /// ``` /// /// [1]: crate::Context::new /// [2]: https://design.ros2.org/articles/ros_command_line_arguments.html - pub fn arguments(mut self, arguments: impl IntoIterator) -> Self { - self.arguments = arguments.into_iter().collect(); - self + fn arguments(self, arguments: Args) -> NodeOptions<'a> + where + Args::Item: ToString, + { + let mut options = self.into_node_options(); + options.arguments = arguments.into_iter().map(|item| item.to_string()).collect(); + options } /// Enables or disables logging to rosout. @@ -230,57 +152,161 @@ impl NodeBuilder { /// standard output. /// /// This option is currently unused in `rclrs`. - pub fn enable_rosout(mut self, enable: bool) -> Self { - self.enable_rosout = enable; - self + fn enable_rosout(self, enable: bool) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.enable_rosout = enable; + options } /// Enables or disables parameter services. /// /// Parameter services can be used to allow external nodes to list, get and set /// parameters for this node. - pub fn start_parameter_services(mut self, start: bool) -> Self { - self.start_parameter_services = start; - self + fn start_parameter_services(self, start: bool) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.start_parameter_services = start; + options } /// Sets the node's clock type. - pub fn clock_type(mut self, clock_type: ClockType) -> Self { - self.clock_type = clock_type; - self + fn clock_type(self, clock_type: ClockType) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.clock_type = clock_type; + options } /// Sets the QoSProfile for the clock subscription. - pub fn clock_qos(mut self, clock_qos: QoSProfile) -> Self { - self.clock_qos = clock_qos; - self + fn clock_qos(self, clock_qos: QoSProfile) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.clock_qos = clock_qos; + options } +} - /// Builds the node instance. +/// A set of options for creating a [`Node`][1]. +/// +/// The builder pattern, implemented through [`IntoNodeOptions`], allows +/// selectively setting some fields, and leaving all others at their default values. +/// +/// The default values for optional fields are: +/// - `namespace: "/"` +/// - `use_global_arguments: true` +/// - `arguments: []` +/// - `enable_rosout: true` +/// - `start_parameter_services: true` +/// - `clock_type: ClockType::RosTime` +/// - `clock_qos: QOS_PROFILE_CLOCK` +/// +/// # Example +/// ``` +/// # use rclrs::{ClockType, Context, IntoNodeOptions, NodeOptions, Node, RclrsError}; +/// let executor = Context::default().create_basic_executor(); +/// +/// // Building a node with default options +/// let node = executor.create_node("foo_node"); +/// +/// // Building a node with a namespace +/// let node = executor.create_node("bar_node".namespace("/bar"))?; +/// assert_eq!(node.name(), "bar_node"); +/// assert_eq!(node.namespace(), "/bar"); +/// +/// // Building a node with a namespace and no parameter services +/// let node = executor.create_node( +/// "baz" +/// .namespace("qux") +/// .start_parameter_services(false) +/// )?; +/// +/// // Building node options step-by-step +/// let mut options = NodeOptions::new("goose"); +/// options = options.namespace("/duck/duck"); +/// options = options.clock_type(ClockType::SteadyTime); +/// +/// let node = executor.create_node(options)?; +/// assert_eq!(node.fully_qualified_name(), "/duck/duck/goose"); +/// # Ok::<(), RclrsError>(()) +/// ``` +/// +/// [1]: crate::Node +pub struct NodeOptions<'a> { + name: &'a str, + namespace: &'a str, + use_global_arguments: bool, + arguments: Vec, + enable_rosout: bool, + start_parameter_services: bool, + clock_type: ClockType, + clock_qos: QoSProfile, +} + +impl<'a> NodeOptions<'a> { + /// Creates a builder for a node with the given name. /// - /// Node name and namespace validation is performed in this method. + /// See the [`Node` docs][1] for general information on node names. /// - /// For example usage, see the [`NodeBuilder`][1] docs. + /// # Rules for valid node names /// - /// [1]: crate::NodeBuilder - pub fn build(&self) -> Result, RclrsError> { - let node_name = - CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul { - err, - s: self.name.clone(), - })?; + /// The rules for a valid node name are checked by the [`rmw_validate_node_name()`][2] + /// function. They are: + /// - Must contain only the `a-z`, `A-Z`, `0-9`, and `_` characters + /// - Must not be empty and not be longer than `RMW_NODE_NAME_MAX_NAME_LENGTH` + /// - Must not start with a number + /// + /// Note that node name validation is delayed until [`Executor::create_node`][3]. + /// + /// # Example + /// ``` + /// # use rclrs::{Context, NodeOptions, RclrsError, RclReturnCode}; + /// let executor = Context::default().create_basic_executor(); + /// // This is a valid node name + /// assert!(executor.create_node(NodeOptions::new("my_node")).is_ok()); + /// // This is another valid node name (although not a good one) + /// assert!(executor.create_node(NodeOptions::new("_______")).is_ok()); + /// // This is an invalid node name + /// assert!(matches!( + /// executor.create_node(NodeOptions::new("röböt")).unwrap_err(), + /// RclrsError::RclError { code: RclReturnCode::NodeInvalidName, .. } + /// )); + /// # Ok::<(), RclrsError>(()) + /// ``` + /// + /// [1]: crate::Node#naming + /// [2]: https://docs.ros2.org/latest/api/rmw/validate__node__name_8h.html#a5690a285aed9735f89ef11950b6e39e3 + /// [3]: crate::Executor::create_node + pub fn new(name: &'a str) -> NodeOptions<'a> { + NodeOptions { + name, + namespace: "/", + use_global_arguments: true, + arguments: vec![], + enable_rosout: true, + start_parameter_services: true, + clock_type: ClockType::RosTime, + clock_qos: QOS_PROFILE_CLOCK, + } + } + + /// Builds the node instance. + /// + /// Only used internally. Downstream users should call + /// [`Executor::create_node`]. + pub(crate) fn build(self, commands: &Arc) -> Result { + let node_name = CString::new(self.name).map_err(|err| RclrsError::StringContainsNul { + err, + s: self.name.to_owned(), + })?; let node_namespace = - CString::new(self.namespace.as_str()).map_err(|err| RclrsError::StringContainsNul { + CString::new(self.namespace).map_err(|err| RclrsError::StringContainsNul { err, - s: self.namespace.clone(), + s: self.namespace.to_owned(), })?; let rcl_node_options = self.create_rcl_node_options()?; - let rcl_context = &mut *self.context.rcl_context.lock().unwrap(); + let rcl_context = &mut *commands.context().handle.rcl_context.lock().unwrap(); let handle = Arc::new(NodeHandle { // SAFETY: Getting a zero-initialized value is always safe. rcl_node: Mutex::new(unsafe { rcl_get_zero_initialized_node() }), - context_handle: Arc::clone(&self.context), + context_handle: Arc::clone(&commands.context().handle), initialized: AtomicBool::new(false), }); @@ -334,23 +360,51 @@ impl NodeBuilder { } }; - let node = Arc::new(Node { - handle, - clients_mtx: Mutex::new(vec![]), - guard_conditions_mtx: Mutex::new(vec![]), - services_mtx: Mutex::new(vec![]), - subscriptions_mtx: Mutex::new(vec![]), + // --- Set up guard condition for graph change events --- + let (graph_change_action, graph_change_receiver) = unbounded(); + let graph_change_execute_sender = graph_change_action.clone(); + + let rcl_graph_change_guard_condition = unsafe { + // SAFETY: The node is valid because we just instantiated it. + rcl_node_get_graph_guard_condition(&*handle.rcl_node.lock().unwrap()) + }; + let (graph_change_guard_condition, graph_change_waitable) = unsafe { + // SAFETY: The guard condition is owned by the rcl_node and will + // remain valid for as long as the rcl_node is alive, so we set the + // owner to be the Arc for the NodeHandle. + GuardCondition::from_rcl( + &commands.context().handle, + rcl_graph_change_guard_condition, + Box::new(Arc::clone(&handle)), + Some(Box::new(move || { + graph_change_execute_sender + .unbounded_send(NodeGraphAction::GraphChange) + .ok(); + })), + ) + }; + commands.add_to_wait_set(graph_change_waitable); + let _ = commands.run(node_graph_task( + graph_change_receiver, + graph_change_guard_condition, + )); + + let node = Arc::new(NodeState { time_source: TimeSource::builder(self.clock_type) .clock_qos(self.clock_qos) .build(), parameter, logger: Logger::new(logger_name)?, + graph_change_action, + commands: Arc::clone(&commands), + handle, }); - node.time_source.attach_node(&node); + if self.start_parameter_services { node.parameter.create_services(&node)?; } + Ok(node) } @@ -395,6 +449,24 @@ impl NodeBuilder { } } +impl<'a> IntoNodeOptions<'a> for NodeOptions<'a> { + fn into_node_options(self) -> NodeOptions<'a> { + self + } +} + +impl<'a, T: Borrow> IntoNodeOptions<'a> for &'a T { + fn into_node_options(self) -> NodeOptions<'a> { + NodeOptions::new(self.borrow()) + } +} + +impl<'a> IntoNodeOptions<'a> for &'a str { + fn into_node_options(self) -> NodeOptions<'a> { + NodeOptions::new(self) + } +} + impl Drop for rcl_node_options_t { fn drop(&mut self) { // SAFETY: Do not finish this struct except here. diff --git a/rclrs/src/node/primitive_options.rs b/rclrs/src/node/primitive_options.rs new file mode 100644 index 000000000..0299c70f0 --- /dev/null +++ b/rclrs/src/node/primitive_options.rs @@ -0,0 +1,253 @@ +use crate::{ + QoSDurabilityPolicy, QoSDuration, QoSHistoryPolicy, QoSLivelinessPolicy, QoSProfile, + QoSReliabilityPolicy, +}; + +use std::{borrow::Borrow, time::Duration}; + +/// `PrimitiveOptions` are the subset of options that are relevant across all +/// primitives (e.g. [`Subscription`][1], [`Publisher`][2], [`Client`][3], and +/// [`Service`][4]). +/// +/// Each different primitive type may have its own defaults for the overall +/// quality of service settings, and we cannot know what the default will be +/// until the `PrimitiveOptions` gets converted into the more specific set of +/// options. Therefore we store each quality of service field separately so that +/// we will only override the settings that the user explicitly asked for, and +/// the rest will be determined by the default settings for each primitive. +/// +/// [1]: crate::Subscription +/// [2]: crate::Publisher +/// [3]: crate::Client +/// [4]: crate::Service +#[derive(Debug, Clone, Copy)] +#[non_exhaustive] +pub struct PrimitiveOptions<'a> { + /// The name that will be used for the primitive + pub name: &'a str, + /// Override the default [`QoSProfile::history`] for the primitive. + pub history: Option, + /// Override the default [`QoSProfile::reliability`] for the primitive. + pub reliability: Option, + /// Override the default [`QoSProfile::durability`] for the primitive. + pub durability: Option, + /// Override the default [`QoSProfile::deadline`] for the primitive. + pub deadline: Option, + /// Override the default [`QoSProfile::lifespan`] for the primitive. + pub lifespan: Option, + /// Override the default [`QoSProfile::liveliness`] for the primitive. + pub liveliness: Option, + /// Override the default [`QoSProfile::liveliness_lease`] for the primitive. + pub liveliness_lease: Option, + /// Override the default [`QoSProfile::avoid_ros_namespace_conventions`] for the primitive. + pub avoid_ros_namespace_conventions: Option, +} + +/// Trait to implicitly convert a compatible object into [`PrimitiveOptions`]. +pub trait IntoPrimitiveOptions<'a>: Sized { + /// Convert the object into [`PrimitiveOptions`] with default settings. + fn into_primitive_options(self) -> PrimitiveOptions<'a>; + + /// Override all the quality of service settings for the primitive. + fn qos(self, profile: QoSProfile) -> PrimitiveOptions<'a> { + self.into_primitive_options().history(profile.history) + } + + /// Use the default topics quality of service profile. + fn topics_qos(self) -> PrimitiveOptions<'a> { + self.qos(QoSProfile::topics_default()) + } + + /// Use the default sensor data quality of service profile. + fn sensor_data_qos(self) -> PrimitiveOptions<'a> { + self.qos(QoSProfile::sensor_data_default()) + } + + /// Use the default services quality of service profile. + fn services_qos(self) -> PrimitiveOptions<'a> { + self.qos(QoSProfile::services_default()) + } + + /// Use the system-defined default quality of service profile. This profile + /// is determined by the underlying RMW implementation, so you cannot rely + /// on this profile being consistent or appropriate for your needs. + fn system_qos(self) -> PrimitiveOptions<'a> { + self.qos(QoSProfile::system_default()) + } + + /// Override the default [`QoSProfile::history`] for the primitive. + fn history(self, history: QoSHistoryPolicy) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.history = Some(history); + options + } + + /// Keep the last `depth` messages for the primitive. + fn keep_last(self, depth: u32) -> PrimitiveOptions<'a> { + self.history(QoSHistoryPolicy::KeepLast { depth }) + } + + /// Keep all messages for the primitive. + fn keep_all(self) -> PrimitiveOptions<'a> { + self.history(QoSHistoryPolicy::KeepAll) + } + + /// Override the default [`QoSProfile::reliability`] for the primitive. + fn reliability(self, reliability: QoSReliabilityPolicy) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.reliability = Some(reliability); + options + } + + /// Set the primitive to have [reliable][QoSReliabilityPolicy::Reliable] communication. + fn reliable(self) -> PrimitiveOptions<'a> { + self.reliability(QoSReliabilityPolicy::Reliable) + } + + /// Set the primitive to have [best-effort][QoSReliabilityPolicy::BestEffort] communication. + fn best_effort(self) -> PrimitiveOptions<'a> { + self.reliability(QoSReliabilityPolicy::BestEffort) + } + + /// Override the default [`QoSProfile::durability`] for the primitive. + fn durability(self, durability: QoSDurabilityPolicy) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.durability = Some(durability); + options + } + + /// Set the primitive to have [volatile][QoSDurabilityPolicy::Volatile] durability. + fn volatile(self) -> PrimitiveOptions<'a> { + self.durability(QoSDurabilityPolicy::Volatile) + } + + /// Set the primitive to have [transient local][QoSDurabilityPolicy::TransientLocal] durability. + fn transient_local(self) -> PrimitiveOptions<'a> { + self.durability(QoSDurabilityPolicy::TransientLocal) + } + + /// Override the default [`QoSProfile::lifespan`] for the primitive. + fn lifespan(self, lifespan: QoSDuration) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.lifespan = Some(lifespan); + options + } + + /// Set a custom duration for the [lifespan][QoSProfile::lifespan] of the primitive. + fn lifespan_duration(self, duration: Duration) -> PrimitiveOptions<'a> { + self.lifespan(QoSDuration::Custom(duration)) + } + + /// Make the [lifespan][QoSProfile::lifespan] of the primitive infinite. + fn infinite_lifespan(self) -> PrimitiveOptions<'a> { + self.lifespan(QoSDuration::Infinite) + } + + /// Override the default [`QoSProfile::deadline`] for the primitive. + fn deadline(self, deadline: QoSDuration) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.deadline = Some(deadline); + options + } + + /// Set the [`QoSProfile::deadline`] to a custom finite value. + fn deadline_duration(self, duration: Duration) -> PrimitiveOptions<'a> { + self.deadline(QoSDuration::Custom(duration)) + } + + /// Do not use a deadline for liveliness for this primitive. + fn no_deadline(self) -> PrimitiveOptions<'a> { + self.deadline(QoSDuration::Infinite) + } + + /// Override the default [`QoSProfile::liveliness_lease`] for the primitive. + fn liveliness_lease(self, lease: QoSDuration) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.liveliness_lease = Some(lease); + options + } + + /// Set a custom duration for the [liveliness lease][QoSProfile::liveliness_lease]. + fn liveliness_lease_duration(self, duration: Duration) -> PrimitiveOptions<'a> { + self.liveliness_lease(QoSDuration::Custom(duration)) + } + + /// [Avoid the ROS namespace conventions][1] for the primitive. + /// + /// [1]: QoSProfile::avoid_ros_namespace_conventions + fn avoid_ros_namespace_conventions(self) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.avoid_ros_namespace_conventions = Some(true); + options + } +} + +impl<'a> IntoPrimitiveOptions<'a> for PrimitiveOptions<'a> { + fn into_primitive_options(self) -> PrimitiveOptions<'a> { + self + } +} + +impl<'a> IntoPrimitiveOptions<'a> for &'a str { + fn into_primitive_options(self) -> PrimitiveOptions<'a> { + PrimitiveOptions::new(self) + } +} + +impl<'a, T: Borrow> IntoPrimitiveOptions<'a> for &'a T { + fn into_primitive_options(self) -> PrimitiveOptions<'a> { + self.borrow().into_primitive_options() + } +} + +impl<'a> PrimitiveOptions<'a> { + /// Begin building a new set of `PrimitiveOptions` with only the name set. + pub fn new(name: &'a str) -> Self { + Self { + name, + history: None, + reliability: None, + durability: None, + deadline: None, + lifespan: None, + liveliness: None, + liveliness_lease: None, + avoid_ros_namespace_conventions: None, + } + } + + /// Apply the user-specified options to a pre-initialized [`QoSProfile`]. + pub fn apply(&self, qos: &mut QoSProfile) { + if let Some(history) = self.history { + qos.history = history; + } + + if let Some(reliability) = self.reliability { + qos.reliability = reliability; + } + + if let Some(durability) = self.durability { + qos.durability = durability; + } + + if let Some(deadline) = self.deadline { + qos.deadline = deadline; + } + + if let Some(lifespan) = self.lifespan { + qos.lifespan = lifespan; + } + + if let Some(liveliness) = self.liveliness { + qos.liveliness = liveliness; + } + + if let Some(liveliness_lease) = self.liveliness_lease { + qos.liveliness_lease = liveliness_lease; + } + + if let Some(convention) = self.avoid_ros_namespace_conventions { + qos.avoid_ros_namespace_conventions = convention; + } + } +} diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index 2a0829eac..2a6913a5f 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -84,7 +84,7 @@ enum DeclaredValue { } /// Builder used to declare a parameter. Obtain this by calling -/// [`crate::Node::declare_parameter`]. +/// [`crate::NodeState::declare_parameter`]. #[must_use] pub struct ParameterBuilder<'a, T: ParameterVariant> { name: Arc, @@ -874,18 +874,25 @@ impl ParameterInterface { #[cfg(test)] mod tests { use super::*; - use crate::{create_node, Context}; + use crate::{Context, InitOptions}; #[test] fn test_parameter_override_errors() { // Create a new node with a few parameter overrides - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("declared_int:=10"), - ]) - .unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); // Declaring a parameter with a different type than what was overridden should return an // error @@ -931,19 +938,26 @@ mod tests { #[test] fn test_parameter_setting_declaring() { // Create a new node with a few parameter overrides - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("declared_int:=10"), - String::from("-p"), - String::from("double_array:=[1.0, 2.0]"), - String::from("-p"), - String::from("optional_bool:=true"), - String::from("-p"), - String::from("non_declared_string:='param'"), - ]) - .unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + String::from("-p"), + String::from("double_array:=[1.0, 2.0]"), + String::from("-p"), + String::from("optional_bool:=true"), + String::from("-p"), + String::from("non_declared_string:='param'"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); let overridden_int = node .declare_parameter("declared_int") @@ -1087,13 +1101,20 @@ mod tests { #[test] fn test_override_undeclared_set_priority() { - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("declared_int:=10"), - ]) - .unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); // If a parameter was set as an override and as an undeclared parameter, the undeclared // value should get priority node.use_undeclared_parameters() @@ -1109,13 +1130,20 @@ mod tests { #[test] fn test_parameter_scope_redeclaring() { - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("declared_int:=10"), - ]) - .unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); { // Setting a parameter with an override let param = node @@ -1160,8 +1188,10 @@ mod tests { #[test] fn test_parameter_ranges() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); // Setting invalid ranges should fail let range = ParameterRange { lower: Some(10), @@ -1288,8 +1318,10 @@ mod tests { #[test] fn test_readonly_parameters() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); let param = node .declare_parameter("int_param") .default(100) @@ -1315,8 +1347,10 @@ mod tests { #[test] fn test_preexisting_value_error() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); node.use_undeclared_parameters() .set("int_param", 100) .unwrap(); @@ -1368,8 +1402,10 @@ mod tests { #[test] fn test_optional_parameter_apis() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node(&format!("param_test_node_{}", line!())) + .unwrap(); node.declare_parameter::("int_param") .optional() .unwrap(); diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 7c8ffe62d..2d7d644d1 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -9,24 +9,24 @@ use rosidl_runtime_rs::Sequence; use super::ParameterMap; use crate::{ parameter::{DeclaredValue, ParameterKind, ParameterStorage}, - rmw_request_id_t, Node, RclrsError, Service, + IntoPrimitiveOptions, Node, QoSProfile, RclrsError, Service, }; // The variables only exist to keep a strong reference to the services and are technically unused. // What is used is the Weak that is stored in the node, and is upgraded when spinning. pub struct ParameterService { #[allow(dead_code)] - describe_parameters_service: Arc>, + describe_parameters_service: Service, #[allow(dead_code)] - get_parameter_types_service: Arc>, + get_parameter_types_service: Service, #[allow(dead_code)] - get_parameters_service: Arc>, + get_parameters_service: Service, #[allow(dead_code)] - list_parameters_service: Arc>, + list_parameters_service: Service, #[allow(dead_code)] - set_parameters_service: Arc>, + set_parameters_service: Service, #[allow(dead_code)] - set_parameters_atomically_service: Arc>, + set_parameters_atomically_service: Service, } fn describe_parameters( @@ -247,47 +247,48 @@ impl ParameterService { // destruction is made for the parameter map. let map = parameter_map.clone(); let describe_parameters_service = node.create_service( - &(fqn.clone() + "/describe_parameters"), - move |_req_id: &rmw_request_id_t, req: DescribeParameters_Request| { + (fqn.clone() + "/describe_parameters").qos(QoSProfile::parameter_services_default()), + move |req: DescribeParameters_Request| { let map = map.lock().unwrap(); describe_parameters(req, &map) }, )?; let map = parameter_map.clone(); let get_parameter_types_service = node.create_service( - &(fqn.clone() + "/get_parameter_types"), - move |_req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { + (fqn.clone() + "/get_parameter_types").qos(QoSProfile::parameter_services_default()), + move |req: GetParameterTypes_Request| { let map = map.lock().unwrap(); get_parameter_types(req, &map) }, )?; let map = parameter_map.clone(); let get_parameters_service = node.create_service( - &(fqn.clone() + "/get_parameters"), - move |_req_id: &rmw_request_id_t, req: GetParameters_Request| { + (fqn.clone() + "/get_parameters").qos(QoSProfile::parameter_services_default()), + move |req: GetParameters_Request| { let map = map.lock().unwrap(); get_parameters(req, &map) }, )?; let map = parameter_map.clone(); let list_parameters_service = node.create_service( - &(fqn.clone() + "/list_parameters"), - move |_req_id: &rmw_request_id_t, req: ListParameters_Request| { + (fqn.clone() + "/list_parameters").qos(QoSProfile::parameter_services_default()), + move |req: ListParameters_Request| { let map = map.lock().unwrap(); list_parameters(req, &map) }, )?; let map = parameter_map.clone(); let set_parameters_service = node.create_service( - &(fqn.clone() + "/set_parameters"), - move |_req_id: &rmw_request_id_t, req: SetParameters_Request| { + (fqn.clone() + "/set_parameters").qos(QoSProfile::parameter_services_default()), + move |req: SetParameters_Request| { let mut map = map.lock().unwrap(); set_parameters(req, &mut map) }, )?; let set_parameters_atomically_service = node.create_service( - &(fqn.clone() + "/set_parameters_atomically"), - move |_req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { + (fqn.clone() + "/set_parameters_atomically") + .qos(QoSProfile::parameter_services_default()), + move |req: SetParametersAtomically_Request| { let mut map = parameter_map.lock().unwrap(); set_parameters_atomically(req, &mut map) }, @@ -312,39 +313,30 @@ mod tests { }, srv::rmw::*, }, - Context, MandatoryParameter, Node, NodeBuilder, ParameterRange, ParameterValue, RclrsError, - ReadOnlyParameter, + Context, Executor, IntoNodeOptions, MandatoryParameter, Node, NodeOptions, ParameterRange, + ParameterValue, RclrsError, ReadOnlyParameter, SpinOptions, RclrsErrorFilter, }; use rosidl_runtime_rs::{seq, Sequence}; - use std::sync::{Arc, RwLock}; + use std::{ + sync::{ + atomic::{AtomicBool, Ordering}, + Arc, + }, + time::Duration, + }; struct TestNode { - node: Arc, + node: Node, bool_param: MandatoryParameter, _ns_param: MandatoryParameter, _read_only_param: ReadOnlyParameter, dynamic_param: MandatoryParameter, } - async fn try_until_timeout(f: F) -> Result<(), ()> - where - F: FnOnce() -> bool + Copy, - { - let mut retry_count = 0; - while !f() { - if retry_count > 50 { - return Err(()); - } - tokio::time::sleep(tokio::time::Duration::from_millis(10)).await; - retry_count += 1; - } - Ok(()) - } - - fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { - let node = NodeBuilder::new(context, "node") - .namespace(ns) - .build() + fn construct_test_nodes(ns: &str) -> (Executor, TestNode, Node) { + let executor = Context::default().create_basic_executor(); + let node = executor + .create_node(NodeOptions::new("node").namespace(ns)) .unwrap(); let range = ParameterRange { lower: Some(0), @@ -375,12 +367,12 @@ mod tests { .mandatory() .unwrap(); - let client = NodeBuilder::new(context, "client") - .namespace(ns) - .build() + let client = executor + .create_node(NodeOptions::new("client").namespace(ns)) .unwrap(); ( + executor, TestNode { node, bool_param, @@ -394,552 +386,542 @@ mod tests { #[test] fn test_parameter_services_names_and_types() -> Result<(), RclrsError> { - let context = Context::new([]).unwrap(); - let (node, _client) = construct_test_nodes(&context, "names_types"); + let (mut executor, test, _client) = construct_test_nodes("names_types"); + + // Avoid flakiness while also finishing faster in most cases by giving + // this more maximum time but checking each time a graph change is detected. + let timeout = Duration::from_secs(1); + let initial_time = std::time::Instant::now(); + + let node = Arc::clone(&test.node); + let promise = + test.node + .notify_on_graph_change_with_period(Duration::from_millis(1), move || { + let mut not_finished = false; + let max_time_reached = initial_time.elapsed() > timeout; + let mut check = |condition: bool| { + if max_time_reached { + assert!(condition); + } else { + not_finished &= !condition; + } + }; + + let names_and_types = node.get_service_names_and_types().unwrap(); + let types = names_and_types + .get("/names_types/node/describe_parameters") + .unwrap(); + check(!types.contains(&"rcl_interfaces/srv/DescribeParameters".to_string())); + let types = names_and_types + .get("/names_types/node/get_parameters") + .unwrap(); + check(!types.contains(&"rcl_interfaces/srv/GetParameters".to_string())); + let types = names_and_types + .get("/names_types/node/set_parameters") + .unwrap(); + check(!types.contains(&"rcl_interfaces/srv/SetParameters".to_string())); + let types = names_and_types + .get("/names_types/node/set_parameters_atomically") + .unwrap(); + check( + !types.contains(&"rcl_interfaces/srv/SetParametersAtomically".to_string()), + ); + let types = names_and_types + .get("/names_types/node/list_parameters") + .unwrap(); + check(types.contains(&"rcl_interfaces/srv/ListParameters".to_string())); + let types = names_and_types + .get("/names_types/node/get_parameter_types") + .unwrap(); + check(types.contains(&"rcl_interfaces/srv/GetParameterTypes".to_string())); + !not_finished + }); - std::thread::sleep(std::time::Duration::from_millis(100)); + executor + .spin( + SpinOptions::new() + .until_promise_resolved(promise) + .timeout(Duration::from_secs(1)), + ) + .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 context = Context::new([]).unwrap(); - let (node, client) = construct_test_nodes(&context, "list"); - let list_client = client.create_client::("/list/node/list_parameters")?; + #[test] + fn test_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(); - try_until_timeout(|| list_client.service_is_ready().unwrap()) - .await + 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 done = Arc::new(RwLock::new(false)); + 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(); - let inner_done = done.clone(); - let rclrs_spin = tokio::task::spawn(async move { - try_until_timeout(|| { - crate::spin_once(node.node.clone(), Some(std::time::Duration::ZERO)).ok(); - crate::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); - *inner_done.read().unwrap() + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .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; - dbg!(&names); - assert_eq!(names.len(), 2); - assert_eq!(names[0].to_string(), "bool"); - assert_eq!(names[1].to_string(), "use_sim_time"); - assert_eq!(response.result.prefixes.len(), 0); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - *done.write().unwrap() = true; - }); - - res.await.unwrap(); - rclrs_spin.await.unwrap(); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .first_error()?; + assert!(callback_ran.load(Ordering::Acquire)); Ok(()) } - #[tokio::test] - async fn test_get_set_parameters_service() -> Result<(), RclrsError> { - let context = Context::new([]).unwrap(); - let (node, client) = construct_test_nodes(&context, "get_set"); - let get_client = client.create_client::("/get_set/node/get_parameters")?; - let set_client = client.create_client::("/get_set/node/set_parameters")?; - let set_atomically_client = client + #[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 done = Arc::new(RwLock::new(false)); - - let inner_node = node.node.clone(); - let inner_done = done.clone(); - let rclrs_spin = tokio::task::spawn(async move { - try_until_timeout(|| { - crate::spin_once(inner_node.clone(), Some(std::time::Duration::ZERO)).ok(); - crate::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); - *inner_done.read().unwrap() + let get_client_inner = Arc::clone(&get_client); + let set_client_inner = Arc::clone(&set_client); + let set_atomically_client_inner = Arc::clone(&set_atomically_client); + let clients_ready_condition = move || { + get_client_inner.service_is_ready().unwrap() + && set_client_inner.service_is_ready().unwrap() + && set_atomically_client_inner.service_is_ready().unwrap() + }; + + let clients_ready = client_node + .notify_on_graph_change_with_period(Duration::from_millis(1), clients_ready_condition); + + executor + .spin(SpinOptions::default().until_promise_resolved(clients_ready)) + .first_error()?; + + // 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 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)); + + // 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(); + + 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!(node.bool_param.get()); - set_client - .async_send_request_with_callback( - &request, - move |response: SetParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.results.len(), 7); - // Setting a bool value set for a bool parameter - assert!(response.results[0].successful); - // Value was set to false, node parameter get should reflect this - assert!(!node.bool_param.get()); - // Setting a parameter to the wrong type - assert!(!response.results[1].successful); - // Setting a read only parameter - assert!(!response.results[2].successful); - // Setting a dynamic parameter to a new type - assert!(response.results[3].successful); - assert_eq!(node.dynamic_param.get(), ParameterValue::Bool(true)); - // Setting a value out of range - assert!(!response.results[4].successful); - // Setting an invalid type - assert!(!response.results[5].successful); - // Setting an undeclared parameter, without allowing undeclared parameters - assert!(!response.results[6].successful); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - // Set the node to use undeclared parameters and try to set one - node.node.use_undeclared_parameters(); - let request = SetParameters_Request { - parameters: seq![undeclared_bool], - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - set_client - .async_send_request_with_callback( - &request, - move |response: SetParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.results.len(), 1); - // Setting the undeclared parameter is now allowed - assert!(response.results[0].successful); - assert_eq!( - node.node.use_undeclared_parameters().get("undeclared_bool"), - Some(ParameterValue::Bool(true)) - ); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - // With set_parameters_atomically, if one fails all should fail - let request = SetParametersAtomically_Request { - parameters: seq![bool_parameter, read_only_parameter], - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - set_atomically_client - .async_send_request_with_callback( - &request, - move |response: SetParametersAtomically_Response| { - *call_done.write().unwrap() = true; - assert!(!response.result.successful); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - *done.write().unwrap() = true; - }); - - res.await.unwrap(); - rclrs_spin.await.unwrap(); + ) + .unwrap(); + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .first_error()?; + assert!(callback_ran.load(Ordering::Acquire)); Ok(()) } - #[tokio::test] - async fn test_describe_get_types_parameters_service() -> Result<(), RclrsError> { - let context = Context::new([]).unwrap(); - let (node, client) = construct_test_nodes(&context, "describe"); - let describe_client = - client.create_client::("/describe/node/describe_parameters")?; + #[test] + fn test_describe_get_types_parameters_service() -> Result<(), RclrsError> { + let (mut executor, _test, client_node) = construct_test_nodes("describe"); + let describe_client = client_node + .create_client::("/describe/node/describe_parameters")?; let get_types_client = - client.create_client::("/describe/node/get_parameter_types")?; + client_node.create_client::("/describe/node/get_parameter_types")?; - try_until_timeout(|| { - describe_client.service_is_ready().unwrap() - && get_types_client.service_is_ready().unwrap() - }) - .await - .unwrap(); - - let done = Arc::new(RwLock::new(false)); - - let inner_done = done.clone(); - let inner_node = node.node.clone(); - let rclrs_spin = tokio::task::spawn(async move { - try_until_timeout(|| { - crate::spin_once(inner_node.clone(), Some(std::time::Duration::ZERO)).ok(); - crate::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); - *inner_done.read().unwrap() + let describe_client_inner = Arc::clone(&describe_client); + let get_types_client_inner = Arc::clone(&get_types_client); + let clients_ready_condition = move || { + describe_client_inner.service_is_ready().unwrap() + && get_types_client_inner.service_is_ready().unwrap() + }; + + let promise = client_node + .notify_on_graph_change_with_period(Duration::from_millis(1), clients_ready_condition); + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .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); }) - .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)); + + // If a describe parameters request is sent with a non existing parameter, an empty + // response should be returned + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let request = DescribeParameters_Request { + names: seq!["bool".into(), "non_existing".into()], + }; + let promise = describe_client + .call_then(&request, move |response: DescribeParameters_Response| { + assert_eq!(response.descriptors[0].name.to_string(), "bool"); + assert_eq!(response.descriptors[0].type_, ParameterType::PARAMETER_BOOL); + assert_eq!(response.descriptors.len(), 2); + assert_eq!(response.descriptors[1].name.to_string(), "non_existing"); + assert_eq!( + response.descriptors[1].type_, + ParameterType::PARAMETER_NOT_SET + ); + callback_ran_inner.store(true, Ordering::Release); + }) + .unwrap(); + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .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); + }) + .unwrap(); + + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .first_error()?; + assert!(callback_ran.load(Ordering::Acquire)); Ok(()) } diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index 82fe31ebb..ff0c86c46 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -537,7 +537,7 @@ impl ParameterValue { #[cfg(test)] mod tests { use super::*; - use crate::{Context, RclrsError, ToResult}; + use crate::{Context, InitOptions, RclrsError, ToResult}; // TODO(luca) tests for all from / to ParameterVariant functions @@ -565,11 +565,14 @@ mod tests { ), ]; for pair in input_output_pairs { - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - format!("foo:={}", pair.0), - ])?; + let ctx = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + format!("foo:={}", pair.0), + ], + InitOptions::default(), + )?; let mut rcl_params = std::ptr::null_mut(); unsafe { rcl_arguments_get_param_overrides( diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index b1cdd93b9..0eef2adfa 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -11,7 +11,7 @@ use crate::{ error::{RclrsError, ToResult}, qos::QoSProfile, rcl_bindings::*, - NodeHandle, ENTITY_LIFECYCLE_MUTEX, + IntoPrimitiveOptions, NodeHandle, ENTITY_LIFECYCLE_MUTEX, }; mod loaned_message; @@ -45,15 +45,32 @@ impl Drop for PublisherHandle { /// Struct for sending messages of type `T`. /// +/// Create a publisher using [`Node::create_publisher`][1]. +/// /// Multiple publishers can be created for the same topic, in different nodes or the same node. +/// A clone of a `Publisher` will refer to the same publisher instance as the original. +/// The underlying instance is tied to [`PublisherState`] which implements the [`Publisher`] API. /// /// The underlying RMW will decide on the concrete delivery mechanism (network stack, shared /// memory, or intraprocess). /// -/// Sending messages does not require calling [`spin`][1] on the publisher's node. +/// Sending messages does not require the node's executor to [spin][2]. +/// +/// [1]: crate::NodeState::create_publisher +/// [2]: crate::Executor::spin +pub type Publisher = Arc>; + +/// The inner state of a [`Publisher`]. +/// +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Publisher`] in a non-owning way. It is +/// generally recommended to manage the `PublisherState` inside of an [`Arc`], +/// and [`Publisher`] is provided as a convenience alias for that. +/// +/// The public API of the [`Publisher`] type is implemented via `PublisherState`. /// -/// [1]: crate::spin -pub struct Publisher +/// [1]: std::sync::Weak +pub struct PublisherState where T: Message, { @@ -66,26 +83,26 @@ where // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread // they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for Publisher where T: Message {} +unsafe impl Send for PublisherState where T: Message {} // SAFETY: The type_support_ptr prevents the default Sync impl. // rosidl_message_type_support_t is a read-only type without interior mutability. -unsafe impl Sync for Publisher where T: Message {} +unsafe impl Sync for PublisherState where T: Message {} -impl Publisher +impl PublisherState where T: Message, { /// Creates a new `Publisher`. /// /// Node and namespace changes are always applied _before_ topic remapping. - pub(crate) fn new( + pub(crate) fn create<'a>( + options: impl Into>, node_handle: Arc, - topic: &str, - qos: QoSProfile, - ) -> Result + ) -> Result, RclrsError> where T: Message, { + let PublisherOptions { topic, qos } = options.into(); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_publisher = unsafe { rcl_get_zero_initialized_publisher() }; let type_support_ptr = @@ -120,14 +137,14 @@ where } } - Ok(Self { + Ok(Arc::new(Self { type_support_ptr, message: PhantomData, handle: PublisherHandle { rcl_publisher: Mutex::new(rcl_publisher), node_handle, }, - }) + })) } /// Returns the topic name of the publisher. @@ -179,7 +196,7 @@ where } } -impl Publisher +impl PublisherState where T: RmwMessage, { @@ -236,7 +253,39 @@ where } } -/// Convenience trait for [`Publisher::publish`]. +/// `PublisherOptions` are used by [`Node::create_publisher`][1] to initialize +/// a [`Publisher`]. +/// +/// [1]: crate::Node::create_publisher +#[derive(Debug, Clone)] +#[non_exhaustive] +pub struct PublisherOptions<'a> { + /// The topic name for the publisher. + pub topic: &'a str, + /// The quality of service settings for the publisher. + pub qos: QoSProfile, +} + +impl<'a> PublisherOptions<'a> { + /// Initialize a new [`PublisherOptions`] with default settings. + pub fn new(topic: &'a str) -> Self { + Self { + topic, + qos: QoSProfile::topics_default(), + } + } +} + +impl<'a, T: IntoPrimitiveOptions<'a>> From for PublisherOptions<'a> { + fn from(value: T) -> Self { + let primitive = value.into_primitive_options(); + let mut options = Self::new(primitive.name); + primitive.apply(&mut options.qos); + options + } +} + +/// Convenience trait for [`PublisherState::publish`]. pub trait MessageCow<'a, T: Message> { /// Wrap the owned or borrowed message in a `Cow`. fn into_cow(self) -> Cow<'a, T>; @@ -267,7 +316,7 @@ mod tests { #[test] fn test_publishers() -> Result<(), RclrsError> { - use crate::{TopicEndpointInfo, QOS_PROFILE_SYSTEM_DEFAULT}; + use crate::TopicEndpointInfo; use test_msgs::msg; let namespace = "/test_publishers_graph"; @@ -275,16 +324,15 @@ mod tests { let node_1_empty_publisher = graph .node1 - .create_publisher::("graph_test_topic_1", QOS_PROFILE_SYSTEM_DEFAULT)?; + .create_publisher::("graph_test_topic_1")?; let topic1 = node_1_empty_publisher.topic_name(); - let node_1_basic_types_publisher = graph.node1.create_publisher::( - "graph_test_topic_2", - QOS_PROFILE_SYSTEM_DEFAULT, - )?; + let node_1_basic_types_publisher = graph + .node1 + .create_publisher::("graph_test_topic_2")?; let topic2 = node_1_basic_types_publisher.topic_name(); let node_2_default_publisher = graph .node2 - .create_publisher::("graph_test_topic_3", QOS_PROFILE_SYSTEM_DEFAULT)?; + .create_publisher::("graph_test_topic_3")?; let topic3 = node_2_default_publisher.topic_name(); std::thread::sleep(std::time::Duration::from_millis(100)); diff --git a/rclrs/src/publisher/loaned_message.rs b/rclrs/src/publisher/loaned_message.rs index 7d29122dc..924e7d21e 100644 --- a/rclrs/src/publisher/loaned_message.rs +++ b/rclrs/src/publisher/loaned_message.rs @@ -2,13 +2,13 @@ use std::ops::{Deref, DerefMut}; use rosidl_runtime_rs::RmwMessage; -use crate::{rcl_bindings::*, Publisher, RclrsError, ToResult}; +use crate::{rcl_bindings::*, PublisherState, RclrsError, ToResult}; /// A message that is owned by the middleware, loaned for publishing. /// /// It dereferences to a `&mut T`. /// -/// This type is returned by [`Publisher::borrow_loaned_message()`], see the documentation of +/// This type is returned by [`PublisherState::borrow_loaned_message()`], see the documentation of /// that function for more information. /// /// The loan is returned by dropping the message or [publishing it][1]. @@ -19,7 +19,7 @@ where T: RmwMessage, { pub(super) msg_ptr: *mut T, - pub(super) publisher: &'a Publisher, + pub(super) publisher: &'a PublisherState, } impl Deref for LoanedMessage<'_, T> diff --git a/rclrs/src/qos.rs b/rclrs/src/qos.rs index b26f01ef8..699576964 100644 --- a/rclrs/src/qos.rs +++ b/rclrs/src/qos.rs @@ -166,7 +166,7 @@ pub struct QoSProfile { /// The time within which the RMW publisher must show that it is alive. /// /// If this is `Infinite`, liveliness is not enforced. - pub liveliness_lease_duration: QoSDuration, + pub liveliness_lease: QoSDuration, /// If true, any ROS specific namespacing conventions will be circumvented. /// /// In the case of DDS and topics, for example, this means the typical @@ -200,7 +200,7 @@ impl From for rmw_qos_profile_t { deadline: qos.deadline.into(), lifespan: qos.lifespan.into(), liveliness: qos.liveliness.into(), - liveliness_lease_duration: qos.liveliness_lease_duration.into(), + liveliness_lease_duration: qos.liveliness_lease.into(), avoid_ros_namespace_conventions: qos.avoid_ros_namespace_conventions, } } @@ -244,22 +244,54 @@ impl QoSProfile { } /// Sets the QoS profile deadline to the specified `Duration`. - pub fn deadline(mut self, deadline: Duration) -> Self { + pub fn deadline_duration(mut self, deadline: Duration) -> Self { self.deadline = QoSDuration::Custom(deadline); self } /// Sets the QoS profile liveliness lease duration to the specified `Duration`. pub fn liveliness_lease_duration(mut self, lease_duration: Duration) -> Self { - self.liveliness_lease_duration = QoSDuration::Custom(lease_duration); + self.liveliness_lease = QoSDuration::Custom(lease_duration); self } /// Sets the QoS profile lifespan to the specified `Duration`. - pub fn lifespan(mut self, lifespan: Duration) -> Self { + pub fn lifespan_duration(mut self, lifespan: Duration) -> Self { self.lifespan = QoSDuration::Custom(lifespan); self } + + /// Get the default QoS profile for ordinary topics. + pub fn topics_default() -> Self { + QOS_PROFILE_DEFAULT + } + + /// Get the default QoS profile for topics that transmit sensor data. + pub fn sensor_data_default() -> Self { + QOS_PROFILE_SENSOR_DATA + } + + /// Get the default QoS profile for services. + pub fn services_default() -> Self { + QOS_PROFILE_SERVICES_DEFAULT + } + + /// Get the default QoS profile for parameter services. + pub fn parameter_services_default() -> Self { + QOS_PROFILE_PARAMETERS + } + + /// Get the default QoS profile for parameter event topics. + pub fn parameter_events_default() -> Self { + QOS_PROFILE_PARAMETER_EVENTS + } + + /// Get the system-defined default quality of service profile. This profile + /// is determined by the underlying RMW implementation, so you cannot rely + /// on this profile being consistent or appropriate for your needs. + pub fn system_default() -> Self { + QOS_PROFILE_SYSTEM_DEFAULT + } } impl From for rmw_qos_history_policy_t { @@ -355,7 +387,7 @@ pub const QOS_PROFILE_SENSOR_DATA: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -370,7 +402,7 @@ pub const QOS_PROFILE_CLOCK: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -384,7 +416,7 @@ pub const QOS_PROFILE_PARAMETERS: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -398,7 +430,7 @@ pub const QOS_PROFILE_DEFAULT: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -412,7 +444,7 @@ pub const QOS_PROFILE_SERVICES_DEFAULT: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -426,7 +458,7 @@ pub const QOS_PROFILE_PARAMETER_EVENTS: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -440,6 +472,6 @@ pub const QOS_PROFILE_SYSTEM_DEFAULT: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; diff --git a/rclrs/src/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 ac43e51a8..e3c0010e6 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -1,107 +1,122 @@ use std::{ boxed::Box, - ffi::CString, - sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, + ffi::{CStr, CString}, + sync::{Arc, Mutex, MutexGuard}, + any::Any, }; -use rosidl_runtime_rs::Message; +use rosidl_runtime_rs::{Message, Service as IdlService}; use crate::{ - error::{RclReturnCode, ToResult}, - rcl_bindings::*, - MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + error::ToResult, rcl_bindings::*, WorkerCommands, IntoPrimitiveOptions, NodeHandle, + QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, Waitable, + WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, MessageCow, Node, Worker, WorkScope, }; -// 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::*; -/// Main class responsible for responding to requests sent by ROS clients. +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] +/// or [`Node::create_async_service`][2]. /// -/// The only available way to instantiate services is via [`Node::create_service()`][1], this is to -/// ensure that [`Node`][2]s can track all the services that have been created. +/// ROS only supports having one service provider for any given fully-qualified +/// service name. "Fully-qualified" means the namespace is also taken into account +/// for uniqueness. A clone of a `Service` will refer to the same service provider +/// instance as the original. The underlying instance is tied to [`ServiceState`] +/// which implements the [`Service`] API. +/// +/// Responding to requests requires the node's executor to [spin][3]. /// /// [1]: crate::Node::create_service -/// [2]: crate::Node -pub struct Service +/// [2]: crate::Node::create_async_service +/// [3]: crate::Executor::spin +/// +pub type Service = Arc>; + +/// Provide a [`Service`] that runs on a [`Worker`]. +/// +/// Create a worker service using [`Worker::create_service`]. +pub type WorkerService = Arc>>; + +/// The inner state of a [`Service`]. +/// +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Service`] in a non-owning way. It is +/// generally recommended to manage the `ServiceState` inside of an [`Arc`], +/// and [`Service`] is provided as a convenience alias for that. +/// +/// The public API of the [`Service`] type is implemented via `ServiceState`. +/// +/// [1]: std::sync::Weak +pub struct ServiceState where - T: rosidl_runtime_rs::Service, + T: IdlService, + Scope: WorkScope, { - pub(crate) handle: Arc, - /// The callback function that runs when a request was received. - pub callback: Mutex>, + /// This handle is used to access the data that rcl holds for this service. + handle: Arc, + /// This is the callback that will be executed each time a request arrives. + callback: Arc>>, + /// Holding onto this keeps the waiter for this service alive in the wait + /// set of the executor. + #[allow(unused)] + lifecycle: WaitableLifecycle, } -impl Service +impl ServiceState where - T: rosidl_runtime_rs::Service, + T: IdlService, + Scope: WorkScope + 'static, { - /// Creates a new service. - pub(crate) fn new( - node_handle: Arc, - topic: &str, - callback: F, - ) -> Result - // This uses pub(crate) visibility to avoid instantiating this struct outside - // [`Node::create_service`], see the struct's documentation for the rationale - where - T: rosidl_runtime_rs::Service, - F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send, - { + /// Returns the name of the service. + /// + /// This returns the service name after remapping, so it is not necessarily the + /// service name which was used when creating the service. + pub fn service_name(&self) -> String { + self.handle.service_name() + } + + /// Used by [`Node`][crate::Node] to create a new service + pub(crate) fn create<'a>( + options: impl Into>, + callback: AnyServiceCallback, + node_handle: &Arc, + commands: &Arc, + ) -> Result, RclrsError> { + let ServiceOptions { name, qos } = options.into(); + let callback = Arc::new(Mutex::new(callback)); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_service = unsafe { rcl_get_zero_initialized_service() }; let type_support = ::get_type_support() as *const rosidl_service_type_support_t; - let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { + let topic_c_string = CString::new(name).map_err(|err| RclrsError::StringContainsNul { err, - s: topic.into(), + s: name.into(), })?; // SAFETY: No preconditions for this function. - let service_options = unsafe { rcl_service_get_default_options() }; + let mut service_options = unsafe { rcl_service_get_default_options() }; + service_options.qos = qos.into(); { let rcl_node = node_handle.rcl_node.lock().unwrap(); @@ -127,14 +142,157 @@ where let handle = Arc::new(ServiceHandle { rcl_service: Mutex::new(rcl_service), - node_handle, - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + node_handle: Arc::clone(&node_handle), }); - Ok(Self { + let (waitable, lifecycle) = Waitable::new( + Box::new(ServiceExecutable:: { + handle: Arc::clone(&handle), + callback: Arc::clone(&callback), + commands: Arc::clone(&commands), + }), + Some(Arc::clone(commands.get_guard_condition())), + ); + + let service = Arc::new(Self { handle, - callback: Mutex::new(Box::new(callback)), - }) + callback, + lifecycle, + }); + commands.add_to_wait_set(waitable); + + Ok(service) + } +} + +impl ServiceState { + /// Set the callback of this service, replacing the callback that was + /// previously set. + /// + /// This can be used even if the service previously used an async callback. + /// + /// 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(); + // TODO(@mxgrey): Log any errors here when logging becomes available + *self.callback.lock().unwrap() = callback; + } + + /// Set the callback of this service, replacing the callback that was + /// previously set. + /// + /// This can be used even if the service previously used a non-async callback. + /// + /// 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; + } +} + +/// `ServiceOptions are used by [`Node::create_service`][1] to initialize a +/// [`Service`] provider. +#[derive(Debug, Clone)] +#[non_exhaustive] +pub struct ServiceOptions<'a> { + /// The name for the service + pub name: &'a str, + /// The quality of service profile for the service. + pub qos: QoSProfile, +} + +impl<'a> ServiceOptions<'a> { + /// Initialize a new [`ServiceOptions`] with default settings. + pub fn new(name: &'a str) -> Self { + Self { + name, + qos: QoSProfile::services_default(), + } + } +} + +impl<'a, T: IntoPrimitiveOptions<'a>> From for ServiceOptions<'a> { + fn from(value: T) -> Self { + let primitive = value.into_primitive_options(); + let mut options = Self::new(primitive.name); + primitive.apply(&mut options.qos); + options + } +} + +struct ServiceExecutable { + handle: Arc, + callback: Arc>>, + commands: Arc, +} + +impl RclPrimitive for ServiceExecutable +where + T: IdlService, + Scope: WorkScope, +{ + unsafe fn execute(&mut self, payload: &mut dyn Any) -> Result<(), RclrsError> { + self.callback + .lock() + .unwrap() + .execute(&self.handle, payload, &self.commands) + } + + 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. @@ -159,17 +317,13 @@ where // | 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; + 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.handle.lock(); + let handle = &*self.lock(); unsafe { - // SAFETY: The three pointers are valid/initialized + // SAFETY: The three pointers are valid and initialized rcl_take_request( handle, &mut request_id_out, @@ -179,37 +333,38 @@ where .ok()?; Ok((T::Request::from_rmw_message(request_out), request_id_out)) } -} -impl ServiceBase for Service -where - T: rosidl_runtime_rs::Service, -{ - fn handle(&self) -> &ServiceHandle { - &self.handle + /// 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 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 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 _, ) } @@ -217,6 +372,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::*; @@ -242,14 +410,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..eec243dc9 --- /dev/null +++ b/rclrs/src/service/any_service_callback.rs @@ -0,0 +1,47 @@ +use rosidl_runtime_rs::Service; + +use crate::{WorkerCommands, RclrsError, ServiceHandle, NodeServiceCallback, 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..5cc53b3c6 --- /dev/null +++ b/rclrs/src/service/into_async_service_callback.rs @@ -0,0 +1,57 @@ +use rosidl_runtime_rs::Service; + +use super::{AnyServiceCallback, NodeServiceCallback, RequestId, ServiceInfo}; + +use std::future::Future; + +/// A trait for async callbacks of services. +/// +// TODO(@mxgrey): Add a description of what callbacks signatures are supported +pub trait 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..c9d2b7b13 --- /dev/null +++ b/rclrs/src/service/into_node_service_callback.rs @@ -0,0 +1,65 @@ +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. +// +// TODO(@mxgrey): Add a description of what callbacks signatures are supported +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..5a7f5fd1d --- /dev/null +++ b/rclrs/src/service/into_worker_service_callback.rs @@ -0,0 +1,95 @@ +use rosidl_runtime_rs::Service; + +use crate::{ + AnyServiceCallback, WorkerServiceCallback, RequestId, ServiceInfo, + Worker, +}; + +/// A trait used to deduce callbacks for services that run on a worker. +/// +/// Users of rclrs never need to use this trait directly. +// +// TODO(@mxgrey): Add a description of what callback signatures are supported +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..df0ce33b0 --- /dev/null +++ b/rclrs/src/service/node_service_callback.rs @@ -0,0 +1,83 @@ +use rosidl_runtime_rs::Service; + +use crate::{ + rcl_bindings::rmw_request_id_t, + WorkerCommands, RclrsError, RequestId, ServiceHandle, ServiceInfo, + log_error, ToLogParams, RclrsErrorFilter, +}; + +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..04eb89173 --- /dev/null +++ b/rclrs/src/service/service_info.rs @@ -0,0 +1,78 @@ +use std::time::SystemTime; + +use crate::{rcl_bindings::*, timestamp_to_system_time}; + +/// Information about an incoming service request. +#[derive(Debug, Clone, Hash, PartialEq, Eq, PartialOrd, Ord)] +pub struct ServiceInfo { + /// Time when the message was published by the publisher. + /// + /// The `rmw` layer does not specify the exact point at which the RMW implementation + /// must take the timestamp, but it should be taken consistently at the same point in the + /// process of publishing a message. + pub source_timestamp: Option, + /// Time when the message was received by the service node. + /// + /// The `rmw` layer does not specify the exact point at which the RMW implementation + /// must take the timestamp, but it should be taken consistently at the same point in the + /// process of receiving a message. + pub received_timestamp: Option, + /// Unique identifier for the request. + pub request_id: RequestId, +} + +impl ServiceInfo { + pub(crate) fn from_rmw_service_info(rmw_service_info: &rmw_service_info_t) -> Self { + Self { + source_timestamp: timestamp_to_system_time(rmw_service_info.source_timestamp), + received_timestamp: timestamp_to_system_time(rmw_service_info.received_timestamp), + request_id: RequestId::from_rmw_request_id(&rmw_service_info.request_id), + } + } + + pub(crate) fn zero_initialized_rmw() -> rmw_service_info_t { + rmw_service_info_t { + source_timestamp: 0, + received_timestamp: 0, + request_id: RequestId::zero_initialized_rmw(), + } + } +} + +/// Unique identifier for a service request. +/// +/// Individually each field in the `RequestId` may be repeated across different +/// requests, but the combination of both values will be unique per request. +#[derive(Debug, Clone, Hash, PartialEq, Eq, PartialOrd, Ord)] +pub struct RequestId { + /// A globally unique identifier for the writer of the request. + pub writer_guid: [i8; 16usize], + /// A number assigned to the request which is unique for the writer who + /// wrote the request. + pub sequence_number: i64, +} + +impl RequestId { + pub(crate) fn from_rmw_request_id(rmw_request_id: &rmw_request_id_t) -> Self { + Self { + writer_guid: rmw_request_id.writer_guid, + sequence_number: rmw_request_id.sequence_number, + } + } + + pub(crate) fn zero_initialized_rmw() -> rmw_request_id_t { + rmw_request_id_t { + writer_guid: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + sequence_number: 0, + } + } +} + +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..590b9f493 --- /dev/null +++ b/rclrs/src/service/worker_service_callback.rs @@ -0,0 +1,72 @@ +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 fbd518c21..e1194884f 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -1,109 +1,124 @@ 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::*, - NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + error::ToResult, qos::QoSProfile, rcl_bindings::*, WorkerCommands, IntoPrimitiveOptions, + Node, NodeHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, Waitable, + WaitableLifecycle, Worker, WorkScope, 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()`][2] +/// or [`Node::create_async_subscription`][3]. +/// /// There can be multiple subscriptions for the same topic, in different nodes or the same node. +/// A clone of a `Subscription` will refer to the same subscription instance as the original. +/// The underlying instance is tied to [`SubscriptionState`] which implements the [`Subscription`] API. /// -/// Receiving messages requires calling [`spin_once`][1] or [`spin`][2] on the subscription's node. +/// Receiving messages requires calling [`spin`][1] on the `Executor` of subscription's [Node][4]. /// /// When a subscription is created, it may take some time to get "matched" with a corresponding /// publisher. /// -/// The only available way to instantiate subscriptions is via [`Node::create_subscription()`][3], this -/// is to ensure that [`Node`][4]s can track all the subscriptions that have been created. -/// -/// [1]: crate::spin_once -/// [2]: crate::spin -/// [3]: crate::Node::create_subscription +/// [1]: crate::Executor::spin +/// [2]: crate::Node::create_subscription +/// [3]: crate::Node::create_async_subscription /// [4]: crate::Node -pub struct Subscription +pub type Subscription = Arc>; + +/// A [`Subscription`] that runs on a [`Worker`]. +/// +/// Create a worker subscription using [`Worker::create_subscription`]. +pub type WorkerSubscription = Arc>>; + +/// The inner state of a [`Subscription`]. +/// +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Subscription`] in a non-owning way. It is +/// generally recommended to manage the `SubscriptionState` inside of an [`Arc`], +/// and [`Subscription`] is provided as a convenience alias for that. +/// +/// The public API of the [`Subscription`] type is implemented via `SubscriptionState`. +/// +/// [1]: std::sync::Weak +pub struct SubscriptionState where T: Message, + Scope: WorkScope, { - pub(crate) handle: Arc, - /// The callback function that runs when a message was received. - pub callback: Mutex>, - message: PhantomData, + /// This handle is used to access the data that rcl holds for this subscription. + handle: Arc, + /// This allows us to replace the callback in the subscription task. + /// + /// Holding onto this sender will keep the subscription task alive. Once + /// this sender is dropped, the subscription task will end itself. + callback: Arc>>, + /// Holding onto this keeps the waiter for this subscription alive in the + /// wait set of the executor. + #[allow(unused)] + lifecycle: WaitableLifecycle, } -impl Subscription +impl SubscriptionState where T: Message, + Scope: WorkScope, { - /// Creates a new subscription. - pub(crate) fn new( - node_handle: Arc, - topic: &str, - qos: QoSProfile, - callback: impl SubscriptionCallback, - ) -> Result - // This uses pub(crate) visibility to avoid instantiating this struct outside - // [`Node::create_subscription`], see the struct's documentation for the rationale - where - T: Message, - { + /// Returns the topic name of the subscription. + /// + /// This returns the topic name after remapping, so it is not necessarily the + /// topic name which was used when creating the subscription. + pub fn topic_name(&self) -> String { + // SAFETY: The subscription handle is valid because its lifecycle is managed by an Arc. + // The unsafe variables get converted to safe types before being returned + unsafe { + let raw_topic_pointer = rcl_subscription_get_topic_name(&*self.handle.lock()); + CStr::from_ptr(raw_topic_pointer) + } + .to_string_lossy() + .into_owned() + } + + /// Used by [`Node`][crate::Node] to create a new subscription. + pub(crate) fn create<'a>( + options: impl Into>, + callback: AnySubscriptionCallback, + node_handle: &Arc, + commands: &Arc, + ) -> Result, RclrsError> { + let SubscriptionOptions { topic, qos } = options.into(); + let callback = Arc::new(Mutex::new(callback)); + // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_subscription = unsafe { rcl_get_zero_initialized_subscription() }; let type_support = @@ -114,8 +129,8 @@ where })?; // SAFETY: No preconditions for this function. - let mut subscription_options = unsafe { rcl_subscription_get_default_options() }; - subscription_options.qos = qos.into(); + let mut rcl_subscription_options = unsafe { rcl_subscription_get_default_options() }; + rcl_subscription_options.qos = qos.into(); { let rcl_node = node_handle.rcl_node.lock().unwrap(); @@ -132,7 +147,7 @@ where &*rcl_node, type_support, topic_c_string.as_ptr(), - &subscription_options, + &rcl_subscription_options, ) .ok()?; } @@ -140,31 +155,151 @@ where let handle = Arc::new(SubscriptionHandle { rcl_subscription: Mutex::new(rcl_subscription), - node_handle, - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + node_handle: Arc::clone(node_handle), }); - Ok(Self { + let (waitable, lifecycle) = Waitable::new( + Box::new(SubscriptionExecutable { + handle: Arc::clone(&handle), + callback: Arc::clone(&callback), + commands: Arc::clone(commands), + }), + Some(Arc::clone(commands.get_guard_condition())), + ); + commands.add_to_wait_set(waitable); + + Ok(Arc::new(Self { handle, - callback: Mutex::new(callback.into_callback()), - message: PhantomData, - }) + callback, + lifecycle, + })) } +} - /// Returns the topic name of the subscription. +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::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(&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. /// @@ -188,18 +323,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)); @@ -207,12 +342,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. @@ -238,14 +373,16 @@ where /// /// [1]: crate::RclrsError /// [2]: crate::Publisher::borrow_loaned_message - pub fn take_loaned(&self) -> Result<(ReadOnlyLoanedMessage<'_, T>, MessageInfo), RclrsError> { + 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(), @@ -254,7 +391,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, @@ -263,57 +400,15 @@ where } } -impl SubscriptionBase for Subscription -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); } } } @@ -326,33 +421,29 @@ mod tests { #[test] fn traits() { - assert_send::>(); - assert_sync::>(); + assert_send::>(); + assert_sync::>(); } #[test] fn test_subscriptions() -> Result<(), RclrsError> { - use crate::{TopicEndpointInfo, QOS_PROFILE_SYSTEM_DEFAULT}; + use crate::TopicEndpointInfo; let namespace = "/test_subscriptions_graph"; let graph = construct_test_graph(namespace)?; - let node_2_empty_subscription = graph.node2.create_subscription::( - "graph_test_topic_1", - QOS_PROFILE_SYSTEM_DEFAULT, - |_msg: msg::Empty| {}, - )?; + let node_2_empty_subscription = graph + .node2 + .create_subscription::("graph_test_topic_1", |_msg: msg::Empty| {})?; let topic1 = node_2_empty_subscription.topic_name(); let node_2_basic_types_subscription = graph.node2.create_subscription::( "graph_test_topic_2", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::BasicTypes| {}, )?; let topic2 = node_2_basic_types_subscription.topic_name(); let node_1_defaults_subscription = graph.node1.create_subscription::( "graph_test_topic_3", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::Defaults| {}, )?; let topic3 = node_1_defaults_subscription.topic_name(); diff --git a/rclrs/src/subscription/any_subscription_callback.rs b/rclrs/src/subscription/any_subscription_callback.rs new file mode 100644 index 000000000..709abea5a --- /dev/null +++ b/rclrs/src/subscription/any_subscription_callback.rs @@ -0,0 +1,49 @@ +use rosidl_runtime_rs::Message; + +use crate::{ + subscription::SubscriptionHandle, WorkerCommands, RclrsError, + NodeSubscriptionCallback, 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..bc6fca887 --- /dev/null +++ b/rclrs/src/subscription/into_async_subscription_callback.rs @@ -0,0 +1,198 @@ +use rosidl_runtime_rs::Message; + +use super::{ + AnySubscriptionCallback, MessageInfo, NodeSubscriptionCallback, +}; +use crate::ReadOnlyLoanedMessage; + +use std::future::Future; + +/// A trait for async callbacks of subscriptions. +/// +// TODO(@mxgrey): Add a description of what callback signatures are supported +pub trait 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..603df1c19 --- /dev/null +++ b/rclrs/src/subscription/into_node_subscription_callback.rs @@ -0,0 +1,224 @@ +use rosidl_runtime_rs::Message; + +use crate::{ + ReadOnlyLoanedMessage, AnySubscriptionCallback, MessageInfo, NodeSubscriptionCallback, +}; + +use std::sync::Arc; + +/// A trait for regular callbacks of subscriptions. +// +// TODO(@mxgrey): Add a description of what callbacks signatures are supported +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..63ed920f0 --- /dev/null +++ b/rclrs/src/subscription/into_worker_subscription_callback.rs @@ -0,0 +1,155 @@ +use rosidl_runtime_rs::Message; + +use crate::{ReadOnlyLoanedMessage, MessageInfo, AnySubscriptionCallback, WorkerSubscriptionCallback}; + +/// A trait for callbacks of subscriptions that run on a worker. +// +// TODO(@mxgrey): Add a description of what callbacks signatures are supported +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..bbb3701bc --- /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::{WorkerCommands, RclrsError, ReadOnlyLoanedMessage, RclrsErrorFilter}; + +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 c6f52e280..9fae1db11 100644 --- a/rclrs/src/subscription/readonly_loaned_message.rs +++ b/rclrs/src/subscription/readonly_loaned_message.rs @@ -1,8 +1,8 @@ -use std::ops::Deref; +use std::{ops::Deref, sync::Arc}; use rosidl_runtime_rs::Message; -use crate::{rcl_bindings::*, Subscription, ToResult}; +use crate::{rcl_bindings::*, subscription::SubscriptionHandle, ToResult}; /// A message that is owned by the middleware, loaned out for reading. /// @@ -14,15 +14,15 @@ use crate::{rcl_bindings::*, Subscription, ToResult}; /// subscription callbacks. /// /// The loan is returned by dropping the `ReadOnlyLoanedMessage`. -pub struct ReadOnlyLoanedMessage<'a, T> +pub struct ReadOnlyLoanedMessage where T: Message, { pub(super) msg_ptr: *const T::RmwMsg, - pub(super) subscription: &'a Subscription, + pub(super) handle: Arc, } -impl Deref for ReadOnlyLoanedMessage<'_, T> +impl Deref for ReadOnlyLoanedMessage where T: Message, { @@ -32,14 +32,14 @@ where } } -impl Drop for ReadOnlyLoanedMessage<'_, T> +impl Drop for ReadOnlyLoanedMessage where T: Message, { fn drop(&mut self) { unsafe { rcl_return_loaned_message_from_subscription( - &*self.subscription.handle.lock(), + &*self.handle.lock(), self.msg_ptr as *mut _, ) .ok() @@ -50,9 +50,9 @@ where // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread // they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for ReadOnlyLoanedMessage<'_, T> where T: Message {} +unsafe impl Send for ReadOnlyLoanedMessage where T: Message {} // SAFETY: This type has no interior mutability, in fact it has no mutability at all. -unsafe impl Sync for ReadOnlyLoanedMessage<'_, T> where T: Message {} +unsafe impl Sync for ReadOnlyLoanedMessage where T: Message {} #[cfg(test)] mod tests { diff --git a/rclrs/src/subscription/worker_subscription_callback.rs b/rclrs/src/subscription/worker_subscription_callback.rs new file mode 100644 index 000000000..4c04cc7e1 --- /dev/null +++ b/rclrs/src/subscription/worker_subscription_callback.rs @@ -0,0 +1,81 @@ +use rosidl_runtime_rs::Message; + +use crate::{ + subscription::SubscriptionHandle, RclrsError, + ReadOnlyLoanedMessage, MessageInfo, RclrsErrorFilter, +}; + +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/test_helpers/graph_helpers.rs b/rclrs/src/test_helpers/graph_helpers.rs index 1e9b581ae..f61f5db96 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -1,19 +1,14 @@ -use crate::{Context, Node, NodeBuilder, RclrsError}; -use std::sync::Arc; +use crate::{Context, IntoNodeOptions, Node, RclrsError}; pub(crate) struct TestGraph { - pub node1: Arc, - pub node2: Arc, + pub node1: Node, + pub node2: Node, } pub(crate) fn construct_test_graph(namespace: &str) -> Result { - let context = Context::new([])?; + let executor = Context::default().create_basic_executor(); Ok(TestGraph { - node1: NodeBuilder::new(&context, "graph_test_node_1") - .namespace(namespace) - .build()?, - node2: NodeBuilder::new(&context, "graph_test_node_2") - .namespace(namespace) - .build()?, + node1: executor.create_node("graph_test_node_1".namespace(namespace))?, + node2: executor.create_node("graph_test_node_2".namespace(namespace))?, }) } diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index 0be0c07ec..d8165b937 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -1,7 +1,8 @@ use crate::{ clock::{Clock, ClockSource, ClockType}, vendor::rosgraph_msgs::msg::Clock as ClockMsg, - Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, + IntoPrimitiveOptions, Node, NodeState, QoSProfile, ReadOnlyParameter, Subscription, + QOS_PROFILE_CLOCK, }; use std::sync::{Arc, Mutex, RwLock, Weak}; @@ -9,12 +10,12 @@ use std::sync::{Arc, Mutex, RwLock, Weak}; /// If the node's `use_sim_time` parameter is set to `true`, the `TimeSource` will subscribe /// to the `/clock` topic and drive the attached clock pub(crate) struct TimeSource { - node: Mutex>, + node: Mutex>, clock: RwLock, clock_source: Arc>>, requested_clock_type: ClockType, clock_qos: QoSProfile, - clock_subscription: Mutex>>>, + clock_subscription: Mutex>>, last_received_time: Arc>>, // TODO(luca) Make this parameter editable when we have parameter callbacks implemented and can // safely change clock type at runtime @@ -85,7 +86,7 @@ impl TimeSource { /// Attaches the given node to to the `TimeSource`, using its interface to read the /// `use_sim_time` parameter and create the clock subscription. - pub(crate) fn attach_node(&self, node: &Arc) { + pub(crate) fn attach_node(&self, node: &Node) { // TODO(luca) Make this parameter editable and register a parameter callback // that calls set_ros_time(bool) once parameter callbacks are implemented. let param = node @@ -122,7 +123,7 @@ impl TimeSource { clock.set_ros_time_override(nanoseconds); } - fn create_clock_sub(&self) -> Arc> { + fn create_clock_sub(&self) -> Subscription { let clock = self.clock_source.clone(); let last_received_time = self.last_received_time.clone(); // Safe to unwrap since the function will only fail if invalid arguments are provided @@ -131,42 +132,51 @@ impl TimeSource { .unwrap() .upgrade() .unwrap() - .create_subscription::("/clock", self.clock_qos, move |msg: ClockMsg| { - let nanoseconds: i64 = - (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64; - *last_received_time.lock().unwrap() = Some(nanoseconds); - if let Some(clock) = clock.lock().unwrap().as_mut() { - Self::update_clock(clock, nanoseconds); - } - }) + .create_subscription::( + "/clock".qos(self.clock_qos), + move |msg: ClockMsg| { + let nanoseconds: i64 = + (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64; + *last_received_time.lock().unwrap() = Some(nanoseconds); + if let Some(clock) = clock.lock().unwrap().as_mut() { + Self::update_clock(clock, nanoseconds); + } + }, + ) .unwrap() } } #[cfg(test)] mod tests { - use crate::{create_node, Context}; + use crate::{Context, InitOptions}; #[test] fn time_source_default_clock() { - let node = create_node( - &Context::new([]).unwrap(), - &format!("time_source_test_node_{}", line!()), - ) - .unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node(&format!("time_source_test_node_{}", line!())) + .unwrap(); // Default clock should be above 0 (use_sim_time is default false) assert!(node.get_clock().now().nsec > 0); } #[test] fn time_source_sim_time() { - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("use_sim_time:=true"), - ]) - .unwrap(); - let node = create_node(&ctx, &format!("time_source_test_node_{}", line!())).unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("use_sim_time:=true"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor + .create_node(&format!("time_source_test_node_{}", line!())) + .unwrap(); // Default sim time value should be 0 (no message received) assert_eq!(node.get_clock().now().nsec, 0); } diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs new file mode 100644 index 000000000..3101142ad --- /dev/null +++ b/rclrs/src/timer.rs @@ -0,0 +1,765 @@ +use crate::{ + clock::Clock, context::ContextHandle, error::RclrsError, log_error, rcl_bindings::*, + ToLogParams, ToResult, ENTITY_LIFECYCLE_MUTEX, Node, RclPrimitive, RclPrimitiveKind, + RclPrimitiveHandle, Worker, WorkerCommands, WorkScope, Waitable, WaitableLifecycle, +}; +// 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 execution of the callbacks is tied to [`spin_once`][1] or [`spin`][2] on the timers's node. +/// +/// Timer can be created via [`Node::create_timer()`][3], this is to ensure that [`Node`][4]s can +/// track all the timers that have been created. However, a user of a `Timer` can also +/// use it standalone. +/// +/// [1]: crate::spin_once +/// [2]: crate::spin +/// [3]: crate::Node::create_timer +/// [4]: crate::Node +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, +} + +/// 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, +} + +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()) + } +} + +/// '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()) }; + } +} + +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)) + } +} + +// 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 crate::*; + use super::TimerExecutable; + 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..faa45fa6e --- /dev/null +++ b/rclrs/src/timer/into_node_timer_callback.rs @@ -0,0 +1,67 @@ +use crate::{Time, Timer, AnyTimerCallback, Node}; + +/// 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