From 9b9de10c071c0366eb10a0a6c052a84fed658d29 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 7 Oct 2024 15:56:27 +0800 Subject: [PATCH 01/48] Drafting traits for generic executors Signed-off-by: Michael X. Grey --- rclrs/src/executor.rs | 61 ++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 60 insertions(+), 1 deletion(-) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 37c43a68e..245ac5e9b 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,8 +1,67 @@ -use crate::{rcl_bindings::rcl_context_is_valid, Node, RclReturnCode, RclrsError, WaitSet}; +use crate::{ + rcl_bindings::rcl_context_is_valid, + Node, RclReturnCode, RclrsError, WaitSet, ContextHandle, +}; use std::{ sync::{Arc, Mutex, Weak}, time::Duration, }; +pub use futures::channel::oneshot::Receiver as Promise; +use futures::future::BoxFuture; + +/// An executor that can be used to run nodes. +pub struct Executor { + context: Arc, + inner: Box, +} + +impl Executor { + + + + /// Creates a new executor using the provided runtime. Users of rclrs should + /// use [`Context::create_executor`]. + pub(crate) fn new(context: Arc, inner: E) -> Self + where + E: 'static + ExecutorRuntime, + { + Self { context, inner: Box::new(inner) } + } +} + +/// This allows commands, such as creating a new node, to be run on the executor +/// while the executor is spinning. +pub struct ExecutorCommands { + context: Arc, + channel: Box, +} + +/// This trait defines the interface for passing new items into an executor to +/// run. +pub trait ExecutorChannel { + /// Add a new item for the executor to run. + fn add(&self, f: BoxFuture<'static, ()>) -> Promise<()>; +} + +/// This trait defines the interface for having an executor run. +pub trait ExecutorRuntime { + /// Get a channel that can add new items for the executor to run. + fn channel(&self) -> Arc; + + /// Tell the executor to spin. + fn spin(&mut self, conditions: SpinConditions); +} + +/// A bundle of conditions that a user may want to impose on how long an +/// executor spins for. +#[non_exhaustive] +pub struct SpinConditions { + /// A limit on how many times the executor should spin before stopping. A + /// [`None`] value will allow the executor to keep spinning indefinitely. + pub spin_limit: Option, + /// The executor will stop spinning if the future is resolved. + pub until_future_resolved: BoxFuture<'static, ()>, +} /// Single-threaded executor implementation. pub struct SingleThreadedExecutor { From 2dba33088600032a86084a09c2aebac337dc4787 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 7 Oct 2024 23:11:05 +0800 Subject: [PATCH 02/48] Fleshing out interfaces for async execution Signed-off-by: Michael X. Grey --- rclrs/src/context.rs | 21 +++- rclrs/src/executor.rs | 125 +++++++++++++++++++--- rclrs/src/executor/basic_executor.rs | 34 ++++++ rclrs/src/lib.rs | 2 +- rclrs/src/node.rs | 23 +--- rclrs/src/node/{builder.rs => options.rs} | 44 ++++---- rclrs/src/parameter/service.rs | 6 +- rclrs/src/test_helpers/graph_helpers.rs | 6 +- 8 files changed, 197 insertions(+), 64 deletions(-) create mode 100644 rclrs/src/executor/basic_executor.rs rename rclrs/src/node/{builder.rs => options.rs} (92%) diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 90c8fbd3c..a34782930 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -6,7 +6,10 @@ use std::{ vec::Vec, }; -use crate::{rcl_bindings::*, RclrsError, ToResult}; +use crate::{ + rcl_bindings::*, RclrsError, ToResult, Executor, ExecutorRuntime, + BasicExecutorRuntime, +}; /// This is locked whenever initializing or dropping any middleware entity /// because we have found issues in RCL and some RMW implementations that @@ -150,6 +153,22 @@ impl Context { }) } + /// Create an executor that uses the [basic executor runtime][1] that comes + /// built into rclrs. + /// + /// [1]: BasicExecutorRuntime + pub fn create_basic_executor(&self) -> Executor { + self.create_executor(BasicExecutorRuntime::default()) + } + + /// Create an [`Executor`] for this context. + pub fn create_executor(&self, runtime: E) -> Executor + where + E: 'static + ExecutorRuntime, + { + Executor::new(Arc::clone(&self.handle), runtime) + } + /// Returns the ROS domain ID that the context is using. /// /// The domain ID controls which nodes can send messages to each other, see the [ROS 2 concept article][1]. diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 245ac5e9b..fe14d25d4 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,66 +1,163 @@ +mod basic_executor; +pub use self::basic_executor::*; + use crate::{ rcl_bindings::rcl_context_is_valid, - Node, RclReturnCode, RclrsError, WaitSet, ContextHandle, + Node, NodeOptions, RclReturnCode, RclrsError, WaitSet, Context, + ContextHandle, }; use std::{ sync::{Arc, Mutex, Weak}, time::Duration, + sync::atomic::{AtomicBool, Ordering}, + future::Future, }; pub use futures::channel::oneshot::Receiver as Promise; -use futures::future::BoxFuture; +use futures::{future::{BoxFuture, Either, select}, channel::oneshot}; -/// An executor that can be used to run nodes. +/// An executor that can be used to create nodes and run their callbacks. pub struct Executor { context: Arc, - inner: Box, + commands: Arc, + runtime: Box, } impl Executor { + /// Access the commands interface for this executor. Use the returned + /// [`ExecutorCommands`] to create [nodes][Node]. + pub fn commands(&self) -> &Arc { + &self.commands + } - + /// Spin the Executor. + pub fn spin(&mut self, options: SpinOptions) { + self.runtime.spin(SpinConditions { + options, + halt: Arc::clone(&self.commands.halt), + context: Context { handle: Arc::clone(&self.context) }, + }); + } /// Creates a new executor using the provided runtime. Users of rclrs should /// use [`Context::create_executor`]. - pub(crate) fn new(context: Arc, inner: E) -> Self + pub(crate) fn new(context: Arc, runtime: E) -> Self where E: 'static + ExecutorRuntime, { - Self { context, inner: Box::new(inner) } + let commands = Arc::new(ExecutorCommands { + context: Context { handle: Arc::clone(&context) }, + channel: runtime.channel(), + halt: Arc::new(AtomicBool::new(false)), + }); + + Self { + context, + commands, + runtime: Box::new(runtime), + } } } /// This allows commands, such as creating a new node, to be run on the executor /// while the executor is spinning. pub struct ExecutorCommands { - context: Arc, + context: Context, channel: Box, + halt: Arc, +} + +impl ExecutorCommands { + + pub fn create_node( + self: &Arc, + options: NodeOptions, + ) -> Result, RclrsError> { + options.build(self) + } + + /// Tell the [`Executor`] to halt its spinning. + pub fn halt(&self) { + self.halt.store(true, Ordering::Relaxed); + } + + /// Run a task on the [`Executor`]. If the returned [`Promise`] is dropped + /// then the task will stop running. + pub fn run(&self, f: F) -> Promise + where + F: 'static + Future + Send + Unpin, + F::Output: Send, + { + let (mut sender, receiver) = oneshot::channel(); + self.channel.add(Box::pin( + async move { + let cancellation = sender.cancellation(); + let result = select(cancellation, f).await; + let output = match result { + Either::Left(_) => return, + Either::Right((output, _)) => output, + }; + sender.send(output).ok(); + } + )); + + receiver + } + + pub fn context(&self) -> &Context { + &self.context + } } /// This trait defines the interface for passing new items into an executor to /// run. pub trait ExecutorChannel { /// Add a new item for the executor to run. - fn add(&self, f: BoxFuture<'static, ()>) -> Promise<()>; + fn add(&self, f: BoxFuture<'static, ()>); } /// This trait defines the interface for having an executor run. pub trait ExecutorRuntime { /// Get a channel that can add new items for the executor to run. - fn channel(&self) -> Arc; + fn channel(&self) -> Box; /// Tell the executor to spin. fn spin(&mut self, conditions: SpinConditions); + + fn async_spin( + self: Box, + conditions: SpinConditions, + ) -> BoxFuture<'static, Box>; } -/// A bundle of conditions that a user may want to impose on how long an -/// executor spins for. +/// 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] -pub struct SpinConditions { +#[derive(Default)] +pub struct SpinOptions { /// A limit on how many times the executor should spin before stopping. A /// [`None`] value will allow the executor to keep spinning indefinitely. pub spin_limit: Option, /// The executor will stop spinning if the future is resolved. - pub until_future_resolved: BoxFuture<'static, ()>, + pub until_future_resolved: Option>, +} + +/// 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 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: Arc, + /// Check ok to make sure that the context is still valid. When the context + /// is invalid, the executor runtime should stop spinning. + pub context: Context, } /// Single-threaded executor implementation. diff --git a/rclrs/src/executor/basic_executor.rs b/rclrs/src/executor/basic_executor.rs new file mode 100644 index 000000000..6ccf45f34 --- /dev/null +++ b/rclrs/src/executor/basic_executor.rs @@ -0,0 +1,34 @@ +use crate::executor::{ExecutorRuntime, ExecutorChannel, SpinConditions}; +use futures::future::BoxFuture; + +#[derive(Default)] +pub struct BasicExecutorRuntime { + +} + +impl ExecutorRuntime for BasicExecutorRuntime { + fn spin(&mut self, conditions: SpinConditions) { + + } + + fn async_spin( + self: Box, + conditions: SpinConditions, + ) -> BoxFuture<'static, Box> { + Box::pin(async move { self as Box }) + } + + fn channel(&self) -> Box { + Box::new(BasicExecutorChannel { }) + } +} + +pub struct BasicExecutorChannel { + +} + +impl ExecutorChannel for BasicExecutorChannel { + fn add(&self, f: futures::future::BoxFuture<'static, ()>) { + + } +} diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 4924b36cb..d05930ee3 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -108,6 +108,6 @@ pub fn create_node(context: &Context, node_name: &str) -> Result, Rclr /// assert_eq!(node.name(), "my_node"); /// # Ok::<(), RclrsError>(()) /// ``` -pub fn create_node_builder(context: &Context, node_name: &str) -> NodeBuilder { +pub fn create_node_builder(context: &Context, node_name: &str) -> NodeOptions { Node::builder(context, node_name) } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 97684d6bc..4fea581af 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,4 +1,4 @@ -mod builder; +mod options; mod graph; use std::{ cmp::PartialEq, @@ -11,7 +11,7 @@ use std::{ use rosidl_runtime_rs::Message; -pub use self::{builder::*, graph::*}; +pub use self::{options::*, graph::*}; use crate::{ rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, @@ -421,25 +421,6 @@ impl Node { interface: &self.parameter, } } - - /// Creates a [`NodeBuilder`][1] with the given name. - /// - /// Convenience function equivalent to [`NodeBuilder::new()`][2]. - /// - /// [1]: crate::NodeBuilder - /// [2]: crate::NodeBuilder::new - /// - /// # 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) - } } // Helper used to implement call_string_getter(), but also used to get the FQN in the Node::new() diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/options.rs similarity index 92% rename from rclrs/src/node/builder.rs rename to rclrs/src/node/options.rs index 011d5d2f3..6e3c01a8b 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/options.rs @@ -4,7 +4,8 @@ use std::{ }; use crate::{ - rcl_bindings::*, ClockType, Context, ContextHandle, Node, NodeHandle, ParameterInterface, + rcl_bindings::*, ClockType, Node, NodeHandle, ParameterInterface, + ExecutorCommands, QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, }; @@ -43,8 +44,7 @@ use crate::{ /// /// [1]: crate::Node /// [2]: crate::Node::builder -pub struct NodeBuilder { - context: Arc, +pub struct NodeOptions { name: String, namespace: String, use_global_arguments: bool, @@ -55,7 +55,7 @@ pub struct NodeBuilder { clock_qos: QoSProfile, } -impl NodeBuilder { +impl NodeOptions { /// Creates a builder for a node with the given name. /// /// See the [`Node` docs][1] for general information on node names. @@ -72,10 +72,10 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, NodeBuilder, RclrsError, RclReturnCode}; - /// let context = Context::new([])?; + /// # use rclrs::{Context, NodeOptions}; + /// let context = Context::new(); /// // This is a valid node name - /// assert!(NodeBuilder::new(&context, "my_node").build().is_ok()); + /// assert!(NodeOptions::new("my_node").is_valid()); /// // This is another valid node name (although not a good one) /// assert!(NodeBuilder::new(&context, "_______").build().is_ok()); /// // This is an invalid node name @@ -91,9 +91,8 @@ impl NodeBuilder { /// [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), + pub fn new(name: impl ToString) -> NodeOptions { + NodeOptions { name: name.to_string(), namespace: "/".to_string(), use_global_arguments: true, @@ -154,7 +153,7 @@ impl NodeBuilder { /// [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 { + pub fn namespace(mut self, namespace: impl ToString) -> Self { self.namespace = namespace.to_string(); self } @@ -218,8 +217,11 @@ impl NodeBuilder { /// /// [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(); + pub fn arguments(mut self, arguments: Args) -> Self + where + Args::Item: ToString, + { + self.arguments = arguments.into_iter().map(|item| item.to_string()).collect(); self } @@ -257,12 +259,12 @@ impl NodeBuilder { /// Builds the node instance. /// - /// Node name and namespace validation is performed in this method. - /// - /// For example usage, see the [`NodeBuilder`][1] docs. - /// - /// [1]: crate::NodeBuilder - pub fn build(&self) -> Result, RclrsError> { + /// Only used internally. Downstream users should call + /// [`ExecutorCommands::create_node`]. + pub(crate) fn build( + self, + commands: &Arc, + ) -> Result, RclrsError> { let node_name = CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul { err, @@ -274,7 +276,7 @@ impl NodeBuilder { s: self.namespace.clone(), })?; 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(); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_node = unsafe { rcl_get_zero_initialized_node() }; @@ -298,7 +300,7 @@ impl NodeBuilder { let handle = Arc::new(NodeHandle { rcl_node: Mutex::new(rcl_node), - context_handle: Arc::clone(&self.context), + context_handle: Arc::clone(&commands.context().handle), }); let parameter = { let rcl_node = handle.rcl_node.lock().unwrap(); diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 7c8ffe62d..62c409437 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -312,7 +312,7 @@ mod tests { }, srv::rmw::*, }, - Context, MandatoryParameter, Node, NodeBuilder, ParameterRange, ParameterValue, RclrsError, + Context, MandatoryParameter, Node, NodeOptions, ParameterRange, ParameterValue, RclrsError, ReadOnlyParameter, }; use rosidl_runtime_rs::{seq, Sequence}; @@ -342,7 +342,7 @@ mod tests { } fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { - let node = NodeBuilder::new(context, "node") + let node = NodeOptions::new(context, "node") .namespace(ns) .build() .unwrap(); @@ -375,7 +375,7 @@ mod tests { .mandatory() .unwrap(); - let client = NodeBuilder::new(context, "client") + let client = NodeOptions::new(context, "client") .namespace(ns) .build() .unwrap(); diff --git a/rclrs/src/test_helpers/graph_helpers.rs b/rclrs/src/test_helpers/graph_helpers.rs index 1e9b581ae..c1df36b65 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -1,4 +1,4 @@ -use crate::{Context, Node, NodeBuilder, RclrsError}; +use crate::{Context, Node, NodeOptions, RclrsError}; use std::sync::Arc; pub(crate) struct TestGraph { @@ -9,10 +9,10 @@ pub(crate) struct TestGraph { pub(crate) fn construct_test_graph(namespace: &str) -> Result { let context = Context::new([])?; Ok(TestGraph { - node1: NodeBuilder::new(&context, "graph_test_node_1") + node1: NodeOptions::new(&context, "graph_test_node_1") .namespace(namespace) .build()?, - node2: NodeBuilder::new(&context, "graph_test_node_2") + node2: NodeOptions::new(&context, "graph_test_node_2") .namespace(namespace) .build()?, }) From f10036ddc258ec8c5e4a75bd4c21ed504c981773 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 8 Oct 2024 17:00:43 +0800 Subject: [PATCH 03/48] Beginning to migrate subscriptions to async Signed-off-by: Michael X. Grey --- rclrs/src/context.rs | 2 + rclrs/src/executor.rs | 113 +++++++++++++--- rclrs/src/executor/basic_executor.rs | 6 +- rclrs/src/node.rs | 96 ++------------ rclrs/src/node/graph.rs | 9 +- rclrs/src/node/options.rs | 6 + rclrs/src/parameter/service.rs | 13 +- rclrs/src/subscription.rs | 26 ++-- rclrs/src/subscription/callback.rs | 125 +++++++++++------- .../subscription/readonly_loaned_message.rs | 18 +-- 10 files changed, 227 insertions(+), 187 deletions(-) diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index a34782930..9c2fcce25 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -199,6 +199,8 @@ impl Context { // SAFETY: No preconditions for this function. unsafe { rcl_context_is_valid(rcl_context) } } + + // TODO(@mxgrey): } /// Additional options for initializing the Context. diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index fe14d25d4..d0354b13b 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -29,13 +29,42 @@ impl Executor { &self.commands } - /// Spin the Executor. + /// Create a [`Node`] that will run on this Executor. + pub fn create_node( + &self, + options: impl Into, + ) -> Result, RclrsError> { + self.commands.create_node(options) + } + + /// 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) { - self.runtime.spin(SpinConditions { - options, - halt: Arc::clone(&self.commands.halt), - context: Context { handle: Arc::clone(&self.context) }, - }); + self.commands.halt.store(false, Ordering::Release); + let conditions = self.make_spin_conditions(options); + self.runtime.spin(conditions); + } + + /// Spin the Executor as an async task. This does not block the current thread. + /// It also does not prevent your `main` function from exiting while it spins, + /// so make sure you have a way to keep the application running. + /// + /// This will consume the Executor so that the task can run on other threads. + /// + /// The async task will run until the [`SpinConditions`] stop the Executor + /// from spinning. The output of the async task will be the restored Executor, + /// which you can use to resume spinning after the task is finished. + pub async fn spin_async(self, options: SpinOptions) -> Self { + self.commands.halt.store(false, Ordering::Release); + let conditions = self.make_spin_conditions(options); + let Self { context, commands, runtime } = self; + + let runtime = runtime.spin_async(conditions).await; + Self { context, commands, runtime } } /// Creates a new executor using the provided runtime. Users of rclrs should @@ -56,6 +85,14 @@ impl Executor { runtime: Box::new(runtime), } } + + fn make_spin_conditions(&self, options: SpinOptions) -> SpinConditions { + SpinConditions { + options, + halt: Arc::clone(&self.commands.halt), + context: Context { handle: Arc::clone(&self.context) }, + } + } } /// This allows commands, such as creating a new node, to be run on the executor @@ -67,33 +104,39 @@ pub struct ExecutorCommands { } impl ExecutorCommands { - + /// Create a new node that will run on the [`Executor`] that is being commanded. pub fn create_node( self: &Arc, - options: NodeOptions, + options: impl Into, ) -> Result, RclrsError> { + let options: NodeOptions = options.into(); options.build(self) } /// Tell the [`Executor`] to halt its spinning. pub fn halt(&self) { - self.halt.store(true, Ordering::Relaxed); + self.halt.store(true, Ordering::Release); + self.channel.wakeup(); } /// Run a task on the [`Executor`]. If the returned [`Promise`] is dropped - /// then the task will stop running. + /// then the task will be dropped, which means it might not run to + /// completion. + /// + /// You can `.await` the output of the promise in an async scope. pub fn run(&self, f: F) -> Promise where - F: 'static + Future + Send + Unpin, + F: 'static + Future + Send, F::Output: Send, { let (mut sender, receiver) = oneshot::channel(); self.channel.add(Box::pin( async move { let cancellation = sender.cancellation(); - let result = select(cancellation, f).await; - let output = match result { + 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(); @@ -103,9 +146,31 @@ impl ExecutorCommands { receiver } + /// Run a task on the [`Executor`]. The task will run to completion even if + /// you drop the returned [`Promise`]. + /// + /// You can `.await` the output of the promise in an async scope. + pub fn run_detached(&self, f: F) -> Promise + where + F: 'static + Future + Send, + F::Output: Send, + { + let (sender, receiver) = oneshot::channel(); + self.channel.add(Box::pin( + async move { + sender.send(f.await).ok(); + } + )); + receiver + } + pub fn context(&self) -> &Context { &self.context } + + pub(crate) fn add(&self, f: BoxFuture<'static, ()>) { + self.channel.add(f); + } } /// This trait defines the interface for passing new items into an executor to @@ -113,6 +178,8 @@ impl ExecutorCommands { pub trait ExecutorChannel { /// Add a new item for the executor to run. fn add(&self, f: BoxFuture<'static, ()>); + + // TODO(@mxgrey): create_guard_condition for waking up the waitset thread } /// This trait defines the interface for having an executor run. @@ -120,10 +187,15 @@ pub trait ExecutorRuntime { /// Get a channel that can add new items for the executor to run. fn channel(&self) -> Box; - /// Tell the executor to spin. + /// Tell the runtime to spin while blocking any further execution until the + /// spinning is complete. fn spin(&mut self, conditions: SpinConditions); - fn async_spin( + /// 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>; @@ -136,9 +208,9 @@ pub trait ExecutorRuntime { #[non_exhaustive] #[derive(Default)] pub struct SpinOptions { - /// A limit on how many times the executor should spin before stopping. A - /// [`None`] value will allow the executor to keep spinning indefinitely. - pub spin_limit: Option, + /// Only perform immediately available work. This is similar to spin_once in + /// rclcpp and rclpy. + pub only_available_work: bool, /// The executor will stop spinning if the future is resolved. pub until_future_resolved: Option>, } @@ -155,8 +227,9 @@ pub struct SpinConditions { pub options: SpinOptions, /// Halt trigger that gets set by [`ExecutorCommands`]. pub halt: Arc, - /// Check ok to make sure that the context is still valid. When the context - /// is invalid, the executor runtime should stop spinning. + /// 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, } diff --git a/rclrs/src/executor/basic_executor.rs b/rclrs/src/executor/basic_executor.rs index 6ccf45f34..d2c75f36b 100644 --- a/rclrs/src/executor/basic_executor.rs +++ b/rclrs/src/executor/basic_executor.rs @@ -11,7 +11,7 @@ impl ExecutorRuntime for BasicExecutorRuntime { } - fn async_spin( + fn spin_async( self: Box, conditions: SpinConditions, ) -> BoxFuture<'static, Box> { @@ -31,4 +31,8 @@ impl ExecutorChannel for BasicExecutorChannel { fn add(&self, f: futures::future::BoxFuture<'static, ()>) { } + + fn wakeup(&self) { + + } } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 4fea581af..daa2a45ba 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -106,14 +106,6 @@ 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() - } - /// Returns the clock associated with this node. pub fn get_clock(&self) -> Clock { self.time_source.get_clock() @@ -188,15 +180,6 @@ impl Node { self.call_string_getter(rcl_node_get_fully_qualified_name) } - // Helper for name(), namespace(), fully_qualified_name() - fn call_string_getter( - &self, - getter: unsafe extern "C" fn(*const rcl_node_t) -> *const c_char, - ) -> String { - let rcl_node = self.handle.rcl_node.lock().unwrap(); - unsafe { call_string_getter_with_rcl_node(&rcl_node, getter) } - } - /// Creates a [`Client`][1]. /// /// [1]: crate::Client @@ -210,47 +193,6 @@ impl Node { Ok(client) } - /// Creates a [`GuardCondition`][1] with no callback. - /// - /// A weak pointer to the `GuardCondition` is stored within this node. - /// When this node is added to a wait set (e.g. when calling `spin_once`[2] - /// with this node as an argument), the guard condition can be used to - /// interrupt the wait. - /// - /// [1]: crate::GuardCondition - /// [2]: crate::spin_once - pub fn create_guard_condition(&self) -> Arc { - let guard_condition = Arc::new(GuardCondition::new_with_context_handle( - Arc::clone(&self.handle.context_handle), - None, - )); - { self.guard_conditions_mtx.lock().unwrap() } - .push(Arc::downgrade(&guard_condition) as Weak); - guard_condition - } - - /// Creates a [`GuardCondition`][1] with a callback. - /// - /// A weak pointer to the `GuardCondition` is stored within this node. - /// When this node is added to a wait set (e.g. when calling `spin_once`[2] - /// with this node as an argument), the guard condition can be used to - /// interrupt the wait. - /// - /// [1]: crate::GuardCondition - /// [2]: crate::spin_once - pub fn create_guard_condition_with_callback(&mut self, callback: F) -> Arc - where - F: Fn() + Send + Sync + 'static, - { - let guard_condition = Arc::new(GuardCondition::new_with_context_handle( - Arc::clone(&self.handle.context_handle), - Some(Box::new(callback) as Box), - )); - { self.guard_conditions_mtx.lock().unwrap() } - .push(Arc::downgrade(&guard_condition) as Weak); - guard_condition - } - /// Creates a [`Publisher`][1]. /// /// [1]: crate::Publisher @@ -315,35 +257,6 @@ impl Node { Ok(subscription) } - /// Returns the subscriptions that have not been dropped yet. - pub(crate) fn live_subscriptions(&self) -> Vec> { - { self.subscriptions_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .collect() - } - - pub(crate) fn live_clients(&self) -> Vec> { - { self.clients_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .collect() - } - - pub(crate) fn live_guard_conditions(&self) -> Vec> { - { self.guard_conditions_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .collect() - } - - pub(crate) fn live_services(&self) -> Vec> { - { self.services_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .collect() - } - /// 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]. @@ -421,6 +334,15 @@ impl Node { interface: &self.parameter, } } + + // 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) } + } } // Helper used to implement call_string_getter(), but also used to get the FQN in the Node::new() diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 343fa61d3..0c8d13dff 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -485,8 +485,9 @@ mod tests { let context = Context::new_with_options([], InitOptions::new().with_domain_id(Some(domain_id))) .unwrap(); + let executor = context.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(); // Test that the graph has no publishers let names_and_topics = node .get_publisher_names_and_types_by_node(node_name, "") @@ -544,8 +545,9 @@ mod tests { #[test] fn test_node_names() { let context = Context::new([]).unwrap(); + let executor = context.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(); @@ -560,8 +562,9 @@ mod tests { #[test] fn test_node_names_with_enclaves() { let context = Context::new([]).unwrap(); + let executor = context.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/options.rs b/rclrs/src/node/options.rs index 6e3c01a8b..e3a53f619 100644 --- a/rclrs/src/node/options.rs +++ b/rclrs/src/node/options.rs @@ -369,6 +369,12 @@ impl NodeOptions { } } +impl From for NodeOptions { + fn from(name: T) -> Self { + NodeOptions::new(name) + } +} + impl Drop for rcl_node_options_t { fn drop(&mut self) { // SAFETY: Do not finish this struct except here. diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 62c409437..72074aa46 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -342,10 +342,11 @@ mod tests { } fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { - let node = NodeOptions::new(context, "node") + let executor = context.create_basic_executor(); + let node = executor.create_node( + NodeOptions::new("node") .namespace(ns) - .build() - .unwrap(); + ).unwrap(); let range = ParameterRange { lower: Some(0), upper: Some(100), @@ -375,10 +376,10 @@ mod tests { .mandatory() .unwrap(); - let client = NodeOptions::new(context, "client") + let client = executor.create_node( + NodeOptions::new("client") .namespace(ns) - .build() - .unwrap(); + ).unwrap(); ( TestNode { diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 36c241d19..47386d6c8 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -84,7 +84,7 @@ where { pub(crate) handle: Arc, /// The callback function that runs when a message was received. - pub callback: Mutex>, + pub callback: Mutex>, message: PhantomData, } @@ -238,7 +238,7 @@ where /// /// [1]: crate::RclrsError /// [2]: crate::Publisher::borrow_loaned_message - pub fn take_loaned(&self) -> Result<(ReadOnlyLoanedMessage<'_, T>, MessageInfo), RclrsError> { + fn take_loaned(&self) -> Result<(ReadOnlyLoanedMessage, MessageInfo), RclrsError> { let mut msg_ptr = std::ptr::null_mut(); let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; unsafe { @@ -254,7 +254,7 @@ where } let read_only_loaned_msg = ReadOnlyLoanedMessage { msg_ptr: msg_ptr as *const T::RmwMsg, - subscription: self, + handle: &self.handle, }; Ok(( read_only_loaned_msg, @@ -276,27 +276,27 @@ where // outside this match match (|| { match &mut *self.callback.lock().unwrap() { - AnySubscriptionCallback::Regular(cb) => { + AsyncSubscriptionCallback::Regular(cb) => { let (msg, _) = self.take()?; cb(msg) } - AnySubscriptionCallback::RegularWithMessageInfo(cb) => { + AsyncSubscriptionCallback::RegularWithMessageInfo(cb) => { let (msg, msg_info) = self.take()?; cb(msg, msg_info) } - AnySubscriptionCallback::Boxed(cb) => { + AsyncSubscriptionCallback::Boxed(cb) => { let (msg, _) = self.take_boxed()?; cb(msg) } - AnySubscriptionCallback::BoxedWithMessageInfo(cb) => { + AsyncSubscriptionCallback::BoxedWithMessageInfo(cb) => { let (msg, msg_info) = self.take_boxed()?; cb(msg, msg_info) } - AnySubscriptionCallback::Loaned(cb) => { + AsyncSubscriptionCallback::Loaned(cb) => { let (msg, _) = self.take_loaned()?; - cb(msg) + cb(msg); } - AnySubscriptionCallback::LoanedWithMessageInfo(cb) => { + AsyncSubscriptionCallback::LoanedWithMessageInfo(cb) => { let (msg, msg_info) = self.take_loaned()?; cb(msg, msg_info) } @@ -316,6 +316,12 @@ where } } +async fn subscription_task( + +) { + +} + #[cfg(test)] mod tests { use super::*; diff --git a/rclrs/src/subscription/callback.rs b/rclrs/src/subscription/callback.rs index d5e9fba8e..16c3ccd7b 100644 --- a/rclrs/src/subscription/callback.rs +++ b/rclrs/src/subscription/callback.rs @@ -3,6 +3,9 @@ use rosidl_runtime_rs::Message; use super::MessageInfo; use crate::ReadOnlyLoanedMessage; +use futures::future::BoxFuture; +use std::future::Future; + /// A trait for allowed callbacks for subscriptions. /// /// See [`AnySubscriptionCallback`] for a list of possible callback signatures. @@ -13,51 +16,53 @@ where /// Converts the callback into an enum. /// /// User code never needs to call this function. - fn into_callback(self) -> AnySubscriptionCallback; + fn into_callback(self) -> AsyncSubscriptionCallback; } /// An enum capturing the various possible function signatures for subscription callbacks. /// /// The correct enum variant is deduced by the [`SubscriptionCallback`] trait. -pub enum AnySubscriptionCallback +pub enum AsyncSubscriptionCallback where T: Message, { /// A callback with only the message as an argument. - Regular(Box), + Regular(Box BoxFuture<'static, ()> + Send>), /// A callback with the message and the message info as arguments. - RegularWithMessageInfo(Box), + RegularWithMessageInfo(Box BoxFuture<'static, ()> + Send>), /// A callback with only the boxed message as an argument. - Boxed(Box) + Send>), + Boxed(Box) -> BoxFuture<'static, ()> + Send>), /// A callback with the boxed message and the message info as arguments. - BoxedWithMessageInfo(Box, MessageInfo) + Send>), + BoxedWithMessageInfo(Box, MessageInfo) -> BoxFuture<'static, ()> + Send>), /// A callback with only the loaned message as an argument. #[allow(clippy::type_complexity)] - Loaned(Box FnMut(ReadOnlyLoanedMessage<'a, T>) + Send>), + Loaned(Box FnMut(ReadOnlyLoanedMessage) -> BoxFuture<'static, ()> + Send>), /// A callback with the loaned message and the message info as arguments. #[allow(clippy::type_complexity)] - LoanedWithMessageInfo(Box FnMut(ReadOnlyLoanedMessage<'a, T>, MessageInfo) + Send>), + LoanedWithMessageInfo(Box FnMut(ReadOnlyLoanedMessage, MessageInfo) -> BoxFuture<'static, ()> + Send>), } // We need one implementation per arity. This was inspired by Bevy's systems. -impl SubscriptionCallback for Func +impl SubscriptionCallback for Func where - Func: FnMut(A0) + Send + 'static, + Func: FnMut(A0) -> F + Send + 'static, (A0,): ArgTuple, T: Message, + F: Future + Send + 'static, { - fn into_callback(self) -> AnySubscriptionCallback { + fn into_callback(self) -> AsyncSubscriptionCallback { <(A0,) as ArgTuple>::into_callback_with_args(self) } } -impl SubscriptionCallback for Func +impl SubscriptionCallback for Func where - Func: FnMut(A0, A1) + Send + 'static, + Func: FnMut(A0, A1) -> F + Send + 'static, (A0, A1): ArgTuple, T: Message, + F: Future + Send + 'static, { - fn into_callback(self) -> AnySubscriptionCallback { + fn into_callback(self) -> AsyncSubscriptionCallback { <(A0, A1) as ArgTuple>::into_callback_with_args(self) } } @@ -70,66 +75,84 @@ trait ArgTuple where T: Message, { - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback; + fn into_callback_with_args(func: Func) -> AsyncSubscriptionCallback; } -impl ArgTuple for (T,) +impl ArgTuple for (T,) where T: Message, - Func: FnMut(T) + Send + 'static, + Func: FnMut(T) -> F + Send + 'static, + F: Future + Send + 'static, { - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback { - AnySubscriptionCallback::Regular(Box::new(func)) + fn into_callback_with_args(mut func: Func) -> AsyncSubscriptionCallback { + AsyncSubscriptionCallback::Regular(Box::new( + move |message| Box::pin(func(message)) + )) } } -impl ArgTuple for (T, MessageInfo) +impl ArgTuple for (T, MessageInfo) where T: Message, - Func: FnMut(T, MessageInfo) + Send + 'static, + Func: FnMut(T, MessageInfo) -> F + Send + 'static, + F: Future + Send + 'static, { - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback { - AnySubscriptionCallback::RegularWithMessageInfo(Box::new(func)) + fn into_callback_with_args(mut func: Func) -> AsyncSubscriptionCallback { + AsyncSubscriptionCallback::RegularWithMessageInfo(Box::new( + move |message, info| Box::pin(func(message, info)) + )) } } -impl ArgTuple for (Box,) +impl ArgTuple for (Box,) where T: Message, - Func: FnMut(Box) + Send + 'static, + Func: FnMut(Box) -> F + Send + 'static, + F: Future + Send + 'static, { - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback { - AnySubscriptionCallback::Boxed(Box::new(func)) + fn into_callback_with_args(mut func: Func) -> AsyncSubscriptionCallback { + AsyncSubscriptionCallback::Boxed(Box::new( + move |message| Box::pin(func(message)) + )) } } -impl ArgTuple for (Box, MessageInfo) +impl ArgTuple for (Box, MessageInfo) where T: Message, - Func: FnMut(Box, MessageInfo) + Send + 'static, + Func: FnMut(Box, MessageInfo) -> F + Send + 'static, + F: Future + Send + 'static, { - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback { - AnySubscriptionCallback::BoxedWithMessageInfo(Box::new(func)) + fn into_callback_with_args(mut func: Func) -> AsyncSubscriptionCallback { + AsyncSubscriptionCallback::BoxedWithMessageInfo(Box::new( + move |message, info| Box::pin(func(message, info)) + )) } } -impl ArgTuple for (ReadOnlyLoanedMessage<'_, T>,) +impl ArgTuple for (ReadOnlyLoanedMessage,) where T: Message, - Func: for<'b> FnMut(ReadOnlyLoanedMessage<'b, T>) + Send + 'static, + Func: FnMut(ReadOnlyLoanedMessage) -> F + Send + 'static, + F: Future + Send + 'static, { - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback { - AnySubscriptionCallback::Loaned(Box::new(func)) + fn into_callback_with_args(mut func: Func) -> AsyncSubscriptionCallback { + AsyncSubscriptionCallback::Loaned(Box::new( + move |message| Box::pin(func(message)) + )) } } -impl ArgTuple for (ReadOnlyLoanedMessage<'_, T>, MessageInfo) +impl ArgTuple for (ReadOnlyLoanedMessage, MessageInfo) where T: Message, - Func: for<'b> FnMut(ReadOnlyLoanedMessage<'b, T>, MessageInfo) + Send + 'static, + Func: FnMut(ReadOnlyLoanedMessage, MessageInfo) -> F + Send + 'static, + F: Future + Send + 'static, { - fn into_callback_with_args(func: Func) -> AnySubscriptionCallback { - AnySubscriptionCallback::LoanedWithMessageInfo(Box::new(func)) + fn into_callback_with_args(mut func: Func) -> AsyncSubscriptionCallback { + AsyncSubscriptionCallback::LoanedWithMessageInfo(Box::new( + move |message, info| Box::pin(func(message, info)) + )) } } @@ -140,35 +163,35 @@ mod tests { #[test] fn callback_conversion() { type Message = test_msgs::msg::BoundedSequences; - let cb = |_msg: Message| {}; + let cb = |_msg: Message| { async { } }; assert!(matches!( cb.into_callback(), - AnySubscriptionCallback::::Regular(_) + AsyncSubscriptionCallback::::Regular(_) )); - let cb = |_msg: Message, _info: MessageInfo| {}; + let cb = |_msg: Message, _info: MessageInfo| { async { } }; assert!(matches!( cb.into_callback(), - AnySubscriptionCallback::::RegularWithMessageInfo(_) + AsyncSubscriptionCallback::::RegularWithMessageInfo(_) )); - let cb = |_msg: Box| {}; + let cb = |_msg: Box| { async { } }; assert!(matches!( cb.into_callback(), - AnySubscriptionCallback::::Boxed(_) + AsyncSubscriptionCallback::::Boxed(_) )); - let cb = |_msg: Box, _info: MessageInfo| {}; + let cb = |_msg: Box, _info: MessageInfo| { async { } }; assert!(matches!( cb.into_callback(), - AnySubscriptionCallback::::BoxedWithMessageInfo(_) + AsyncSubscriptionCallback::::BoxedWithMessageInfo(_) )); - let cb = |_msg: ReadOnlyLoanedMessage<'_, Message>| {}; + let cb = |_msg: ReadOnlyLoanedMessage| { async { } }; assert!(matches!( cb.into_callback(), - AnySubscriptionCallback::::Loaned(_) + AsyncSubscriptionCallback::::Loaned(_) )); - let cb = |_msg: ReadOnlyLoanedMessage<'_, Message>, _info: MessageInfo| {}; + let cb = |_msg: ReadOnlyLoanedMessage, _info: MessageInfo| { async { } }; assert!(matches!( cb.into_callback(), - AnySubscriptionCallback::::LoanedWithMessageInfo(_) + AsyncSubscriptionCallback::::LoanedWithMessageInfo(_) )); } } diff --git a/rclrs/src/subscription/readonly_loaned_message.rs b/rclrs/src/subscription/readonly_loaned_message.rs index df4ad6a5b..7da2fdc23 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::*, 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<'a, T> Deref for ReadOnlyLoanedMessage<'a, T> +impl Deref for ReadOnlyLoanedMessage where T: Message, { @@ -32,14 +32,14 @@ where } } -impl<'a, T> Drop for ReadOnlyLoanedMessage<'a, 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<'a, T> Send for ReadOnlyLoanedMessage<'a, T> where T: Message {} +unsafe impl<'a, T> Send for ReadOnlyLoanedMessage where T: Message {} // SAFETY: This type has no interior mutability, in fact it has no mutability at all. -unsafe impl<'a, T> Sync for ReadOnlyLoanedMessage<'a, T> where T: Message {} +unsafe impl<'a, T> Sync for ReadOnlyLoanedMessage where T: Message {} #[cfg(test)] mod tests { From b1dda94c0e01a5b9e7dd464241838c0d31aaea22 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 8 Oct 2024 23:55:50 +0800 Subject: [PATCH 04/48] Implementing async execution for subscriptions Signed-off-by: Michael X. Grey --- rclrs/src/node.rs | 4 +- rclrs/src/subscription.rs | 174 ++------------ rclrs/src/subscription/any_callback.rs | 185 +++++++++++++++ rclrs/src/subscription/async_callback.rs | 227 ++++++++++++++++++ rclrs/src/subscription/callback.rs | 245 ++++++++++++-------- rclrs/src/subscription/subscription_task.rs | 37 +++ 6 files changed, 623 insertions(+), 249 deletions(-) create mode 100644 rclrs/src/subscription/any_callback.rs create mode 100644 rclrs/src/subscription/async_callback.rs create mode 100644 rclrs/src/subscription/subscription_task.rs diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index daa2a45ba..34c97b3c2 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -15,7 +15,7 @@ pub use self::{options::*, graph::*}; use crate::{ rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, + RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionAsyncCallback, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; @@ -240,7 +240,7 @@ impl Node { &self, topic: &str, qos: QoSProfile, - callback: impl SubscriptionCallback, + callback: impl SubscriptionAsyncCallback, ) -> Result>, RclrsError> where T: Message, diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 47386d6c8..af0d0e799 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -6,6 +6,8 @@ use std::{ use rosidl_runtime_rs::{Message, RmwMessage}; +use futures::channel::mpsc::UnboundedSender; + use crate::{ error::{RclReturnCode, ToResult}, qos::QoSProfile, @@ -13,13 +15,24 @@ use crate::{ NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; +mod any_callback; +pub use any_callback::*; + +mod async_callback; +pub use async_callback::*; + mod callback; -mod message_info; -mod readonly_loaned_message; pub use callback::*; + +mod message_info; pub use message_info::*; + +mod readonly_loaned_message; pub use readonly_loaned_message::*; +mod subscription_task; +use subscription_task::*; + // 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 {} @@ -59,7 +72,7 @@ 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>; + fn execute(&self); } /// Struct for receiving messages of type `T`. @@ -82,9 +95,8 @@ pub struct Subscription where T: Message, { - pub(crate) handle: Arc, - /// The callback function that runs when a message was received. - pub callback: Mutex>, + handle: Arc, + action: UnboundedSender>, message: PhantomData, } @@ -93,11 +105,11 @@ where T: Message, { /// Creates a new subscription. - pub(crate) fn new( + pub(crate) fn new( node_handle: Arc, topic: &str, qos: QoSProfile, - callback: impl SubscriptionCallback, + callback: impl SubscriptionAsyncCallback, ) -> Result // This uses pub(crate) visibility to avoid instantiating this struct outside // [`Node::create_subscription`], see the struct's documentation for the rationale @@ -165,102 +177,6 @@ where .into_owned() } } - - /// Fetches a new message. - /// - /// When there is no new message, this will return a - /// [`SubscriptionTakeFailed`][1]. - /// - /// [1]: crate::RclrsError - // - // ```text - // +-------------+ - // | rclrs::take | - // +------+------+ - // | - // | - // +------v------+ - // | rcl_take | - // +------+------+ - // | - // | - // +------v------+ - // | rmw_take | - // +-------------+ - // ``` - pub fn take(&self) -> Result<(T, MessageInfo), RclrsError> { - let mut rmw_message = ::RmwMsg::default(); - let message_info = self.take_inner(&mut rmw_message)?; - Ok((T::from_rmw_message(rmw_message), message_info)) - } - - /// This is a version of take() that returns a boxed message. - /// - /// This can be more efficient for messages containing large arrays. - pub fn take_boxed(&self) -> Result<(Box, MessageInfo), RclrsError> { - let mut rmw_message = Box::<::RmwMsg>::default(); - let message_info = self.take_inner(&mut *rmw_message)?; - // TODO: This will still use the stack in general. Change signature of - // from_rmw_message to allow placing the result in a Box directly. - let message = Box::new(T::from_rmw_message(*rmw_message)); - Ok((message, message_info)) - } - - // Inner function, to be used by both regular and boxed versions. - fn take_inner( - &self, - rmw_message: &mut ::RmwMsg, - ) -> Result { - let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; - let rcl_subscription = &mut *self.handle.lock(); - unsafe { - // SAFETY: The first two pointers are valid/initialized, and do not need to be valid - // beyond the function call. - // The latter two pointers are explicitly allowed to be NULL. - rcl_take( - rcl_subscription, - rmw_message as *mut ::RmwMsg as *mut _, - &mut message_info, - std::ptr::null_mut(), - ) - .ok()? - }; - Ok(MessageInfo::from_rmw_message_info(&message_info)) - } - - /// Obtains a read-only handle to a message owned by the middleware. - /// - /// When there is no new message, this will return a - /// [`SubscriptionTakeFailed`][1]. - /// - /// This is the counterpart to [`Publisher::borrow_loaned_message()`][2]. See its documentation - /// for more information. - /// - /// [1]: crate::RclrsError - /// [2]: crate::Publisher::borrow_loaned_message - fn take_loaned(&self) -> 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(), - &mut msg_ptr, - &mut message_info, - std::ptr::null_mut(), - ) - .ok()?; - } - let read_only_loaned_msg = ReadOnlyLoanedMessage { - msg_ptr: msg_ptr as *const T::RmwMsg, - handle: &self.handle, - }; - Ok(( - read_only_loaned_msg, - MessageInfo::from_rmw_message_info(&message_info), - )) - } } impl SubscriptionBase for Subscription @@ -271,57 +187,13 @@ where &self.handle } - fn execute(&self) -> Result<(), RclrsError> { - // Immediately evaluated closure, to handle SubscriptionTakeFailed - // outside this match - match (|| { - match &mut *self.callback.lock().unwrap() { - AsyncSubscriptionCallback::Regular(cb) => { - let (msg, _) = self.take()?; - cb(msg) - } - AsyncSubscriptionCallback::RegularWithMessageInfo(cb) => { - let (msg, msg_info) = self.take()?; - cb(msg, msg_info) - } - AsyncSubscriptionCallback::Boxed(cb) => { - let (msg, _) = self.take_boxed()?; - cb(msg) - } - AsyncSubscriptionCallback::BoxedWithMessageInfo(cb) => { - let (msg, msg_info) = self.take_boxed()?; - cb(msg, msg_info) - } - AsyncSubscriptionCallback::Loaned(cb) => { - let (msg, _) = self.take_loaned()?; - cb(msg); - } - AsyncSubscriptionCallback::LoanedWithMessageInfo(cb) => { - let (msg, msg_info) = self.take_loaned()?; - cb(msg, msg_info) - } - } - Ok(()) - })() { - 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, + fn execute(&self) { + if let Err(_) = self.action.unbounded_send(SubscriptionAction::Execute) { + // TODO(@mxgrey): Log the error here once logging is implemented } } } -async fn subscription_task( - -) { - -} - #[cfg(test)] mod tests { use super::*; diff --git a/rclrs/src/subscription/any_callback.rs b/rclrs/src/subscription/any_callback.rs new file mode 100644 index 000000000..d62b5acb8 --- /dev/null +++ b/rclrs/src/subscription/any_callback.rs @@ -0,0 +1,185 @@ +use rosidl_runtime_rs::Message; + +use super::{MessageInfo, SubscriptionHandle}; +use crate::{ + error::ToResult, + rcl_bindings::*, + ReadOnlyLoanedMessage, ExecutorCommands, RclrsError, RclReturnCode, +}; + +use futures::future::BoxFuture; + +use std::sync::Arc; + +/// An enum capturing the various possible function signatures for subscription callbacks. +/// +/// The correct enum variant is deduced by the [`SubscriptionCallback`] trait. +pub enum AnySubscriptionCallback +where + T: Message, +{ + /// A callback with only the message as an argument. + Regular(Box BoxFuture<'static, ()> + Send>), + /// A callback with the message and the message info as arguments. + RegularWithMessageInfo(Box BoxFuture<'static, ()> + Send>), + /// A callback with only the boxed message as an argument. + Boxed(Box) -> BoxFuture<'static, ()> + Send>), + /// A callback with the boxed message and the message info as arguments. + BoxedWithMessageInfo(Box, MessageInfo) -> BoxFuture<'static, ()> + Send>), + /// A callback with only the loaned message as an argument. + #[allow(clippy::type_complexity)] + Loaned(Box FnMut(ReadOnlyLoanedMessage) -> BoxFuture<'static, ()> + Send>), + /// A callback with the loaned message and the message info as arguments. + #[allow(clippy::type_complexity)] + LoanedWithMessageInfo(Box FnMut(ReadOnlyLoanedMessage, MessageInfo) -> BoxFuture<'static, ()> + Send>), +} + +impl AnySubscriptionCallback { + pub(super) fn execute( + &mut self, + handle: &Arc, + commands: &Arc, + ) -> Result<(), RclrsError> { + // Immediately evaluated closure, to handle SubscriptionTakeFailed + // outside this match + let mut evaluate = || { + match self { + AnySubscriptionCallback::Regular(cb) => { + let (msg, _) = Self::take(handle)?; + commands.run_detached(cb(msg)); + } + AnySubscriptionCallback::RegularWithMessageInfo(cb) => { + let (msg, msg_info) = Self::take(handle)?; + commands.run_detached(cb(msg, msg_info)); + } + AnySubscriptionCallback::Boxed(cb) => { + let (msg, _) = Self::take_boxed(handle)?; + commands.run_detached(cb(msg)); + } + AnySubscriptionCallback::BoxedWithMessageInfo(cb) => { + let (msg, msg_info) = Self::take_boxed(handle)?; + commands.run_detached(cb(msg, msg_info)); + } + AnySubscriptionCallback::Loaned(cb) => { + let (msg, _) = Self::take_loaned(handle)?; + commands.run_detached(cb(msg)); + } + AnySubscriptionCallback::LoanedWithMessageInfo(cb) => { + let (msg, msg_info) = Self::take_loaned(handle)?; + commands.run_detached(cb(msg, msg_info)); + } + } + Ok(()) + }; + + match evaluate() { + Err(RclrsError::RclError { + code: RclReturnCode::SubscriptionTakeFailed, + .. + }) => { + // Spurious wakeup – this may happen even when a waitset indicated that this + // subscription was ready, so it shouldn't be an error. + Ok(()) + } + other => other, + } + } + + /// Fetches a new message. + /// + /// When there is no new message, this will return a + /// [`SubscriptionTakeFailed`][1]. + /// + /// [1]: crate::RclrsError + // + // ```text + // +-------------+ + // | rclrs::take | + // +------+------+ + // | + // | + // +------v------+ + // | rcl_take | + // +------+------+ + // | + // | + // +------v------+ + // | rmw_take | + // +-------------+ + // ``` + fn take(handle: &Arc) -> Result<(T, MessageInfo), RclrsError> { + let mut rmw_message = ::RmwMsg::default(); + let message_info = Self::take_inner(handle, &mut rmw_message)?; + Ok((T::from_rmw_message(rmw_message), message_info)) + } + + /// This is a version of take() that returns a boxed message. + /// + /// This can be more efficient for messages containing large arrays. + fn take_boxed(handle: &Arc) -> Result<(Box, MessageInfo), RclrsError> { + let mut rmw_message = Box::<::RmwMsg>::default(); + let message_info = Self::take_inner(handle, &mut *rmw_message)?; + // TODO: This will still use the stack in general. Change signature of + // from_rmw_message to allow placing the result in a Box directly. + let message = Box::new(T::from_rmw_message(*rmw_message)); + Ok((message, message_info)) + } + + // Inner function, to be used by both regular and boxed versions. + fn take_inner( + handle: &Arc, + rmw_message: &mut ::RmwMsg, + ) -> Result { + let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; + let rcl_subscription = &mut *handle.lock(); + unsafe { + // SAFETY: The first two pointers are valid/initialized, and do not need to be valid + // beyond the function call. + // The latter two pointers are explicitly allowed to be NULL. + rcl_take( + rcl_subscription, + rmw_message as *mut ::RmwMsg as *mut _, + &mut message_info, + std::ptr::null_mut(), + ) + .ok()? + }; + Ok(MessageInfo::from_rmw_message_info(&message_info)) + } + + /// Obtains a read-only handle to a message owned by the middleware. + /// + /// When there is no new message, this will return a + /// [`SubscriptionTakeFailed`][1]. + /// + /// This is the counterpart to [`Publisher::borrow_loaned_message()`][2]. See its documentation + /// for more information. + /// + /// [1]: crate::RclrsError + /// [2]: crate::Publisher::borrow_loaned_message + fn take_loaned( + handle: &Arc, + ) -> Result<(ReadOnlyLoanedMessage, MessageInfo), RclrsError> { + let mut msg_ptr = std::ptr::null_mut(); + let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; + unsafe { + // SAFETY: The third argument (message_info) and fourth argument (allocation) may be null. + // The second argument (loaned_message) contains a null ptr as expected. + rcl_take_loaned_message( + &*handle.lock(), + &mut msg_ptr, + &mut message_info, + std::ptr::null_mut(), + ) + .ok()?; + } + let read_only_loaned_msg = ReadOnlyLoanedMessage { + msg_ptr: msg_ptr as *const T::RmwMsg, + handle: Arc::clone(&handle), + }; + Ok(( + read_only_loaned_msg, + MessageInfo::from_rmw_message_info(&message_info), + )) + } +} diff --git a/rclrs/src/subscription/async_callback.rs b/rclrs/src/subscription/async_callback.rs new file mode 100644 index 000000000..3130e4529 --- /dev/null +++ b/rclrs/src/subscription/async_callback.rs @@ -0,0 +1,227 @@ +use rosidl_runtime_rs::Message; + +use super::{ + MessageInfo, + any_callback::AnySubscriptionCallback, +}; +use crate::ReadOnlyLoanedMessage; + +use std::future::Future; + +/// A trait for async callbacks of subscriptions. +/// +/// TODO(@mxgrey): Add a description of what callback signatures are supported +pub trait SubscriptionAsyncCallback: Send + 'static +where + T: Message, +{ + /// Converts the callback into an enum. + /// + /// User code never needs to call this function. + fn into_async_callback(self) -> AnySubscriptionCallback; +} + +// We need one implementation per arity. This was inspired by Bevy's systems. +impl SubscriptionAsyncCallback for Func +where + T: Message, + (A0,): SubscriptionAsyncArgs, + Out: Future + Send + 'static, + Func: FnMut(A0) -> Out + Send + 'static, +{ + fn into_async_callback(self) -> AnySubscriptionCallback { + <(A0,) as SubscriptionAsyncArgs>::into_any_callback(self) + } +} + +impl SubscriptionAsyncCallback for Func +where + T: Message, + (A0, A1): SubscriptionAsyncArgs, + Out: Future + Send + 'static, + Func: FnMut(A0, A1) -> Out + Send + 'static, +{ + fn into_async_callback(self) -> AnySubscriptionCallback { + <(A0, A1) as SubscriptionAsyncArgs>::into_any_callback(self) + } +} + +/// Helper trait for SubscriptionCallback. +/// +/// For each tuple of args, it provides conversion from a function with +/// these args to the correct enum variant. +trait SubscriptionAsyncArgs +where + T: Message, +{ + fn into_any_callback(func: Func) -> AnySubscriptionCallback; +} + +impl SubscriptionAsyncArgs for (T,) +where + T: Message, + Func: FnMut(T) -> Out + Send + 'static, + Out: Future + Send + 'static, +{ + fn into_any_callback(mut func: Func) -> AnySubscriptionCallback { + AnySubscriptionCallback::Regular(Box::new( + move |message| Box::pin(func(message)) + )) + } +} + +impl SubscriptionAsyncArgs for (T, MessageInfo) +where + T: Message, + Func: FnMut(T, MessageInfo) -> Out + Send + 'static, + Out: Future + Send + 'static, +{ + fn into_any_callback(mut func: Func) -> AnySubscriptionCallback { + AnySubscriptionCallback::RegularWithMessageInfo(Box::new( + move |message, info| Box::pin(func(message, info)) + )) + } +} + +impl SubscriptionAsyncArgs for (Box,) +where + T: Message, + Func: FnMut(Box) -> Out + Send + 'static, + Out: Future + Send + 'static, +{ + fn into_any_callback(mut func: Func) -> AnySubscriptionCallback { + AnySubscriptionCallback::Boxed(Box::new( + move |message| Box::pin(func(message)) + )) + } +} + +impl SubscriptionAsyncArgs for (Box, MessageInfo) +where + T: Message, + Func: FnMut(Box, MessageInfo) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_any_callback(mut func: Func) -> AnySubscriptionCallback { + AnySubscriptionCallback::BoxedWithMessageInfo(Box::new( + move |message, info| Box::pin(func(message, info)) + )) + } +} + +impl SubscriptionAsyncArgs for (ReadOnlyLoanedMessage,) +where + T: Message, + Func: FnMut(ReadOnlyLoanedMessage) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_any_callback(mut func: Func) -> AnySubscriptionCallback { + AnySubscriptionCallback::Loaned(Box::new( + move |message| Box::pin(func(message)) + )) + } +} + +impl SubscriptionAsyncArgs for (ReadOnlyLoanedMessage, MessageInfo) +where + T: Message, + Func: FnMut(ReadOnlyLoanedMessage, MessageInfo) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_any_callback(mut func: Func) -> AnySubscriptionCallback { + AnySubscriptionCallback::LoanedWithMessageInfo(Box::new( + move |message, info| Box::pin(func(message, info)) + )) + } +} + + +#[cfg(test)] +mod tests { + use super::*; + + type TestMessage = test_msgs::msg::BoundedSequences; + + #[test] + fn callback_conversion() { + let cb = |_msg: TestMessage| { async { } }; + assert!(matches!( + cb.into_async_callback(), + AnySubscriptionCallback::::Regular(_) + )); + let cb = |_msg: TestMessage, _info: MessageInfo| { async { } }; + assert!(matches!( + cb.into_async_callback(), + AnySubscriptionCallback::::RegularWithMessageInfo(_) + )); + let cb = |_msg: Box| { async { } }; + assert!(matches!( + cb.into_async_callback(), + AnySubscriptionCallback::::Boxed(_) + )); + let cb = |_msg: Box, _info: MessageInfo| { async { } }; + assert!(matches!( + cb.into_async_callback(), + AnySubscriptionCallback::::BoxedWithMessageInfo(_) + )); + let cb = |_msg: ReadOnlyLoanedMessage| { async { } }; + assert!(matches!( + cb.into_async_callback(), + AnySubscriptionCallback::::Loaned(_) + )); + let cb = |_msg: ReadOnlyLoanedMessage, _info: MessageInfo| { async {}}; + assert!(matches!( + cb.into_async_callback(), + AnySubscriptionCallback::::LoanedWithMessageInfo(_) + )); + + assert!(matches!( + test_regular.into_async_callback(), + AnySubscriptionCallback::::Regular(_), + )); + assert!(matches!( + test_regular_with_info.into_async_callback(), + AnySubscriptionCallback::::RegularWithMessageInfo(_), + )); + assert!(matches!( + test_boxed.into_async_callback(), + AnySubscriptionCallback::::Boxed(_), + )); + assert!(matches!( + test_boxed_with_info.into_async_callback(), + AnySubscriptionCallback::::BoxedWithMessageInfo(_), + )); + assert!(matches!( + test_loaned.into_async_callback(), + AnySubscriptionCallback::::Loaned(_), + )); + assert!(matches!( + test_loaned_with_info.into_async_callback(), + AnySubscriptionCallback::::LoanedWithMessageInfo(_), + )); + } + + async fn test_regular(_msg: TestMessage) { + + } + + async fn test_regular_with_info(_msg: TestMessage, _info: MessageInfo) { + + } + + async fn test_boxed(_msg: Box) { + + } + + async fn test_boxed_with_info(_msg: Box, _info: MessageInfo) { + + } + + async fn test_loaned(_msg: ReadOnlyLoanedMessage) { + + } + + async fn test_loaned_with_info(_msg: ReadOnlyLoanedMessage) { + + } +} diff --git a/rclrs/src/subscription/callback.rs b/rclrs/src/subscription/callback.rs index 16c3ccd7b..da334b306 100644 --- a/rclrs/src/subscription/callback.rs +++ b/rclrs/src/subscription/callback.rs @@ -1,14 +1,16 @@ use rosidl_runtime_rs::Message; -use super::MessageInfo; +use super::{ + MessageInfo, + any_callback::AnySubscriptionCallback, +}; use crate::ReadOnlyLoanedMessage; -use futures::future::BoxFuture; -use std::future::Future; +use std::sync::Arc; -/// A trait for allowed callbacks for subscriptions. +/// A trait for regular callbacks of subscriptions. /// -/// See [`AnySubscriptionCallback`] for a list of possible callback signatures. +/// TODO(@mxgrey): Add a description of what callbacks signatures are supported pub trait SubscriptionCallback: Send + 'static where T: Message, @@ -16,142 +18,143 @@ where /// Converts the callback into an enum. /// /// User code never needs to call this function. - fn into_callback(self) -> AsyncSubscriptionCallback; -} - -/// An enum capturing the various possible function signatures for subscription callbacks. -/// -/// The correct enum variant is deduced by the [`SubscriptionCallback`] trait. -pub enum AsyncSubscriptionCallback -where - T: Message, -{ - /// A callback with only the message as an argument. - Regular(Box BoxFuture<'static, ()> + Send>), - /// A callback with the message and the message info as arguments. - RegularWithMessageInfo(Box BoxFuture<'static, ()> + Send>), - /// A callback with only the boxed message as an argument. - Boxed(Box) -> BoxFuture<'static, ()> + Send>), - /// A callback with the boxed message and the message info as arguments. - BoxedWithMessageInfo(Box, MessageInfo) -> BoxFuture<'static, ()> + Send>), - /// A callback with only the loaned message as an argument. - #[allow(clippy::type_complexity)] - Loaned(Box FnMut(ReadOnlyLoanedMessage) -> BoxFuture<'static, ()> + Send>), - /// A callback with the loaned message and the message info as arguments. - #[allow(clippy::type_complexity)] - LoanedWithMessageInfo(Box FnMut(ReadOnlyLoanedMessage, MessageInfo) -> BoxFuture<'static, ()> + Send>), + fn into_callback(self) -> AnySubscriptionCallback; } // We need one implementation per arity. This was inspired by Bevy's systems. -impl SubscriptionCallback for Func +impl SubscriptionCallback for Func where - Func: FnMut(A0) -> F + Send + 'static, - (A0,): ArgTuple, T: Message, - F: Future + Send + 'static, + (A0,): SubscriptionArgs, + Func: Fn(A0) + Send + Sync + 'static, { - fn into_callback(self) -> AsyncSubscriptionCallback { - <(A0,) as ArgTuple>::into_callback_with_args(self) + fn into_callback(self) -> AnySubscriptionCallback { + <(A0,) as SubscriptionArgs>::into_any_callback(self) } } -impl SubscriptionCallback for Func +impl SubscriptionCallback for Func where - Func: FnMut(A0, A1) -> F + Send + 'static, - (A0, A1): ArgTuple, T: Message, - F: Future + Send + 'static, + (A0, A1): SubscriptionArgs, + Func: Fn(A0, A1) + Clone + Send + 'static, { - fn into_callback(self) -> AsyncSubscriptionCallback { - <(A0, A1) as ArgTuple>::into_callback_with_args(self) + fn into_callback(self) -> AnySubscriptionCallback { + <(A0, A1) as SubscriptionArgs>::into_any_callback(self) } } -// Helper trait for SubscriptionCallback. -// -// For each tuple of args, it provides conversion from a function with -// these args to the correct enum variant. -trait ArgTuple +trait SubscriptionArgs where T: Message, { - fn into_callback_with_args(func: Func) -> AsyncSubscriptionCallback; + fn into_any_callback(func: Func) -> AnySubscriptionCallback; } -impl ArgTuple for (T,) +impl SubscriptionArgs for (T,) where T: Message, - Func: FnMut(T) -> F + Send + 'static, - F: Future + Send + 'static, + Func: Fn(T) + Send + Sync + 'static, { - fn into_callback_with_args(mut func: Func) -> AsyncSubscriptionCallback { - AsyncSubscriptionCallback::Regular(Box::new( - move |message| Box::pin(func(message)) + fn into_any_callback(func: Func) -> AnySubscriptionCallback { + let func = Arc::new(func); + AnySubscriptionCallback::Regular(Box::new( + move |message| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message); + }) + } )) } } -impl ArgTuple for (T, MessageInfo) +impl SubscriptionArgs for (T, MessageInfo) where T: Message, - Func: FnMut(T, MessageInfo) -> F + Send + 'static, - F: Future + Send + 'static, + Func: Fn(T, MessageInfo) + Send + Sync + 'static, { - fn into_callback_with_args(mut func: Func) -> AsyncSubscriptionCallback { - AsyncSubscriptionCallback::RegularWithMessageInfo(Box::new( - move |message, info| Box::pin(func(message, info)) + fn into_any_callback(func: Func) -> AnySubscriptionCallback { + let func = Arc::new(func); + AnySubscriptionCallback::RegularWithMessageInfo(Box::new( + move |message, info| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message, info); + }) + } )) } } -impl ArgTuple for (Box,) +impl SubscriptionArgs for (Box,) where T: Message, - Func: FnMut(Box) -> F + Send + 'static, - F: Future + Send + 'static, + Func: Fn(Box) + Send + Sync + 'static, { - fn into_callback_with_args(mut func: Func) -> AsyncSubscriptionCallback { - AsyncSubscriptionCallback::Boxed(Box::new( - move |message| Box::pin(func(message)) + fn into_any_callback(func: Func) -> AnySubscriptionCallback { + let func = Arc::new(func); + AnySubscriptionCallback::Boxed(Box::new( + move |message| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message); + }) + } )) } } -impl ArgTuple for (Box, MessageInfo) +impl SubscriptionArgs for (Box, MessageInfo) where T: Message, - Func: FnMut(Box, MessageInfo) -> F + Send + 'static, - F: Future + Send + 'static, + Func: Fn(Box, MessageInfo) + Send + Sync + 'static, { - fn into_callback_with_args(mut func: Func) -> AsyncSubscriptionCallback { - AsyncSubscriptionCallback::BoxedWithMessageInfo(Box::new( - move |message, info| Box::pin(func(message, info)) + fn into_any_callback(func: Func) -> AnySubscriptionCallback { + let func = Arc::new(func); + AnySubscriptionCallback::BoxedWithMessageInfo(Box::new( + move |message, info| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message, info); + }) + } )) } } -impl ArgTuple for (ReadOnlyLoanedMessage,) +impl SubscriptionArgs for (ReadOnlyLoanedMessage,) where T: Message, - Func: FnMut(ReadOnlyLoanedMessage) -> F + Send + 'static, - F: Future + Send + 'static, + Func: Fn(ReadOnlyLoanedMessage) + Send + Sync + 'static, { - fn into_callback_with_args(mut func: Func) -> AsyncSubscriptionCallback { - AsyncSubscriptionCallback::Loaned(Box::new( - move |message| Box::pin(func(message)) + fn into_any_callback(func: Func) -> AnySubscriptionCallback { + let func = Arc::new(func); + AnySubscriptionCallback::Loaned(Box::new( + move |message| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message); + }) + } )) } } -impl ArgTuple for (ReadOnlyLoanedMessage, MessageInfo) +impl SubscriptionArgs for (ReadOnlyLoanedMessage, MessageInfo) where T: Message, - Func: FnMut(ReadOnlyLoanedMessage, MessageInfo) -> F + Send + 'static, - F: Future + Send + 'static, + Func: Fn(ReadOnlyLoanedMessage, MessageInfo) + Send + Sync + 'static, { - fn into_callback_with_args(mut func: Func) -> AsyncSubscriptionCallback { - AsyncSubscriptionCallback::LoanedWithMessageInfo(Box::new( - move |message, info| Box::pin(func(message, info)) + fn into_any_callback(func: Func) -> AnySubscriptionCallback { + let func = Arc::new(func); + AnySubscriptionCallback::LoanedWithMessageInfo(Box::new( + move |message, info| { + let f = Arc::clone(&func); + Box::pin(async move { + f(message, info); + }) + } )) } } @@ -160,38 +163,88 @@ where mod tests { use super::*; + type TestMessage = test_msgs::msg::BoundedSequences; + #[test] fn callback_conversion() { - type Message = test_msgs::msg::BoundedSequences; - let cb = |_msg: Message| { async { } }; + let cb = |_msg: TestMessage| { }; assert!(matches!( cb.into_callback(), - AsyncSubscriptionCallback::::Regular(_) + AnySubscriptionCallback::::Regular(_) )); - let cb = |_msg: Message, _info: MessageInfo| { async { } }; + let cb = |_msg: TestMessage, _info: MessageInfo| { }; assert!(matches!( cb.into_callback(), - AsyncSubscriptionCallback::::RegularWithMessageInfo(_) + AnySubscriptionCallback::::RegularWithMessageInfo(_) )); - let cb = |_msg: Box| { async { } }; + let cb = |_msg: Box| { }; assert!(matches!( cb.into_callback(), - AsyncSubscriptionCallback::::Boxed(_) + AnySubscriptionCallback::::Boxed(_) )); - let cb = |_msg: Box, _info: MessageInfo| { async { } }; + let cb = |_msg: Box, _info: MessageInfo| { }; assert!(matches!( cb.into_callback(), - AsyncSubscriptionCallback::::BoxedWithMessageInfo(_) + AnySubscriptionCallback::::BoxedWithMessageInfo(_) )); - let cb = |_msg: ReadOnlyLoanedMessage| { async { } }; + let cb = |_msg: ReadOnlyLoanedMessage| { }; assert!(matches!( cb.into_callback(), - AsyncSubscriptionCallback::::Loaned(_) + AnySubscriptionCallback::::Loaned(_) )); - let cb = |_msg: ReadOnlyLoanedMessage, _info: MessageInfo| { async { } }; + let cb = |_msg: ReadOnlyLoanedMessage, _info: MessageInfo| { }; assert!(matches!( cb.into_callback(), - AsyncSubscriptionCallback::::LoanedWithMessageInfo(_) + AnySubscriptionCallback::::LoanedWithMessageInfo(_) + )); + + assert!(matches!( + test_regular.into_callback(), + AnySubscriptionCallback::::Regular(_), + )); + assert!(matches!( + test_regular_with_info.into_callback(), + AnySubscriptionCallback::::RegularWithMessageInfo(_), + )); + assert!(matches!( + test_boxed.into_callback(), + AnySubscriptionCallback::::Boxed(_), + )); + assert!(matches!( + test_boxed_with_info.into_callback(), + AnySubscriptionCallback::::BoxedWithMessageInfo(_), + )); + assert!(matches!( + test_loaned.into_callback(), + AnySubscriptionCallback::::Loaned(_), + )); + assert!(matches!( + test_loaned_with_info.into_callback(), + AnySubscriptionCallback::::LoanedWithMessageInfo(_), )); } + + fn test_regular(_msg: TestMessage) { + + } + + fn test_regular_with_info(_msg: TestMessage, _info: MessageInfo) { + + } + + fn test_boxed(_msg: Box) { + + } + + fn test_boxed_with_info(_msg: Box, _info: MessageInfo) { + + } + + fn test_loaned(_msg: ReadOnlyLoanedMessage) { + + } + + fn test_loaned_with_info(_msg: ReadOnlyLoanedMessage) { + + } } diff --git a/rclrs/src/subscription/subscription_task.rs b/rclrs/src/subscription/subscription_task.rs new file mode 100644 index 000000000..8a206369d --- /dev/null +++ b/rclrs/src/subscription/subscription_task.rs @@ -0,0 +1,37 @@ +use rosidl_runtime_rs::Message; + +use crate::{ + AnySubscriptionCallback, ExecutorCommands, SubscriptionHandle, +}; + +use futures::{ + Stream, StreamExt, + channel::mpsc::UnboundedReceiver +}; + +use std::sync::Arc; + +pub(super) enum SubscriptionAction { + Execute, + SetCallback(AnySubscriptionCallback), +} + +pub(super) async fn subscription_task( + mut callback: AnySubscriptionCallback, + mut receiver: UnboundedReceiver>, + handle: Arc, + commands: Arc, +) { + while let Some(action) = receiver.next().await { + match action { + SubscriptionAction::SetCallback(new_callback) => { + callback = new_callback; + } + SubscriptionAction::Execute => { + if let Err(_) = callback.execute(&handle, &commands) { + // TODO(@mxgrey): Log the error here once logging is implemented + } + } + } + } +} From c31ad7d1c83572ffc982235cef0a6357f28f53cf Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 10 Oct 2024 01:26:00 +0800 Subject: [PATCH 05/48] Update wait set behavior Signed-off-by: Michael X. Grey --- rclrs/Cargo.toml | 3 + rclrs/src/executor.rs | 9 +- rclrs/src/node.rs | 38 +- rclrs/src/node/options.rs | 1 + rclrs/src/subscription.rs | 119 ++++-- rclrs/src/wait.rs | 559 +++++++++++++++------------- rclrs/src/wait/exclusivity_guard.rs | 58 --- 7 files changed, 419 insertions(+), 368 deletions(-) delete mode 100644 rclrs/src/wait/exclusivity_guard.rs diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index f489ea3f0..2807e6fc6 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -17,6 +17,9 @@ path = "src/lib.rs" # Needed for dynamically finding type support libraries ament_rs = { version = "0.2", optional = true } +# Used to ensure uniqueness across entity handles +by_address = "1.2.1" + # Needed for uploading documentation to docs.rs cfg-if = "1.0.0" diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index d0354b13b..6c765d884 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -4,7 +4,7 @@ pub use self::basic_executor::*; use crate::{ rcl_bindings::rcl_context_is_valid, Node, NodeOptions, RclReturnCode, RclrsError, WaitSet, Context, - ContextHandle, + ContextHandle, WaitSetEntities, }; use std::{ sync::{Arc, Mutex, Weak}, @@ -175,10 +175,13 @@ impl ExecutorCommands { /// This trait defines the interface for passing new items into an executor to /// run. -pub trait ExecutorChannel { +pub trait ExecutorChannel: Send + Sync { /// Add a new item for the executor to run. fn add(&self, f: BoxFuture<'static, ()>); + /// Add new entities to the waitset of the executor. + fn add_to_waitset(new_entities: WaitSetEntities); + // TODO(@mxgrey): create_guard_condition for waking up the waitset thread } @@ -280,7 +283,7 @@ impl SingleThreadedExecutor { let ready_entities = wait_set.wait(timeout)?; for ready_subscription in ready_entities.subscriptions { - ready_subscription.execute()?; + ready_subscription.execute(); } for ready_client in ready_entities.clients { diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 34c97b3c2..8a63972a9 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -13,9 +13,10 @@ use rosidl_runtime_rs::Message; pub use self::{options::*, graph::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, + rcl_bindings::*, Client, ClientBase, Clock, ContextHandle, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionAsyncCallback, + RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, + SubscriptionAsyncCallback, ExecutorCommands, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; @@ -65,6 +66,7 @@ pub struct Node { pub(crate) subscriptions_mtx: Mutex>>, time_source: TimeSource, parameter: ParameterInterface, + commands: Arc, pub(crate) handle: Arc, } @@ -237,6 +239,25 @@ impl Node { /// [1]: crate::Subscription // TODO: make subscription's lifetime depend on node's lifetime pub fn create_subscription( + &self, + topic: &str, + qos: QoSProfile, + callback: impl SubscriptionCallback, + ) -> Result>, RclrsError> + where + T: Message, + { + Subscription::::create( + topic, + qos, + callback.into_callback(), + &self.handle, + &self.commands, + ) + } + + /// Creates a subscription with an async callback + pub fn create_async_subscription( &self, topic: &str, qos: QoSProfile, @@ -245,16 +266,13 @@ impl Node { where T: Message, { - let subscription = Arc::new(Subscription::::new( - Arc::clone(&self.handle), + Subscription::::create( topic, qos, - callback, - )?); - { self.subscriptions_mtx.lock() } - .unwrap() - .push(Arc::downgrade(&subscription) as Weak); - Ok(subscription) + callback.into_async_callback(), + &self.handle, + &self.commands, + ) } /// Returns the ROS domain ID that the node is using. diff --git a/rclrs/src/node/options.rs b/rclrs/src/node/options.rs index e3a53f619..433b38d33 100644 --- a/rclrs/src/node/options.rs +++ b/rclrs/src/node/options.rs @@ -320,6 +320,7 @@ impl NodeOptions { .clock_qos(self.clock_qos) .build(), parameter, + commands: Arc::clone(&commands), }); node.time_source.attach_node(&node); if self.start_parameter_services { diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index af0d0e799..66a653bc2 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -1,18 +1,17 @@ use std::{ ffi::{CStr, CString}, - marker::PhantomData, - sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, + sync::{Arc, Mutex, MutexGuard}, }; use rosidl_runtime_rs::{Message, RmwMessage}; -use futures::channel::mpsc::UnboundedSender; +use futures::channel::mpsc::{unbounded, UnboundedSender, TrySendError}; use crate::{ - error::{RclReturnCode, ToResult}, + error::ToResult, qos::QoSProfile, rcl_bindings::*, - NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + ExecutorCommands, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; mod any_callback; @@ -45,7 +44,6 @@ unsafe impl Send for rcl_subscription_t {} pub struct SubscriptionHandle { rcl_subscription: Mutex, node_handle: Arc, - pub(crate) in_use_by_wait_set: Arc, } impl SubscriptionHandle { @@ -79,37 +77,102 @@ pub trait SubscriptionBase: Send + Sync { /// /// There can be multiple subscriptions for the same topic, in different nodes or the same node. /// -/// 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. +/// The only available way to instantiate subscriptions is via [`Node::create_subscription()`][2] +/// or [`Node::create_async_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 where T: Message, { + /// This handle is used to access the data that rcl holds for this subscription. handle: Arc, + /// This allows us to trigger the callback or replace the callback in the + /// subscription task. + /// + /// Holding onto this sender will keep the subscription task running. Once + /// this sender is dropped, the subscription task will end itself. action: UnboundedSender>, - message: PhantomData, } impl Subscription where T: Message, { - /// Creates a new subscription. - pub(crate) fn new( - node_handle: Arc, + /// Returns the topic name of the subscription. + /// + /// This returns the topic name after remapping, so it is not necessarily the + /// topic name which was used when creating the subscription. + pub fn topic_name(&self) -> String { + // SAFETY: No preconditions for the function used + // The unsafe variables get converted to safe types before being returned + unsafe { + let raw_topic_pointer = rcl_subscription_get_topic_name(&*self.handle.lock()); + CStr::from_ptr(raw_topic_pointer) + .to_string_lossy() + .into_owned() + } + } + + /// Set the callback of this subscription, replacing the callback that was + /// previously set. + /// + /// This can be used even if the subscription previously used an async callback. + pub fn set_callback( + &self, + callback: impl SubscriptionCallback, + ) -> Result<(), TrySendError>> { + let callback = callback.into_callback(); + self.action.unbounded_send(SubscriptionAction::SetCallback(callback)) + } + + /// Set the callback of this subscription, replacing the callback that was + /// previously set. + /// + /// This can be used even if the subscription previously used a non-async callback. + pub fn set_async_callback( + &self, + callback: impl SubscriptionAsyncCallback, + ) -> Result<(), TrySendError>> { + let callback = callback.into_async_callback(); + self.action.unbounded_send(SubscriptionAction::SetCallback(callback)) + } + + /// Used by [`Node`][crate::Node] to create a new subscription. + pub(crate) fn create( topic: &str, qos: QoSProfile, - callback: impl SubscriptionAsyncCallback, + callback: AnySubscriptionCallback, + node_handle: &Arc, + commands: &Arc, + ) -> Result, RclrsError> { + let (sender, receiver) = unbounded(); + let subscription = Arc::new(Self::new(topic, qos, sender, Arc::clone(&node_handle))?); + + commands.run_detached(subscription_task( + callback, + receiver, + Arc::clone(&subscription.handle), + Arc::clone(&commands), + )); + + Ok(subscription) + } + + /// Instantiate the Subscription. + fn new( + topic: &str, + qos: QoSProfile, + action: UnboundedSender>, + node_handle: Arc, ) -> Result // This uses pub(crate) visibility to avoid instantiating this struct outside // [`Node::create_subscription`], see the struct's documentation for the rationale @@ -153,29 +216,9 @@ where let handle = Arc::new(SubscriptionHandle { rcl_subscription: Mutex::new(rcl_subscription), node_handle, - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), }); - Ok(Self { - handle, - callback: Mutex::new(callback.into_callback()), - message: PhantomData, - }) - } - - /// Returns the topic name of the subscription. - /// - /// This returns the topic name after remapping, so it is not necessarily the - /// topic name which was used when creating the subscription. - pub fn topic_name(&self) -> String { - // SAFETY: No preconditions for the function used - // The unsafe variables get converted to safe types before being returned - unsafe { - let raw_topic_pointer = rcl_subscription_get_topic_name(&*self.handle.lock()); - CStr::from_ptr(raw_topic_pointer) - .to_string_lossy() - .into_owned() - } + Ok(Self { handle, action }) } } diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 2ef99c026..3d7bedf6d 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -15,70 +15,24 @@ // DISTRIBUTION A. Approved for public release; distribution unlimited. // OPSEC #4584. -use std::{sync::Arc, time::Duration, vec::Vec}; +use std::{sync::Arc, time::Duration, vec::Vec, collections::HashSet}; +use by_address::ByAddress; use crate::{ error::{to_rclrs_result, RclReturnCode, RclrsError, ToResult}, rcl_bindings::*, - ClientBase, Context, ContextHandle, Node, ServiceBase, SubscriptionBase, + ClientBase, Context, ContextHandle, ServiceBase, SubscriptionBase, }; -mod exclusivity_guard; mod guard_condition; -use exclusivity_guard::*; pub use guard_condition::*; -/// Manage the lifecycle of an `rcl_wait_set_t`, including managing its dependency -/// on `rcl_context_t` by ensuring that this dependency is [dropped after][1] the -/// `rcl_wait_set_t`. -/// -/// [1]: -struct WaitSetHandle { - rcl_wait_set: rcl_wait_set_t, - // Used to ensure the context is alive while the wait set is alive. - #[allow(dead_code)] - context_handle: Arc, -} - /// A struct for waiting on subscriptions and other waitable entities to become ready. pub struct WaitSet { - // The subscriptions that are currently registered in the wait set. - // This correspondence is an invariant that must be maintained by all functions, - // even in the error case. - subscriptions: Vec>>, - clients: Vec>>, - // The guard conditions that are currently registered in the wait set. - guard_conditions: Vec>>, - services: Vec>>, + entities: WaitSetEntities, handle: WaitSetHandle, } -/// A list of entities that are ready, returned by [`WaitSet::wait`]. -pub struct ReadyEntities { - /// A list of subscriptions that have potentially received messages. - pub subscriptions: Vec>, - /// A list of clients that have potentially received responses. - pub clients: Vec>, - /// A list of guard conditions that have been triggered. - pub guard_conditions: Vec>, - /// A list of services that have potentially received requests. - pub services: Vec>, -} - -impl Drop for rcl_wait_set_t { - fn drop(&mut self) { - // SAFETY: No preconditions for this function (besides passing in a valid wait set). - let rc = unsafe { rcl_wait_set_fini(self) }; - if let Err(e) = to_rclrs_result(rc) { - panic!("Unable to release WaitSet. {:?}", e) - } - } -} - -// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread -// they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for rcl_wait_set_t {} - // SAFETY: While the rcl_wait_set_t does have some interior mutability (because it has // members of non-const pointer type), this interior mutability is hidden/not used by // the WaitSet type. Therefore, sharing &WaitSet between threads does not risk data races. @@ -90,14 +44,11 @@ impl WaitSet { /// The given number of subscriptions is a capacity, corresponding to how often /// [`WaitSet::add_subscription`] may be called. pub fn new( - number_of_subscriptions: usize, - number_of_guard_conditions: usize, - number_of_timers: usize, - number_of_clients: usize, - number_of_services: usize, - number_of_events: usize, + mut entities: WaitSetEntities, context: &Context, ) -> Result { + entities.dedup(); + let rcl_wait_set = unsafe { // SAFETY: Getting a zero-initialized value is always safe let mut rcl_wait_set = rcl_get_zero_initialized_wait_set(); @@ -106,67 +57,44 @@ impl WaitSet { // There are no other preconditions. rcl_wait_set_init( &mut rcl_wait_set, - number_of_subscriptions, - number_of_guard_conditions, - number_of_timers, - number_of_clients, - number_of_services, - number_of_events, + entities.subscriptions.len(), + entities.guard_conditions.len(), + 0, + entities.clients.len(), + entities.services.len(), + 0, &mut *rcl_context, rcutils_get_default_allocator(), ) .ok()?; rcl_wait_set }; - Ok(Self { - subscriptions: Vec::new(), - guard_conditions: Vec::new(), - clients: Vec::new(), - services: Vec::new(), - handle: WaitSetHandle { - rcl_wait_set, - context_handle: Arc::clone(&context.handle), - }, - }) - } - /// Creates a new wait set and adds all waitable entities in the node to it. - /// - /// The wait set is sized to fit the node exactly, so there is no capacity for adding other entities. - pub fn new_for_node(node: &Node) -> Result { - let live_subscriptions = node.live_subscriptions(); - let live_clients = node.live_clients(); - let live_guard_conditions = node.live_guard_conditions(); - let live_services = node.live_services(); - let ctx = Context { - handle: Arc::clone(&node.handle.context_handle), + let handle = WaitSetHandle { + rcl_wait_set, + context_handle: Arc::clone(&context.handle), }; - let mut wait_set = WaitSet::new( - live_subscriptions.len(), - live_guard_conditions.len(), - 0, - live_clients.len(), - live_services.len(), - 0, - &ctx, - )?; - - for live_subscription in &live_subscriptions { - wait_set.add_subscription(live_subscription.clone())?; - } - for live_client in &live_clients { - wait_set.add_client(live_client.clone())?; - } + let mut wait_set = Self { entities, handle }; + wait_set.register_rcl_entities()?; + Ok(wait_set) + } - for live_guard_condition in &live_guard_conditions { - wait_set.add_guard_condition(live_guard_condition.clone())?; - } + /// Take all the items out of `entities` and move them into this wait set. + pub fn add(&mut self, entities: &mut WaitSetEntities) -> Result<(), RclrsError> { + self.entities.append(entities); + self.entities.dedup(); + self.resize_rcl_containers()?; + self.register_rcl_entities()?; + Ok(()) + } - for live_service in &live_services { - wait_set.add_service(live_service.clone())?; - } - Ok(wait_set) + /// Remove the specified entities from this wait set. + fn remove(&mut self, entities: &WaitSetEntities) -> Result<(), RclrsError> { + self.entities.remove_and_dedup(entities); + self.resize_rcl_containers()?; + self.register_rcl_entities()?; + Ok(()) } /// Removes all entities from the wait set. @@ -174,10 +102,7 @@ impl WaitSet { /// This effectively resets the wait set to the state it was in after being created by /// [`WaitSet::new`]. pub fn clear(&mut self) { - self.subscriptions.clear(); - self.guard_conditions.clear(); - self.clients.clear(); - self.services.clear(); + self.entities.clear(); // This cannot fail – the rcl_wait_set_clear function only checks that the input handle is // valid, which it always is in our case. Hence, only debug_assert instead of returning // Result. @@ -186,131 +111,6 @@ impl WaitSet { debug_assert_eq!(ret, 0); } - /// Adds a subscription to the wait set. - /// - /// # Errors - /// - If the subscription was already added to this wait set or another one, - /// [`AlreadyAddedToWaitSet`][1] will be returned - /// - If the number of subscriptions in the wait set is larger than the - /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned - /// - /// [1]: crate::RclrsError - /// [2]: crate::RclReturnCode - pub fn add_subscription( - &mut self, - subscription: Arc, - ) -> Result<(), RclrsError> { - let exclusive_subscription = ExclusivityGuard::new( - Arc::clone(&subscription), - Arc::clone(&subscription.handle().in_use_by_wait_set), - )?; - unsafe { - // SAFETY: I'm not sure if it's required, but the subscription pointer will remain valid - // for as long as the wait set exists, because it's stored in self.subscriptions. - // Passing in a null pointer for the third argument is explicitly allowed. - rcl_wait_set_add_subscription( - &mut self.handle.rcl_wait_set, - &*subscription.handle().lock(), - std::ptr::null_mut(), - ) - } - .ok()?; - self.subscriptions.push(exclusive_subscription); - Ok(()) - } - - /// Adds a guard condition to the wait set. - /// - /// # Errors - /// - If the guard condition was already added to this wait set or another one, - /// [`AlreadyAddedToWaitSet`][1] will be returned - /// - If the number of guard conditions in the wait set is larger than the - /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned - /// - /// [1]: crate::RclrsError - /// [2]: crate::RclReturnCode - pub fn add_guard_condition( - &mut self, - guard_condition: Arc, - ) -> Result<(), RclrsError> { - let exclusive_guard_condition = ExclusivityGuard::new( - Arc::clone(&guard_condition), - Arc::clone(&guard_condition.in_use_by_wait_set), - )?; - - unsafe { - // SAFETY: Safe if the wait set and guard condition are initialized - rcl_wait_set_add_guard_condition( - &mut self.handle.rcl_wait_set, - &*guard_condition.handle.rcl_guard_condition.lock().unwrap(), - std::ptr::null_mut(), - ) - .ok()?; - } - self.guard_conditions.push(exclusive_guard_condition); - Ok(()) - } - - /// Adds a client to the wait set. - /// - /// # Errors - /// - If the client was already added to this wait set or another one, - /// [`AlreadyAddedToWaitSet`][1] will be returned - /// - If the number of clients in the wait set is larger than the - /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned - /// - /// [1]: crate::RclrsError - /// [2]: crate::RclReturnCode - pub fn add_client(&mut self, client: Arc) -> Result<(), RclrsError> { - let exclusive_client = ExclusivityGuard::new( - Arc::clone(&client), - Arc::clone(&client.handle().in_use_by_wait_set), - )?; - unsafe { - // SAFETY: I'm not sure if it's required, but the client pointer will remain valid - // for as long as the wait set exists, because it's stored in self.clients. - // Passing in a null pointer for the third argument is explicitly allowed. - rcl_wait_set_add_client( - &mut self.handle.rcl_wait_set, - &*client.handle().lock() as *const _, - core::ptr::null_mut(), - ) - } - .ok()?; - self.clients.push(exclusive_client); - Ok(()) - } - - /// Adds a service to the wait set. - /// - /// # Errors - /// - If the service was already added to this wait set or another one, - /// [`AlreadyAddedToWaitSet`][1] will be returned - /// - If the number of services in the wait set is larger than the - /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned - /// - /// [1]: crate::RclrsError - /// [2]: crate::RclReturnCode - pub fn add_service(&mut self, service: Arc) -> Result<(), RclrsError> { - let exclusive_service = ExclusivityGuard::new( - Arc::clone(&service), - Arc::clone(&service.handle().in_use_by_wait_set), - )?; - unsafe { - // SAFETY: I'm not sure if it's required, but the service pointer will remain valid - // for as long as the wait set exists, because it's stored in self.services. - // Passing in a null pointer for the third argument is explicitly allowed. - rcl_wait_set_add_service( - &mut self.handle.rcl_wait_set, - &*service.handle().lock() as *const _, - core::ptr::null_mut(), - ) - } - .ok()?; - self.services.push(exclusive_service); - Ok(()) - } - /// Blocks until the wait set is ready, or until the timeout has been exceeded. /// /// If the timeout is `None` then this function will block indefinitely until @@ -334,7 +134,11 @@ impl WaitSet { /// This list is not comprehensive, since further errors may occur in the `rmw` or `rcl` layers. /// /// [1]: std::time::Duration::ZERO - pub fn wait(mut self, timeout: Option) -> Result { + pub fn wait( + mut self, + timeout: Option, + mut f: impl FnMut(WaitSetEntity) -> Result<(), RclrsError>, + ) -> Result<(), RclrsError> { let timeout_ns = match timeout.map(|d| d.as_nanos()) { None => -1, Some(ns) if ns <= i64::MAX as u128 => ns as i64, @@ -360,57 +164,284 @@ impl WaitSet { _ => return Err(error), }, } - let mut ready_entities = ReadyEntities { - subscriptions: Vec::new(), - clients: Vec::new(), - guard_conditions: Vec::new(), - services: Vec::new(), - }; - for (i, subscription) in self.subscriptions.iter().enumerate() { + + for (i, subscription) in self.entities.subscriptions.iter().enumerate() { // SAFETY: The `subscriptions` entry is an array of pointers, and this dereferencing is // equivalent to // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 let wait_set_entry = unsafe { *self.handle.rcl_wait_set.subscriptions.add(i) }; if !wait_set_entry.is_null() { - ready_entities - .subscriptions - .push(Arc::clone(&subscription.waitable)); + f(WaitSetEntity::Subscription(subscription))?; } } - for (i, client) in self.clients.iter().enumerate() { + for (i, client) in self.entities.clients.iter().enumerate() { // SAFETY: The `clients` entry is an array of pointers, and this dereferencing is // equivalent to // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 let wait_set_entry = unsafe { *self.handle.rcl_wait_set.clients.add(i) }; if !wait_set_entry.is_null() { - ready_entities.clients.push(Arc::clone(&client.waitable)); + f(WaitSetEntity::Client(client))?; } } - for (i, guard_condition) in self.guard_conditions.iter().enumerate() { + for (i, guard_condition) in self.entities.guard_conditions.iter().enumerate() { // SAFETY: The `clients` entry is an array of pointers, and this dereferencing is // equivalent to // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 let wait_set_entry = unsafe { *self.handle.rcl_wait_set.guard_conditions.add(i) }; if !wait_set_entry.is_null() { - ready_entities - .guard_conditions - .push(Arc::clone(&guard_condition.waitable)); + f(WaitSetEntity::GuardCondition(guard_condition))?; } } - for (i, service) in self.services.iter().enumerate() { + for (i, service) in self.entities.services.iter().enumerate() { // SAFETY: The `services` entry is an array of pointers, and this dereferencing is // equivalent to // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 let wait_set_entry = unsafe { *self.handle.rcl_wait_set.services.add(i) }; if !wait_set_entry.is_null() { - ready_entities.services.push(Arc::clone(&service.waitable)); + f(WaitSetEntity::Service(service))?; } } - Ok(ready_entities) + Ok(()) } + + fn resize_rcl_containers(&mut self) -> Result<(), RclrsError> { + unsafe { + rcl_wait_set_resize( + &mut self.handle.rcl_wait_set, + self.entities.subscriptions.len(), + self.entities.guard_conditions.len(), + 0, + self.entities.clients.len(), + self.entities.services.len(), + 0, + ) + } + .ok() + } + + fn register_rcl_entities(&mut self) -> Result<(), RclrsError> { + self.register_rcl_subscriptions()?; + self.register_rcl_guard_conditions()?; + self.register_rcl_clients()?; + self.register_rcl_services()?; + Ok(()) + } + + /// Adds a subscription to the wait set. + /// + /// # Errors + /// - If the subscription was already added to this wait set or another one, + /// [`AlreadyAddedToWaitSet`][1] will be returned + /// - If the number of subscriptions in the wait set is larger than the + /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned + /// + /// [1]: crate::RclrsError + /// [2]: crate::RclReturnCode + fn register_rcl_subscriptions( + &mut self, + ) -> Result<(), RclrsError> { + for subscription in &self.entities.subscriptions { + unsafe { + // SAFETY: I'm not sure if it's required, but the subscription pointer will remain valid + // for as long as the wait set exists, because it's stored in self.subscriptions. + // Passing in a null pointer for the third argument is explicitly allowed. + rcl_wait_set_add_subscription( + &mut self.handle.rcl_wait_set, + &*subscription.handle().lock(), + std::ptr::null_mut(), + ) + } + .ok()?; + } + Ok(()) + } + + /// Adds a guard condition to the wait set. + /// + /// # Errors + /// - If the guard condition was already added to this wait set or another one, + /// [`AlreadyAddedToWaitSet`][1] will be returned + /// - If the number of guard conditions in the wait set is larger than the + /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned + /// + /// [1]: crate::RclrsError + /// [2]: crate::RclReturnCode + fn register_rcl_guard_conditions( + &mut self, + ) -> Result<(), RclrsError> { + for guard_condition in &self.entities.guard_conditions { + unsafe { + // SAFETY: Safe if the wait set and guard condition are initialized + rcl_wait_set_add_guard_condition( + &mut self.handle.rcl_wait_set, + &*guard_condition.handle.rcl_guard_condition.lock().unwrap(), + std::ptr::null_mut(), + ) + .ok()?; + } + } + Ok(()) + } + + /// Adds a client to the wait set. + /// + /// # Errors + /// - If the client was already added to this wait set or another one, + /// [`AlreadyAddedToWaitSet`][1] will be returned + /// - If the number of clients in the wait set is larger than the + /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned + /// + /// [1]: crate::RclrsError + /// [2]: crate::RclReturnCode + fn register_rcl_clients(&mut self) -> Result<(), RclrsError> { + for client in &self.entities.clients { + unsafe { + // SAFETY: I'm not sure if it's required, but the client pointer will remain valid + // for as long as the wait set exists, because it's stored in self.clients. + // Passing in a null pointer for the third argument is explicitly allowed. + rcl_wait_set_add_client( + &mut self.handle.rcl_wait_set, + &*client.handle().lock() as *const _, + core::ptr::null_mut(), + ) + } + .ok()?; + } + Ok(()) + } + + /// Adds a service to the wait set. + /// + /// # Errors + /// - If the service was already added to this wait set or another one, + /// [`AlreadyAddedToWaitSet`][1] will be returned + /// - If the number of services in the wait set is larger than the + /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned + /// + /// [1]: crate::RclrsError + /// [2]: crate::RclReturnCode + fn register_rcl_services(&mut self) -> Result<(), RclrsError> { + for service in &self.entities.services { + unsafe { + // SAFETY: I'm not sure if it's required, but the service pointer will remain valid + // for as long as the wait set exists, because it's stored in self.services. + // Passing in a null pointer for the third argument is explicitly allowed. + rcl_wait_set_add_service( + &mut self.handle.rcl_wait_set, + &*service.handle().lock() as *const _, + core::ptr::null_mut(), + ) + } + .ok()?; + } + Ok(()) + } +} + +/// This is a container for all wait set entities that rclrs currently supports +#[derive(Clone, Default)] +pub struct WaitSetEntities { + // The subscriptions that are currently registered in the wait set. + // This correspondence is an invariant that must be maintained by all functions, + // even in the error case. + pub subscriptions: Vec>, + pub clients: Vec>, + // The guard conditions that are currently registered in the wait set. + pub guard_conditions: Vec>, + pub services: Vec>, +} + +impl WaitSetEntities { + /// Create a new empty container + pub fn new() -> Self { + Self::default() + } + + /// Ensure there is only one instance of each entity in the collection. + pub fn dedup(&mut self) { + dedup_vec_by_arc_address(&mut self.subscriptions); + dedup_vec_by_arc_address(&mut self.clients); + dedup_vec_by_arc_address(&mut self.guard_conditions); + dedup_vec_by_arc_address(&mut self.services); + } + + /// Move all entities out of `other` into `self`, leaving `other` empty. + pub fn append(&mut self, other: &mut Self) { + self.subscriptions.append(&mut other.subscriptions); + self.clients.append(&mut other.clients); + self.guard_conditions.append(&mut other.guard_conditions); + self.services.append(&mut other.services); + } + + /// Remove all entities that are present in `other`. This will also + /// deduplicate any items that were already in `self`. + pub fn remove_and_dedup(&mut self, other: &Self) { + remove_vec_by_arc_address(&mut self.subscriptions, &other.subscriptions); + remove_vec_by_arc_address(&mut self.clients, &other.clients); + remove_vec_by_arc_address(&mut self.guard_conditions, &other.guard_conditions); + remove_vec_by_arc_address(&mut self.services, &other.services); + } + + /// Clear all items from the container + pub fn clear(&mut self) { + self.subscriptions.clear(); + self.clients.clear(); + self.guard_conditions.clear(); + self.services.clear(); + } +} + +fn dedup_vec_by_arc_address(v: &mut Vec>) { + let mut set = HashSet::new(); + v.retain(|item| set.insert(ByAddress(Arc::clone(item)))); +} + +fn remove_vec_by_arc_address( + v: &mut Vec>, + remove: &Vec>, +) { + let mut set = HashSet::new(); + for r in remove { + set.insert(ByAddress(Arc::clone(r))); + } + + v.retain(|item| set.insert(ByAddress(Arc::clone(item)))); +} + +pub enum WaitSetEntity<'a> { + Subscription(&'a Arc), + Client(&'a Arc), + Service(&'a Arc), + GuardCondition(&'a Arc), +} + +impl Drop for rcl_wait_set_t { + fn drop(&mut self) { + // SAFETY: No preconditions for this function (besides passing in a valid wait set). + let rc = unsafe { rcl_wait_set_fini(self) }; + if let Err(e) = to_rclrs_result(rc) { + panic!("Unable to release WaitSet. {:?}", e) + } + } +} + +// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread +// they are running in. Therefore, this type can be safely sent to another thread. +unsafe impl Send for rcl_wait_set_t {} + +/// Manage the lifecycle of an `rcl_wait_set_t`, including managing its dependency +/// on `rcl_context_t` by ensuring that this dependency is [dropped after][1] the +/// `rcl_wait_set_t`. +/// +/// [1]: +struct WaitSetHandle { + rcl_wait_set: rcl_wait_set_t, + // Used to ensure the context is alive while the wait set is alive. + #[allow(dead_code)] + context_handle: Arc, } #[cfg(test)] @@ -431,12 +462,22 @@ mod tests { let guard_condition = Arc::new(GuardCondition::new(&context)); - let mut wait_set = WaitSet::new(0, 1, 0, 0, 0, 0, &context)?; - wait_set.add_guard_condition(Arc::clone(&guard_condition))?; + let mut entities = WaitSetEntities::new(); + entities.guard_conditions.push(Arc::clone(&guard_condition)); + + let wait_set = WaitSet::new(entities, &context)?; guard_condition.trigger()?; - let readies = wait_set.wait(Some(std::time::Duration::from_millis(10)))?; - assert!(readies.guard_conditions.contains(&guard_condition)); + let mut triggered = false; + wait_set.wait( + Some(std::time::Duration::from_millis(10)), + |entity| { + assert!(matches!(entity, WaitSetEntity::GuardCondition(_))); + triggered = true; + Ok(()) + }, + )?; + assert!(triggered); Ok(()) } diff --git a/rclrs/src/wait/exclusivity_guard.rs b/rclrs/src/wait/exclusivity_guard.rs deleted file mode 100644 index 7c18ba66d..000000000 --- a/rclrs/src/wait/exclusivity_guard.rs +++ /dev/null @@ -1,58 +0,0 @@ -use std::sync::{ - atomic::{AtomicBool, Ordering}, - Arc, -}; - -use crate::RclrsError; - -/// A helper struct for tracking whether the waitable is currently in a wait set. -/// -/// When this struct is constructed, which happens when adding an entity to the wait set, -/// it checks that the atomic boolean is false and sets it to true. -/// When it is dropped, which happens when it is removed from the wait set, -/// or the wait set itself is dropped, it sets the atomic bool to false. -pub(super) struct ExclusivityGuard { - in_use_by_wait_set: Arc, - pub(super) waitable: T, -} - -impl Drop for ExclusivityGuard { - fn drop(&mut self) { - self.in_use_by_wait_set.store(false, Ordering::Relaxed) - } -} - -impl ExclusivityGuard { - pub fn new(waitable: T, in_use_by_wait_set: Arc) -> Result { - if in_use_by_wait_set - .compare_exchange(false, true, Ordering::Relaxed, Ordering::Relaxed) - .is_err() - { - return Err(RclrsError::AlreadyAddedToWaitSet); - } - Ok(Self { - in_use_by_wait_set, - waitable, - }) - } -} - -#[cfg(test)] -mod tests { - use std::sync::{ - atomic::{AtomicBool, Ordering}, - Arc, - }; - - use super::*; - - #[test] - fn test_exclusivity_guard() { - let atomic = Arc::new(AtomicBool::new(false)); - let eg = ExclusivityGuard::new((), Arc::clone(&atomic)).unwrap(); - assert!(ExclusivityGuard::new((), Arc::clone(&atomic)).is_err()); - drop(eg); - assert!(!atomic.load(Ordering::Relaxed)); - assert!(ExclusivityGuard::new((), Arc::clone(&atomic)).is_ok()); - } -} From cda131d5316da300b74081221fdc42ac8ae994f1 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 10 Oct 2024 09:32:21 +0000 Subject: [PATCH 06/48] Reimagining the way wait sets are handled Signed-off-by: Michael X. Grey --- rclrs/src/lib.rs | 40 ------------- rclrs/src/subscription.rs | 37 ++++++------ rclrs/src/wait.rs | 68 +++++++++++----------- rclrs/src/wait/waitable.rs | 112 +++++++++++++++++++++++++++++++++++++ 4 files changed, 165 insertions(+), 92 deletions(-) create mode 100644 rclrs/src/wait/waitable.rs diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index d05930ee3..641836cc2 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -71,43 +71,3 @@ pub fn spin(node: Arc) -> Result<(), RclrsError> { 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) -> NodeOptions { - Node::builder(context, node_name) -} diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 66a653bc2..20515916c 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -11,7 +11,8 @@ use crate::{ error::ToResult, qos::QoSProfile, rcl_bindings::*, - ExecutorCommands, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + ExecutorCommands, NodeHandle, RclrsError, Waitable, Executable, + ENTITY_LIFECYCLE_MUTEX, }; mod any_callback; @@ -65,14 +66,6 @@ impl Drop for SubscriptionHandle { } } -/// 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); -} - /// Struct for receiving messages of type `T`. /// /// There can be multiple subscriptions for the same topic, in different nodes or the same node. @@ -222,18 +215,24 @@ where } } -impl SubscriptionBase for Subscription -where - T: Message, -{ - fn handle(&self) -> &SubscriptionHandle { - &self.handle +struct SubscriptionWaitable { + handle: Arc, + action: UnboundedSender>, +} + +impl Executable for SubscriptionWaitable { + fn execute(&mut self) -> Result<(), RclrsError> { + self.action.unbounded_send(SubscriptionAction::Execute).ok(); + Ok(()) } +} + +impl Waitable for SubscriptionWaitable { + unsafe fn add_to_wait_set( + &mut self, + wait_set: &mut crate::WaitSet, + ) -> Result { - fn execute(&self) { - if let Err(_) = self.action.unbounded_send(SubscriptionAction::Execute) { - // TODO(@mxgrey): Log the error here once logging is implemented - } } } diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 3d7bedf6d..72c5188c6 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -15,21 +15,24 @@ // DISTRIBUTION A. Approved for public release; distribution unlimited. // OPSEC #4584. -use std::{sync::Arc, time::Duration, vec::Vec, collections::HashSet}; +use std::{sync::Arc, time::Duration, vec::Vec, collections::{HashSet, HashMap}}; use by_address::ByAddress; use crate::{ error::{to_rclrs_result, RclReturnCode, RclrsError, ToResult}, rcl_bindings::*, - ClientBase, Context, ContextHandle, ServiceBase, SubscriptionBase, + Context, ContextHandle, }; mod guard_condition; pub use guard_condition::*; +mod waitable; +pub use waitable::*; + /// A struct for waiting on subscriptions and other waitable entities to become ready. pub struct WaitSet { - entities: WaitSetEntities, + entities: HashMap>, handle: WaitSetHandle, } @@ -49,25 +52,9 @@ impl WaitSet { ) -> Result { entities.dedup(); + let count = WaitableCount::new(); let rcl_wait_set = unsafe { - // SAFETY: Getting a zero-initialized value is always safe - let mut rcl_wait_set = rcl_get_zero_initialized_wait_set(); - let mut rcl_context = context.handle.rcl_context.lock().unwrap(); - // SAFETY: We're passing in a zero-initialized wait set and a valid context. - // There are no other preconditions. - rcl_wait_set_init( - &mut rcl_wait_set, - entities.subscriptions.len(), - entities.guard_conditions.len(), - 0, - entities.clients.len(), - entities.services.len(), - 0, - &mut *rcl_context, - rcutils_get_default_allocator(), - ) - .ok()?; - rcl_wait_set + count.initialize(&mut context.handle.rcl_context.lock().unwrap())? }; let handle = WaitSetHandle { @@ -123,6 +110,9 @@ impl WaitSet { /// that period of time has elapsed or the wait set becomes ready, which ever /// comes first. /// + /// Once one or more items in the wait set are ready, `f` will be triggered + /// for each ready item. + /// /// This function does not change the entities registered in the wait set. /// /// # Errors @@ -135,7 +125,7 @@ impl WaitSet { /// /// [1]: std::time::Duration::ZERO pub fn wait( - mut self, + &mut self, timeout: Option, mut f: impl FnMut(WaitSetEntity) -> Result<(), RclrsError>, ) -> Result<(), RclrsError> { @@ -204,22 +194,34 @@ impl WaitSet { f(WaitSetEntity::Service(service))?; } } + + // Each time we call rcl_wait, the rcl_wait_set_t handle will have some + // of its entities set to null, so we need to put them back in. We do + // not need to resize the rcl_wait_set_t because no new entities could + // have been added while we had the mutable borrow of the WaitSet. + + // Note that self.clear() will not change the allocated size of each rcl + // entity container, so we do not need to resize before re-registering + // the rcl entities. + self.clear(); + self.register_rcl_entities(); + Ok(()) } + pub fn count(&self) -> WaitableCount { + let mut c = WaitableCount::new(); + for (kind, collection) in &self.entities { + c.add(*kind, collection.len()); + } + } + fn resize_rcl_containers(&mut self) -> Result<(), RclrsError> { + let count = self.count(); unsafe { - rcl_wait_set_resize( - &mut self.handle.rcl_wait_set, - self.entities.subscriptions.len(), - self.entities.guard_conditions.len(), - 0, - self.entities.clients.len(), - self.entities.services.len(), - 0, - ) + count.resize(&mut self.handle.rcl_wait_set)?; } - .ok() + Ok(()) } fn register_rcl_entities(&mut self) -> Result<(), RclrsError> { @@ -465,7 +467,7 @@ mod tests { let mut entities = WaitSetEntities::new(); entities.guard_conditions.push(Arc::clone(&guard_condition)); - let wait_set = WaitSet::new(entities, &context)?; + let mut wait_set = WaitSet::new(entities, &context)?; guard_condition.trigger()?; let mut triggered = false; diff --git a/rclrs/src/wait/waitable.rs b/rclrs/src/wait/waitable.rs new file mode 100644 index 000000000..6e097bb96 --- /dev/null +++ b/rclrs/src/wait/waitable.rs @@ -0,0 +1,112 @@ +use crate::{ + rcl_bindings::*, + RclrsError, +}; + +#[derive(Clone, Copy)] +pub(crate) enum WaitableKind { + Subscription, + GuardCondition, + Timer, + Client, + Service, + Event, +} + +#[derive(Default, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] +pub struct WaitableCount { + pub subscriptions: usize, + pub guard_conditions: usize, + pub timers: usize, + pub clients: usize, + pub services: usize, + pub events: usize, +} + +impl WaitableCount { + pub fn new() -> Self { + Self::default() + } + + pub(super) fn add(&mut self, kind: WaitableKind, count: usize) { + match kind { + WaitableKind::Subscription => self.subscriptions += count, + WaitableKind::GuardCondition => self.guard_conditions += count, + WaitableKind::Timer => self.timers += count, + WaitableKind::Client => self.clients += count, + WaitableKind::Service => self.services += count, + WaitableKind::Event => self.events += count, + } + } + + pub(super) unsafe fn initialize( + &self, + rcl_context: &mut rcl_context_s, + ) -> Result { + unsafe { + // SAFETY: Getting a zero-initialized value is always safe + let mut rcl_wait_set = rcl_get_zero_initialized_wait_set(); + // SAFETY: We're passing in a zero-initialized wait set and a valid context. + // There are no other preconditions. + rcl_wait_set_init( + &mut rcl_wait_set, + self.subscriptions, + self.guard_conditions, + self.timers, + self.clients, + self.services, + self.events, + &mut *rcl_context, + rcutils_get_default_allocator(), + ) + .ok()?; + Ok(rcl_wait_set) + } + } + + pub(super) unsafe fn resize( + &self, + rcl_wait_set: &mut rcl_wait_set_t, + ) -> Result<(), RclrsError> { + unsafe { + rcl_wait_set_resize( + rcl_wait_set, + self.subscriptions, + self.guard_conditions, + self.timers, + self.clients, + self.services, + self.events, + ) + } + .ok() + } +} + +pub trait Executable { + fn execute(&mut self) -> Result<(), RclrsError>; +} + +pub(crate) trait Waitable: Executable { + unsafe fn add_to_wait_set( + &mut self, + wait_set: &mut rcl_wait_set_t, + ) -> Result; + + fn kind(&self) -> WaitableKind; +} + +pub struct Waiter { + waitable: Box, + index_in_wait_set: Option, +} + +impl Waiter { + pub fn new(waitable: T) -> Self { + Self { + waitable: Box::new(waitable), + index_in_wait_set: None, + } + } + +} From d28ed70e3e0bfcf9fff2238dfd6017ba212702e3 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 10 Oct 2024 22:06:47 +0800 Subject: [PATCH 07/48] Finished reworking wait sets -- need to migrate all waitables Signed-off-by: Michael X. Grey --- rclrs/src/executor.rs | 20 +- rclrs/src/executor/basic_executor.rs | 2 +- rclrs/src/subscription.rs | 48 ++++- rclrs/src/wait.rs | 286 ++++----------------------- rclrs/src/wait/waitable.rs | 80 +++++++- 5 files changed, 163 insertions(+), 273 deletions(-) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 6c765d884..847f416fd 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -4,7 +4,7 @@ pub use self::basic_executor::*; use crate::{ rcl_bindings::rcl_context_is_valid, Node, NodeOptions, RclReturnCode, RclrsError, WaitSet, Context, - ContextHandle, WaitSetEntities, + ContextHandle, Waiter, }; use std::{ sync::{Arc, Mutex, Weak}, @@ -130,7 +130,7 @@ impl ExecutorCommands { F::Output: Send, { let (mut sender, receiver) = oneshot::channel(); - self.channel.add(Box::pin( + self.channel.add_async_task(Box::pin( async move { let cancellation = sender.cancellation(); let output = match select(cancellation, std::pin::pin!(f)).await { @@ -156,7 +156,7 @@ impl ExecutorCommands { F::Output: Send, { let (sender, receiver) = oneshot::channel(); - self.channel.add(Box::pin( + self.channel.add_async_task(Box::pin( async move { sender.send(f.await).ok(); } @@ -168,8 +168,12 @@ impl ExecutorCommands { &self.context } - pub(crate) fn add(&self, f: BoxFuture<'static, ()>) { - self.channel.add(f); + pub(crate) fn add_async_task(&self, f: BoxFuture<'static, ()>) { + self.channel.add_async_task(f); + } + + pub(crate) fn add_to_wait_set(&self, waiter: Waiter) { + self.channel.add_to_waitset(waiter); } } @@ -177,12 +181,10 @@ impl ExecutorCommands { /// run. pub trait ExecutorChannel: Send + Sync { /// Add a new item for the executor to run. - fn add(&self, f: BoxFuture<'static, ()>); + fn add_async_task(&self, f: BoxFuture<'static, ()>); /// Add new entities to the waitset of the executor. - fn add_to_waitset(new_entities: WaitSetEntities); - - // TODO(@mxgrey): create_guard_condition for waking up the waitset thread + fn add_to_waitset(&self, new_entity: Waiter); } /// This trait defines the interface for having an executor run. diff --git a/rclrs/src/executor/basic_executor.rs b/rclrs/src/executor/basic_executor.rs index d2c75f36b..096889384 100644 --- a/rclrs/src/executor/basic_executor.rs +++ b/rclrs/src/executor/basic_executor.rs @@ -28,7 +28,7 @@ pub struct BasicExecutorChannel { } impl ExecutorChannel for BasicExecutorChannel { - fn add(&self, f: futures::future::BoxFuture<'static, ()>) { + fn add_async_task(&self, f: futures::future::BoxFuture<'static, ()>) { } diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 20515916c..a9c423f79 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -11,8 +11,8 @@ use crate::{ error::ToResult, qos::QoSProfile, rcl_bindings::*, - ExecutorCommands, NodeHandle, RclrsError, Waitable, Executable, - ENTITY_LIFECYCLE_MUTEX, + ExecutorCommands, NodeHandle, RclrsError, Waitable, Executable, WaitableKind, + WaitSet, Waiter, WaiterLifecycle, ENTITY_LIFECYCLE_MUTEX, }; mod any_callback; @@ -94,6 +94,9 @@ where /// Holding onto this sender will keep the subscription task running. Once /// this sender is dropped, the subscription task will end itself. action: UnboundedSender>, + /// Holding onto this keeps the waiter for this subscription alive in the + /// wait set of the executor. + lifecycle: WaiterLifecycle, } impl Subscription @@ -148,7 +151,7 @@ where commands: &Arc, ) -> Result, RclrsError> { let (sender, receiver) = unbounded(); - let subscription = Arc::new(Self::new(topic, qos, sender, Arc::clone(&node_handle))?); + let (subscription, waiter) = Self::new(topic, qos, sender, Arc::clone(&node_handle))?; commands.run_detached(subscription_task( callback, @@ -157,6 +160,8 @@ where Arc::clone(&commands), )); + commands.add_to_wait_set(waiter); + Ok(subscription) } @@ -166,7 +171,7 @@ where qos: QoSProfile, action: UnboundedSender>, node_handle: Arc, - ) -> Result + ) -> Result<(Arc, Waiter), RclrsError> // This uses pub(crate) visibility to avoid instantiating this struct outside // [`Node::create_subscription`], see the struct's documentation for the rationale where @@ -211,7 +216,14 @@ where node_handle, }); - Ok(Self { handle, action }) + let (waiter, lifecycle) = Waiter::new(Box::new(SubscriptionWaitable { + handle: Arc::clone(&handle), + action: action.clone(), + })); + + let subscription = Arc::new(Self { handle, action, lifecycle }); + + Ok((subscription, waiter)) } } @@ -225,13 +237,37 @@ impl Executable for SubscriptionWaitable { self.action.unbounded_send(SubscriptionAction::Execute).ok(); Ok(()) } + + fn kind(&self) -> WaitableKind { + WaitableKind::Subscription + } } impl Waitable for SubscriptionWaitable { unsafe fn add_to_wait_set( &mut self, - wait_set: &mut crate::WaitSet, + wait_set: &mut WaitSet, ) -> Result { + let index: usize = 0; + unsafe { + // SAFETY: We are holding an Arc of the handle, so the subscription + // is still valid. + rcl_wait_set_add_subscription( + &mut self.handle.rcl_wait_set, + &*self.handle().lock(), + &mut index, + ) + } + .ok()?; + + Ok(index) + } + + unsafe fn is_ready( + &self, + wait_set: &rcl_wait_set_t, + index: usize, + ) -> bool { } } diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 72c5188c6..5e293da4f 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -15,8 +15,7 @@ // DISTRIBUTION A. Approved for public release; distribution unlimited. // OPSEC #4584. -use std::{sync::Arc, time::Duration, vec::Vec, collections::{HashSet, HashMap}}; -use by_address::ByAddress; +use std::{sync::Arc, time::Duration, vec::Vec, collections::HashMap}; use crate::{ error::{to_rclrs_result, RclReturnCode, RclrsError, ToResult}, @@ -42,16 +41,8 @@ pub struct WaitSet { unsafe impl Sync for WaitSet {} impl WaitSet { - /// Creates a new wait set. - /// - /// The given number of subscriptions is a capacity, corresponding to how often - /// [`WaitSet::add_subscription`] may be called. - pub fn new( - mut entities: WaitSetEntities, - context: &Context, - ) -> Result { - entities.dedup(); - + /// Creates a new empty wait set. + pub fn new(context: &Context) -> Result { let count = WaitableCount::new(); let rcl_wait_set = unsafe { count.initialize(&mut context.handle.rcl_context.lock().unwrap())? @@ -62,23 +53,23 @@ impl WaitSet { context_handle: Arc::clone(&context.handle), }; - let mut wait_set = Self { entities, handle }; + let mut wait_set = Self { entities: HashMap::new(), handle }; wait_set.register_rcl_entities()?; Ok(wait_set) } /// Take all the items out of `entities` and move them into this wait set. - pub fn add(&mut self, entities: &mut WaitSetEntities) -> Result<(), RclrsError> { - self.entities.append(entities); - self.entities.dedup(); - self.resize_rcl_containers()?; - self.register_rcl_entities()?; - Ok(()) - } - - /// Remove the specified entities from this wait set. - fn remove(&mut self, entities: &WaitSetEntities) -> Result<(), RclrsError> { - self.entities.remove_and_dedup(entities); + pub fn add( + &mut self, + entities: impl IntoIterator, + ) -> Result<(), RclrsError> { + for entity in entities { + if entity.in_wait_set() { + return Err(RclrsError::AlreadyAddedToWaitSet); + } + let kind = entity.waitable.kind(); + self.entities.insert(kind, entity); + } self.resize_rcl_containers()?; self.register_rcl_entities()?; Ok(()) @@ -127,7 +118,7 @@ impl WaitSet { pub fn wait( &mut self, timeout: Option, - mut f: impl FnMut(WaitSetEntity) -> Result<(), RclrsError>, + mut f: impl FnMut(&mut Executable) -> Result<(), RclrsError>, ) -> Result<(), RclrsError> { let timeout_ns = match timeout.map(|d| d.as_nanos()) { None => -1, @@ -155,50 +146,24 @@ impl WaitSet { }, } - for (i, subscription) in self.entities.subscriptions.iter().enumerate() { - // SAFETY: The `subscriptions` entry is an array of pointers, and this dereferencing is - // equivalent to - // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 - let wait_set_entry = unsafe { *self.handle.rcl_wait_set.subscriptions.add(i) }; - if !wait_set_entry.is_null() { - f(WaitSetEntity::Subscription(subscription))?; - } - } - - for (i, client) in self.entities.clients.iter().enumerate() { - // SAFETY: The `clients` entry is an array of pointers, and this dereferencing is - // equivalent to - // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 - let wait_set_entry = unsafe { *self.handle.rcl_wait_set.clients.add(i) }; - if !wait_set_entry.is_null() { - f(WaitSetEntity::Client(client))?; - } + // Remove any waitables that are no longer being used + for waiter in self.entities.values_mut() { + waiter.retain(|w| w.in_use()); } - for (i, guard_condition) in self.entities.guard_conditions.iter().enumerate() { - // SAFETY: The `clients` entry is an array of pointers, and this dereferencing is - // equivalent to - // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 - let wait_set_entry = unsafe { *self.handle.rcl_wait_set.guard_conditions.add(i) }; - if !wait_set_entry.is_null() { - f(WaitSetEntity::GuardCondition(guard_condition))?; - } - } - - for (i, service) in self.entities.services.iter().enumerate() { - // SAFETY: The `services` entry is an array of pointers, and this dereferencing is - // equivalent to - // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 - let wait_set_entry = unsafe { *self.handle.rcl_wait_set.services.add(i) }; - if !wait_set_entry.is_null() { - f(WaitSetEntity::Service(service))?; + // For the remaining entities, check if they were activated and then run + // the callback for those that were. + for waiter in self.entities.values_mut().flat_map(|v| v) { + if waiter.is_ready(&self.handle.rcl_wait_set) { + f(&mut waiter.waitable)?; } } // Each time we call rcl_wait, the rcl_wait_set_t handle will have some // of its entities set to null, so we need to put them back in. We do // not need to resize the rcl_wait_set_t because no new entities could - // have been added while we had the mutable borrow of the WaitSet. + // have been added while we had the mutable borrow of the WaitSet. Some + // entities could have been removed, but that does not require a resizing. // Note that self.clear() will not change the allocated size of each rcl // entity container, so we do not need to resize before re-registering @@ -209,6 +174,7 @@ impl WaitSet { Ok(()) } + /// Get a count of the different kinds of entities in the wait set. pub fn count(&self) -> WaitableCount { let mut c = WaitableCount::new(); for (kind, collection) in &self.entities { @@ -224,202 +190,22 @@ impl WaitSet { Ok(()) } - fn register_rcl_entities(&mut self) -> Result<(), RclrsError> { - self.register_rcl_subscriptions()?; - self.register_rcl_guard_conditions()?; - self.register_rcl_clients()?; - self.register_rcl_services()?; - Ok(()) - } - - /// Adds a subscription to the wait set. + /// Registers all the waitable entities with the rcl wait set. /// /// # Errors - /// - If the subscription was already added to this wait set or another one, - /// [`AlreadyAddedToWaitSet`][1] will be returned /// - If the number of subscriptions in the wait set is larger than the - /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned - /// - /// [1]: crate::RclrsError - /// [2]: crate::RclReturnCode - fn register_rcl_subscriptions( - &mut self, - ) -> Result<(), RclrsError> { - for subscription in &self.entities.subscriptions { - unsafe { - // SAFETY: I'm not sure if it's required, but the subscription pointer will remain valid - // for as long as the wait set exists, because it's stored in self.subscriptions. - // Passing in a null pointer for the third argument is explicitly allowed. - rcl_wait_set_add_subscription( - &mut self.handle.rcl_wait_set, - &*subscription.handle().lock(), - std::ptr::null_mut(), - ) - } - .ok()?; - } - Ok(()) - } - - /// Adds a guard condition to the wait set. - /// - /// # Errors - /// - If the guard condition was already added to this wait set or another one, - /// [`AlreadyAddedToWaitSet`][1] will be returned - /// - If the number of guard conditions in the wait set is larger than the - /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned - /// - /// [1]: crate::RclrsError - /// [2]: crate::RclReturnCode - fn register_rcl_guard_conditions( - &mut self, - ) -> Result<(), RclrsError> { - for guard_condition in &self.entities.guard_conditions { - unsafe { - // SAFETY: Safe if the wait set and guard condition are initialized - rcl_wait_set_add_guard_condition( - &mut self.handle.rcl_wait_set, - &*guard_condition.handle.rcl_guard_condition.lock().unwrap(), - std::ptr::null_mut(), - ) - .ok()?; - } - } - Ok(()) - } - - /// Adds a client to the wait set. - /// - /// # Errors - /// - If the client was already added to this wait set or another one, - /// [`AlreadyAddedToWaitSet`][1] will be returned - /// - If the number of clients in the wait set is larger than the - /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned + /// allocated size [`WaitSetFull`][1] will be returned. If this happens + /// then there is a bug in rclrs. /// - /// [1]: crate::RclrsError - /// [2]: crate::RclReturnCode - fn register_rcl_clients(&mut self) -> Result<(), RclrsError> { - for client in &self.entities.clients { - unsafe { - // SAFETY: I'm not sure if it's required, but the client pointer will remain valid - // for as long as the wait set exists, because it's stored in self.clients. - // Passing in a null pointer for the third argument is explicitly allowed. - rcl_wait_set_add_client( - &mut self.handle.rcl_wait_set, - &*client.handle().lock() as *const _, - core::ptr::null_mut(), - ) - } - .ok()?; - } - Ok(()) - } - - /// Adds a service to the wait set. - /// - /// # Errors - /// - If the service was already added to this wait set or another one, - /// [`AlreadyAddedToWaitSet`][1] will be returned - /// - If the number of services in the wait set is larger than the - /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned - /// - /// [1]: crate::RclrsError - /// [2]: crate::RclReturnCode - fn register_rcl_services(&mut self) -> Result<(), RclrsError> { - for service in &self.entities.services { - unsafe { - // SAFETY: I'm not sure if it's required, but the service pointer will remain valid - // for as long as the wait set exists, because it's stored in self.services. - // Passing in a null pointer for the third argument is explicitly allowed. - rcl_wait_set_add_service( - &mut self.handle.rcl_wait_set, - &*service.handle().lock() as *const _, - core::ptr::null_mut(), - ) - } - .ok()?; + /// [1]: crate::RclReturnCode + fn register_rcl_entities(&mut self) -> Result<(), RclrsError> { + for entity in self.entities.values_mut().flat_map(|c| c) { + entity.add_to_wait_set(&self.handle.rcl_wait_set)?; } Ok(()) } } -/// This is a container for all wait set entities that rclrs currently supports -#[derive(Clone, Default)] -pub struct WaitSetEntities { - // The subscriptions that are currently registered in the wait set. - // This correspondence is an invariant that must be maintained by all functions, - // even in the error case. - pub subscriptions: Vec>, - pub clients: Vec>, - // The guard conditions that are currently registered in the wait set. - pub guard_conditions: Vec>, - pub services: Vec>, -} - -impl WaitSetEntities { - /// Create a new empty container - pub fn new() -> Self { - Self::default() - } - - /// Ensure there is only one instance of each entity in the collection. - pub fn dedup(&mut self) { - dedup_vec_by_arc_address(&mut self.subscriptions); - dedup_vec_by_arc_address(&mut self.clients); - dedup_vec_by_arc_address(&mut self.guard_conditions); - dedup_vec_by_arc_address(&mut self.services); - } - - /// Move all entities out of `other` into `self`, leaving `other` empty. - pub fn append(&mut self, other: &mut Self) { - self.subscriptions.append(&mut other.subscriptions); - self.clients.append(&mut other.clients); - self.guard_conditions.append(&mut other.guard_conditions); - self.services.append(&mut other.services); - } - - /// Remove all entities that are present in `other`. This will also - /// deduplicate any items that were already in `self`. - pub fn remove_and_dedup(&mut self, other: &Self) { - remove_vec_by_arc_address(&mut self.subscriptions, &other.subscriptions); - remove_vec_by_arc_address(&mut self.clients, &other.clients); - remove_vec_by_arc_address(&mut self.guard_conditions, &other.guard_conditions); - remove_vec_by_arc_address(&mut self.services, &other.services); - } - - /// Clear all items from the container - pub fn clear(&mut self) { - self.subscriptions.clear(); - self.clients.clear(); - self.guard_conditions.clear(); - self.services.clear(); - } -} - -fn dedup_vec_by_arc_address(v: &mut Vec>) { - let mut set = HashSet::new(); - v.retain(|item| set.insert(ByAddress(Arc::clone(item)))); -} - -fn remove_vec_by_arc_address( - v: &mut Vec>, - remove: &Vec>, -) { - let mut set = HashSet::new(); - for r in remove { - set.insert(ByAddress(Arc::clone(r))); - } - - v.retain(|item| set.insert(ByAddress(Arc::clone(item)))); -} - -pub enum WaitSetEntity<'a> { - Subscription(&'a Arc), - Client(&'a Arc), - Service(&'a Arc), - GuardCondition(&'a Arc), -} - impl Drop for rcl_wait_set_t { fn drop(&mut self) { // SAFETY: No preconditions for this function (besides passing in a valid wait set). @@ -448,6 +234,7 @@ struct WaitSetHandle { #[cfg(test)] mod tests { + use std::sync::atomic::AtomicBool; use super::*; #[test] @@ -464,7 +251,8 @@ mod tests { let guard_condition = Arc::new(GuardCondition::new(&context)); - let mut entities = WaitSetEntities::new(); + let in_use = Arc::new(AtomicBool::new(true)); + let waiter = Waiter::new(..); entities.guard_conditions.push(Arc::clone(&guard_condition)); let mut wait_set = WaitSet::new(entities, &context)?; diff --git a/rclrs/src/wait/waitable.rs b/rclrs/src/wait/waitable.rs index 6e097bb96..7cfae9bf8 100644 --- a/rclrs/src/wait/waitable.rs +++ b/rclrs/src/wait/waitable.rs @@ -1,9 +1,14 @@ +use std::sync::{ + atomic::{AtomicBool, Ordering}, + Arc, +}; + use crate::{ rcl_bindings::*, - RclrsError, + RclrsError, WaitSet, }; -#[derive(Clone, Copy)] +#[derive(Clone, Copy, Hash, PartialEq, Eq, PartialOrd, Ord)] pub(crate) enum WaitableKind { Subscription, GuardCondition, @@ -83,30 +88,89 @@ impl WaitableCount { } } +/// This provides the public API for executing a waitable item. pub trait Executable { + /// Indicate what kind of executable this is. + fn kind(&self) -> WaitableKind; + + /// Trigger this executable to run. fn execute(&mut self) -> Result<(), RclrsError>; } +/// This provides the internal APIs for a waitable item to interact with the +/// wait set that manages it. pub(crate) trait Waitable: Executable { + /// Add this to a wait set unsafe fn add_to_wait_set( &mut self, wait_set: &mut rcl_wait_set_t, ) -> Result; - fn kind(&self) -> WaitableKind; + unsafe fn is_ready( + &self, + wait_set: &rcl_wait_set_t, + index: usize, + ) -> bool; } pub struct Waiter { - waitable: Box, + pub(super) waitable: Box, + in_use: Arc, index_in_wait_set: Option, } impl Waiter { - pub fn new(waitable: T) -> Self { - Self { - waitable: Box::new(waitable), + pub fn new(waitable: Box) -> (Self, WaiterLifecycle) { + let in_use = Arc::new(AtomicBool::new(true)); + let waiter = Self { + waitable, + in_use: Arc::clone(&in_use), index_in_wait_set: None, - } + }; + + let lifecycle = WaiterLifecycle { in_use }; + (waiter, lifecycle) + } + + pub(super) fn in_wait_set(&self) -> bool { + self.index_in_wait_set.is_some() + } + + pub(super) fn in_use(&self) -> bool { + self.in_use.load(Ordering::Relaxed) + } + + pub(super) fn is_ready(&self, wait_set: &rcl_wait_set_t) -> bool { + self.index_in_wait_set.is_some_and(|index| + unsafe { + // SAFETY: The Waitable::is_ready function is marked as unsafe + // because this is the only place that it makes sense to use it. + self.waitable.is_ready(wait_set, index) + } + ) } + pub(super) fn add_to_wait_set( + &mut self, + wait_set: &mut WaitSet, + ) -> Result<(), RclrsError> { + self.index_in_wait_set = Some( + unsafe { + // SAFETY: The Waitable::add_to_wait_set function is marked as + // unsafe because this is the only place that it makes sense to use it. + self.waitable.add_to_wait_set(wait_set)? + } + ); + Ok(()) + } +} + +pub struct WaiterLifecycle { + in_use: Arc, +} + +impl Drop for WaiterLifecycle { + fn drop(&mut self) { + self.in_use.store(false, Ordering::Relaxed); + } } From abb83675d62d1310942395e1269e37156ebb39e2 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 11 Oct 2024 22:52:05 +0800 Subject: [PATCH 08/48] Finished migrating guard conditions and services Signed-off-by: Michael X. Grey --- rclrs/src/executor.rs | 9 +- rclrs/src/lib.rs | 2 + rclrs/src/node.rs | 106 ++++-- rclrs/src/qos.rs | 5 + rclrs/src/service.rs | 351 +++++++++++------- rclrs/src/service/any_service_callback.rs | 162 ++++++++ rclrs/src/service/service_async_callback.rs | 60 +++ rclrs/src/service/service_callback.rs | 75 ++++ rclrs/src/service/service_info.rs | 72 ++++ rclrs/src/service/service_task.rs | 38 ++ rclrs/src/subscription.rs | 146 ++++---- ...llback.rs => any_subscription_callback.rs} | 14 +- rclrs/src/subscription/message_info.rs | 37 +- .../subscription/readonly_loaned_message.rs | 2 +- ...back.rs => subscription_async_callback.rs} | 34 +- .../{callback.rs => subscription_callback.rs} | 34 +- rclrs/src/subscription/subscription_task.rs | 7 +- rclrs/src/wait.rs | 12 +- rclrs/src/wait/guard_condition.rs | 184 +++++---- rclrs/src/wait/waitable.rs | 30 +- 20 files changed, 996 insertions(+), 384 deletions(-) create mode 100644 rclrs/src/service/any_service_callback.rs create mode 100644 rclrs/src/service/service_async_callback.rs create mode 100644 rclrs/src/service/service_callback.rs create mode 100644 rclrs/src/service/service_info.rs create mode 100644 rclrs/src/service/service_task.rs rename rclrs/src/subscription/{any_callback.rs => any_subscription_callback.rs} (94%) rename rclrs/src/subscription/{async_callback.rs => subscription_async_callback.rs} (85%) rename rclrs/src/subscription/{callback.rs => subscription_callback.rs} (86%) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 847f416fd..a38970777 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -4,7 +4,7 @@ pub use self::basic_executor::*; use crate::{ rcl_bindings::rcl_context_is_valid, Node, NodeOptions, RclReturnCode, RclrsError, WaitSet, Context, - ContextHandle, Waiter, + ContextHandle, Waiter, GuardCondition, }; use std::{ sync::{Arc, Mutex, Weak}, @@ -175,6 +175,10 @@ impl ExecutorCommands { pub(crate) fn add_to_wait_set(&self, waiter: Waiter) { self.channel.add_to_waitset(waiter); } + + pub(crate) fn get_guard_condition(&self) -> &Arc { + &self.channel.get_guard_condition() + } } /// This trait defines the interface for passing new items into an executor to @@ -185,6 +189,9 @@ pub trait ExecutorChannel: Send + Sync { /// Add new entities to the waitset of the executor. fn add_to_waitset(&self, new_entity: Waiter); + + /// Get a guard condition that can be used to wake up the wait set of the executor. + fn get_guard_condition(&self) -> &Arc; } /// This trait defines the interface for having an executor run. diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 641836cc2..829561b3a 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -11,6 +11,7 @@ mod clock; mod context; mod error; mod executor; +mod guard_condition; mod node; mod parameter; mod publisher; @@ -38,6 +39,7 @@ pub use clock::*; pub use context::*; pub use error::*; pub use executor::*; +pub use guard_condition::*; pub use node::*; pub use parameter::*; pub use publisher::*; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 8a63972a9..f8e619499 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -15,9 +15,8 @@ pub use self::{options::*, graph::*}; use crate::{ rcl_bindings::*, Client, ClientBase, Clock, ContextHandle, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - SubscriptionAsyncCallback, ExecutorCommands, - TimeSource, ENTITY_LIFECYCLE_MUTEX, + RclrsError, Service, Subscription, SubscriptionCallback, SubscriptionAsyncCallback, + ServiceCallback, ServiceAsyncCallback, ExecutorCommands, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -60,10 +59,6 @@ unsafe impl Send for rcl_node_t {} /// [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>>, time_source: TimeSource, parameter: ParameterInterface, commands: Arc, @@ -211,33 +206,80 @@ impl Node { Ok(publisher) } - /// Creates a [`Service`][1]. + /// Creates a [`Service`] with an ordinary callback. /// - /// [1]: crate::Service - // TODO: make service's lifetime depend on node's lifetime - pub fn create_service( + /// 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>`. + // + // TODO(@mxgrey): Add examples showing each supported signature + pub fn create_service( &self, topic: &str, - callback: F, + qos: QoSProfile, + callback: impl ServiceCallback, ) -> Result>, RclrsError> where T: rosidl_runtime_rs::Service, - F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send, { - let service = Arc::new(Service::::new( - Arc::clone(&self.handle), + Service::::create( topic, - callback, - )?); - { self.services_mtx.lock().unwrap() } - .push(Arc::downgrade(&service) as Weak); - Ok(service) + qos, + callback.into_service_callback(), + &self.handle, + &self.commands, + ) } - /// Creates a [`Subscription`][1]. + /// Creates a [`Service`] with an async callback. + /// + /// 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. /// - /// [1]: crate::Subscription - // TODO: make subscription's lifetime depend on node's lifetime + /// 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). + // + // TODO(@mxgrey): Add examples showing each supported signature + pub fn create_async_service( + &self, + topic: &str, + qos: QoSProfile, + callback: impl ServiceAsyncCallback, + ) -> Result>, RclrsError> + where + T: rosidl_runtime_rs::Service, + { + Service::::create( + topic, + qos, + callback.into_service_async_callback(), + &self.handle, + &self.commands, + ) + } + + /// Creates a [`Subscription`] with an ordinary callback. + /// + /// 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>`. + // + // TODO(@mxgrey): Add examples showing each supported signature pub fn create_subscription( &self, topic: &str, @@ -250,13 +292,25 @@ impl Node { Subscription::::create( topic, qos, - callback.into_callback(), + callback.into_subscription_callback(), &self.handle, &self.commands, ) } - /// Creates a subscription with an async callback + /// Creates a [`Subscription`] with an async callback. + /// + /// 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>`. + // + // TODO(@mxgrey): Add examples showing each supported signature pub fn create_async_subscription( &self, topic: &str, @@ -269,7 +323,7 @@ impl Node { Subscription::::create( topic, qos, - callback.into_async_callback(), + callback.into_subscription_async_callback(), &self.handle, &self.commands, ) diff --git a/rclrs/src/qos.rs b/rclrs/src/qos.rs index b26f01ef8..20aefc48c 100644 --- a/rclrs/src/qos.rs +++ b/rclrs/src/qos.rs @@ -260,6 +260,11 @@ impl QoSProfile { self.lifespan = QoSDuration::Custom(lifespan); self } + + /// Get the default QoS profile for services. + pub fn services_default() -> Self { + QOS_PROFILE_SERVICES_DEFAULT + } } impl From for rmw_qos_history_policy_t { diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index ac43e51a8..b62c5009d 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -1,96 +1,140 @@ use std::{ boxed::Box, - ffi::CString, - sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, + ffi::{CStr, CString}, + sync::{Arc, Mutex, MutexGuard}, }; -use rosidl_runtime_rs::Message; +use futures::channel::mpsc::{unbounded, UnboundedSender, TrySendError}; use crate::{ - error::{RclReturnCode, ToResult}, + error::ToResult, rcl_bindings::*, - MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + NodeHandle, RclrsError, Waiter, WaiterLifecycle, GuardCondition, QoSProfile, + Executable, Waitable, WaitableKind, ENTITY_LIFECYCLE_MUTEX, ExecutorCommands, }; -// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread -// they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for rcl_service_t {} +mod any_service_callback; +pub use any_service_callback::*; -/// Manage the lifecycle of an `rcl_service_t`, including managing its dependencies -/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are -/// [dropped after][1] the `rcl_service_t`. -/// -/// [1]: -pub struct ServiceHandle { - rcl_service: Mutex, - node_handle: Arc, - pub(crate) in_use_by_wait_set: Arc, -} +mod service_async_callback; +pub use service_async_callback::*; -impl ServiceHandle { - pub(crate) fn lock(&self) -> MutexGuard { - self.rcl_service.lock().unwrap() - } -} +mod service_callback; +pub use service_callback::*; -impl Drop for ServiceHandle { - fn drop(&mut self) { - let rcl_service = self.rcl_service.get_mut().unwrap(); - let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); - let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); - // SAFETY: The entity lifecycle mutex is locked to protect against the risk of - // global variables in the rmw implementation being unsafely modified during cleanup. - unsafe { - rcl_service_fini(rcl_service, &mut *rcl_node); - } - } -} - -/// 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 service_info; +pub use service_info::*; -type ServiceCallback = - Box Response + 'static + Send>; +mod service_task; +use service_task::*; -/// Main class responsible for responding to requests sent by ROS clients. +/// Struct for responding to requests sent by ROS service clients. /// -/// 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. +/// The only way to instantiate a service is via [`Node::create_service()`][1] +/// or [`Node::create_async_service`][2]. +/// +/// Services should be unique per service name (including namespace) throughout +/// an entire ROS system. ROS does not currently support multiple services that +/// use the same name (including namespace). /// /// [1]: crate::Node::create_service -/// [2]: crate::Node +/// [2]: crate::Node::create_async_service pub struct Service where T: rosidl_runtime_rs::Service, { - pub(crate) handle: Arc, - /// The callback function that runs when a request was received. - pub callback: Mutex>, + /// This handle is used to access the data that rcl holds for this service. + handle: Arc, + /// This allows us to replace the callback in the service task. + /// + /// Holding onto this sender will keep the service task alive. Once this + /// sender is dropped, the service task will end itself. + action: UnboundedSender>, + /// Holding onto this keeps the waiter for this service alive in the wait + /// set of the executor. + lifecycle: WaiterLifecycle, } impl Service where T: rosidl_runtime_rs::Service, { - /// Creates a new service. - pub(crate) fn new( - node_handle: Arc, + /// Returns the name of the service. + /// + /// This returns the service name after remapping, so it is not necessarily the + /// service name which was used when creating the service. + pub fn service_name(&self) -> String { + // SAFETY: The service handle is valid because its lifecycle is managed by an Arc. + // The unsafe variables get converted to safe types before being returned + unsafe { + let raw_service_pointer = rcl_service_get_service_name(&*self.handle.lock()); + CStr::from_ptr(raw_service_pointer) + } + .to_string_lossy() + .into_owned() + } + + /// Set the callback of this service, replacing the callback that was + /// previously set. + /// + /// This can be used even if the service previously used an async callback. + pub fn set_callback( + &self, + callback: impl ServiceCallback, + ) -> Result<(), TrySendError>> { + let callback = callback.into_service_callback(); + self.action.unbounded_send(ServiceAction::SetCallback(callback)) + } + + /// Set the callback of this service, replacing the callback that was + /// previously set. + /// + /// This can be used even if the service previously used a non-async callback. + pub fn set_async_callback( + &self, + callback: impl ServiceAsyncCallback, + ) -> Result<(), TrySendError>> { + let callback = callback.into_service_async_callback(); + self.action.unbounded_send(ServiceAction::SetCallback(callback)) + } + + /// Used by [`Node`][crate::Node] to create a new service + pub(crate) fn create( 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, - { + qos: QoSProfile, + callback: AnyServiceCallback, + node_handle: &Arc, + commands: &Arc, + ) -> Result, RclrsError> { + let (sender, receiver) = unbounded(); + let (service, waiter) = Self::new( + topic, + qos, + sender, + Arc::clone(&node_handle), + Arc::clone(commands.get_guard_condition()), + )?; + + commands.run_detached(service_task( + callback, + receiver, + Arc::clone(&service.handle), + Arc::clone(commands), + )); + + commands.add_to_wait_set(waiter); + + Ok(service) + } + + /// Instantiate the service. + fn new( + topic: &str, + qos: QoSProfile, + action: UnboundedSender>, + node_handle: Arc, + guard_condition: Arc, + ) -> Result<(Arc, Waiter), RclrsError> { // 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() @@ -101,7 +145,8 @@ where })?; // 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(); @@ -128,92 +173,110 @@ where let handle = Arc::new(ServiceHandle { rcl_service: Mutex::new(rcl_service), node_handle, - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), }); - Ok(Self { - handle, - callback: Mutex::new(Box::new(callback)), - }) + let (waiter, lifecycle) = Waiter::new( + Box::new(ServiceWaitable { + handle: Arc::clone(&handle), + action: action.clone(), + }), + Some(guard_condition), + ); + + let service = Arc::new(Self { handle, action, lifecycle }); + + Ok((service, waiter)) } +} - /// Fetches a new request. - /// - /// When there is no new message, this will return a - /// [`ServiceTakeFailed`][1]. - /// - /// [1]: crate::RclrsError - // - // ```text - // +---------------------+ - // | rclrs::take_request | - // +----------+----------+ - // | - // | - // +----------v----------+ - // | rcl_take_request | - // +----------+----------+ - // | - // | - // +----------v----------+ - // | rmw_take | - // +---------------------+ - // ``` - pub fn take_request(&self) -> Result<(T::Request, rmw_request_id_t), RclrsError> { - let mut request_id_out = rmw_request_id_t { - writer_guid: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], - sequence_number: 0, - }; - type RmwMsg = - <::Request as rosidl_runtime_rs::Message>::RmwMsg; - let mut request_out = RmwMsg::::default(); - let handle = &*self.handle.lock(); - unsafe { - // SAFETY: The three pointers are valid/initialized - rcl_take_request( - handle, - &mut request_id_out, - &mut request_out as *mut RmwMsg as *mut _, - ) - } - .ok()?; - Ok((T::Request::from_rmw_message(request_out), request_id_out)) +struct ServiceWaitable { + handle: Arc, + action: UnboundedSender>, +} + +impl Executable for ServiceWaitable +where + T: rosidl_runtime_rs::Service, +{ + fn execute(&mut self) -> Result<(), RclrsError> { + self.action.unbounded_send(ServiceAction::Execute).ok(); + Ok(()) + } + + fn kind(&self) -> WaitableKind { + WaitableKind::Service } } -impl ServiceBase for Service +impl Waitable for ServiceWaitable where T: rosidl_runtime_rs::Service, { - fn handle(&self) -> &ServiceHandle { - &self.handle + unsafe fn add_to_wait_set( + &mut self, + wait_set: &mut rcl_wait_set_t, + ) -> Result { + let mut index: usize = 0; + unsafe { + // SAFETY: We are holding an Arc of the handle, so the service is + // still valid. + rcl_wait_set_add_service( + wait_set, + &*self.handle.lock(), + &mut index, + ) + } + .ok()?; + + Ok(index) } - 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), + unsafe fn is_ready( + &self, + wait_set: &rcl_wait_set_t, + index: usize, + ) -> bool { + let entity = unsafe { + // SAFETY: The `services` field is an array of pointers, and this + // dereferencing is equivalent to getting the element of the array + // at `index`. + *wait_set.services.add(index) }; - let res = (*self.callback.lock().unwrap())(&req_id, req); - let rmw_message = ::into_rmw_message(res.into_cow()); - let handle = &*self.handle.lock(); + + !entity.is_null() + } +} + +// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread +// they are running in. Therefore, this type can be safely sent to another thread. +unsafe impl Send for rcl_service_t {} + +/// Manage the lifecycle of an `rcl_service_t`, including managing its dependencies +/// on `rcl_node_t` and `rcl_context_t` by ensuring that these dependencies are +/// [dropped after][1] the `rcl_service_t`. +/// +/// [1]: +pub struct ServiceHandle { + rcl_service: Mutex, + node_handle: Arc, +} + +impl ServiceHandle { + pub(crate) fn lock(&self) -> MutexGuard { + self.rcl_service.lock().unwrap() + } +} + +impl Drop for ServiceHandle { + fn drop(&mut self) { + let rcl_service = self.rcl_service.get_mut().unwrap(); + let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of + // global variables in the rmw implementation being unsafely modified during cleanup. unsafe { - // SAFETY: The response type is guaranteed to match the service type by the type system. - rcl_send_response( - handle, - &mut req_id, - rmw_message.as_ref() as *const ::RmwMsg as *mut _, - ) + rcl_service_fini(rcl_service, &mut *rcl_node); } - .ok() } } @@ -245,11 +308,15 @@ mod tests { let _node_1_empty_service = graph .node1 - .create_service::("graph_test_topic_4", |_, _| { - srv::Empty_Response { - structure_needs_at_least_one_member: 0, - } - })?; + .create_service::( + "graph_test_topic_4", + QoSProfile::services_default(), + |_| { + 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..f220cedf9 --- /dev/null +++ b/rclrs/src/service/any_service_callback.rs @@ -0,0 +1,162 @@ +use rosidl_runtime_rs::{Service, Message}; + +use crate::{ + error::ToResult, + rcl_bindings::{ + rmw_request_id_t, rmw_service_info_t, rcl_take_request, rcl_take_request_with_info, + rcl_send_response, + }, + RequestId, ServiceInfo, ServiceHandle, ExecutorCommands, + RclrsError, RclReturnCode, MessageCow, +}; + +use futures::future::BoxFuture; + +use std::sync::Arc; + +/// An enum capturing the various possible function signatures for service callbacks. +pub enum AnyServiceCallback +where + T: Service, +{ + /// A callback that only takes in the request value + OnlyRequest(Box BoxFuture<'static, T::Response> + Send>), + /// A callback that takes in the request value and the ID of the request + WithId(Box BoxFuture<'static, T::Response> + Send>), + /// A callback that takes in the request value and all available + WithInfo(Box BoxFuture<'static, T::Response> + Send>), +} + +impl AnyServiceCallback { + pub(super) fn execute( + &mut self, + handle: &Arc, + commands: &Arc, + ) -> Result<(), RclrsError> { + let mut evaluate = || { + match self { + AnyServiceCallback::OnlyRequest(cb) => { + let (msg, mut rmw_request_id) = Self::take_request(handle)?; + let handle = Arc::clone(&handle); + let response = cb(msg); + commands.run_detached(async move { + // TODO(@mxgrey): Log any errors here when logging is available + Self::send_response(&handle, &mut rmw_request_id, response.await).ok(); + }); + } + AnyServiceCallback::WithId(cb) => { + let (msg, mut rmw_request_id) = Self::take_request(handle)?; + let request_id = RequestId::from_rmw_request_id(&rmw_request_id); + let handle = Arc::clone(&handle); + let response = cb(msg, request_id); + commands.run_detached(async move { + // TODO(@mxgrey): Log any errors here when logging is available + Self::send_response(&handle, &mut rmw_request_id, response.await).ok(); + }); + } + AnyServiceCallback::WithInfo(cb) => { + let (msg, rmw_service_info) = Self::take_request_with_info(handle)?; + let mut rmw_request_id = rmw_request_id_t { + writer_guid: rmw_service_info.request_id.writer_guid, + sequence_number: rmw_service_info.request_id.sequence_number, + }; + let service_info = ServiceInfo::from_rmw_service_info(&rmw_service_info); + let handle = Arc::clone(&handle); + let response = cb(msg, service_info); + commands.run_detached(async move { + // TODO(@mxgrey): Log any errors here when logging is available + Self::send_response(&handle, &mut rmw_request_id, response.await).ok(); + }); + } + } + + Ok(()) + }; + + match evaluate() { + Err(RclrsError::RclError { + code: RclReturnCode::ServiceTakeFailed, + .. + }) => { + // Spurious wakeup - this may happen even when a waitlist indicated that this + // subscription was ready, so it shouldn't be an error. + Ok(()) + } + other => other, + } + } + + /// Fetches a new request. + /// + /// When there is no new message, this will return a + /// [`ServiceTakeFailed`][1]. + /// + /// [1]: crate::RclrsError + // + // ```text + // +---------------------+ + // | rclrs::take_request | + // +----------+----------+ + // | + // | + // +----------v----------+ + // | rcl_take_request | + // +----------+----------+ + // | + // | + // +----------v----------+ + // | rmw_take | + // +---------------------+ + // ``` + fn take_request(handle: &ServiceHandle) -> Result<(T::Request, rmw_request_id_t), RclrsError> { + let mut request_id_out = RequestId::zero_initialized_rmw(); + type RmwMsg = <::Request as Message>::RmwMsg; + let mut request_out = RmwMsg::::default(); + let handle = &*handle.lock(); + unsafe { + // SAFETY: The three pointers are valid and initialized + rcl_take_request( + handle, + &mut request_id_out, + &mut request_out as *mut RmwMsg as *mut _, + ) + } + .ok()?; + Ok((T::Request::from_rmw_message(request_out), request_id_out)) + } + + /// Same as [`Self::take_request`] but includes additional info about the service + fn take_request_with_info(handle: &ServiceHandle) -> Result<(T::Request, rmw_service_info_t), RclrsError> { + let mut service_info_out = ServiceInfo::zero_initialized_rmw(); + type RmwMsg = <::Request as Message>::RmwMsg; + let mut request_out = RmwMsg::::default(); + let handle = &*handle.lock(); + unsafe { + // SAFETY: The three pointers are valid and initialized + rcl_take_request_with_info( + handle, + &mut service_info_out, + &mut request_out as *mut RmwMsg as *mut _, + ) + } + .ok()?; + Ok((T::Request::from_rmw_message(request_out), service_info_out)) + } + + fn send_response( + handle: &Arc, + request_id: &mut rmw_request_id_t, + response: T::Response, + ) -> Result<(), RclrsError> { + let rmw_message = ::into_rmw_message(response.into_cow()); + let handle = &*handle.lock(); + unsafe { + rcl_send_response( + handle, + request_id, + rmw_message.as_ref() as *const ::RmwMsg as *mut _, + ) + } + .ok() + } +} diff --git a/rclrs/src/service/service_async_callback.rs b/rclrs/src/service/service_async_callback.rs new file mode 100644 index 000000000..c8750ee30 --- /dev/null +++ b/rclrs/src/service/service_async_callback.rs @@ -0,0 +1,60 @@ +use rosidl_runtime_rs::Service; + +use super::{ + ServiceInfo, RequestId, + any_service_callback::AnyServiceCallback, +}; + +use std::future::Future; + +/// A trait for async callbacks of services. +/// +// TODO(@mxgrey): Add a description of what callbacks signatures are supported +pub trait ServiceAsyncCallback: Send + 'static +where + T: Service, +{ + /// Converts the callback into an enum. + /// + /// User code never needs to call this function. + fn into_service_async_callback(self) -> AnyServiceCallback; +} + +impl ServiceAsyncCallback for Func +where + T: Service, + Func: FnMut(T::Request) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_service_async_callback(mut self) -> AnyServiceCallback { + AnyServiceCallback::OnlyRequest(Box::new( + move |request| Box::pin(self(request)) + )) + } +} + +impl ServiceAsyncCallback for Func +where + T: Service, + Func: FnMut(T::Request, RequestId) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_service_async_callback(mut self) -> AnyServiceCallback { + AnyServiceCallback::WithId(Box::new( + move |request, request_id| Box::pin(self(request, request_id)) + )) + } +} + +impl ServiceAsyncCallback for Func +where + T: Service, + Func: FnMut(T::Request, ServiceInfo) -> F + Send + 'static, + F: Future + Send + 'static, +{ + fn into_service_async_callback(mut self) -> AnyServiceCallback { + AnyServiceCallback::WithInfo(Box::new( + move |request, service_info| Box::pin(self(request, service_info)) + )) + } +} diff --git a/rclrs/src/service/service_callback.rs b/rclrs/src/service/service_callback.rs new file mode 100644 index 000000000..2d0bffd4f --- /dev/null +++ b/rclrs/src/service/service_callback.rs @@ -0,0 +1,75 @@ +use rosidl_runtime_rs::Service; + +use super::{ + ServiceInfo, RequestId, + any_service_callback::AnyServiceCallback, +}; + +use std::sync::Arc; + +/// A trait for regular callbacks of services. +/// +// TODO(@mxgrey): Add a description of what callbacks signatures are supported +pub trait ServiceCallback: Send + 'static +where + T: Service, +{ + /// Converts the callback into an enum. + /// + /// User code never needs to call this function. + fn into_service_callback(self) -> AnyServiceCallback; +} + +impl ServiceCallback for Func +where + T: Service, + Func: Fn(T::Request) -> T::Response + Send + Sync + 'static, +{ + fn into_service_callback(self) -> AnyServiceCallback { + let func = Arc::new(self); + AnyServiceCallback::OnlyRequest(Box::new( + move |request| { + let f = Arc::clone(&func); + Box::pin(async move { + f(request) + }) + } + )) + } +} + +impl ServiceCallback for Func +where + T: Service, + Func: Fn(T::Request, RequestId) -> T::Response + Send + Sync + 'static, +{ + fn into_service_callback(self) -> AnyServiceCallback { + let func = Arc::new(self); + AnyServiceCallback::WithId(Box::new( + move |request, request_id| { + let f = Arc::clone(&func); + Box::pin(async move { + f(request, request_id) + }) + } + )) + } +} + +impl ServiceCallback for Func +where + T: Service, + Func: Fn(T::Request, ServiceInfo) -> T::Response + Send + Sync + 'static, +{ + fn into_service_callback(self) -> AnyServiceCallback { + let func = Arc::new(self); + AnyServiceCallback::WithInfo(Box::new( + move |request, service_info| { + let f = Arc::clone(&func); + Box::pin(async move { + f(request, service_info) + }) + } + )) + } +} diff --git a/rclrs/src/service/service_info.rs b/rclrs/src/service/service_info.rs new file mode 100644 index 000000000..99f6d49ec --- /dev/null +++ b/rclrs/src/service/service_info.rs @@ -0,0 +1,72 @@ +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(super) 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(super) 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(super) fn zero_initialized_rmw() -> rmw_request_id_t { + rmw_request_id_t { + writer_guid: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + sequence_number: 0, + } + } +} diff --git a/rclrs/src/service/service_task.rs b/rclrs/src/service/service_task.rs new file mode 100644 index 000000000..9bb45be07 --- /dev/null +++ b/rclrs/src/service/service_task.rs @@ -0,0 +1,38 @@ +use rosidl_runtime_rs::Service; + +use crate::{ + service::ServiceHandle, + AnyServiceCallback, ExecutorCommands, +}; + +use futures::{ + channel::mpsc::UnboundedReceiver, + StreamExt, +}; + +use std::sync::Arc; + +pub(super) enum ServiceAction { + Execute, + SetCallback(AnyServiceCallback), +} + +pub(super) async fn service_task( + mut callback: AnyServiceCallback, + mut receiver: UnboundedReceiver>, + handle: Arc, + commands: Arc, +) { + while let Some(action) = receiver.next().await { + match action { + ServiceAction::SetCallback(new_callback) => { + callback = new_callback; + } + ServiceAction::Execute => { + if let Err(_) = callback.execute(&handle, &commands) { + // TODO(@mxgrey): Log the error here once logging is implemented + } + } + } + } +} diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index a9c423f79..fead30176 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -12,17 +12,17 @@ use crate::{ qos::QoSProfile, rcl_bindings::*, ExecutorCommands, NodeHandle, RclrsError, Waitable, Executable, WaitableKind, - WaitSet, Waiter, WaiterLifecycle, ENTITY_LIFECYCLE_MUTEX, + GuardCondition, Waiter, WaiterLifecycle, ENTITY_LIFECYCLE_MUTEX, }; -mod any_callback; -pub use any_callback::*; +mod any_subscription_callback; +pub use any_subscription_callback::*; -mod async_callback; -pub use async_callback::*; +mod subscription_async_callback; +pub use subscription_async_callback::*; -mod callback; -pub use callback::*; +mod subscription_callback; +pub use subscription_callback::*; mod message_info; pub use message_info::*; @@ -33,41 +33,11 @@ pub use readonly_loaned_message::*; mod subscription_task; use subscription_task::*; -// 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]: -pub struct SubscriptionHandle { - rcl_subscription: Mutex, - node_handle: Arc, -} - -impl SubscriptionHandle { - pub(crate) fn lock(&self) -> MutexGuard { - self.rcl_subscription.lock().unwrap() - } -} - -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); - } - } -} - /// Struct for receiving messages of type `T`. /// +/// The only way to instantiate a subscription is via [`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. /// /// Receiving messages requires calling [`spin`][1] on the `Executor` of subscription's [Node][4]. @@ -75,9 +45,6 @@ impl Drop for SubscriptionHandle { /// 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()`][2] -/// or [`Node::create_async_subscription`][3]. This is to ensure that [`Node`][4]s can track all the subscriptions that have been created. -/// /// [1]: crate::Executor::spin /// [2]: crate::Node::create_subscription /// [3]: crate::Node::create_async_subscription @@ -88,10 +55,9 @@ where { /// This handle is used to access the data that rcl holds for this subscription. handle: Arc, - /// This allows us to trigger the callback or replace the callback in the - /// subscription task. + /// This allows us to replace the callback in the subscription task. /// - /// Holding onto this sender will keep the subscription task running. Once + /// Holding onto this sender will keep the subscription task alive. Once /// this sender is dropped, the subscription task will end itself. action: UnboundedSender>, /// Holding onto this keeps the waiter for this subscription alive in the @@ -108,14 +74,14 @@ where /// 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 + // 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() } + .to_string_lossy() + .into_owned() } /// Set the callback of this subscription, replacing the callback that was @@ -126,7 +92,7 @@ where &self, callback: impl SubscriptionCallback, ) -> Result<(), TrySendError>> { - let callback = callback.into_callback(); + let callback = callback.into_subscription_callback(); self.action.unbounded_send(SubscriptionAction::SetCallback(callback)) } @@ -138,7 +104,7 @@ where &self, callback: impl SubscriptionAsyncCallback, ) -> Result<(), TrySendError>> { - let callback = callback.into_async_callback(); + let callback = callback.into_subscription_async_callback(); self.action.unbounded_send(SubscriptionAction::SetCallback(callback)) } @@ -151,7 +117,13 @@ where commands: &Arc, ) -> Result, RclrsError> { let (sender, receiver) = unbounded(); - let (subscription, waiter) = Self::new(topic, qos, sender, Arc::clone(&node_handle))?; + let (subscription, waiter) = Self::new( + topic, + qos, + sender, + Arc::clone(&node_handle), + Arc::clone(commands.get_guard_condition()), + )?; commands.run_detached(subscription_task( callback, @@ -171,6 +143,7 @@ where qos: QoSProfile, action: UnboundedSender>, node_handle: Arc, + guard_condition: Arc, ) -> Result<(Arc, Waiter), RclrsError> // This uses pub(crate) visibility to avoid instantiating this struct outside // [`Node::create_subscription`], see the struct's documentation for the rationale @@ -216,10 +189,13 @@ where node_handle, }); - let (waiter, lifecycle) = Waiter::new(Box::new(SubscriptionWaitable { - handle: Arc::clone(&handle), - action: action.clone(), - })); + let (waiter, lifecycle) = Waiter::new( + Box::new(SubscriptionWaitable { + handle: Arc::clone(&handle), + action: action.clone(), + }), + Some(guard_condition), + ); let subscription = Arc::new(Self { handle, action, lifecycle }); @@ -232,7 +208,10 @@ struct SubscriptionWaitable { action: UnboundedSender>, } -impl Executable for SubscriptionWaitable { +impl Executable for SubscriptionWaitable +where + T: Message, +{ fn execute(&mut self) -> Result<(), RclrsError> { self.action.unbounded_send(SubscriptionAction::Execute).ok(); Ok(()) @@ -243,18 +222,21 @@ impl Executable for SubscriptionWaitable { } } -impl Waitable for SubscriptionWaitable { +impl Waitable for SubscriptionWaitable +where + T: Message, +{ unsafe fn add_to_wait_set( &mut self, - wait_set: &mut WaitSet, + wait_set: &mut rcl_wait_set_t, ) -> Result { - let index: usize = 0; + let mut index: usize = 0; unsafe { // SAFETY: We are holding an Arc of the handle, so the subscription // is still valid. rcl_wait_set_add_subscription( - &mut self.handle.rcl_wait_set, - &*self.handle().lock(), + wait_set, + &*self.handle.lock(), &mut index, ) } @@ -268,7 +250,47 @@ impl Waitable for SubscriptionWaitable { wait_set: &rcl_wait_set_t, index: usize, ) -> bool { + let entity = unsafe { + // SAFETY: The `subscriptions` field is an array of pointers, and this + // dereferencing is equivalent to getting the element of the array + // at `index`. + *wait_set.subscriptions.add(index) + }; + + !entity.is_null() + } +} + +// 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() + } +} + +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); + } } } diff --git a/rclrs/src/subscription/any_callback.rs b/rclrs/src/subscription/any_subscription_callback.rs similarity index 94% rename from rclrs/src/subscription/any_callback.rs rename to rclrs/src/subscription/any_subscription_callback.rs index d62b5acb8..8a048bb10 100644 --- a/rclrs/src/subscription/any_callback.rs +++ b/rclrs/src/subscription/any_subscription_callback.rs @@ -13,7 +13,11 @@ use std::sync::Arc; /// An enum capturing the various possible function signatures for subscription callbacks. /// -/// The correct enum variant is deduced by the [`SubscriptionCallback`] trait. +/// The correct enum variant is deduced by the [`SubscriptionCallback`][1] or +/// [`SubscriptionAsyncCallback`][2] trait. +/// +/// [1]: crate::SubscriptionCallback +/// [2]: crate::SubscriptionAsyncCallback pub enum AnySubscriptionCallback where T: Message, @@ -107,7 +111,7 @@ impl AnySubscriptionCallback { // | rmw_take | // +-------------+ // ``` - fn take(handle: &Arc) -> Result<(T, MessageInfo), RclrsError> { + fn take(handle: &SubscriptionHandle) -> Result<(T, MessageInfo), RclrsError> { let mut rmw_message = ::RmwMsg::default(); let message_info = Self::take_inner(handle, &mut rmw_message)?; Ok((T::from_rmw_message(rmw_message), message_info)) @@ -116,7 +120,7 @@ impl AnySubscriptionCallback { /// This is a version of take() that returns a boxed message. /// /// This can be more efficient for messages containing large arrays. - fn take_boxed(handle: &Arc) -> Result<(Box, MessageInfo), RclrsError> { + fn take_boxed(handle: &SubscriptionHandle) -> Result<(Box, MessageInfo), RclrsError> { let mut rmw_message = Box::<::RmwMsg>::default(); let message_info = Self::take_inner(handle, &mut *rmw_message)?; // TODO: This will still use the stack in general. Change signature of @@ -127,7 +131,7 @@ impl AnySubscriptionCallback { // Inner function, to be used by both regular and boxed versions. fn take_inner( - handle: &Arc, + handle: &SubscriptionHandle, rmw_message: &mut ::RmwMsg, ) -> Result { let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; @@ -175,7 +179,7 @@ impl AnySubscriptionCallback { } let read_only_loaned_msg = ReadOnlyLoanedMessage { msg_ptr: msg_ptr as *const T::RmwMsg, - handle: Arc::clone(&handle), + handle: Arc::clone(handle), }; Ok(( read_only_loaned_msg, diff --git a/rclrs/src/subscription/message_info.rs b/rclrs/src/subscription/message_info.rs index 010bf28ec..091f520d6 100644 --- a/rclrs/src/subscription/message_info.rs +++ b/rclrs/src/subscription/message_info.rs @@ -24,7 +24,7 @@ use crate::rcl_bindings::*; /// behavior is not defined here. /// However, this should be avoided, if at all possible, by the RMW implementation, /// and should be unlikely to happen in practice. -#[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], @@ -46,7 +46,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. /// @@ -104,30 +104,29 @@ pub struct MessageInfo { impl MessageInfo { pub(crate) fn from_rmw_message_info(rmw_message_info: &rmw_message_info_t) -> Self { - let source_timestamp = match rmw_message_info.source_timestamp { - 0 => None, - ts if ts < 0 => Some(SystemTime::UNIX_EPOCH - Duration::from_nanos(ts.unsigned_abs())), - ts => Some(SystemTime::UNIX_EPOCH + Duration::from_nanos(ts.unsigned_abs())), - }; - let received_timestamp = match rmw_message_info.received_timestamp { - 0 => None, - ts if ts < 0 => Some(SystemTime::UNIX_EPOCH - Duration::from_nanos(ts.unsigned_abs())), - ts => Some(SystemTime::UNIX_EPOCH + Duration::from_nanos(ts.unsigned_abs())), - }; - let publisher_gid = PublisherGid { - data: rmw_message_info.publisher_gid.data, - implementation_identifier: rmw_message_info.publisher_gid.implementation_identifier, - }; Self { - source_timestamp, - received_timestamp, + source_timestamp: timestamp_to_system_time(rmw_message_info.source_timestamp), + received_timestamp: timestamp_to_system_time(rmw_message_info.received_timestamp), publication_sequence_number: rmw_message_info.publication_sequence_number, reception_sequence_number: rmw_message_info.reception_sequence_number, - publisher_gid, + publisher_gid: PublisherGid { + data: rmw_message_info.publisher_gid.data, + implementation_identifier: rmw_message_info.publisher_gid.implementation_identifier, + }, } } } +pub(crate) fn timestamp_to_system_time( + timestamp: rmw_time_point_value_t +) -> Option { + match timestamp { + 0 => None, + ts if ts < 0 => Some(SystemTime::UNIX_EPOCH - Duration::from_nanos(ts.unsigned_abs())), + ts => Some(SystemTime::UNIX_EPOCH + Duration::from_nanos(ts.unsigned_abs())), + } +} + #[cfg(test)] mod tests { use super::*; diff --git a/rclrs/src/subscription/readonly_loaned_message.rs b/rclrs/src/subscription/readonly_loaned_message.rs index 7da2fdc23..d1aad4296 100644 --- a/rclrs/src/subscription/readonly_loaned_message.rs +++ b/rclrs/src/subscription/readonly_loaned_message.rs @@ -2,7 +2,7 @@ use std::{ops::Deref, sync::Arc}; use rosidl_runtime_rs::Message; -use crate::{rcl_bindings::*, SubscriptionHandle, ToResult}; +use crate::{rcl_bindings::*, subscription::SubscriptionHandle, ToResult}; /// A message that is owned by the middleware, loaned out for reading. /// diff --git a/rclrs/src/subscription/async_callback.rs b/rclrs/src/subscription/subscription_async_callback.rs similarity index 85% rename from rclrs/src/subscription/async_callback.rs rename to rclrs/src/subscription/subscription_async_callback.rs index 3130e4529..f75c2e34e 100644 --- a/rclrs/src/subscription/async_callback.rs +++ b/rclrs/src/subscription/subscription_async_callback.rs @@ -2,7 +2,7 @@ use rosidl_runtime_rs::Message; use super::{ MessageInfo, - any_callback::AnySubscriptionCallback, + any_subscription_callback::AnySubscriptionCallback, }; use crate::ReadOnlyLoanedMessage; @@ -10,7 +10,7 @@ use std::future::Future; /// A trait for async callbacks of subscriptions. /// -/// TODO(@mxgrey): Add a description of what callback signatures are supported +// TODO(@mxgrey): Add a description of what callback signatures are supported pub trait SubscriptionAsyncCallback: Send + 'static where T: Message, @@ -18,7 +18,7 @@ where /// Converts the callback into an enum. /// /// User code never needs to call this function. - fn into_async_callback(self) -> AnySubscriptionCallback; + fn into_subscription_async_callback(self) -> AnySubscriptionCallback; } // We need one implementation per arity. This was inspired by Bevy's systems. @@ -29,7 +29,7 @@ where Out: Future + Send + 'static, Func: FnMut(A0) -> Out + Send + 'static, { - fn into_async_callback(self) -> AnySubscriptionCallback { + fn into_subscription_async_callback(self) -> AnySubscriptionCallback { <(A0,) as SubscriptionAsyncArgs>::into_any_callback(self) } } @@ -41,7 +41,7 @@ where Out: Future + Send + 'static, Func: FnMut(A0, A1) -> Out + Send + 'static, { - fn into_async_callback(self) -> AnySubscriptionCallback { + fn into_subscription_async_callback(self) -> AnySubscriptionCallback { <(A0, A1) as SubscriptionAsyncArgs>::into_any_callback(self) } } @@ -146,57 +146,57 @@ mod tests { fn callback_conversion() { let cb = |_msg: TestMessage| { async { } }; assert!(matches!( - cb.into_async_callback(), + cb.into_subscription_async_callback(), AnySubscriptionCallback::::Regular(_) )); let cb = |_msg: TestMessage, _info: MessageInfo| { async { } }; assert!(matches!( - cb.into_async_callback(), + cb.into_subscription_async_callback(), AnySubscriptionCallback::::RegularWithMessageInfo(_) )); let cb = |_msg: Box| { async { } }; assert!(matches!( - cb.into_async_callback(), + cb.into_subscription_async_callback(), AnySubscriptionCallback::::Boxed(_) )); let cb = |_msg: Box, _info: MessageInfo| { async { } }; assert!(matches!( - cb.into_async_callback(), + cb.into_subscription_async_callback(), AnySubscriptionCallback::::BoxedWithMessageInfo(_) )); let cb = |_msg: ReadOnlyLoanedMessage| { async { } }; assert!(matches!( - cb.into_async_callback(), + cb.into_subscription_async_callback(), AnySubscriptionCallback::::Loaned(_) )); let cb = |_msg: ReadOnlyLoanedMessage, _info: MessageInfo| { async {}}; assert!(matches!( - cb.into_async_callback(), + cb.into_subscription_async_callback(), AnySubscriptionCallback::::LoanedWithMessageInfo(_) )); assert!(matches!( - test_regular.into_async_callback(), + test_regular.into_subscription_async_callback(), AnySubscriptionCallback::::Regular(_), )); assert!(matches!( - test_regular_with_info.into_async_callback(), + test_regular_with_info.into_subscription_async_callback(), AnySubscriptionCallback::::RegularWithMessageInfo(_), )); assert!(matches!( - test_boxed.into_async_callback(), + test_boxed.into_subscription_async_callback(), AnySubscriptionCallback::::Boxed(_), )); assert!(matches!( - test_boxed_with_info.into_async_callback(), + test_boxed_with_info.into_subscription_async_callback(), AnySubscriptionCallback::::BoxedWithMessageInfo(_), )); assert!(matches!( - test_loaned.into_async_callback(), + test_loaned.into_subscription_async_callback(), AnySubscriptionCallback::::Loaned(_), )); assert!(matches!( - test_loaned_with_info.into_async_callback(), + test_loaned_with_info.into_subscription_async_callback(), AnySubscriptionCallback::::LoanedWithMessageInfo(_), )); } diff --git a/rclrs/src/subscription/callback.rs b/rclrs/src/subscription/subscription_callback.rs similarity index 86% rename from rclrs/src/subscription/callback.rs rename to rclrs/src/subscription/subscription_callback.rs index da334b306..5e2aec292 100644 --- a/rclrs/src/subscription/callback.rs +++ b/rclrs/src/subscription/subscription_callback.rs @@ -2,7 +2,7 @@ use rosidl_runtime_rs::Message; use super::{ MessageInfo, - any_callback::AnySubscriptionCallback, + any_subscription_callback::AnySubscriptionCallback, }; use crate::ReadOnlyLoanedMessage; @@ -10,7 +10,7 @@ use std::sync::Arc; /// A trait for regular callbacks of subscriptions. /// -/// TODO(@mxgrey): Add a description of what callbacks signatures are supported +// TODO(@mxgrey): Add a description of what callbacks signatures are supported pub trait SubscriptionCallback: Send + 'static where T: Message, @@ -18,7 +18,7 @@ where /// Converts the callback into an enum. /// /// User code never needs to call this function. - fn into_callback(self) -> AnySubscriptionCallback; + fn into_subscription_callback(self) -> AnySubscriptionCallback; } // We need one implementation per arity. This was inspired by Bevy's systems. @@ -28,7 +28,7 @@ where (A0,): SubscriptionArgs, Func: Fn(A0) + Send + Sync + 'static, { - fn into_callback(self) -> AnySubscriptionCallback { + fn into_subscription_callback(self) -> AnySubscriptionCallback { <(A0,) as SubscriptionArgs>::into_any_callback(self) } } @@ -39,7 +39,7 @@ where (A0, A1): SubscriptionArgs, Func: Fn(A0, A1) + Clone + Send + 'static, { - fn into_callback(self) -> AnySubscriptionCallback { + fn into_subscription_callback(self) -> AnySubscriptionCallback { <(A0, A1) as SubscriptionArgs>::into_any_callback(self) } } @@ -169,57 +169,57 @@ mod tests { fn callback_conversion() { let cb = |_msg: TestMessage| { }; assert!(matches!( - cb.into_callback(), + cb.into_subscription_callback(), AnySubscriptionCallback::::Regular(_) )); let cb = |_msg: TestMessage, _info: MessageInfo| { }; assert!(matches!( - cb.into_callback(), + cb.into_subscription_callback(), AnySubscriptionCallback::::RegularWithMessageInfo(_) )); let cb = |_msg: Box| { }; assert!(matches!( - cb.into_callback(), + cb.into_subscription_callback(), AnySubscriptionCallback::::Boxed(_) )); let cb = |_msg: Box, _info: MessageInfo| { }; assert!(matches!( - cb.into_callback(), + cb.into_subscription_callback(), AnySubscriptionCallback::::BoxedWithMessageInfo(_) )); let cb = |_msg: ReadOnlyLoanedMessage| { }; assert!(matches!( - cb.into_callback(), + cb.into_subscription_callback(), AnySubscriptionCallback::::Loaned(_) )); let cb = |_msg: ReadOnlyLoanedMessage, _info: MessageInfo| { }; assert!(matches!( - cb.into_callback(), + cb.into_subscription_callback(), AnySubscriptionCallback::::LoanedWithMessageInfo(_) )); assert!(matches!( - test_regular.into_callback(), + test_regular.into_subscription_callback(), AnySubscriptionCallback::::Regular(_), )); assert!(matches!( - test_regular_with_info.into_callback(), + test_regular_with_info.into_subscription_callback(), AnySubscriptionCallback::::RegularWithMessageInfo(_), )); assert!(matches!( - test_boxed.into_callback(), + test_boxed.into_subscription_callback(), AnySubscriptionCallback::::Boxed(_), )); assert!(matches!( - test_boxed_with_info.into_callback(), + test_boxed_with_info.into_subscription_callback(), AnySubscriptionCallback::::BoxedWithMessageInfo(_), )); assert!(matches!( - test_loaned.into_callback(), + test_loaned.into_subscription_callback(), AnySubscriptionCallback::::Loaned(_), )); assert!(matches!( - test_loaned_with_info.into_callback(), + test_loaned_with_info.into_subscription_callback(), AnySubscriptionCallback::::LoanedWithMessageInfo(_), )); } diff --git a/rclrs/src/subscription/subscription_task.rs b/rclrs/src/subscription/subscription_task.rs index 8a206369d..8e0210a93 100644 --- a/rclrs/src/subscription/subscription_task.rs +++ b/rclrs/src/subscription/subscription_task.rs @@ -1,12 +1,13 @@ use rosidl_runtime_rs::Message; use crate::{ - AnySubscriptionCallback, ExecutorCommands, SubscriptionHandle, + subscription::SubscriptionHandle, + AnySubscriptionCallback, ExecutorCommands, }; use futures::{ - Stream, StreamExt, - channel::mpsc::UnboundedReceiver + channel::mpsc::UnboundedReceiver, + StreamExt, }; use std::sync::Arc; diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 5e293da4f..db08bb0c4 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -32,7 +32,7 @@ pub use waitable::*; /// A struct for waiting on subscriptions and other waitable entities to become ready. pub struct WaitSet { entities: HashMap>, - handle: WaitSetHandle, + pub(crate) handle: WaitSetHandle, } // SAFETY: While the rcl_wait_set_t does have some interior mutability (because it has @@ -68,7 +68,7 @@ impl WaitSet { return Err(RclrsError::AlreadyAddedToWaitSet); } let kind = entity.waitable.kind(); - self.entities.insert(kind, entity); + self.entities.entry(kind).or_default().push(entity); } self.resize_rcl_containers()?; self.register_rcl_entities()?; @@ -118,7 +118,7 @@ impl WaitSet { pub fn wait( &mut self, timeout: Option, - mut f: impl FnMut(&mut Executable) -> Result<(), RclrsError>, + mut f: impl FnMut(&mut dyn Executable) -> Result<(), RclrsError>, ) -> Result<(), RclrsError> { let timeout_ns = match timeout.map(|d| d.as_nanos()) { None => -1, @@ -155,7 +155,7 @@ impl WaitSet { // the callback for those that were. for waiter in self.entities.values_mut().flat_map(|v| v) { if waiter.is_ready(&self.handle.rcl_wait_set) { - f(&mut waiter.waitable)?; + f(waiter.waitable.as_executable())?; } } @@ -200,7 +200,7 @@ impl WaitSet { /// [1]: crate::RclReturnCode fn register_rcl_entities(&mut self) -> Result<(), RclrsError> { for entity in self.entities.values_mut().flat_map(|c| c) { - entity.add_to_wait_set(&self.handle.rcl_wait_set)?; + entity.add_to_wait_set(&mut self.handle.rcl_wait_set)?; } Ok(()) } @@ -226,7 +226,7 @@ unsafe impl Send for rcl_wait_set_t {} /// /// [1]: struct WaitSetHandle { - rcl_wait_set: rcl_wait_set_t, + pub(crate) rcl_wait_set: rcl_wait_set_t, // Used to ensure the context is alive while the wait set is alive. #[allow(dead_code)] context_handle: Arc, diff --git a/rclrs/src/wait/guard_condition.rs b/rclrs/src/wait/guard_condition.rs index 92a6acd00..c7721bf2e 100644 --- a/rclrs/src/wait/guard_condition.rs +++ b/rclrs/src/wait/guard_condition.rs @@ -1,15 +1,18 @@ -use std::sync::{atomic::AtomicBool, Arc, Mutex}; +use std::sync::{Arc, Mutex}; -use crate::{rcl_bindings::*, Context, ContextHandle, RclrsError, ToResult}; +use crate::{ + rcl_bindings::*, + ContextHandle, RclrsError, ToResult, WaiterLifecycle, Executable, + Waitable, WaitableKind, ExecutorCommands, Waiter, +}; /// A waitable entity used for waking up a wait set manually. /// -/// If a wait set that is currently waiting on events should be interrupted from a separate thread, this can be done -/// by adding an `Arc` to the wait set, and calling `trigger()` on the same `GuardCondition` while -/// the wait set is waiting. +/// If a wait set that is currently waiting on events should be interrupted from +/// a separate thread, trigger a `GuardCondition`. /// -/// The guard condition may be reused multiple times, but like other waitable entities, can not be used in -/// multiple wait sets concurrently. +/// A guard condition may be triggered any number of times, but can only be +/// associated with one wait set. /// /// # Example /// ``` @@ -43,80 +46,25 @@ use crate::{rcl_bindings::*, Context, ContextHandle, RclrsError, ToResult}; /// # Ok::<(), RclrsError>(()) /// ``` pub struct GuardCondition { - /// The rcl_guard_condition_t that this struct encapsulates. - pub(crate) handle: GuardConditionHandle, - /// An optional callback to call when this guard condition is triggered. - callback: Option>, - /// A flag to indicate if this guard condition has already been assigned to a wait set. - pub(crate) in_use_by_wait_set: Arc, + /// The rcl_guard_condition_t that this struct encapsulates. Holding onto + /// this keeps the rcl_guard_condition alive and allows us to trigger it. + handle: Arc, + /// This manages the lifecycle of this guard condition's waiter. Dropping + /// this will remove the guard condition from its wait set. + lifecycle: WaiterLifecycle, } -/// Manage the lifecycle of an `rcl_guard_condition_t`, including managing its dependency -/// on `rcl_context_t` by ensuring that this dependency is [dropped after][1] the -/// `rcl_guard_condition_t`. -/// -/// [1]: -pub(crate) struct GuardConditionHandle { - pub(crate) rcl_guard_condition: Mutex, - /// Keep the context alive for the whole lifecycle of the guard condition - #[allow(dead_code)] - pub(crate) context_handle: Arc, -} - -impl Drop for GuardCondition { - fn drop(&mut self) { - unsafe { - // SAFETY: No precondition for this function (besides passing in a valid guard condition) - rcl_guard_condition_fini(&mut *self.handle.rcl_guard_condition.lock().unwrap()); - } - } -} - -impl PartialEq for GuardCondition { - fn eq(&self, other: &Self) -> bool { - // Because GuardCondition controls the creation of the rcl_guard_condition, each unique GuardCondition should have a unique - // rcl_guard_condition. Thus comparing equality of this member should be enough. - std::ptr::eq( - &self.handle.rcl_guard_condition.lock().unwrap().impl_, - &other.handle.rcl_guard_condition.lock().unwrap().impl_, - ) - } -} - -impl Eq for GuardCondition {} - // SAFETY: rcl_guard_condition is the only member that doesn't implement Send, and it is designed to be accessed from other threads unsafe impl Send for rcl_guard_condition_t {} impl GuardCondition { /// Creates a new guard condition with no callback. - pub fn new(context: &Context) -> Self { - Self::new_with_context_handle(Arc::clone(&context.handle), None) - } - - /// Creates a new guard condition with a callback. - pub fn new_with_callback(context: &Context, callback: F) -> Self - where - F: Fn() + Send + Sync + 'static, - { - Self::new_with_context_handle( - Arc::clone(&context.handle), - Some(Box::new(callback) as Box), - ) - } - - /// Creates a new guard condition by providing the rcl_context_t and an optional callback. - /// Note this function enables calling `Node::create_guard_condition`[1] without providing the Context separately - /// - /// [1]: Node::create_guard_condition - pub(crate) fn new_with_context_handle( - context_handle: Arc, - callback: Option>, - ) -> Self { + pub(crate) fn new(commands: &ExecutorCommands) -> Self { + let context = commands.context(); let rcl_guard_condition = { // SAFETY: Getting a zero initialized value is always safe let mut guard_condition = unsafe { rcl_get_zero_initialized_guard_condition() }; - let mut rcl_context = context_handle.rcl_context.lock().unwrap(); + let mut rcl_context = context.handle.rcl_context.lock().unwrap(); unsafe { // SAFETY: The context must be valid, and the guard condition must be zero-initialized rcl_guard_condition_init( @@ -129,14 +77,21 @@ impl GuardCondition { Mutex::new(guard_condition) }; - Self { - handle: GuardConditionHandle { - rcl_guard_condition, - context_handle, + let handle = Arc::new(GuardConditionHandle { + rcl_guard_condition, + context_handle: Arc::clone(&context.handle), + }); + + let (waiter, lifecycle) = Waiter::new( + GuardConditionWaitable { + handle: Arc::clone(&handle), }, - callback, - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), - } + None, + ); + + commands.add_to_wait_set(waiter); + + Self { handle, lifecycle } } /// Triggers this guard condition, activating the wait set, and calling the optionally assigned callback. @@ -146,11 +101,80 @@ impl GuardCondition { rcl_trigger_guard_condition(&mut *self.handle.rcl_guard_condition.lock().unwrap()) .ok()?; } - if let Some(callback) = &self.callback { - callback(); + Ok(()) + } +} + +/// Manage the lifecycle of an `rcl_guard_condition_t`, including managing its dependency +/// on `rcl_context_t` by ensuring that this dependency is [dropped after][1] the +/// `rcl_guard_condition_t`. +/// +/// [1]: +struct GuardConditionHandle { + rcl_guard_condition: Mutex, + /// Keep the context alive for the whole lifecycle of the guard condition + #[allow(dead_code)] + context_handle: Arc, +} + +impl Drop for GuardConditionHandle { + fn drop(&mut self) { + unsafe { + // SAFETY: No precondition for this function (besides passing in a valid guard condition) + rcl_guard_condition_fini(&mut self.rcl_guard_condition); } + } +} + +struct GuardConditionWaitable { + handle: Arc, +} + +impl Executable for GuardConditionWaitable { + fn execute(&mut self) -> Result<(), RclrsError> { + // Do nothing Ok(()) } + + fn kind(&self) -> super::WaitableKind { + WaitableKind::GuardCondition + } +} + +impl Waitable for GuardConditionWaitable { + unsafe fn add_to_wait_set( + &mut self, + wait_set: &mut rcl_wait_set_t, + ) -> Result { + let mut index: usize = 0; + unsafe { + // SAFETY: We are holding onto an Arc of the handle, so the guard + // condition is still valid. + rcl_wait_set_add_guard_condition( + wait_set, + &mut *self.handle.rcl_guard_condition.lock(), + &mut index, + ) + } + .ok()?; + + Ok(index) + } + + unsafe fn is_ready( + &self, + wait_set: &rcl_wait_set_t, + index: usize, + ) -> bool { + let entity = unsafe { + // SAFETY: The `guard_condition` entry is an array of pointers, and this + // dereferencing is equivalent to getting the element of the array + // at `index`. + *wait_set.guard_conditions.add(index) + }; + + !entity.is_null() + } } #[cfg(test)] diff --git a/rclrs/src/wait/waitable.rs b/rclrs/src/wait/waitable.rs index 7cfae9bf8..44007a1da 100644 --- a/rclrs/src/wait/waitable.rs +++ b/rclrs/src/wait/waitable.rs @@ -4,8 +4,9 @@ use std::sync::{ }; use crate::{ + error::ToResult, rcl_bindings::*, - RclrsError, WaitSet, + RclrsError, WaitSet, GuardCondition, }; #[derive(Clone, Copy, Hash, PartialEq, Eq, PartialOrd, Ord)] @@ -113,6 +114,7 @@ pub(crate) trait Waitable: Executable { ) -> bool; } +#[must_use = "If you do not give the Waiter to a WaitSet then it will never be useful"] pub struct Waiter { pub(super) waitable: Box, in_use: Arc, @@ -120,7 +122,10 @@ pub struct Waiter { } impl Waiter { - pub fn new(waitable: Box) -> (Self, WaiterLifecycle) { + pub fn new( + waitable: Box, + guard_condition: Option>, + ) -> (Self, WaiterLifecycle) { let in_use = Arc::new(AtomicBool::new(true)); let waiter = Self { waitable, @@ -128,7 +133,7 @@ impl Waiter { index_in_wait_set: None, }; - let lifecycle = WaiterLifecycle { in_use }; + let lifecycle = WaiterLifecycle { in_use, guard_condition }; (waiter, lifecycle) } @@ -152,7 +157,7 @@ impl Waiter { pub(super) fn add_to_wait_set( &mut self, - wait_set: &mut WaitSet, + wait_set: &mut rcl_wait_set_t, ) -> Result<(), RclrsError> { self.index_in_wait_set = Some( unsafe { @@ -165,12 +170,27 @@ impl Waiter { } } +pub trait AsExecutable { + fn as_executable(&mut self) -> &mut dyn Executable; +} + +impl AsExecutable for T { + fn as_executable(&mut self) -> &mut dyn Executable { + self + } +} + +#[must_use = "If you do not hold onto the WaiterLifecycle, then its Waiter will be immediately dropped"] pub struct WaiterLifecycle { in_use: Arc, + guard_condition: Option>, } impl Drop for WaiterLifecycle { fn drop(&mut self) { - self.in_use.store(false, Ordering::Relaxed); + self.in_use.store(false, Ordering::Release); + if let Some(guard_condition) = &self.guard_condition { + guard_condition.trigger(); + } } } From 3a851be3a91bf37b25df725d9742e38f391242bf Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 12 Oct 2024 16:59:18 +0800 Subject: [PATCH 09/48] Change Waitable from trait to struct Signed-off-by: Michael X. Grey --- rclrs/src/executor.rs | 40 ++++++-- rclrs/src/executor/basic_executor.rs | 2 - rclrs/src/lib.rs | 2 - rclrs/src/node.rs | 3 +- rclrs/src/node/options.rs | 4 - rclrs/src/parameter.rs | 47 +++++---- rclrs/src/parameter/service.rs | 20 ++-- rclrs/src/qos.rs | 10 ++ rclrs/src/service.rs | 51 ++-------- rclrs/src/subscription.rs | 51 ++-------- rclrs/src/test_helpers/graph_helpers.rs | 10 +- rclrs/src/time_source.rs | 16 ++- rclrs/src/wait.rs | 48 ++++----- rclrs/src/wait/guard_condition.rs | 52 ++-------- rclrs/src/wait/waitable.rs | 129 ++++++++++++++---------- 15 files changed, 222 insertions(+), 263 deletions(-) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index a38970777..643a15387 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -4,7 +4,7 @@ pub use self::basic_executor::*; use crate::{ rcl_bindings::rcl_context_is_valid, Node, NodeOptions, RclReturnCode, RclrsError, WaitSet, Context, - ContextHandle, Waiter, GuardCondition, + ContextHandle, Waitable, GuardCondition, }; use std::{ sync::{Arc, Mutex, Weak}, @@ -116,7 +116,7 @@ impl ExecutorCommands { /// Tell the [`Executor`] to halt its spinning. pub fn halt(&self) { self.halt.store(true, Ordering::Release); - self.channel.wakeup(); + self.channel.get_guard_condition().trigger(); } /// Run a task on the [`Executor`]. If the returned [`Promise`] is dropped @@ -172,7 +172,7 @@ impl ExecutorCommands { self.channel.add_async_task(f); } - pub(crate) fn add_to_wait_set(&self, waiter: Waiter) { + pub(crate) fn add_to_wait_set(&self, waiter: Waitable) { self.channel.add_to_waitset(waiter); } @@ -188,7 +188,7 @@ pub trait ExecutorChannel: Send + Sync { fn add_async_task(&self, f: BoxFuture<'static, ()>); /// Add new entities to the waitset of the executor. - fn add_to_waitset(&self, new_entity: Waiter); + fn add_to_waitset(&self, new_entity: Waitable); /// Get a guard condition that can be used to wake up the wait set of the executor. fn get_guard_condition(&self) -> &Arc; @@ -220,11 +220,39 @@ pub trait ExecutorRuntime { #[non_exhaustive] #[derive(Default)] pub struct SpinOptions { - /// Only perform immediately available work. This is similar to spin_once in + /// Only perform the next available work. This is similar to spin_once in /// rclcpp and rclpy. - pub only_available_work: bool, + /// + /// 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 future is resolved. pub until_future_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 { + /// Behave like spin_once in rclcpp and rclpy. + pub fn spin_once() -> Self { + Self { + only_next_available_work: true, + ..Default::default() + } + } + + /// Stop spinning once this future is resolved. + pub fn until_future_resolved(mut self, f: F) -> Self { + self.until_future_resolved = Some(Box::pin(async { f.await; })); + self + } + + /// Stop spinning once this durtion of time is reached. + pub fn timeout(mut self, timeout: Duration) -> Self { + self.timeout = Some(timeout); + self + } } /// A bundle of conditions that tell the [`ExecutorRuntime`] how long to keep diff --git a/rclrs/src/executor/basic_executor.rs b/rclrs/src/executor/basic_executor.rs index 096889384..1c733c791 100644 --- a/rclrs/src/executor/basic_executor.rs +++ b/rclrs/src/executor/basic_executor.rs @@ -32,7 +32,5 @@ impl ExecutorChannel for BasicExecutorChannel { } - fn wakeup(&self) { - } } diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 829561b3a..641836cc2 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -11,7 +11,6 @@ mod clock; mod context; mod error; mod executor; -mod guard_condition; mod node; mod parameter; mod publisher; @@ -39,7 +38,6 @@ pub use clock::*; pub use context::*; pub use error::*; pub use executor::*; -pub use guard_condition::*; pub use node::*; pub use parameter::*; pub use publisher::*; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index f8e619499..d04b4a97a 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -6,14 +6,13 @@ use std::{ fmt, os::raw::c_char, sync::{Arc, Mutex, Weak}, - vec::Vec, }; use rosidl_runtime_rs::Message; pub use self::{options::*, graph::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, ContextHandle, GuardCondition, + rcl_bindings::*, Client, ClientBase, Clock, ContextHandle, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, Subscription, SubscriptionCallback, SubscriptionAsyncCallback, ServiceCallback, ServiceAsyncCallback, ExecutorCommands, TimeSource, ENTITY_LIFECYCLE_MUTEX, diff --git a/rclrs/src/node/options.rs b/rclrs/src/node/options.rs index 433b38d33..1defee488 100644 --- a/rclrs/src/node/options.rs +++ b/rclrs/src/node/options.rs @@ -312,10 +312,6 @@ impl NodeOptions { }; 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![]), time_source: TimeSource::builder(self.clock_type) .clock_qos(self.clock_qos) .build(), diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index c8a710eeb..97a9d9206 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -838,18 +838,19 @@ impl ParameterInterface { #[cfg(test)] mod tests { use super::*; - use crate::{create_node, Context}; + use crate::Context; #[test] fn test_parameter_override_errors() { // Create a new node with a few parameter overrides - let ctx = Context::new([ + let executor = Context::new([ String::from("--ros-args"), String::from("-p"), String::from("declared_int:=10"), ]) - .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + .unwrap() + .create_basic_executor(); + let node = executor.create_node("param_test_node").unwrap(); // Declaring a parameter with a different type than what was overridden should return an // error @@ -895,7 +896,7 @@ mod tests { #[test] fn test_parameter_setting_declaring() { // Create a new node with a few parameter overrides - let ctx = Context::new([ + let executor = Context::new([ String::from("--ros-args"), String::from("-p"), String::from("declared_int:=10"), @@ -906,8 +907,9 @@ mod tests { String::from("-p"), String::from("non_declared_string:='param'"), ]) - .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + .unwrap() + .create_basic_executor(); + let node = executor.create_node("param_test_node").unwrap(); let overridden_int = node .declare_parameter("declared_int") @@ -1051,13 +1053,14 @@ mod tests { #[test] fn test_override_undeclared_set_priority() { - let ctx = Context::new([ + let executor = Context::new([ String::from("--ros-args"), String::from("-p"), String::from("declared_int:=10"), ]) - .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + .unwrap() + .create_basic_executor(); + let node = executor.create_node("param_test_node").unwrap(); // If a parameter was set as an override and as an undeclared parameter, the undeclared // value should get priority node.use_undeclared_parameters() @@ -1073,13 +1076,15 @@ mod tests { #[test] fn test_parameter_scope_redeclaring() { - let ctx = Context::new([ + let executor = Context::new([ String::from("--ros-args"), String::from("-p"), String::from("declared_int:=10"), ]) - .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + .unwrap() + .create_basic_executor(); + + let node = executor.create_node("param_test_node").unwrap(); { // Setting a parameter with an override let param = node @@ -1124,8 +1129,8 @@ mod tests { #[test] fn test_parameter_ranges() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let executor = Context::new([]).unwrap().create_basic_executor(); + let node = executor.create_node("param_test_node").unwrap(); // Setting invalid ranges should fail let range = ParameterRange { lower: Some(10), @@ -1252,8 +1257,8 @@ mod tests { #[test] fn test_readonly_parameters() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let executor = Context::new([]).unwrap().create_basic_executor(); + let node = executor.create_node("param_test_node").unwrap(); let param = node .declare_parameter("int_param") .default(100) @@ -1279,8 +1284,8 @@ mod tests { #[test] fn test_preexisting_value_error() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let executor = Context::new([]).unwrap().create_basic_executor(); + let node = executor.create_node("param_test_node").unwrap(); node.use_undeclared_parameters() .set("int_param", 100) .unwrap(); @@ -1332,8 +1337,8 @@ mod tests { #[test] fn test_optional_parameter_apis() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let executor = Context::new([]).unwrap().create_basic_executor(); + let node = executor.create_node("param_test_node").unwrap(); node.declare_parameter::("int_param") .optional() .unwrap(); diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 72074aa46..1fea50af5 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -9,7 +9,7 @@ use rosidl_runtime_rs::Sequence; use super::ParameterMap; use crate::{ parameter::{DeclaredValue, ParameterKind, ParameterStorage}, - rmw_request_id_t, Node, RclrsError, Service, + Node, RclrsError, Service, QoSProfile, }; // The variables only exist to keep a strong reference to the services and are technically unused. @@ -248,7 +248,8 @@ impl ParameterService { 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| { + QoSProfile::parameter_services_default(), + move |req: DescribeParameters_Request| { let map = map.lock().unwrap(); describe_parameters(req, &map) }, @@ -256,7 +257,8 @@ impl ParameterService { 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| { + QoSProfile::parameter_services_default(), + move |req: GetParameterTypes_Request| { let map = map.lock().unwrap(); get_parameter_types(req, &map) }, @@ -264,7 +266,8 @@ impl ParameterService { 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| { + QoSProfile::parameter_services_default(), + move |req: GetParameters_Request| { let map = map.lock().unwrap(); get_parameters(req, &map) }, @@ -272,7 +275,8 @@ impl ParameterService { 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| { + QoSProfile::parameter_services_default(), + move |req: ListParameters_Request| { let map = map.lock().unwrap(); list_parameters(req, &map) }, @@ -280,14 +284,16 @@ impl ParameterService { 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| { + 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| { + QoSProfile::parameter_services_default(), + move |req: SetParametersAtomically_Request| { let mut map = parameter_map.lock().unwrap(); set_parameters_atomically(req, &mut map) }, diff --git a/rclrs/src/qos.rs b/rclrs/src/qos.rs index 20aefc48c..1574eceb4 100644 --- a/rclrs/src/qos.rs +++ b/rclrs/src/qos.rs @@ -265,6 +265,16 @@ impl QoSProfile { 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 + } } impl From for rmw_qos_history_policy_t { diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index b62c5009d..acc2430e8 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -9,8 +9,8 @@ use futures::channel::mpsc::{unbounded, UnboundedSender, TrySendError}; use crate::{ error::ToResult, rcl_bindings::*, - NodeHandle, RclrsError, Waiter, WaiterLifecycle, GuardCondition, QoSProfile, - Executable, Waitable, WaitableKind, ENTITY_LIFECYCLE_MUTEX, ExecutorCommands, + NodeHandle, RclrsError, Waitable, WaiterLifecycle, GuardCondition, QoSProfile, + Executable, ExecutableKind, ExecutableHandle, ENTITY_LIFECYCLE_MUTEX, ExecutorCommands, }; mod any_service_callback; @@ -134,7 +134,7 @@ where action: UnboundedSender>, node_handle: Arc, guard_condition: Arc, - ) -> Result<(Arc, Waiter), RclrsError> { + ) -> Result<(Arc, Waitable), RclrsError> { // 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() @@ -175,7 +175,7 @@ where node_handle, }); - let (waiter, lifecycle) = Waiter::new( + let (waiter, lifecycle) = Waitable::new( Box::new(ServiceWaitable { handle: Arc::clone(&handle), action: action.clone(), @@ -203,47 +203,12 @@ where Ok(()) } - fn kind(&self) -> WaitableKind { - WaitableKind::Service + fn kind(&self) -> crate::ExecutableKind { + ExecutableKind::Service } -} - -impl Waitable for ServiceWaitable -where - T: rosidl_runtime_rs::Service, -{ - unsafe fn add_to_wait_set( - &mut self, - wait_set: &mut rcl_wait_set_t, - ) -> Result { - let mut index: usize = 0; - unsafe { - // SAFETY: We are holding an Arc of the handle, so the service is - // still valid. - rcl_wait_set_add_service( - wait_set, - &*self.handle.lock(), - &mut index, - ) - } - .ok()?; - - Ok(index) - } - - unsafe fn is_ready( - &self, - wait_set: &rcl_wait_set_t, - index: usize, - ) -> bool { - let entity = unsafe { - // SAFETY: The `services` field is an array of pointers, and this - // dereferencing is equivalent to getting the element of the array - // at `index`. - *wait_set.services.add(index) - }; - !entity.is_null() + fn handle(&self) -> ExecutableHandle { + ExecutableHandle::Service(self.handle.lock()) } } diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index fead30176..32e1a0dbf 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -11,8 +11,8 @@ use crate::{ error::ToResult, qos::QoSProfile, rcl_bindings::*, - ExecutorCommands, NodeHandle, RclrsError, Waitable, Executable, WaitableKind, - GuardCondition, Waiter, WaiterLifecycle, ENTITY_LIFECYCLE_MUTEX, + ExecutorCommands, NodeHandle, RclrsError, Waitable, Executable, ExecutableHandle, + ExecutableKind, GuardCondition, WaiterLifecycle, ENTITY_LIFECYCLE_MUTEX, }; mod any_subscription_callback; @@ -144,7 +144,7 @@ where action: UnboundedSender>, node_handle: Arc, guard_condition: Arc, - ) -> Result<(Arc, Waiter), RclrsError> + ) -> Result<(Arc, Waitable), RclrsError> // This uses pub(crate) visibility to avoid instantiating this struct outside // [`Node::create_subscription`], see the struct's documentation for the rationale where @@ -189,7 +189,7 @@ where node_handle, }); - let (waiter, lifecycle) = Waiter::new( + let (waiter, lifecycle) = Waitable::new( Box::new(SubscriptionWaitable { handle: Arc::clone(&handle), action: action.clone(), @@ -217,47 +217,12 @@ where Ok(()) } - fn kind(&self) -> WaitableKind { - WaitableKind::Subscription + fn kind(&self) -> crate::ExecutableKind { + ExecutableKind::Subscription } -} - -impl Waitable for SubscriptionWaitable -where - T: Message, -{ - unsafe fn add_to_wait_set( - &mut self, - wait_set: &mut rcl_wait_set_t, - ) -> Result { - let mut index: usize = 0; - unsafe { - // SAFETY: We are holding an Arc of the handle, so the subscription - // is still valid. - rcl_wait_set_add_subscription( - wait_set, - &*self.handle.lock(), - &mut index, - ) - } - .ok()?; - Ok(index) - } - - unsafe fn is_ready( - &self, - wait_set: &rcl_wait_set_t, - index: usize, - ) -> bool { - let entity = unsafe { - // SAFETY: The `subscriptions` field is an array of pointers, and this - // dereferencing is equivalent to getting the element of the array - // at `index`. - *wait_set.subscriptions.add(index) - }; - - !entity.is_null() + fn handle(&self) -> ExecutableHandle { + ExecutableHandle::Subscription(self.handle.lock()) } } diff --git a/rclrs/src/test_helpers/graph_helpers.rs b/rclrs/src/test_helpers/graph_helpers.rs index c1df36b65..ca51b8467 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -7,13 +7,9 @@ pub(crate) struct TestGraph { } pub(crate) fn construct_test_graph(namespace: &str) -> Result { - let context = Context::new([])?; + let executor = Context::new([])?.create_basic_executor(); Ok(TestGraph { - node1: NodeOptions::new(&context, "graph_test_node_1") - .namespace(namespace) - .build()?, - node2: NodeOptions::new(&context, "graph_test_node_2") - .namespace(namespace) - .build()?, + node1: executor.create_node(NodeOptions::new("graph_test_node_1").namespace(namespace))?, + node2: executor.create_node(NodeOptions::new("graph_test_node_2").namespace(namespace))?, }) } diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index a6b600800..a5883c22c 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -145,24 +145,30 @@ impl TimeSource { #[cfg(test)] mod tests { - use crate::{create_node, Context}; + use crate::Context; #[test] fn time_source_default_clock() { - let node = create_node(&Context::new([]).unwrap(), "test_node").unwrap(); + let node = Context::new([]) + .unwrap() + .create_basic_executor() + .create_node("test_node") + .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([ + let executor = Context::new([ String::from("--ros-args"), String::from("-p"), String::from("use_sim_time:=true"), ]) - .unwrap(); - let node = create_node(&ctx, "test_node").unwrap(); + .unwrap() + .create_basic_executor(); + + let node = executor.create_node("test_node").unwrap(); // Default sim time value should be 0 (no message received) assert_eq!(node.get_clock().now().nsec, 0); } diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index db08bb0c4..ebac1a422 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -31,7 +31,7 @@ pub use waitable::*; /// A struct for waiting on subscriptions and other waitable entities to become ready. pub struct WaitSet { - entities: HashMap>, + entities: HashMap>, pub(crate) handle: WaitSetHandle, } @@ -61,13 +61,13 @@ impl WaitSet { /// Take all the items out of `entities` and move them into this wait set. pub fn add( &mut self, - entities: impl IntoIterator, + entities: impl IntoIterator, ) -> Result<(), RclrsError> { for entity in entities { if entity.in_wait_set() { return Err(RclrsError::AlreadyAddedToWaitSet); } - let kind = entity.waitable.kind(); + let kind = entity.executable.kind(); self.entities.entry(kind).or_default().push(entity); } self.resize_rcl_containers()?; @@ -155,7 +155,7 @@ impl WaitSet { // the callback for those that were. for waiter in self.entities.values_mut().flat_map(|v| v) { if waiter.is_ready(&self.handle.rcl_wait_set) { - f(waiter.waitable.as_executable())?; + f(&mut *waiter.executable)?; } } @@ -180,6 +180,7 @@ impl WaitSet { for (kind, collection) in &self.entities { c.add(*kind, collection.len()); } + c } fn resize_rcl_containers(&mut self) -> Result<(), RclrsError> { @@ -234,8 +235,7 @@ struct WaitSetHandle { #[cfg(test)] mod tests { - use std::sync::atomic::AtomicBool; - use super::*; + use crate::*; #[test] fn traits() { @@ -247,27 +247,27 @@ mod tests { #[test] fn guard_condition_in_wait_set_readies() -> Result<(), RclrsError> { - let context = Context::new([])?; + let mut executor = Context::new([])?.create_basic_executor(); - let guard_condition = Arc::new(GuardCondition::new(&context)); - - let in_use = Arc::new(AtomicBool::new(true)); - let waiter = Waiter::new(..); - entities.guard_conditions.push(Arc::clone(&guard_condition)); - - let mut wait_set = WaitSet::new(entities, &context)?; + let guard_condition = GuardCondition::new(&executor.commands()); guard_condition.trigger()?; - let mut triggered = false; - wait_set.wait( - Some(std::time::Duration::from_millis(10)), - |entity| { - assert!(matches!(entity, WaitSetEntity::GuardCondition(_))); - triggered = true; - Ok(()) - }, - )?; - assert!(triggered); + let start = std::time::Instant::now(); + // This should stop spinning right away because the guard condition was + // already triggered. + executor.spin( + SpinOptions::spin_once() + .timeout(Duration::from_secs(10)) + ); + + // If it took more than a second to finish spinning then something is + // probably wrong. + // + // Note that this test could theoretically be flaky if it runs on a + // machine with very strange CPU scheduling behaviors. To have a test + // that is guaranteed to be stable we could write a custom executor for + // testing that will give us more introspection. + assert!(std::time::Instant::now() - start < Duration::from_secs(1)); Ok(()) } diff --git a/rclrs/src/wait/guard_condition.rs b/rclrs/src/wait/guard_condition.rs index c7721bf2e..587801fd6 100644 --- a/rclrs/src/wait/guard_condition.rs +++ b/rclrs/src/wait/guard_condition.rs @@ -3,7 +3,7 @@ use std::sync::{Arc, Mutex}; use crate::{ rcl_bindings::*, ContextHandle, RclrsError, ToResult, WaiterLifecycle, Executable, - Waitable, WaitableKind, ExecutorCommands, Waiter, + Waitable, ExecutableKind, ExecutableHandle, ExecutorCommands, }; /// A waitable entity used for waking up a wait set manually. @@ -82,10 +82,8 @@ impl GuardCondition { context_handle: Arc::clone(&context.handle), }); - let (waiter, lifecycle) = Waiter::new( - GuardConditionWaitable { - handle: Arc::clone(&handle), - }, + let (waiter, lifecycle) = Waitable::new( + Box::new(GuardConditionWaitable { handle: Arc::clone(&handle) }), None, ); @@ -121,7 +119,7 @@ impl Drop for GuardConditionHandle { fn drop(&mut self) { unsafe { // SAFETY: No precondition for this function (besides passing in a valid guard condition) - rcl_guard_condition_fini(&mut self.rcl_guard_condition); + rcl_guard_condition_fini(&mut *self.rcl_guard_condition.lock().unwrap()); } } } @@ -136,44 +134,14 @@ impl Executable for GuardConditionWaitable { Ok(()) } - fn kind(&self) -> super::WaitableKind { - WaitableKind::GuardCondition + fn kind(&self) -> ExecutableKind { + ExecutableKind::GuardCondition } -} - -impl Waitable for GuardConditionWaitable { - unsafe fn add_to_wait_set( - &mut self, - wait_set: &mut rcl_wait_set_t, - ) -> Result { - let mut index: usize = 0; - unsafe { - // SAFETY: We are holding onto an Arc of the handle, so the guard - // condition is still valid. - rcl_wait_set_add_guard_condition( - wait_set, - &mut *self.handle.rcl_guard_condition.lock(), - &mut index, - ) - } - .ok()?; - - Ok(index) - } - - unsafe fn is_ready( - &self, - wait_set: &rcl_wait_set_t, - index: usize, - ) -> bool { - let entity = unsafe { - // SAFETY: The `guard_condition` entry is an array of pointers, and this - // dereferencing is equivalent to getting the element of the array - // at `index`. - *wait_set.guard_conditions.add(index) - }; - !entity.is_null() + fn handle(&self) -> super::ExecutableHandle { + ExecutableHandle::GuardCondition( + self.handle.rcl_guard_condition.lock().unwrap() + ) } } diff --git a/rclrs/src/wait/waitable.rs b/rclrs/src/wait/waitable.rs index 44007a1da..26456a5f3 100644 --- a/rclrs/src/wait/waitable.rs +++ b/rclrs/src/wait/waitable.rs @@ -1,16 +1,17 @@ use std::sync::{ atomic::{AtomicBool, Ordering}, - Arc, + Arc, MutexGuard, }; use crate::{ error::ToResult, rcl_bindings::*, - RclrsError, WaitSet, GuardCondition, + RclrsError, GuardCondition, }; +/// Enum to describe the kind of an executable. #[derive(Clone, Copy, Hash, PartialEq, Eq, PartialOrd, Ord)] -pub(crate) enum WaitableKind { +pub enum ExecutableKind { Subscription, GuardCondition, Timer, @@ -19,6 +20,16 @@ pub(crate) enum WaitableKind { Event, } +/// Used by the wait set to obtain the handle of an executable. +pub enum ExecutableHandle<'a> { + Subscription(MutexGuard<'a, rcl_subscription_t>), + GuardCondition(MutexGuard<'a, rcl_guard_condition_t>), + Timer(MutexGuard<'a, rcl_timer_t>), + Client(MutexGuard<'a, rcl_client_t>), + Service(MutexGuard<'a, rcl_service_t>), + Event(MutexGuard<'a, rcl_event_t>), +} + #[derive(Default, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] pub struct WaitableCount { pub subscriptions: usize, @@ -34,14 +45,14 @@ impl WaitableCount { Self::default() } - pub(super) fn add(&mut self, kind: WaitableKind, count: usize) { + pub(super) fn add(&mut self, kind: ExecutableKind, count: usize) { match kind { - WaitableKind::Subscription => self.subscriptions += count, - WaitableKind::GuardCondition => self.guard_conditions += count, - WaitableKind::Timer => self.timers += count, - WaitableKind::Client => self.clients += count, - WaitableKind::Service => self.services += count, - WaitableKind::Event => self.events += count, + ExecutableKind::Subscription => self.subscriptions += count, + ExecutableKind::GuardCondition => self.guard_conditions += count, + ExecutableKind::Timer => self.timers += count, + ExecutableKind::Client => self.clients += count, + ExecutableKind::Service => self.services += count, + ExecutableKind::Event => self.events += count, } } @@ -91,44 +102,31 @@ impl WaitableCount { /// This provides the public API for executing a waitable item. pub trait Executable { - /// Indicate what kind of executable this is. - fn kind(&self) -> WaitableKind; - /// Trigger this executable to run. fn execute(&mut self) -> Result<(), RclrsError>; -} -/// This provides the internal APIs for a waitable item to interact with the -/// wait set that manages it. -pub(crate) trait Waitable: Executable { - /// Add this to a wait set - unsafe fn add_to_wait_set( - &mut self, - wait_set: &mut rcl_wait_set_t, - ) -> Result; + /// Indicate what kind of executable this is. + fn kind(&self) -> ExecutableKind; - unsafe fn is_ready( - &self, - wait_set: &rcl_wait_set_t, - index: usize, - ) -> bool; + /// Provide the handle for this executable + fn handle(&self) -> ExecutableHandle; } #[must_use = "If you do not give the Waiter to a WaitSet then it will never be useful"] -pub struct Waiter { - pub(super) waitable: Box, +pub struct Waitable { + pub(super) executable: Box, in_use: Arc, index_in_wait_set: Option, } -impl Waiter { +impl Waitable { pub fn new( - waitable: Box, + waitable: Box, guard_condition: Option>, ) -> (Self, WaiterLifecycle) { let in_use = Arc::new(AtomicBool::new(true)); let waiter = Self { - waitable, + executable: waitable, in_use: Arc::clone(&in_use), index_in_wait_set: None, }; @@ -146,37 +144,58 @@ impl Waiter { } pub(super) fn is_ready(&self, wait_set: &rcl_wait_set_t) -> bool { - self.index_in_wait_set.is_some_and(|index| - unsafe { - // SAFETY: The Waitable::is_ready function is marked as unsafe - // because this is the only place that it makes sense to use it. - self.waitable.is_ready(wait_set, index) - } - ) + self.index_in_wait_set.is_some_and(|index| { + let ptr_is_null = unsafe { + // SAFETY: Each field in the wait set is an array of points. + // The dereferencing that we do is equivalent to obtaining the + // element of the array at the index-th position. + match self.executable.kind() { + ExecutableKind::Subscription => wait_set.subscriptions.add(index).is_null(), + ExecutableKind::GuardCondition => wait_set.guard_conditions.add(index).is_null(), + ExecutableKind::Service => wait_set.services.add(index).is_null(), + ExecutableKind::Client => wait_set.clients.add(index).is_null(), + ExecutableKind::Timer => wait_set.timers.add(index).is_null(), + ExecutableKind::Event => wait_set.events.add(index).is_null(), + } + }; + !ptr_is_null + }) } pub(super) fn add_to_wait_set( &mut self, wait_set: &mut rcl_wait_set_t, ) -> Result<(), RclrsError> { - self.index_in_wait_set = Some( - unsafe { - // SAFETY: The Waitable::add_to_wait_set function is marked as - // unsafe because this is the only place that it makes sense to use it. - self.waitable.add_to_wait_set(wait_set)? - } - ); - Ok(()) - } -} -pub trait AsExecutable { - fn as_executable(&mut self) -> &mut dyn Executable; -} + let mut index = 0; + unsafe { + // SAFETY: The Executable is responsible for maintaining the lifecycle + // of the handle, so it is guaranteed to be valid here. + match self.executable.handle() { + ExecutableHandle::Subscription(handle) => { + rcl_wait_set_add_subscription(wait_set, &*handle, &mut index) + } + ExecutableHandle::GuardCondition(handle) => { + rcl_wait_set_add_guard_condition(wait_set, &*handle, &mut index) + } + ExecutableHandle::Service(handle) => { + rcl_wait_set_add_service(wait_set, &*handle, &mut index) + } + ExecutableHandle::Client(handle) => { + rcl_wait_set_add_client(wait_set, &*handle, &mut index) + } + ExecutableHandle::Timer(handle) => { + rcl_wait_set_add_timer(wait_set, &*handle, &mut index) + } + ExecutableHandle::Event(handle) => { + rcl_wait_set_add_event(wait_set, &*handle, &mut index) + } + } + } + .ok()?; -impl AsExecutable for T { - fn as_executable(&mut self) -> &mut dyn Executable { - self + self.index_in_wait_set = Some(index); + Ok(()) } } From 2f9108344b24c3241cb014c6a1ab2090478b87ce Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 13 Oct 2024 00:00:29 +0800 Subject: [PATCH 10/48] Migrate clients to new async implementation Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 406 ++++++++++------------ rclrs/src/client/client_async_callback.rs | 54 +++ rclrs/src/client/client_callback.rs | 45 +++ rclrs/src/client/client_output.rs | 71 ++++ rclrs/src/client/client_task.rs | 96 +++++ rclrs/src/node.rs | 17 +- rclrs/src/parameter/service.rs | 41 ++- rclrs/src/service.rs | 16 +- rclrs/src/service/service_callback.rs | 8 +- rclrs/src/service/service_info.rs | 6 +- rclrs/src/subscription.rs | 10 +- rclrs/src/wait/guard_condition.rs | 95 +---- rclrs/src/wait/waitable.rs | 8 +- 13 files changed, 524 insertions(+), 349 deletions(-) create mode 100644 rclrs/src/client/client_async_callback.rs create mode 100644 rclrs/src/client/client_callback.rs create mode 100644 rclrs/src/client/client_output.rs create mode 100644 rclrs/src/client/client_task.rs diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index b308f1de2..7deebdd3a 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -1,66 +1,32 @@ use std::{ boxed::Box, - collections::HashMap, ffi::CString, - sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, + sync::{Arc, Mutex, MutexGuard}, }; -use futures::channel::oneshot; +use futures::channel::mpsc::{UnboundedSender, unbounded}; + use rosidl_runtime_rs::Message; use crate::{ - error::{RclReturnCode, ToResult}, + error::ToResult, rcl_bindings::*, - MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + MessageCow, NodeHandle, RclrsError, Promise, ENTITY_LIFECYCLE_MUTEX, + ExecutorCommands, Executable, QoSProfile, Waitable, WaitableLifecycle, + ExecutableHandle, ExecutableKind, ServiceInfo, }; -// 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 {} +mod client_async_callback; +pub use client_async_callback::*; -/// 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_callback; +pub use client_callback::*; -impl ClientHandle { - pub(crate) fn lock(&self) -> MutexGuard { - self.rcl_client.lock().unwrap() - } -} +mod client_output; +pub use client_output::*; -impl Drop for ClientHandle { - fn drop(&mut self) { - let rcl_client = self.rcl_client.get_mut().unwrap(); - let mut rcl_node = self.node_handle.rcl_node.lock().unwrap(); - let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); - // SAFETY: The entity lifecycle mutex is locked to protect against the risk of - // global variables in the rmw implementation being unsafely modified during cleanup. - unsafe { - rcl_client_fini(rcl_client, &mut *rcl_node); - } - } -} - -/// Trait to be implemented by concrete Client structs. -/// -/// See [`Client`] for an example. -pub trait ClientBase: Send + Sync { - /// Internal function to get a reference to the `rcl` handle. - fn handle(&self) -> &ClientHandle; - /// Tries to take a new response and run the callback or future with it. - fn execute(&self) -> Result<(), RclrsError>; -} - -type RequestValue = Box; - -type RequestId = i64; +mod client_task; +use client_task::*; /// Main class responsible for sending requests to a ROS service. /// @@ -73,17 +39,125 @@ pub struct Client where T: rosidl_runtime_rs::Service, { - pub(crate) handle: Arc, - requests: Mutex>>, - futures: Arc>>>, + handle: Arc, + action: UnboundedSender>, + lifecycle: WaitableLifecycle, + commands: Arc, } impl Client 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]`)` + /// + /// [1]: crate::RequestId + /// [2]: crate::ServiceInfo + pub fn call<'a, Req, Out>( + &self, + request: Req, + ) -> Result, RclrsError> + where + Req: MessageCow<'a, T::Request>, + Out: ClientOutput, + { + let (sender, promise) = Out::create_channel(); + let rmw_message = T::Request::into_rmw_message(request.into_cow()); + let mut sequence_number = -1; + unsafe { + // SAFETY: The client handle ensures the rcl_client is valid and + // our generic system ensures it has the correct type. + rcl_send_request( + &*self.handle.lock() as *const _, + rmw_message.as_ref() as *const ::RmwMsg as *mut _, + &mut sequence_number, + ) + } + .ok()?; + + self.action.unbounded_send( + ClientAction::NewRequest { sequence_number, sender } + ); + + Ok(promise) + } + + /// Call this service and then handle its response with a regular callback. + // + // 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. + // + // 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)?; + self.commands.run_detached(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(()) + } + + /// Check if a service server is available. + /// + /// Will return true if there is a service server available, false if unavailable. + // + // TODO(@mxgrey): Provide an async function to await on when the service 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) + } + /// Creates a new client. - pub(crate) fn new(node_handle: Arc, topic: &str) -> Result + pub(crate) fn create( + topic: &str, + qos: QoSProfile, + node_handle: &Arc, + commands: &Arc, + ) -> 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 @@ -99,7 +173,8 @@ where })?; // 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(); @@ -126,187 +201,86 @@ where let handle = Arc::new(ClientHandle { rcl_client: Mutex::new(rcl_client), - node_handle, - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + node_handle: Arc::clone(&node_handle), }); - Ok(Self { + let (action, receiver) = unbounded(); + commands.run_detached(client_task(receiver, Arc::clone(&handle))); + + let (waitable, lifecycle) = Waitable::new( + Box::new(ClientExecutable { + handle: Arc::clone(&handle), + action: action.clone(), + }), + 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(), - )), - }) + action, + lifecycle, + commands: Arc::clone(&commands), + })) } +} - /// Sends a request with a callback to be called with the response. - /// - /// The [`MessageCow`] trait is implemented by any - /// [`Message`] as well as any reference to a `Message`. - /// - /// The reason for allowing owned messages is that publishing owned messages can be more - /// efficient in the case of idiomatic messages[^note]. - /// - /// [^note]: See the [`Message`] trait for an explanation of "idiomatic". - /// - /// Hence, when a message will not be needed anymore after publishing, pass it by value. - /// When a message will be needed again after publishing, pass it by reference, instead of cloning and passing by value. - pub fn async_send_request_with_callback<'a, M: MessageCow<'a, T::Request>, F>( - &self, - message: M, - callback: F, - ) -> Result<(), RclrsError> - where - F: FnOnce(T::Response) + 'static + Send, - { - let rmw_message = T::Request::into_rmw_message(message.into_cow()); - let mut sequence_number = -1; - unsafe { - // SAFETY: The request type is guaranteed to match the client type by the type system. - rcl_send_request( - &*self.handle.lock() as *const _, - rmw_message.as_ref() as *const ::RmwMsg as *mut _, - &mut sequence_number, - ) - } - .ok()?; - let requests = &mut *self.requests.lock().unwrap(); - requests.insert(sequence_number, Box::new(callback)); +struct ClientExecutable { + handle: Arc, + action: UnboundedSender> +} + +impl Executable for ClientExecutable +where + T: rosidl_runtime_rs::Service, +{ + fn execute(&mut self) -> Result<(), RclrsError> { + self.action.unbounded_send(ClientAction::TakeResponse).ok(); 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()) + fn handle(&self) -> ExecutableHandle { + ExecutableHandle::Client(self.handle.lock()) } - /// 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)) + fn kind(&self) -> ExecutableKind { + ExecutableKind::Client } +} - /// Check if a service server is available. - /// - /// Will return true if there is a service server available, false if unavailable. - /// - pub fn service_is_ready(&self) -> Result { - let mut is_ready = false; - let client = &mut *self.handle.rcl_client.lock().unwrap(); - let node = &mut *self.handle.node_handle.rcl_node.lock().unwrap(); - - unsafe { - // SAFETY both node and client are guaranteed to be valid here - // client is guaranteed to have been generated with node - rcl_service_server_is_available(node as *const _, client as *const _, &mut is_ready) - } - .ok()?; - Ok(is_ready) - } +/// 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, + node_handle: Arc, } -impl ClientBase for Client -where - T: rosidl_runtime_rs::Service, -{ - fn handle(&self) -> &ClientHandle { - &self.handle +impl ClientHandle { + fn lock(&self) -> MutexGuard { + self.rcl_client.lock().unwrap() } +} - 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(()); - } - 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); +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); } - Ok(()) } } +// 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::*; @@ -325,7 +299,7 @@ mod tests { let graph = construct_test_graph(namespace)?; let _node_2_empty_client = graph .node2 - .create_client::("graph_test_topic_4")?; + .create_client::("graph_test_topic_4", QoSProfile::services_default())?; std::thread::sleep(std::time::Duration::from_millis(200)); diff --git a/rclrs/src/client/client_async_callback.rs b/rclrs/src/client/client_async_callback.rs new file mode 100644 index 000000000..b97c8472d --- /dev/null +++ b/rclrs/src/client/client_async_callback.rs @@ -0,0 +1,54 @@ +use rosidl_runtime_rs::Service; + +use std::future::Future; + +use crate::{ServiceInfo, RequestId}; + +/// 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, +{ + type Task: Future + Send; + 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..d34fb6980 --- /dev/null +++ b/rclrs/src/client/client_callback.rs @@ -0,0 +1,45 @@ +use rosidl_runtime_rs::Service; + +use crate::{ServiceInfo, RequestId}; + +/// 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, +{ + 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..e0f7b8c44 --- /dev/null +++ b/rclrs/src/client/client_output.rs @@ -0,0 +1,71 @@ +use rosidl_runtime_rs::Message; + +use futures::channel::oneshot::{Sender, channel}; + +use crate::{ + rcl_bindings::rmw_service_info_t, + RequestId, ServiceInfo, Promise, +}; + +/// 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 { + fn create_channel() -> (AnyClientOutputSender, Promise); +} + +impl ClientOutput for Response { + fn create_channel() -> (AnyClientOutputSender, Promise) { + let (sender, receiver) = channel(); + (AnyClientOutputSender::RequestOnly(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 { + RequestOnly(Sender), + WithId(Sender<(Response, RequestId)>), + WithServiceInfo(Sender<(Response, ServiceInfo)>), +} + +impl AnyClientOutputSender { + pub(super) fn send_response( + self, + response: Response, + service_info: rmw_service_info_t, + ) { + match self { + Self::RequestOnly(sender) => { + sender.send(response); + } + Self::WithId(sender) => { + sender.send(( + response, + RequestId::from_rmw_request_id(&service_info.request_id), + )); + } + Self::WithServiceInfo(sender) => { + sender.send(( + response, + ServiceInfo::from_rmw_service_info(&service_info), + )); + } + } + } +} diff --git a/rclrs/src/client/client_task.rs b/rclrs/src/client/client_task.rs new file mode 100644 index 000000000..91ede050d --- /dev/null +++ b/rclrs/src/client/client_task.rs @@ -0,0 +1,96 @@ +use rosidl_runtime_rs::{Message, Service}; + +use futures::{ + channel::mpsc::UnboundedReceiver, + StreamExt, +}; + +use std::{ + sync::Arc, + collections::HashMap, +}; + +use crate::{ + error::ToResult, + rcl_bindings::*, + client::ClientHandle, + AnyClientOutputSender, ServiceInfo, + RclrsError, +}; + +pub enum ClientAction { + TakeResponse, + NewRequest{ + sequence_number: SequenceNumber, + sender: AnyClientOutputSender, + }, +} + +pub(super) type SequenceNumber = i64; + +pub(super) async fn client_task( + mut receiver: UnboundedReceiver>, + handle: Arc, +) { + // This stores all active requests that have not received a response yet + let mut active_requests: HashMap> = HashMap::new(); + + // This holds responses that came in when no active request matched the + // sequence number. This could happen if the TakeResponse arrives before the + // NewRequest for the same sequence number. That is extremely unlikely to + // ever happen but is theoretically possible, so we should account for it. + let mut loose_responses: HashMap = HashMap::new(); + + while let Some(action) = receiver.next().await { + match action { + ClientAction::TakeResponse => { + match take_response::(&handle) { + Ok((response, info)) => { + let seq = info.request_id.sequence_number; + if let Some(sender) = 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. + loose_responses.insert(seq, (response, info)); + } + } + Err(_err) => { + // TODO(@mxgrey): Log the error here once logging is available + } + } + } + ClientAction::NewRequest { sequence_number, sender } => { + if let Some((response, info)) = loose_responses.remove(&sequence_number) { + // The response for this request already arrive, so we'll + // send it off immediately. + sender.send_response(response, info); + } else { + active_requests.insert(sequence_number, sender); + } + } + } + } +} + +fn take_response( + handle: &Arc, +) -> Result<(Response, rmw_service_info_t), RclrsError> { + let mut service_info_out = ServiceInfo::zero_initialized_rmw(); + let mut response_out = Response::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 Response::RmwMsg as *mut _, + ) + } + .ok() + .map(|_| ( + Response::from_rmw_message(response_out), + service_info_out, + )) +} diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index d04b4a97a..37ec6a299 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -5,14 +5,14 @@ use std::{ ffi::CStr, fmt, os::raw::c_char, - sync::{Arc, Mutex, Weak}, + sync::{Arc, Mutex}, }; use rosidl_runtime_rs::Message; pub use self::{options::*, graph::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, ContextHandle, + rcl_bindings::*, Client, Clock, ContextHandle, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, Subscription, SubscriptionCallback, SubscriptionAsyncCallback, ServiceCallback, ServiceAsyncCallback, ExecutorCommands, TimeSource, ENTITY_LIFECYCLE_MUTEX, @@ -180,13 +180,15 @@ impl Node { /// /// [1]: crate::Client // TODO: make client's lifetime depend on node's lifetime - pub fn create_client(&self, topic: &str) -> Result>, RclrsError> + pub fn create_client( + &self, + topic: &str, + qos: QoSProfile, + ) -> 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) + Client::::create(topic, qos, &self.handle, &self.commands) } /// Creates a [`Publisher`][1]. @@ -201,8 +203,7 @@ impl Node { where T: Message, { - let publisher = Arc::new(Publisher::::new(Arc::clone(&self.handle), topic, qos)?); - Ok(publisher) + Ok(Arc::new(Publisher::::new(Arc::clone(&self.handle), topic, qos)?)) } /// Creates a [`Service`] with an ordinary callback. diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 1fea50af5..b0cb0404b 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -319,7 +319,7 @@ mod tests { srv::rmw::*, }, Context, MandatoryParameter, Node, NodeOptions, ParameterRange, ParameterValue, RclrsError, - ReadOnlyParameter, + ReadOnlyParameter, QoSProfile, }; use rosidl_runtime_rs::{seq, Sequence}; use std::sync::{Arc, RwLock}; @@ -438,7 +438,10 @@ mod tests { 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")?; + let list_client = client.create_client::( + "/list/node/list_parameters", + QoSProfile::services_default(), + )?; try_until_timeout(|| list_client.service_is_ready().unwrap()) .await @@ -466,7 +469,7 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); list_client - .async_send_request_with_callback( + .call_then( &request, move |response: ListParameters_Response| { // use_sim_time + all the manually defined ones @@ -497,7 +500,7 @@ mod tests { let call_done = client_finished.clone(); *call_done.write().unwrap() = false; list_client - .async_send_request_with_callback( + .call_then( &request, move |response: ListParameters_Response| { *call_done.write().unwrap() = true; @@ -520,7 +523,7 @@ mod tests { let call_done = client_finished.clone(); *call_done.write().unwrap() = false; list_client - .async_send_request_with_callback( + .call_then( &request, move |response: ListParameters_Response| { *call_done.write().unwrap() = true; @@ -544,7 +547,7 @@ mod tests { let call_done = client_finished.clone(); *call_done.write().unwrap() = false; list_client - .async_send_request_with_callback( + .call_then( &request, move |response: ListParameters_Response| { *call_done.write().unwrap() = true; @@ -573,10 +576,10 @@ mod tests { 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 get_client = client.create_client::("/get_set/node/get_parameters", QoSProfile::services_default())?; + let set_client = client.create_client::("/get_set/node/set_parameters", QoSProfile::services_default())?; let set_atomically_client = client - .create_client::("/get_set/node/set_parameters_atomically")?; + .create_client::("/get_set/node/set_parameters_atomically", QoSProfile::services_default())?; try_until_timeout(|| { get_client.service_is_ready().unwrap() @@ -608,7 +611,7 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); get_client - .async_send_request_with_callback( + .call_then( &request, move |response: GetParameters_Response| { *call_done.write().unwrap() = true; @@ -631,7 +634,7 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); get_client - .async_send_request_with_callback( + .call_then( &request, move |response: GetParameters_Response| { *call_done.write().unwrap() = true; @@ -720,7 +723,7 @@ mod tests { // Parameter is assigned a default of true at declaration time assert!(node.bool_param.get()); set_client - .async_send_request_with_callback( + .call_then( &request, move |response: SetParameters_Response| { *call_done.write().unwrap() = true; @@ -757,7 +760,7 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); set_client - .async_send_request_with_callback( + .call_then( &request, move |response: SetParameters_Response| { *call_done.write().unwrap() = true; @@ -782,7 +785,7 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); set_atomically_client - .async_send_request_with_callback( + .call_then( &request, move |response: SetParametersAtomically_Response| { *call_done.write().unwrap() = true; @@ -807,9 +810,9 @@ mod tests { let context = Context::new([]).unwrap(); let (node, client) = construct_test_nodes(&context, "describe"); let describe_client = - client.create_client::("/describe/node/describe_parameters")?; + client.create_client::("/describe/node/describe_parameters", QoSProfile::services_default())?; let get_types_client = - client.create_client::("/describe/node/get_parameter_types")?; + client.create_client::("/describe/node/get_parameter_types", QoSProfile::services_default())?; try_until_timeout(|| { describe_client.service_is_ready().unwrap() @@ -845,7 +848,7 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); describe_client - .async_send_request_with_callback( + .call_then( &request, move |response: DescribeParameters_Response| { *call_done.write().unwrap() = true; @@ -892,7 +895,7 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); describe_client - .async_send_request_with_callback( + .call_then( &request, move |response: DescribeParameters_Response| { *call_done.write().unwrap() = true; @@ -924,7 +927,7 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); get_types_client - .async_send_request_with_callback( + .call_then( &request, move |response: GetParameterTypes_Response| { *call_done.write().unwrap() = true; diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index acc2430e8..2c306ce76 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -9,7 +9,7 @@ use futures::channel::mpsc::{unbounded, UnboundedSender, TrySendError}; use crate::{ error::ToResult, rcl_bindings::*, - NodeHandle, RclrsError, Waitable, WaiterLifecycle, GuardCondition, QoSProfile, + NodeHandle, RclrsError, Waitable, WaitableLifecycle, GuardCondition, QoSProfile, Executable, ExecutableKind, ExecutableHandle, ENTITY_LIFECYCLE_MUTEX, ExecutorCommands, }; @@ -52,7 +52,7 @@ where action: UnboundedSender>, /// Holding onto this keeps the waiter for this service alive in the wait /// set of the executor. - lifecycle: WaiterLifecycle, + lifecycle: WaitableLifecycle, } impl Service @@ -107,7 +107,7 @@ where commands: &Arc, ) -> Result, RclrsError> { let (sender, receiver) = unbounded(); - let (service, waiter) = Self::new( + let (service, waitable) = Self::new( topic, qos, sender, @@ -122,7 +122,7 @@ where Arc::clone(commands), )); - commands.add_to_wait_set(waiter); + commands.add_to_wait_set(waitable); Ok(service) } @@ -176,7 +176,7 @@ where }); let (waiter, lifecycle) = Waitable::new( - Box::new(ServiceWaitable { + Box::new(ServiceExecutable { handle: Arc::clone(&handle), action: action.clone(), }), @@ -189,12 +189,12 @@ where } } -struct ServiceWaitable { +struct ServiceExecutable { handle: Arc, action: UnboundedSender>, } -impl Executable for ServiceWaitable +impl Executable for ServiceExecutable where T: rosidl_runtime_rs::Service, { @@ -284,7 +284,7 @@ mod tests { )?; let _node_2_empty_client = graph .node2 - .create_client::("graph_test_topic_4")?; + .create_client::("graph_test_topic_4", QoSProfile::services_default())?; std::thread::sleep(std::time::Duration::from_millis(100)); diff --git a/rclrs/src/service/service_callback.rs b/rclrs/src/service/service_callback.rs index 2d0bffd4f..317b42586 100644 --- a/rclrs/src/service/service_callback.rs +++ b/rclrs/src/service/service_callback.rs @@ -1,14 +1,16 @@ use rosidl_runtime_rs::Service; -use super::{ +use crate::{ ServiceInfo, RequestId, - any_service_callback::AnyServiceCallback, + service::any_service_callback::AnyServiceCallback, }; use std::sync::Arc; -/// A trait for regular callbacks of services. +/// A trait to deduce regular callbacks of services. /// +/// Users of rclrs never need to use this trait directly. +// // TODO(@mxgrey): Add a description of what callbacks signatures are supported pub trait ServiceCallback: Send + 'static where diff --git a/rclrs/src/service/service_info.rs b/rclrs/src/service/service_info.rs index 99f6d49ec..f5e1e7ecf 100644 --- a/rclrs/src/service/service_info.rs +++ b/rclrs/src/service/service_info.rs @@ -33,7 +33,7 @@ impl ServiceInfo { } } - pub(super) fn zero_initialized_rmw() -> rmw_service_info_t { + pub(crate) fn zero_initialized_rmw() -> rmw_service_info_t { rmw_service_info_t { source_timestamp: 0, received_timestamp: 0, @@ -56,14 +56,14 @@ pub struct RequestId { } impl RequestId { - pub(super) fn from_rmw_request_id(rmw_request_id: &rmw_request_id_t) -> Self { + 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(super) fn zero_initialized_rmw() -> rmw_request_id_t { + pub(crate) fn zero_initialized_rmw() -> rmw_request_id_t { rmw_request_id_t { writer_guid: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], sequence_number: 0, diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 32e1a0dbf..d7a284191 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -12,7 +12,7 @@ use crate::{ qos::QoSProfile, rcl_bindings::*, ExecutorCommands, NodeHandle, RclrsError, Waitable, Executable, ExecutableHandle, - ExecutableKind, GuardCondition, WaiterLifecycle, ENTITY_LIFECYCLE_MUTEX, + ExecutableKind, GuardCondition, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, }; mod any_subscription_callback; @@ -62,7 +62,7 @@ where action: UnboundedSender>, /// Holding onto this keeps the waiter for this subscription alive in the /// wait set of the executor. - lifecycle: WaiterLifecycle, + lifecycle: WaitableLifecycle, } impl Subscription @@ -190,7 +190,7 @@ where }); let (waiter, lifecycle) = Waitable::new( - Box::new(SubscriptionWaitable { + Box::new(SubscriptionExecutable { handle: Arc::clone(&handle), action: action.clone(), }), @@ -203,12 +203,12 @@ where } } -struct SubscriptionWaitable { +struct SubscriptionExecutable { handle: Arc, action: UnboundedSender>, } -impl Executable for SubscriptionWaitable +impl Executable for SubscriptionExecutable where T: Message, { diff --git a/rclrs/src/wait/guard_condition.rs b/rclrs/src/wait/guard_condition.rs index 587801fd6..2b612d849 100644 --- a/rclrs/src/wait/guard_condition.rs +++ b/rclrs/src/wait/guard_condition.rs @@ -2,56 +2,27 @@ use std::sync::{Arc, Mutex}; use crate::{ rcl_bindings::*, - ContextHandle, RclrsError, ToResult, WaiterLifecycle, Executable, + ContextHandle, RclrsError, ToResult, WaitableLifecycle, Executable, Waitable, ExecutableKind, ExecutableHandle, ExecutorCommands, }; /// A waitable entity used for waking up a wait set manually. /// -/// If a wait set that is currently waiting on events should be interrupted from -/// a separate thread, trigger a `GuardCondition`. +/// This is used internally to wake up the executor's wait set to check if its +/// spin conditions are still valid or to perform cleanup (releasing waitable +/// entries that have been dropped by the user). /// -/// A guard condition may be triggered any number of times, but can only be -/// associated with one wait set. -/// -/// # Example -/// ``` -/// # use rclrs::{Context, GuardCondition, WaitSet, RclrsError}; -/// # use std::sync::{Arc, atomic::Ordering}; -/// -/// let context = Context::new([])?; -/// -/// let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false)); -/// let atomic_bool_for_closure = Arc::clone(&atomic_bool); -/// -/// let gc = Arc::new(GuardCondition::new_with_callback( -/// &context, -/// move || { -/// atomic_bool_for_closure.store(true, Ordering::Relaxed); -/// }, -/// )); -/// -/// let mut ws = WaitSet::new(0, 1, 0, 0, 0, 0, &context)?; -/// ws.add_guard_condition(Arc::clone(&gc))?; -/// -/// // Trigger the guard condition, firing the callback and waking the wait set being waited on, if any. -/// gc.trigger()?; -/// -/// // The provided callback has now been called. -/// assert_eq!(atomic_bool.load(Ordering::Relaxed), true); -/// -/// // The wait call will now immediately return. -/// ws.wait(Some(std::time::Duration::from_millis(10)))?; -/// -/// # Ok::<(), RclrsError>(()) -/// ``` +/// Users of rclrs have no reason to use this struct unless you are +/// implementing a custom executor. If you want to trigger a function to run on +/// the executor you should use [`ExecutorCommands::run`] or +/// [`ExecutorCommands::run_detached`]. pub struct GuardCondition { /// The rcl_guard_condition_t that this struct encapsulates. Holding onto /// this keeps the rcl_guard_condition alive and allows us to trigger it. handle: Arc, /// This manages the lifecycle of this guard condition's waiter. Dropping /// this will remove the guard condition from its wait set. - lifecycle: WaiterLifecycle, + lifecycle: WaitableLifecycle, } // SAFETY: rcl_guard_condition is the only member that doesn't implement Send, and it is designed to be accessed from other threads @@ -83,7 +54,7 @@ impl GuardCondition { }); let (waiter, lifecycle) = Waitable::new( - Box::new(GuardConditionWaitable { handle: Arc::clone(&handle) }), + Box::new(GuardConditionExecutable { handle: Arc::clone(&handle) }), None, ); @@ -124,11 +95,11 @@ impl Drop for GuardConditionHandle { } } -struct GuardConditionWaitable { +struct GuardConditionExecutable { handle: Arc, } -impl Executable for GuardConditionWaitable { +impl Executable for GuardConditionExecutable { fn execute(&mut self) -> Result<(), RclrsError> { // Do nothing Ok(()) @@ -147,49 +118,7 @@ impl Executable for GuardConditionWaitable { #[cfg(test)] mod tests { - use std::sync::atomic::Ordering; - use super::*; - use crate::WaitSet; - - #[test] - fn test_guard_condition() -> Result<(), RclrsError> { - let context = Context::new([])?; - - let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false)); - let atomic_bool_for_closure = Arc::clone(&atomic_bool); - - let guard_condition = GuardCondition::new_with_callback(&context, move || { - atomic_bool_for_closure.store(true, Ordering::Relaxed); - }); - - guard_condition.trigger()?; - - assert!(atomic_bool.load(Ordering::Relaxed)); - - Ok(()) - } - - #[test] - fn test_guard_condition_wait() -> Result<(), RclrsError> { - let context = Context::new([])?; - - let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false)); - let atomic_bool_for_closure = Arc::clone(&atomic_bool); - - let guard_condition = Arc::new(GuardCondition::new_with_callback(&context, move || { - atomic_bool_for_closure.store(true, Ordering::Relaxed); - })); - - let mut wait_set = WaitSet::new(0, 1, 0, 0, 0, 0, &context)?; - wait_set.add_guard_condition(Arc::clone(&guard_condition))?; - guard_condition.trigger()?; - - assert!(atomic_bool.load(Ordering::Relaxed)); - wait_set.wait(Some(std::time::Duration::from_millis(10)))?; - - Ok(()) - } #[test] fn traits() { diff --git a/rclrs/src/wait/waitable.rs b/rclrs/src/wait/waitable.rs index 26456a5f3..3329315be 100644 --- a/rclrs/src/wait/waitable.rs +++ b/rclrs/src/wait/waitable.rs @@ -123,7 +123,7 @@ impl Waitable { pub fn new( waitable: Box, guard_condition: Option>, - ) -> (Self, WaiterLifecycle) { + ) -> (Self, WaitableLifecycle) { let in_use = Arc::new(AtomicBool::new(true)); let waiter = Self { executable: waitable, @@ -131,7 +131,7 @@ impl Waitable { index_in_wait_set: None, }; - let lifecycle = WaiterLifecycle { in_use, guard_condition }; + let lifecycle = WaitableLifecycle { in_use, guard_condition }; (waiter, lifecycle) } @@ -200,12 +200,12 @@ impl Waitable { } #[must_use = "If you do not hold onto the WaiterLifecycle, then its Waiter will be immediately dropped"] -pub struct WaiterLifecycle { +pub struct WaitableLifecycle { in_use: Arc, guard_condition: Option>, } -impl Drop for WaiterLifecycle { +impl Drop for WaitableLifecycle { fn drop(&mut self) { self.in_use.store(false, Ordering::Release); if let Some(guard_condition) = &self.guard_condition { From cc5b26495599a4b6f6a383924924ed6049b7bf59 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 13 Oct 2024 14:23:02 +0800 Subject: [PATCH 11/48] Fleshing out basic executor Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 4 +- rclrs/src/context.rs | 13 +- rclrs/src/executor.rs | 175 ++++++------------ rclrs/src/executor/basic_executor.rs | 117 +++++++++++- rclrs/src/executor/wait_set_runner.rs | 114 ++++++++++++ rclrs/src/lib.rs | 23 --- rclrs/src/service.rs | 2 +- rclrs/src/service/any_service_callback.rs | 6 +- rclrs/src/subscription.rs | 2 +- .../subscription/any_subscription_callback.rs | 12 +- rclrs/src/wait.rs | 3 +- rclrs/src/wait/guard_condition.rs | 38 ++-- 12 files changed, 324 insertions(+), 185 deletions(-) create mode 100644 rclrs/src/executor/wait_set_runner.rs diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index 7deebdd3a..f61ec1acb 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -118,7 +118,7 @@ where Req: MessageCow<'a, T::Request>, { let response: Promise<(T::Response, ServiceInfo)> = self.call(request)?; - self.commands.run_detached(async move { + self.commands.run(async move { match response.await { Ok((response, info)) => { callback.run_client_async_callback(response, info).await; @@ -205,7 +205,7 @@ where }); let (action, receiver) = unbounded(); - commands.run_detached(client_task(receiver, Arc::clone(&handle))); + commands.run(client_task(receiver, Arc::clone(&handle))); let (waitable, lifecycle) = Waitable::new( Box::new(ClientExecutable { diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 9c2fcce25..eeda8f725 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -73,6 +73,14 @@ pub(crate) struct ContextHandle { pub(crate) rcl_context: Mutex, } +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([]).expect("Failed to instantiate a default context") + } +} + impl Context { /// Creates a new context. /// @@ -158,7 +166,8 @@ impl Context { /// /// [1]: BasicExecutorRuntime pub fn create_basic_executor(&self) -> Executor { - self.create_executor(BasicExecutorRuntime::default()) + let runtime = BasicExecutorRuntime::new(self); + self.create_executor(runtime) } /// Create an [`Executor`] for this context. @@ -199,8 +208,6 @@ impl Context { // SAFETY: No preconditions for this function. unsafe { rcl_context_is_valid(rcl_context) } } - - // TODO(@mxgrey): } /// Additional options for initializing the Context. diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 643a15387..fbe1def44 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,19 +1,22 @@ mod basic_executor; pub use self::basic_executor::*; +mod wait_set_runner; +pub use self::wait_set_runner::*; + use crate::{ - rcl_bindings::rcl_context_is_valid, - Node, NodeOptions, RclReturnCode, RclrsError, WaitSet, Context, - ContextHandle, Waitable, GuardCondition, + Node, NodeOptions, RclrsError, Context, ContextHandle, Waitable, GuardCondition, }; use std::{ - sync::{Arc, Mutex, Weak}, + sync::{Arc, atomic::{AtomicBool, Ordering}}, time::Duration, - sync::atomic::{AtomicBool, Ordering}, future::Future, }; pub use futures::channel::oneshot::Receiver as Promise; -use futures::{future::{BoxFuture, Either, select}, channel::oneshot}; +use futures::{ + future::{BoxFuture, Either, select}, + channel::oneshot, +}; /// An executor that can be used to create nodes and run their callbacks. pub struct Executor { @@ -44,7 +47,6 @@ impl Executor { /// certain conditions are met. Use `SpinOptions::default()` to allow the /// Executor to keep spinning indefinitely. pub fn spin(&mut self, options: SpinOptions) { - self.commands.halt.store(false, Ordering::Release); let conditions = self.make_spin_conditions(options); self.runtime.spin(conditions); } @@ -59,7 +61,6 @@ impl 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 { - self.commands.halt.store(false, Ordering::Release); let conditions = self.make_spin_conditions(options); let Self { context, commands, runtime } = self; @@ -73,12 +74,16 @@ impl Executor { where E: 'static + ExecutorRuntime, { + let (guard_condition, waitable) = GuardCondition::new(&context); let commands = Arc::new(ExecutorCommands { context: Context { handle: Arc::clone(&context) }, channel: runtime.channel(), - halt: Arc::new(AtomicBool::new(false)), + halt_spinning: Arc::new(AtomicBool::new(false)), + guard_condition: Arc::new(guard_condition), }); + commands.add_to_wait_set(waitable); + Self { context, commands, @@ -87,10 +92,12 @@ impl Executor { } fn make_spin_conditions(&self, options: SpinOptions) -> SpinConditions { + self.commands.halt_spinning.store(false, Ordering::Release); SpinConditions { options, - halt: Arc::clone(&self.commands.halt), + halt_spinning: Arc::clone(&self.commands.halt_spinning), context: Context { handle: Arc::clone(&self.context) }, + guard_condition: Arc::clone(&self.commands.guard_condition), } } } @@ -100,7 +107,8 @@ impl Executor { pub struct ExecutorCommands { context: Context, channel: Box, - halt: Arc, + halt_spinning: Arc, + guard_condition: Arc, } impl ExecutorCommands { @@ -114,17 +122,25 @@ impl ExecutorCommands { } /// Tell the [`Executor`] to halt its spinning. - pub fn halt(&self) { - self.halt.store(true, Ordering::Release); - self.channel.get_guard_condition().trigger(); + pub fn halt_spinning(&self) { + self.halt_spinning.store(true, Ordering::Release); + // TODO(@mxgrey): Log errors here when logging becomes available + self.guard_condition.trigger().ok(); } /// Run a task on the [`Executor`]. If the returned [`Promise`] is dropped /// then the task will be dropped, which means it might not run to /// completion. /// - /// You can `.await` the output of the promise in an async scope. - pub fn run(&self, f: F) -> Promise + /// 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, @@ -149,8 +165,15 @@ impl ExecutorCommands { /// Run a task on the [`Executor`]. The task will run to completion even if /// you drop the returned [`Promise`]. /// - /// You can `.await` the output of the promise in an async scope. - pub fn run_detached(&self, f: F) -> 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 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 run(&self, f: F) -> Promise where F: 'static + Future + Send, F::Output: Send, @@ -164,20 +187,18 @@ impl ExecutorCommands { receiver } + /// Get the context that the executor is associated with. pub fn context(&self) -> &Context { &self.context } - pub(crate) fn add_async_task(&self, f: BoxFuture<'static, ()>) { - self.channel.add_async_task(f); - } - - pub(crate) fn add_to_wait_set(&self, waiter: Waitable) { - self.channel.add_to_waitset(waiter); + pub(crate) fn add_to_wait_set(&self, waitable: Waitable) { + self.channel.add_to_waitset(waitable); } + /// 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.channel.get_guard_condition() + &self.guard_condition } } @@ -189,9 +210,6 @@ pub trait ExecutorChannel: Send + Sync { /// Add new entities to the waitset of the executor. fn add_to_waitset(&self, new_entity: Waitable); - - /// Get a guard condition that can be used to wake up the wait set of the executor. - fn get_guard_condition(&self) -> &Arc; } /// This trait defines the interface for having an executor run. @@ -226,8 +244,11 @@ pub struct SpinOptions { /// To only process work that is immediately available without waiting at all, /// set a timeout of zero. pub only_next_available_work: bool, - /// The executor will stop spinning if the future is resolved. - pub until_future_resolved: Option>, + /// 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, @@ -242,9 +263,9 @@ impl SpinOptions { } } - /// Stop spinning once this future is resolved. - pub fn until_future_resolved(mut self, f: F) -> Self { - self.until_future_resolved = Some(Box::pin(async { f.await; })); + /// Stop spinning once this promise is resolved. + pub fn until_promise_resolved(mut self, promise: Promise<()>) -> Self { + self.until_promise_resolved = Some(promise); self } @@ -259,95 +280,19 @@ impl SpinOptions { /// spinning. This combines conditions that users specify with [`SpinOptions`] /// and standard conditions that are set by the [`Executor`]. /// -/// This struct is for users who are implementing custom executors. Users who -/// are writing applications should use [`SpinOptions`]. +/// 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: Arc, + 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, -} - -/// Single-threaded executor implementation. -pub struct SingleThreadedExecutor { - nodes_mtx: Mutex>>, -} - -impl Default for SingleThreadedExecutor { - fn default() -> Self { - Self::new() - } -} - -impl SingleThreadedExecutor { - /// Creates a new executor. - pub fn new() -> Self { - SingleThreadedExecutor { - nodes_mtx: Mutex::new(Vec::new()), - } - } - - /// 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(()) - } - - /// 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(()) - } - - /// Polls the nodes for new messages and executes the corresponding callbacks. - /// - /// This function additionally checks that the context is still valid. - pub fn spin_once(&self, timeout: Option) -> Result<(), RclrsError> { - for node in { self.nodes_mtx.lock().unwrap() } - .iter() - .filter_map(Weak::upgrade) - .filter(|node| unsafe { - rcl_context_is_valid(&*node.handle.context_handle.rcl_context.lock().unwrap()) - }) - { - let wait_set = WaitSet::new_for_node(&node)?; - let ready_entities = wait_set.wait(timeout)?; - - for ready_subscription in ready_entities.subscriptions { - ready_subscription.execute(); - } - - for ready_client in ready_entities.clients { - ready_client.execute()?; - } - - for ready_service in ready_entities.services { - ready_service.execute()?; - } - } - - Ok(()) - } - - /// 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, - } - } - - Ok(()) - } + /// This is a guard condition which is present in the wait set. The executor + /// can use this to wake up the wait set. + pub guard_condition: Arc, } diff --git a/rclrs/src/executor/basic_executor.rs b/rclrs/src/executor/basic_executor.rs index 1c733c791..5121cda6e 100644 --- a/rclrs/src/executor/basic_executor.rs +++ b/rclrs/src/executor/basic_executor.rs @@ -1,36 +1,135 @@ -use crate::executor::{ExecutorRuntime, ExecutorChannel, SpinConditions}; -use futures::future::BoxFuture; +use futures::{ + future::{BoxFuture, FutureExt}, + task::{waker_ref, ArcWake}, +}; +use std::{ + future::Future, + sync::mpsc::{channel, Receiver, Sender}, + sync::{Arc, Mutex}, + task::Context as TaskContext, + time::Duration, +}; -#[derive(Default)] -pub struct BasicExecutorRuntime { +use crate::{ + executor::{ExecutorRuntime, ExecutorChannel, SpinConditions}, + WaitSet, Waitable, Context, WaitSetRunner, +}; +/// 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_runner: WaitSetRunner, } impl ExecutorRuntime for BasicExecutorRuntime { - fn spin(&mut self, conditions: SpinConditions) { + fn spin(&mut self, mut conditions: SpinConditions) { + self.process_spin_conditions(&mut conditions); + } fn spin_async( self: Box, - conditions: SpinConditions, + mut conditions: SpinConditions, ) -> BoxFuture<'static, Box> { + self.process_spin_conditions(&mut conditions); + Box::pin(async move { self as Box }) } fn channel(&self) -> Box { - Box::new(BasicExecutorChannel { }) + Box::new(BasicExecutorChannel { + task_sender: self.task_sender.clone(), + }) } } -pub struct BasicExecutorChannel { +impl BasicExecutorRuntime { + pub(crate) fn new(context: &Context) -> Self { + let (task_sender, ready_queue) = channel(); + + Self { + ready_queue, + task_sender: TaskSender { task_sender }, + wait_set_runner: WaitSetRunner::new(context), + } + } + + fn process_spin_conditions(&self, conditions: &mut SpinConditions) { + if let Some(promise) = conditions.options.until_promise_resolved.take() { + let guard_condition = Arc::clone(&conditions.guard_condition); + self.task_sender.add_async_task(Box::pin(async move { + if let Err(err) = promise.await { + // TODO(@mxgrey): We should change this to a log when logging + // becomes available. + eprintln!( + "Sender for SpinOptions::until_promise_resolved was \ + dropped, so the Promise will never be fulfilled. \ + Spinning will stop now. Error message: {err}" + ); + } + // TODO(@mxgrey): Log errors here when logging becomes available. + guard_condition.trigger().ok(); + })); + } + } +} +#[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(); + } + +} + +pub struct BasicExecutorChannel { + task_sender: TaskSender, } impl ExecutorChannel for BasicExecutorChannel { - fn add_async_task(&self, f: futures::future::BoxFuture<'static, ()>) { + fn add_async_task(&self, f: BoxFuture<'static, ()>) { + self.task_sender.add_async_task(f); + } + + fn add_to_waitset(&self, new_entity: Waitable) { } +} +struct Task { + 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); + } } diff --git a/rclrs/src/executor/wait_set_runner.rs b/rclrs/src/executor/wait_set_runner.rs new file mode 100644 index 000000000..002475aa4 --- /dev/null +++ b/rclrs/src/executor/wait_set_runner.rs @@ -0,0 +1,114 @@ +use futures::channel::{ + oneshot::channel, + mpsc::{UnboundedSender, UnboundedReceiver, unbounded}, +}; + +use std::sync::atomic::Ordering; + +use crate::{ + WaitSet, Context, SpinConditions, Promise, Waitable, +}; + +/// This is a utility class that executors can use to easily run and manage +/// their wait set. +pub struct WaitSetRunner { + wait_set: WaitSet, + waitable_sender: UnboundedSender, + waitable_receiver: UnboundedReceiver, +} + +impl WaitSetRunner { + /// Create a new WaitSetRunner. + pub fn new(context: &Context) -> Self { + let (waitable_sender, waitable_receiver) = unbounded(); + Self { + wait_set: WaitSet::new(context) + // SAFETY: This only gets called from Context which ensures that + // everything is valid when creating a wait set. + .expect("Unable to create wait set for basic executor"), + waitable_sender, + waitable_receiver, + } + } + + /// Get the sender that allows users to send new [`Waitables`] to this + /// `WaitSetRunner`. + pub fn sender(&self) -> UnboundedSender { + self.waitable_sender.clone() + } + + /// Spawn a thread to run the wait set. You will receive a Promise that will + /// be resolved once the wait set stops spinning. + /// + /// Note that if the user gives a [`SpinOptions::until_promise_resolved`], + /// the best practice is for your executor runtime to swap that out with a + /// new promise which ensures that the [`SpinConditions::guard_condition`] + /// will be triggered after the user-provided promise is resolved. + pub fn run(mut self, conditions: SpinConditions) -> Promise { + let (sender, promise) = channel(); + std::thread::spawn(move || { + self.run_blocking(conditions); + // TODO(@mxgrey): Log any error here when logging becomes available + sender.send(self).ok(); + }); + + promise + } + + /// Run the wait set on the current thread. This will block the execution of + /// the current thread until the wait set is finished waiting. + /// + /// Note that if the user gives a [`SpinOptions::until_promise_resolved`], + /// the best practice is for your executor runtime to swap that out with a + /// new promise which ensures that the [`SpinConditions::guard_condition`] + /// will be triggered after the user-provided promise is resolved. + pub fn run_blocking(&mut self, mut conditions: SpinConditions) { + let mut first_spin = true; + loop { + // TODO(@mxgrey): SmallVec would be better suited here if we are + // okay with adding that as a dependency. + let mut new_waitables = Vec::new(); + while let Ok(Some(new_waitable)) = self.waitable_receiver.try_next() { + new_waitables.push(new_waitable); + } + if !new_waitables.is_empty() { + // TODO(@mxgrey): Log any error here when logging becomes available + self.wait_set.add(new_waitables).ok(); + } + + if conditions.options.only_next_available_work && !first_spin { + // We've already completed a spin and were asked to only do one, + // so break here + break; + } + first_spin = false; + + if let Some(promise) = &mut conditions.options.until_promise_resolved { + let r = promise.try_recv(); + if r.is_ok_and(|r| r.is_some()) || r.is_err() { + // The promise has been resolved, so we should stop spinning. + break; + } + } + + if conditions.halt_spinning.load(Ordering::Acquire) { + // The user has manually asked for the spinning to stop + break; + } + + if !conditions.context.ok() { + // The ROS context has switched to being invalid, so we should + // stop spinning. + break; + } + + if let Err(err) = self.wait_set.wait( + conditions.options.timeout, + |executable| executable.execute(), + ) { + // TODO(@mxgrey): Change this to a log when logging becomes available + eprintln!("Error while processing wait set: {err}"); + } + } + } +} diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 641836cc2..8d8832cfb 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -48,26 +48,3 @@ 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() -} diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index 2c306ce76..b32f1306f 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -115,7 +115,7 @@ where Arc::clone(commands.get_guard_condition()), )?; - commands.run_detached(service_task( + commands.run(service_task( callback, receiver, Arc::clone(&service.handle), diff --git a/rclrs/src/service/any_service_callback.rs b/rclrs/src/service/any_service_callback.rs index f220cedf9..411f2b271 100644 --- a/rclrs/src/service/any_service_callback.rs +++ b/rclrs/src/service/any_service_callback.rs @@ -39,7 +39,7 @@ impl AnyServiceCallback { let (msg, mut rmw_request_id) = Self::take_request(handle)?; let handle = Arc::clone(&handle); let response = cb(msg); - commands.run_detached(async move { + commands.run(async move { // TODO(@mxgrey): Log any errors here when logging is available Self::send_response(&handle, &mut rmw_request_id, response.await).ok(); }); @@ -49,7 +49,7 @@ impl AnyServiceCallback { let request_id = RequestId::from_rmw_request_id(&rmw_request_id); let handle = Arc::clone(&handle); let response = cb(msg, request_id); - commands.run_detached(async move { + commands.run(async move { // TODO(@mxgrey): Log any errors here when logging is available Self::send_response(&handle, &mut rmw_request_id, response.await).ok(); }); @@ -63,7 +63,7 @@ impl AnyServiceCallback { let service_info = ServiceInfo::from_rmw_service_info(&rmw_service_info); let handle = Arc::clone(&handle); let response = cb(msg, service_info); - commands.run_detached(async move { + commands.run(async move { // TODO(@mxgrey): Log any errors here when logging is available Self::send_response(&handle, &mut rmw_request_id, response.await).ok(); }); diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index d7a284191..2b61ae603 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -125,7 +125,7 @@ where Arc::clone(commands.get_guard_condition()), )?; - commands.run_detached(subscription_task( + commands.run(subscription_task( callback, receiver, Arc::clone(&subscription.handle), diff --git a/rclrs/src/subscription/any_subscription_callback.rs b/rclrs/src/subscription/any_subscription_callback.rs index 8a048bb10..437070912 100644 --- a/rclrs/src/subscription/any_subscription_callback.rs +++ b/rclrs/src/subscription/any_subscription_callback.rs @@ -50,27 +50,27 @@ impl AnySubscriptionCallback { match self { AnySubscriptionCallback::Regular(cb) => { let (msg, _) = Self::take(handle)?; - commands.run_detached(cb(msg)); + commands.run(cb(msg)); } AnySubscriptionCallback::RegularWithMessageInfo(cb) => { let (msg, msg_info) = Self::take(handle)?; - commands.run_detached(cb(msg, msg_info)); + commands.run(cb(msg, msg_info)); } AnySubscriptionCallback::Boxed(cb) => { let (msg, _) = Self::take_boxed(handle)?; - commands.run_detached(cb(msg)); + commands.run(cb(msg)); } AnySubscriptionCallback::BoxedWithMessageInfo(cb) => { let (msg, msg_info) = Self::take_boxed(handle)?; - commands.run_detached(cb(msg, msg_info)); + commands.run(cb(msg, msg_info)); } AnySubscriptionCallback::Loaned(cb) => { let (msg, _) = Self::take_loaned(handle)?; - commands.run_detached(cb(msg)); + commands.run(cb(msg)); } AnySubscriptionCallback::LoanedWithMessageInfo(cb) => { let (msg, msg_info) = Self::take_loaned(handle)?; - commands.run_detached(cb(msg, msg_info)); + commands.run(cb(msg, msg_info)); } } Ok(()) diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index ebac1a422..c64c6606f 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -249,8 +249,7 @@ mod tests { fn guard_condition_in_wait_set_readies() -> Result<(), RclrsError> { let mut executor = Context::new([])?.create_basic_executor(); - let guard_condition = GuardCondition::new(&executor.commands()); - guard_condition.trigger()?; + executor.commands().get_guard_condition().trigger(); let start = std::time::Instant::now(); // This should stop spinning right away because the guard condition was diff --git a/rclrs/src/wait/guard_condition.rs b/rclrs/src/wait/guard_condition.rs index 2b612d849..ba027c29d 100644 --- a/rclrs/src/wait/guard_condition.rs +++ b/rclrs/src/wait/guard_condition.rs @@ -3,7 +3,7 @@ use std::sync::{Arc, Mutex}; use crate::{ rcl_bindings::*, ContextHandle, RclrsError, ToResult, WaitableLifecycle, Executable, - Waitable, ExecutableKind, ExecutableHandle, ExecutorCommands, + Waitable, ExecutableKind, ExecutableHandle, }; /// A waitable entity used for waking up a wait set manually. @@ -29,13 +29,23 @@ pub struct GuardCondition { unsafe impl Send for rcl_guard_condition_t {} impl GuardCondition { - /// Creates a new guard condition with no callback. - pub(crate) fn new(commands: &ExecutorCommands) -> Self { - let context = commands.context(); + /// Triggers this guard condition, activating the wait set, and calling the optionally assigned callback. + pub fn trigger(&self) -> Result<(), RclrsError> { + unsafe { + // SAFETY: The rcl_guard_condition_t is valid. + rcl_trigger_guard_condition(&mut *self.handle.rcl_guard_condition.lock().unwrap()) + .ok()?; + } + Ok(()) + } + + /// Creates a new guard condition. This is only for internal use. Ordinary + /// users of rclrs do not need to access guard conditions. + pub(crate) fn new(context: &Arc) -> (Self, Waitable) { let rcl_guard_condition = { // SAFETY: Getting a zero initialized value is always safe let mut guard_condition = unsafe { rcl_get_zero_initialized_guard_condition() }; - let mut rcl_context = context.handle.rcl_context.lock().unwrap(); + let mut rcl_context = context.rcl_context.lock().unwrap(); unsafe { // SAFETY: The context must be valid, and the guard condition must be zero-initialized rcl_guard_condition_init( @@ -50,27 +60,15 @@ impl GuardCondition { let handle = Arc::new(GuardConditionHandle { rcl_guard_condition, - context_handle: Arc::clone(&context.handle), + context_handle: Arc::clone(&context), }); - let (waiter, lifecycle) = Waitable::new( + let (waitable, lifecycle) = Waitable::new( Box::new(GuardConditionExecutable { handle: Arc::clone(&handle) }), None, ); - commands.add_to_wait_set(waiter); - - Self { handle, lifecycle } - } - - /// Triggers this guard condition, activating the wait set, and calling the optionally assigned callback. - pub fn trigger(&self) -> Result<(), RclrsError> { - unsafe { - // SAFETY: The rcl_guard_condition_t is valid. - rcl_trigger_guard_condition(&mut *self.handle.rcl_guard_condition.lock().unwrap()) - .ok()?; - } - Ok(()) + (Self { handle, lifecycle }, waitable) } } From 06cb0ab52e7bb29a47e2626fef21d9a10b084280 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 13 Oct 2024 17:15:51 +0800 Subject: [PATCH 12/48] Finished implementing basic executor -- needs testing Signed-off-by: Michael X. Grey --- rclrs/src/context.rs | 2 +- rclrs/src/executor.rs | 4 +- rclrs/src/executor/basic_executor.rs | 161 +++++++++++++++++++++++---- rclrs/src/parameter/service.rs | 8 +- 4 files changed, 144 insertions(+), 31 deletions(-) diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index eeda8f725..1b4d4f5d3 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -173,7 +173,7 @@ impl Context { /// Create an [`Executor`] for this context. pub fn create_executor(&self, runtime: E) -> Executor where - E: 'static + ExecutorRuntime, + E: 'static + ExecutorRuntime + Send, { Executor::new(Arc::clone(&self.handle), runtime) } diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index fbe1def44..c1a84398d 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -72,7 +72,7 @@ impl Executor { /// use [`Context::create_executor`]. pub(crate) fn new(context: Arc, runtime: E) -> Self where - E: 'static + ExecutorRuntime, + E: 'static + ExecutorRuntime + Send, { let (guard_condition, waitable) = GuardCondition::new(&context); let commands = Arc::new(ExecutorCommands { @@ -213,7 +213,7 @@ pub trait ExecutorChannel: Send + Sync { } /// This trait defines the interface for having an executor run. -pub trait ExecutorRuntime { +pub trait ExecutorRuntime: Send { /// Get a channel that can add new items for the executor to run. fn channel(&self) -> Box; diff --git a/rclrs/src/executor/basic_executor.rs b/rclrs/src/executor/basic_executor.rs index 5121cda6e..545d28b06 100644 --- a/rclrs/src/executor/basic_executor.rs +++ b/rclrs/src/executor/basic_executor.rs @@ -1,11 +1,15 @@ use futures::{ future::{BoxFuture, FutureExt}, task::{waker_ref, ArcWake}, + channel::{oneshot, mpsc::UnboundedSender}, }; use std::{ future::Future, - sync::mpsc::{channel, Receiver, Sender}, - sync::{Arc, Mutex}, + sync::{ + mpsc::{Sender, Receiver, channel}, + atomic::{AtomicBool, Ordering}, + Arc, Mutex, + }, task::Context as TaskContext, time::Duration, }; @@ -31,28 +35,115 @@ use crate::{ pub struct BasicExecutorRuntime { ready_queue: Receiver>, task_sender: TaskSender, - wait_set_runner: WaitSetRunner, + /// We use an Option here because we need to hand the WaitSetRunner off to + /// another thread while spinning. It should only be None while the spin + /// function is active. At any other time this should contain Some. + wait_set_runner: Option, } impl ExecutorRuntime for BasicExecutorRuntime { fn spin(&mut self, mut conditions: SpinConditions) { self.process_spin_conditions(&mut conditions); + let wait_set_runner = self.wait_set_runner.take().expect( + "The wait set runner of the basic executor is missing while beginning to spin. \ + This is a critical bug in rclrs. \ + Please report this bug to the maintainers of rclrs by providing a minimum reproduction of the problem." + ); + + let wait_set_promise = wait_set_runner.run(conditions); + // futures::channel::oneshot::Receiver is only suitable for async, but + // we need to block this function from exiting until the WaitSetRunner + // is returned to self. Therefore we create this blocking channel to + // prevent the function from returning until the WaitSetRunner has been + // re-obtained. + let (wait_set_sender, wait_set_receiver) = channel(); + + // Use this atomic bool to recognize when we should stop spinning. + let wait_set_finished = Arc::new(AtomicBool::new(false)); + + // Use this to terminate the spinning once the wait set is finished. + let wait_set_finished_clone = Arc::clone(&wait_set_finished); + self.task_sender.add_async_task(Box::pin(async move { + let wait_set_runner = wait_set_promise.await.expect( + "The wait set thread of the basic executor dropped prematurely. \ + This is a critical bug in rclrs. \ + Please report this bug to the maintainers of rclrs by providing a minimum reproduction of the problem." + ); + // TODO(@mxgrey): Log errors here when logging becomes available. + wait_set_sender.send(wait_set_runner).ok(); + + // Notify the main loop that it should stop + wait_set_finished_clone.store(true, Ordering::Release); + })); + + while let Ok(task) = self.next_task(&wait_set_finished) { + // SAFETY: If the mutex is poisoned then we have unrecoverable situation. + let mut future_slot = task.future.lock().unwrap(); + if let Some(mut future) = future_slot.take() { + let waker = waker_ref(&task); + let task_context = &mut TaskContext::from_waker(&waker); + // Poll the future inside the task so it can do some work and + // tell us its state. + if future.as_mut().poll(task_context).is_pending() { + // The task is still pending, so return the future to its + // task so it can be processed again when it's ready to + // continue. + *future_slot = Some(future); + } + } + + } + self.wait_set_runner = Some( + wait_set_receiver.recv().expect( + "Basic executor failed to receive the WaitSetRunner at the end of its spinning. \ + This is a critical bug in rclrs. \ + Please report this bug to the maintainers of rclrs by providing a minimum reproduction of the problem." + ) + ); } fn spin_async( - self: Box, - mut conditions: SpinConditions, + mut self: Box, + conditions: SpinConditions, ) -> BoxFuture<'static, Box> { - self.process_spin_conditions(&mut conditions); + 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 || { + self.spin(conditions); + sender.send(self as Box).ok(); + }); - Box::pin(async move { self as Box }) + Box::pin(async move { + receiver.await.expect( + "The basic executor async spin thread was dropped without finishing. \ + This is a critical bug in rclrs. \ + Please report this bug to the maintainers of rclrs by providing a minimum reproduction of the problem." + ) + }) } fn channel(&self) -> Box { + let waitable_sender = self.wait_set_runner.as_ref().expect( + "The wait set runner of the basic executor is missing while creating a channel. \ + This is a critical bug in rclrs. \ + Please report this bug to the maintainers of rclrs by providing a minimum reproduction of the problem." + ) + .sender(); + Box::new(BasicExecutorChannel { task_sender: self.task_sender.clone(), + waitable_sender, }) } } @@ -60,11 +151,10 @@ impl ExecutorRuntime for BasicExecutorRuntime { impl BasicExecutorRuntime { pub(crate) fn new(context: &Context) -> Self { let (task_sender, ready_queue) = channel(); - Self { ready_queue, task_sender: TaskSender { task_sender }, - wait_set_runner: WaitSetRunner::new(context), + wait_set_runner: Some(WaitSetRunner::new(context)), } } @@ -86,6 +176,32 @@ impl BasicExecutorRuntime { })); } } + + 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(|_| ()) + } + } +} + +pub struct BasicExecutorChannel { + task_sender: TaskSender, + waitable_sender: UnboundedSender, +} + +impl ExecutorChannel for BasicExecutorChannel { + fn add_async_task(&self, f: BoxFuture<'static, ()>) { + self.task_sender.add_async_task(f); + } + + fn add_to_waitset(&self, new_entity: Waitable) { + // TODO(@mxgrey): Log errors here once logging becomes available. + self.waitable_sender.unbounded_send(new_entity).ok(); + } } #[derive(Clone)] @@ -106,21 +222,14 @@ impl TaskSender { } -pub struct BasicExecutorChannel { - task_sender: TaskSender, -} - -impl ExecutorChannel for BasicExecutorChannel { - fn add_async_task(&self, f: BoxFuture<'static, ()>) { - self.task_sender.add_async_task(f); - } - - fn add_to_waitset(&self, new_entity: Waitable) { - - } -} - 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>, } @@ -130,6 +239,10 @@ struct Task { impl ArcWake for Task { fn wake_by_ref(arc_self: &Arc) { let cloned = Arc::clone(arc_self); - arc_self.task_sender.send(cloned); + if let Err(err) = arc_self.task_sender.send(cloned) { + eprintln!( + "Unable to resume a task because sending it to the basic executor failed: {err}" + ); + } } } diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index b0cb0404b..be1ed298c 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -437,8 +437,8 @@ mod tests { #[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::( + let (test_node, client_node) = construct_test_nodes(&context, "list"); + let list_client = client_node.create_client::( "/list/node/list_parameters", QoSProfile::services_default(), )?; @@ -452,8 +452,8 @@ mod tests { 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(); + crate::spin_once(test_node.node.clone(), Some(std::time::Duration::ZERO)).ok(); + crate::spin_once(client_node.clone(), Some(std::time::Duration::ZERO)).ok(); *inner_done.read().unwrap() }) .await From 40d874608de3c75ceadc82f85e5b1a319e6f448f Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 13 Oct 2024 21:20:58 +0800 Subject: [PATCH 13/48] Add support for waiting on graph events Signed-off-by: Michael X. Grey --- rclrs/Cargo.toml | 8 +- rclrs/src/client.rs | 60 +++++++++++---- rclrs/src/error.rs | 12 +++ rclrs/src/executor.rs | 18 +++-- rclrs/src/node.rs | 108 +++++++++++++++++++++++--- rclrs/src/node/node_graph_task.rs | 42 ++++++++++ rclrs/src/node/options.rs | 40 +++++++++- rclrs/src/parameter/service.rs | 2 +- rclrs/src/wait/guard_condition.rs | 122 ++++++++++++++++++++++++++---- rclrs/src/wait/waitable.rs | 8 +- 10 files changed, 362 insertions(+), 58 deletions(-) create mode 100644 rclrs/src/node/node_graph_task.rs diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index 2807e6fc6..f27f53d5e 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -17,15 +17,15 @@ path = "src/lib.rs" # Needed for dynamically finding type support libraries ament_rs = { version = "0.2", optional = true } -# Used to ensure uniqueness across entity handles -by_address = "1.2.1" - # Needed for uploading documentation to docs.rs cfg-if = "1.0.0" # Needed for clients futures = "0.3" +# We import only the time module, which is needed to support timeouts in async blocks +tokio = { version = "1", features = ["time"] } + # Needed for dynamic messages libloading = { version = "0.8", optional = true } @@ -38,7 +38,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 f61ec1acb..c48fa84f4 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -11,7 +11,7 @@ use rosidl_runtime_rs::Message; use crate::{ error::ToResult, rcl_bindings::*, - MessageCow, NodeHandle, RclrsError, Promise, ENTITY_LIFECYCLE_MUTEX, + MessageCow, Node, RclrsError, Promise, ENTITY_LIFECYCLE_MUTEX, ExecutorCommands, Executable, QoSProfile, Waitable, WaitableLifecycle, ExecutableHandle, ExecutableKind, ServiceInfo, }; @@ -41,8 +41,8 @@ where { handle: Arc, action: UnboundedSender>, + #[allow(unused)] lifecycle: WaitableLifecycle, - commands: Arc, } impl Client @@ -58,6 +58,10 @@ where /// - `(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>( @@ -82,21 +86,27 @@ where } .ok()?; + // TODO(@mxgrey): Log errors here when logging becomes available. self.action.unbounded_send( ClientAction::NewRequest { sequence_number, sender } - ); + ).ok(); 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> + ) -> Result, RclrsError> where Req: MessageCow<'a, T::Request>, { @@ -107,18 +117,23 @@ where } /// 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> + ) -> Result, RclrsError> where Req: MessageCow<'a, T::Request>, { let response: Promise<(T::Response, ServiceInfo)> = self.call(request)?; - self.commands.run(async move { + let promise = self.handle.node.commands().run(async move { match response.await { Ok((response, info)) => { callback.run_client_async_callback(response, info).await; @@ -129,7 +144,7 @@ where } }); - Ok(()) + Ok(promise) } /// Check if a service server is available. @@ -140,7 +155,7 @@ where 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(); + let node = &mut *self.handle.node.handle().rcl_node.lock().unwrap(); unsafe { // SAFETY both node and client are guaranteed to be valid here @@ -151,12 +166,22 @@ where 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 create( topic: &str, qos: QoSProfile, - node_handle: &Arc, - commands: &Arc, + node: &Arc, ) -> Result, RclrsError> // This uses pub(crate) visibility to avoid instantiating this struct outside // [`Node::create_client`], see the struct's documentation for the rationale @@ -177,7 +202,7 @@ where client_options.qos = qos.into(); { - let rcl_node = node_handle.rcl_node.lock().unwrap(); + let rcl_node = node.handle().rcl_node.lock().unwrap(); let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); // SAFETY: @@ -201,11 +226,13 @@ where let handle = Arc::new(ClientHandle { rcl_client: Mutex::new(rcl_client), - node_handle: Arc::clone(&node_handle), + node: Arc::clone(&node), }); + let commands = node.commands(); + let (action, receiver) = unbounded(); - commands.run(client_task(receiver, Arc::clone(&handle))); + let _ = commands.run(client_task(receiver, Arc::clone(&handle))); let (waitable, lifecycle) = Waitable::new( Box::new(ClientExecutable { @@ -220,7 +247,6 @@ where handle, action, lifecycle, - commands: Arc::clone(&commands), })) } } @@ -255,7 +281,9 @@ where /// [1]: struct ClientHandle { rcl_client: Mutex, - node_handle: Arc, + /// We store the whole node here because we use some of its user-facing API + /// in some of the Client methods. + node: Arc, } impl ClientHandle { @@ -267,7 +295,7 @@ impl ClientHandle { 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 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. diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index 3eba2549f..6d6684ad8 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -32,6 +32,9 @@ pub enum RclrsError { }, /// It was attempted to add a waitable to a wait set twice. AlreadyAddedToWaitSet, + /// The guard condition that you tried to trigger is not owned by the + /// [`GuardCondition`][crate::GuardCondition] instance. + UnownedGuardCondition, } impl Display for RclrsError { @@ -48,6 +51,12 @@ impl Display for RclrsError { "Could not add entity to wait set because it was already added to a wait set" ) } + RclrsError::UnownedGuardCondition => { + write!( + f, + "Could not trigger guard condition because it is not owned by rclrs" + ) + } } } } @@ -79,7 +88,10 @@ impl Error for RclrsError { RclrsError::RclError { msg, .. } => msg.as_ref().map(|e| e as &dyn Error), RclrsError::UnknownRclError { msg, .. } => msg.as_ref().map(|e| e as &dyn Error), RclrsError::StringContainsNul { err, .. } => Some(err).map(|e| e as &dyn Error), + // TODO(@mxgrey): We should provide source information for these other types. + // It should be easy to do this using the thiserror crate. RclrsError::AlreadyAddedToWaitSet => None, + RclrsError::UnownedGuardCondition => None, } } } diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index c1a84398d..48ef73679 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -74,12 +74,12 @@ impl Executor { where E: 'static + ExecutorRuntime + Send, { - let (guard_condition, waitable) = GuardCondition::new(&context); + let (wakeup_wait_set, waitable) = GuardCondition::new(&context, None); let commands = Arc::new(ExecutorCommands { context: Context { handle: Arc::clone(&context) }, channel: runtime.channel(), halt_spinning: Arc::new(AtomicBool::new(false)), - guard_condition: Arc::new(guard_condition), + wakeup_wait_set: Arc::new(wakeup_wait_set), }); commands.add_to_wait_set(waitable); @@ -97,7 +97,7 @@ impl Executor { options, halt_spinning: Arc::clone(&self.commands.halt_spinning), context: Context { handle: Arc::clone(&self.context) }, - guard_condition: Arc::clone(&self.commands.guard_condition), + guard_condition: Arc::clone(&self.commands.wakeup_wait_set), } } } @@ -108,7 +108,7 @@ pub struct ExecutorCommands { context: Context, channel: Box, halt_spinning: Arc, - guard_condition: Arc, + wakeup_wait_set: Arc, } impl ExecutorCommands { @@ -125,7 +125,7 @@ impl ExecutorCommands { pub fn halt_spinning(&self) { self.halt_spinning.store(true, Ordering::Release); // TODO(@mxgrey): Log errors here when logging becomes available - self.guard_condition.trigger().ok(); + self.wakeup_wait_set.trigger().ok(); } /// Run a task on the [`Executor`]. If the returned [`Promise`] is dropped @@ -170,7 +170,11 @@ impl ExecutorCommands { /// If you want to ensure that the task always runs to completion, then this /// `run` method is what you want. /// - /// You have two ways to obtain the output of the promise: + /// 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 @@ -198,7 +202,7 @@ impl ExecutorCommands { /// 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.guard_condition + &self.wakeup_wait_set } } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 37ec6a299..7703f7de1 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,27 +1,38 @@ mod options; +pub use 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::{Arc, Mutex}, + time::Duration, +}; + +use futures::{ + StreamExt, + channel::{oneshot, mpsc::{unbounded, UnboundedSender}}, }; +use tokio::time::timeout; + use rosidl_runtime_rs::Message; -pub use self::{options::*, graph::*}; use crate::{ - rcl_bindings::*, Client, Clock, ContextHandle, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, - RclrsError, Service, Subscription, SubscriptionCallback, SubscriptionAsyncCallback, - ServiceCallback, ServiceAsyncCallback, ExecutorCommands, TimeSource, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, + Client, Clock, ContextHandle, Promise, ParameterBuilder, ParameterInterface, + ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, + Subscription, SubscriptionCallback, SubscriptionAsyncCallback, ServiceCallback, + ServiceAsyncCallback, ExecutorCommands, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; -// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread -// they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for rcl_node_t {} - /// A processing unit that can communicate with other nodes. /// /// Nodes are a core concept in ROS 2. Refer to the official ["Understanding ROS 2 nodes"][1] @@ -61,7 +72,8 @@ pub struct Node { time_source: TimeSource, parameter: ParameterInterface, commands: Arc, - pub(crate) handle: Arc, + graph_change_action: UnboundedSender, + handle: Arc, } /// This struct manages the lifetime of an `rcl_node_t`, and accounts for its @@ -181,14 +193,14 @@ impl Node { /// [1]: crate::Client // TODO: make client's lifetime depend on node's lifetime pub fn create_client( - &self, + self: &Arc, topic: &str, qos: QoSProfile, ) -> Result>, RclrsError> where T: rosidl_runtime_rs::Service, { - Client::::create(topic, qos, &self.handle, &self.commands) + Client::::create(topic, qos, &self) } /// Creates a [`Publisher`][1]. @@ -407,6 +419,70 @@ impl Node { } } + /// 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(condition, Duration::from_millis(100)) + } + + /// This function allows you to track when a specific graph change happens. + /// + /// Provide a function that will be called each time a graph change occurs. + /// You will be given a [`Promise`] that will be fulfilled when the condition + /// returns true. The condition will be checked under these conditions: + /// - once immediately as this function is run + /// - each time rcl notifies us that a graph change has happened + /// - each time the period elapses + /// + /// We specify a period because it is possible that race conditions at the + /// rcl layer could trigger a notification of a graph change before your + /// API calls will be able to observe it. + /// + /// + pub fn notify_on_graph_change_with_period( + &self, + mut condition: impl FnMut() -> bool + Send + 'static, + period: Duration, + ) -> 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 + } + // Helper for name(), namespace(), fully_qualified_name() fn call_string_getter( &self, @@ -415,6 +491,10 @@ impl Node { 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 + } } // Helper used to implement call_string_getter(), but also used to get the FQN in the Node::new() @@ -434,6 +514,10 @@ 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::*; diff --git a/rclrs/src/node/node_graph_task.rs b/rclrs/src/node/node_graph_task.rs new file mode 100644 index 000000000..30a5d752f --- /dev/null +++ b/rclrs/src/node/node_graph_task.rs @@ -0,0 +1,42 @@ +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/options.rs b/rclrs/src/node/options.rs index 1defee488..a93eecb3e 100644 --- a/rclrs/src/node/options.rs +++ b/rclrs/src/node/options.rs @@ -3,9 +3,12 @@ use std::{ sync::{Arc, Mutex}, }; +use futures::channel::mpsc::unbounded; + use crate::{ - rcl_bindings::*, ClockType, Node, NodeHandle, ParameterInterface, - ExecutorCommands, + node::node_graph_task::{node_graph_task, NodeGraphAction}, + rcl_bindings::*, + ClockType, Node, NodeHandle, ParameterInterface, GuardCondition, ExecutorCommands, QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, }; @@ -310,18 +313,51 @@ impl NodeOptions { &rcl_context.global_arguments, )? }; + + // --- 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(Node { handle, time_source: TimeSource::builder(self.clock_type) .clock_qos(self.clock_qos) .build(), parameter, + graph_change_action, commands: Arc::clone(&commands), }); node.time_source.attach_node(&node); + if self.start_parameter_services { node.parameter.create_services(&node)?; } + Ok(node) } diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index be1ed298c..a07f9eb75 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -436,7 +436,7 @@ mod tests { #[tokio::test] async fn test_list_parameters_service() -> Result<(), RclrsError> { - let context = Context::new([]).unwrap(); + let context = Context::default(); let (test_node, client_node) = construct_test_nodes(&context, "list"); let list_client = client_node.create_client::( "/list/node/list_parameters", diff --git a/rclrs/src/wait/guard_condition.rs b/rclrs/src/wait/guard_condition.rs index ba027c29d..6c887c9ab 100644 --- a/rclrs/src/wait/guard_condition.rs +++ b/rclrs/src/wait/guard_condition.rs @@ -1,4 +1,7 @@ -use std::sync::{Arc, Mutex}; +use std::{ + any::Any, + sync::{Arc, Mutex} +}; use crate::{ rcl_bindings::*, @@ -22,26 +25,36 @@ pub struct GuardCondition { handle: Arc, /// This manages the lifecycle of this guard condition's waiter. Dropping /// this will remove the guard condition from its wait set. + #[allow(unused)] lifecycle: WaitableLifecycle, } // SAFETY: rcl_guard_condition is the only member that doesn't implement Send, and it is designed to be accessed from other threads unsafe impl Send for rcl_guard_condition_t {} +// SAFETY: We make sure internally to defend all invariants of the unowned +// pointer. +unsafe impl Send for InnerGuardConditionHandle {} impl GuardCondition { /// Triggers this guard condition, activating the wait set, and calling the optionally assigned callback. pub fn trigger(&self) -> Result<(), RclrsError> { - unsafe { - // SAFETY: The rcl_guard_condition_t is valid. - rcl_trigger_guard_condition(&mut *self.handle.rcl_guard_condition.lock().unwrap()) - .ok()?; + // SAFETY: The rcl_guard_condition_t is valid. + if let Some(handle) = self.handle.rcl_guard_condition.lock().unwrap().owned_mut() { + unsafe { + rcl_trigger_guard_condition(handle).ok()?; + } + } else { + return Err(RclrsError::UnownedGuardCondition); } Ok(()) } /// Creates a new guard condition. This is only for internal use. Ordinary /// users of rclrs do not need to access guard conditions. - pub(crate) fn new(context: &Arc) -> (Self, Waitable) { + pub(crate) fn new( + context: &Arc, + callback: Option>, + ) -> (Self, Waitable) { let rcl_guard_condition = { // SAFETY: Getting a zero initialized value is always safe let mut guard_condition = unsafe { rcl_get_zero_initialized_guard_condition() }; @@ -55,7 +68,7 @@ impl GuardCondition { ); } - Mutex::new(guard_condition) + Mutex::new(InnerGuardConditionHandle::Owned(guard_condition)) }; let handle = Arc::new(GuardConditionHandle { @@ -64,7 +77,40 @@ impl GuardCondition { }); let (waitable, lifecycle) = Waitable::new( - Box::new(GuardConditionExecutable { handle: Arc::clone(&handle) }), + Box::new(GuardConditionExecutable { + handle: Arc::clone(&handle), + callback, + }), + None, + ); + + (Self { handle, lifecycle }, waitable) + } + + /// SAFETY: The caller is responsible for ensuring that the pointer being + /// passed in is valid and non-null, and also that as long as `owner` is + /// held, the pointer will remain valid. + pub(crate) unsafe fn from_rcl( + context: &Arc, + rcl_guard_condition: *const rcl_guard_condition_t, + owner: Box, + callback: Option>, + ) -> (Self, Waitable) { + let rcl_guard_condition = Mutex::new(InnerGuardConditionHandle::Unowned { + handle: rcl_guard_condition, + owner, + }); + + let handle = Arc::new(GuardConditionHandle { + rcl_guard_condition, + context_handle: Arc::clone(&context), + }); + + let (waitable, lifecycle) = Waitable::new( + Box::new(GuardConditionExecutable { + handle: Arc::clone(&handle), + callback, + }), None, ); @@ -78,28 +124,78 @@ impl GuardCondition { /// /// [1]: struct GuardConditionHandle { - rcl_guard_condition: Mutex, + rcl_guard_condition: Mutex, /// Keep the context alive for the whole lifecycle of the guard condition #[allow(dead_code)] context_handle: Arc, } +/// We need to wrap the underlying condition variable with this because some +/// condition variables are owned at the rclrs layer while others were obtained +/// from rcl and either have static lifetimes or lifetimes that depend on +/// something else. +pub enum InnerGuardConditionHandle { + Owned(rcl_guard_condition_t), + Unowned { + handle: *const rcl_guard_condition_t, + owner: Box, + }, +} + +impl InnerGuardConditionHandle { + pub fn owned(&self) -> Option<&rcl_guard_condition_t> { + match self { + Self::Owned(handle) => Some(handle), + _ => None, + } + } + + pub fn owned_mut(&mut self) -> Option<&mut rcl_guard_condition_t> { + match self { + Self::Owned(handle) => Some(handle), + _ => None, + } + } + + pub fn use_handle(&self, f: impl FnOnce(&rcl_guard_condition_t) -> Out) -> Out { + match self { + Self::Owned(handle) => f(handle), + Self::Unowned { handle, .. } => f( + unsafe { + // SAFETY: The enum ensures that the pointer remains valid + handle.as_ref().unwrap() + } + ), + } + } +} + impl Drop for GuardConditionHandle { fn drop(&mut self) { - unsafe { + if let InnerGuardConditionHandle::Owned( + rcl_guard_condition + ) = &mut *self.rcl_guard_condition.lock().unwrap() { + unsafe { // SAFETY: No precondition for this function (besides passing in a valid guard condition) - rcl_guard_condition_fini(&mut *self.rcl_guard_condition.lock().unwrap()); + rcl_guard_condition_fini(rcl_guard_condition); + } } } } struct GuardConditionExecutable { handle: Arc, + /// The callback that will be triggered when execute is called. Not all + /// guard conditions need to have a callback associated, so this could be + /// [`None`]. + callback: Option>, } impl Executable for GuardConditionExecutable { fn execute(&mut self) -> Result<(), RclrsError> { - // Do nothing + if let Some(callback) = &mut self.callback { + callback(); + } Ok(()) } @@ -107,7 +203,7 @@ impl Executable for GuardConditionExecutable { ExecutableKind::GuardCondition } - fn handle(&self) -> super::ExecutableHandle { + fn handle(&self) -> ExecutableHandle { ExecutableHandle::GuardCondition( self.handle.rcl_guard_condition.lock().unwrap() ) diff --git a/rclrs/src/wait/waitable.rs b/rclrs/src/wait/waitable.rs index 3329315be..f7ba71219 100644 --- a/rclrs/src/wait/waitable.rs +++ b/rclrs/src/wait/waitable.rs @@ -6,7 +6,7 @@ use std::sync::{ use crate::{ error::ToResult, rcl_bindings::*, - RclrsError, GuardCondition, + RclrsError, GuardCondition, InnerGuardConditionHandle, }; /// Enum to describe the kind of an executable. @@ -23,7 +23,7 @@ pub enum ExecutableKind { /// Used by the wait set to obtain the handle of an executable. pub enum ExecutableHandle<'a> { Subscription(MutexGuard<'a, rcl_subscription_t>), - GuardCondition(MutexGuard<'a, rcl_guard_condition_t>), + GuardCondition(MutexGuard<'a, InnerGuardConditionHandle>), Timer(MutexGuard<'a, rcl_timer_t>), Client(MutexGuard<'a, rcl_client_t>), Service(MutexGuard<'a, rcl_service_t>), @@ -176,7 +176,9 @@ impl Waitable { rcl_wait_set_add_subscription(wait_set, &*handle, &mut index) } ExecutableHandle::GuardCondition(handle) => { - rcl_wait_set_add_guard_condition(wait_set, &*handle, &mut index) + handle.use_handle(|handle| { + rcl_wait_set_add_guard_condition(wait_set, &*handle, &mut index) + }) } ExecutableHandle::Service(handle) => { rcl_wait_set_add_service(wait_set, &*handle, &mut index) From 329cafbcd31d0171888f84a898731c5e99036a1d Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 13 Oct 2024 23:08:56 +0800 Subject: [PATCH 14/48] Migrate parameter service tests to new async executor Signed-off-by: Michael X. Grey --- rclrs/src/executor.rs | 5 + rclrs/src/node.rs | 4 +- rclrs/src/parameter/service.rs | 1064 ++++++++++++++++---------------- 3 files changed, 543 insertions(+), 530 deletions(-) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 48ef73679..62a8200b4 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -259,6 +259,11 @@ pub struct SpinOptions { } 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 { diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 7703f7de1..9ee44cfe2 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -425,7 +425,7 @@ impl Node { &self, condition: impl FnMut() -> bool + Send + 'static, ) -> Promise<()> { - self.notify_on_graph_change_with_period(condition, Duration::from_millis(100)) + self.notify_on_graph_change_with_period(Duration::from_millis(100), condition) } /// This function allows you to track when a specific graph change happens. @@ -444,8 +444,8 @@ impl Node { /// pub fn notify_on_graph_change_with_period( &self, - mut condition: impl FnMut() -> bool + Send + 'static, 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 { diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index a07f9eb75..e724a0b6b 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -319,10 +319,10 @@ mod tests { srv::rmw::*, }, Context, MandatoryParameter, Node, NodeOptions, ParameterRange, ParameterValue, RclrsError, - ReadOnlyParameter, QoSProfile, + ReadOnlyParameter, QoSProfile, Executor, SpinOptions, Duration, }; use rosidl_runtime_rs::{seq, Sequence}; - use std::sync::{Arc, RwLock}; + use std::sync::{Arc, atomic::{AtomicBool, Ordering}}; struct TestNode { node: Arc, @@ -332,22 +332,7 @@ mod tests { 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) { + fn construct_test_nodes(context: &Context, ns: &str) -> (Executor, TestNode, Arc) { let executor = context.create_basic_executor(); let node = executor.create_node( NodeOptions::new("node") @@ -388,6 +373,7 @@ mod tests { ).unwrap(); ( + executor, TestNode { node, bool_param, @@ -402,554 +388,576 @@ 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(&context, "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(10); + 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; + } + }; - std::thread::sleep(std::time::Duration::from_millis(100)); + 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 + } + ); + + executor.spin(SpinOptions::new().until_promise_resolved(promise)); - 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> { + #[test] + fn test_list_parameters_service() -> Result<(), RclrsError> { let context = Context::default(); - let (test_node, client_node) = construct_test_nodes(&context, "list"); + let (mut executor, _test, client_node) = construct_test_nodes(&context, "list"); let list_client = client_node.create_client::( "/list/node/list_parameters", QoSProfile::services_default(), )?; - try_until_timeout(|| list_client.service_is_ready().unwrap()) - .await + executor.spin( + SpinOptions::default() + .until_promise_resolved(list_client.notify_on_service_ready()) + ); + + // List all parameters + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let request = ListParameters_Request { + prefixes: seq![], + depth: 0, + }; + let promise = list_client + .call_then( + &request, + move |response: ListParameters_Response| { + // use_sim_time + all the manually defined ones + let names = response.result.names; + assert_eq!(names.len(), 5); + // Parameter names are returned in alphabetical order + assert_eq!(names[0].to_string(), "bool"); + assert_eq!(names[1].to_string(), "dynamic"); + assert_eq!(names[2].to_string(), "ns1.ns2.ns3.int"); + assert_eq!(names[3].to_string(), "read_only"); + assert_eq!(names[4].to_string(), "use_sim_time"); + // Only one prefix + assert_eq!(response.result.prefixes.len(), 1); + assert_eq!(response.result.prefixes[0].to_string(), "ns1.ns2.ns3"); + callback_ran_inner.store(true, Ordering::Release); + }, + ) .unwrap(); - let done = Arc::new(RwLock::new(false)); + executor.spin( + SpinOptions::default() + .until_promise_resolved(promise) + ); + assert!(callback_ran.load(Ordering::Acquire)); + + // Limit depth, namespaced parameter is not returned + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let request = ListParameters_Request { + prefixes: seq![], + depth: 1, + }; + let promise = list_client + .call_then( + &request, + move |response: ListParameters_Response| { + let names = response.result.names; + assert_eq!(names.len(), 4); + assert!(names.iter().all(|n| n.to_string() != "ns1.ns2.ns3.int")); + assert_eq!(response.result.prefixes.len(), 0); + callback_ran_inner.store(true, Ordering::Release); + }, + ) + .unwrap(); - let inner_done = done.clone(); - let rclrs_spin = tokio::task::spawn(async move { - try_until_timeout(|| { - crate::spin_once(test_node.node.clone(), Some(std::time::Duration::ZERO)).ok(); - crate::spin_once(client_node.clone(), Some(std::time::Duration::ZERO)).ok(); - *inner_done.read().unwrap() - }) - .await + executor.spin( + SpinOptions::default() + .until_promise_resolved(promise) + ); + 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 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 - .call_then( - &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 - .call_then( - &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 - .call_then( - &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 - .call_then( - &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) + ); + 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; + 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); + callback_ran_inner.store(true, Ordering::Release); + }, + ) + .unwrap(); + + executor.spin( + SpinOptions::default() + .until_promise_resolved(promise) + ); + assert!(callback_ran.load(Ordering::Acquire)); Ok(()) } - #[tokio::test] - async fn test_get_set_parameters_service() -> Result<(), RclrsError> { + #[test] + 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", QoSProfile::services_default())?; - let set_client = client.create_client::("/get_set/node/set_parameters", QoSProfile::services_default())?; - let set_atomically_client = client + let (mut executor, test, client_node) = construct_test_nodes(&context, "get_set"); + let get_client = client_node.create_client::("/get_set/node/get_parameters", QoSProfile::services_default())?; + let set_client = client_node.create_client::("/get_set/node/set_parameters", QoSProfile::services_default())?; + let set_atomically_client = client_node .create_client::("/get_set/node/set_parameters_atomically", QoSProfile::services_default())?; - 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() - }) - .await - .unwrap(); - }); + let get_client_inner = Arc::clone(&get_client); + let set_client_inner = Arc::clone(&set_client); + let set_atomically_client_inner = Arc::clone(&set_atomically_client); + let clients_ready_condition = move || { + get_client_inner.service_is_ready().unwrap() + && set_client_inner.service_is_ready().unwrap() + && set_atomically_client_inner.service_is_ready().unwrap() + }; - let 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 - .call_then( - &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 - .call_then( - &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 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) + ); + + // 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); }, - }; - let dynamic_parameter = RmwParameter { - name: "dynamic".into(), - value: RmwParameterValue { - type_: ParameterType::PARAMETER_BOOL, - bool_value: true, - ..Default::default() + ) + .unwrap(); + + executor.spin( + SpinOptions::default() + .until_promise_resolved(promise) + ); + 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); }, - }; - let out_of_range_parameter = RmwParameter { - name: "ns1.ns2.ns3.int".into(), - value: RmwParameterValue { - type_: ParameterType::PARAMETER_INTEGER, - integer_value: 1000, - ..Default::default() + ) + .unwrap(); + + executor.spin( + SpinOptions::default() + .until_promise_resolved(promise) + ); + 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); }, - }; - let invalid_parameter_type = RmwParameter { - name: "dynamic".into(), - value: RmwParameterValue { - type_: 200, - integer_value: 1000, - ..Default::default() + ) + .unwrap(); + + executor.spin( + SpinOptions::default() + .until_promise_resolved(promise) + ); + 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], + }; + 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); }, - }; - let undeclared_bool = RmwParameter { - name: "undeclared_bool".into(), - value: RmwParameterValue { - type_: ParameterType::PARAMETER_BOOL, - bool_value: true, - ..Default::default() + ) + .unwrap(); + + executor.spin( + SpinOptions::default() + .until_promise_resolved(promise) + ); + 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 - .call_then( - &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 - .call_then( - &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 - .call_then( - &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) + ); + assert!(callback_ran.load(Ordering::Acquire)); Ok(()) } - #[tokio::test] - async fn test_describe_get_types_parameters_service() -> Result<(), RclrsError> { + #[test] + fn test_describe_get_types_parameters_service() -> Result<(), RclrsError> { let context = Context::new([]).unwrap(); - let (node, client) = construct_test_nodes(&context, "describe"); + let (mut executor, _test, client_node) = construct_test_nodes(&context, "describe"); let describe_client = - client.create_client::("/describe/node/describe_parameters", QoSProfile::services_default())?; + client_node.create_client::("/describe/node/describe_parameters", QoSProfile::services_default())?; let get_types_client = - client.create_client::("/describe/node/get_parameter_types", QoSProfile::services_default())?; + client_node.create_client::("/describe/node/get_parameter_types", QoSProfile::services_default())?; - 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() - }) - .await + 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) + ); + + // Describe all parameters + let request = DescribeParameters_Request { + names: seq![ + "bool".into(), + "ns1.ns2.ns3.int".into(), + "read_only".into(), + "dynamic".into() + ], + }; + let callback_ran = Arc::new(AtomicBool::new(false)); + let callback_ran_inner = Arc::clone(&callback_ran); + let promise = describe_client + .call_then( + &request, + move |response: DescribeParameters_Response| { + let desc = response.descriptors; + assert_eq!(desc.len(), 4); + // Descriptors are returned in the requested order + assert_eq!(desc[0].name.to_string(), "bool"); + assert_eq!(desc[0].type_, ParameterType::PARAMETER_BOOL); + assert_eq!(desc[0].description.to_string(), "A boolean value"); + assert!(!desc[0].read_only); + assert!(!desc[0].dynamic_typing); + assert_eq!(desc[1].name.to_string(), "ns1.ns2.ns3.int"); + assert_eq!(desc[1].type_, ParameterType::PARAMETER_INTEGER); + assert_eq!(desc[1].integer_range.len(), 1); + assert_eq!(desc[1].integer_range[0].from_value, 0); + assert_eq!(desc[1].integer_range[0].to_value, 100); + assert_eq!(desc[1].integer_range[0].step, 0); + assert!(!desc[1].read_only); + assert!(!desc[1].dynamic_typing); + assert_eq!( + desc[1].additional_constraints.to_string(), + "Only the answer" + ); + assert_eq!(desc[2].name.to_string(), "read_only"); + assert_eq!(desc[2].type_, ParameterType::PARAMETER_DOUBLE); + assert!(desc[2].read_only); + assert!(!desc[2].dynamic_typing); + assert_eq!(desc[3].name.to_string(), "dynamic"); + assert_eq!(desc[3].type_, ParameterType::PARAMETER_STRING); + assert!(desc[3].dynamic_typing); + assert!(!desc[3].read_only); + callback_ran_inner.store(true, Ordering::Release); + }, + ) .unwrap(); - }); - - let 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 - .call_then( - &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 - .call_then( - &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 - .call_then( - &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) + ); + 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) + ); + 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) + ); + assert!(callback_ran.load(Ordering::Acquire)); Ok(()) } From d15f7625fb8334e81b95caa96abfb81f973399aa Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 13 Oct 2024 23:21:16 +0800 Subject: [PATCH 15/48] Try async-std timeouts instead of tokio Signed-off-by: Michael X. Grey --- rclrs/Cargo.toml | 4 ++-- rclrs/src/node.rs | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index f27f53d5e..6a8fda34c 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -23,8 +23,8 @@ cfg-if = "1.0.0" # Needed for clients futures = "0.3" -# We import only the time module, which is needed to support timeouts in async blocks -tokio = { version = "1", features = ["time"] } +# Needed for the runtime-agnostic timeout feature +async-std = "1.13" # Needed for dynamic messages libloading = { version = "0.8", optional = true } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 9ee44cfe2..56e69677c 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -18,10 +18,10 @@ use std::{ use futures::{ StreamExt, - channel::{oneshot, mpsc::{unbounded, UnboundedSender}}, + channel::mpsc::{unbounded, UnboundedSender}, }; -use tokio::time::timeout; +use async_std::future::timeout; use rosidl_runtime_rs::Message; From fe56cc920918da4879ad9a66cafac94b3869234b Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 14 Oct 2024 01:17:59 +0800 Subject: [PATCH 16/48] Debugging strange client failure to take Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 5 +- rclrs/src/client/client_task.rs | 29 +++++++++-- rclrs/src/executor.rs | 13 ++++- rclrs/src/executor/basic_executor.rs | 22 ++++++--- rclrs/src/executor/wait_set_runner.rs | 31 ++++++++++-- rclrs/src/lib.rs | 2 - rclrs/src/node.rs | 49 ++++++++++++++----- rclrs/src/parameter/service.rs | 33 +++++++++++-- rclrs/src/service.rs | 14 +++--- rclrs/src/service/service_task.rs | 6 ++- .../subscription_async_callback.rs | 2 +- .../src/subscription/subscription_callback.rs | 2 +- rclrs/src/wait.rs | 3 +- 13 files changed, 164 insertions(+), 47 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index c48fa84f4..502d2b0ca 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -150,8 +150,9 @@ where /// Check if a service server is available. /// /// Will return true if there is a service server available, false if unavailable. - // - // TODO(@mxgrey): Provide an async function to await on when the service is ready + /// + /// 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(); diff --git a/rclrs/src/client/client_task.rs b/rclrs/src/client/client_task.rs index 91ede050d..a5b2c3cd3 100644 --- a/rclrs/src/client/client_task.rs +++ b/rclrs/src/client/client_task.rs @@ -10,12 +10,13 @@ use std::{ collections::HashMap, }; +use std::io::Write; + use crate::{ error::ToResult, rcl_bindings::*, client::ClientHandle, - AnyClientOutputSender, ServiceInfo, - RclrsError, + AnyClientOutputSender, ServiceInfo, RclrsError, RclReturnCode, }; pub enum ClientAction { @@ -32,6 +33,8 @@ pub(super) async fn client_task( mut receiver: UnboundedReceiver>, handle: Arc, ) { + dbg!(); + std::io::stdout().lock().flush().unwrap(); // This stores all active requests that have not received a response yet let mut active_requests: HashMap> = HashMap::new(); @@ -44,29 +47,47 @@ pub(super) async fn client_task( while let Some(action) = receiver.next().await { match action { ClientAction::TakeResponse => { + dbg!(); + std::io::stdout().lock().flush().unwrap(); match take_response::(&handle) { Ok((response, info)) => { let seq = info.request_id.sequence_number; if let Some(sender) = active_requests.remove(&seq) { + dbg!(); // The active request is available, so send this response off sender.send_response(response, info); } else { + dbg!(); // Weirdly there isn't an active request for this, so save // it in the loose responses map. loose_responses.insert(seq, (response, info)); } } - Err(_err) => { - // TODO(@mxgrey): Log the error here once logging is available + Err(err) => { + match err { + RclrsError::RclError { code: RclReturnCode::ClientTakeFailed, .. } => { + // This is okay, it means a spurious wakeup happened + dbg!(); + } + err => { + dbg!(); + // TODO(@mxgrey): Log the error here once logging is available + eprintln!("Error while taking a response for a client: {err}"); + } + } } } } ClientAction::NewRequest { sequence_number, sender } => { + dbg!(); + std::io::stdout().lock().flush().unwrap(); if let Some((response, info)) = loose_responses.remove(&sequence_number) { // The response for this request already arrive, so we'll // send it off immediately. + dbg!(); sender.send_response(response, info); } else { + dbg!(); active_requests.insert(sequence_number, sender); } } diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 62a8200b4..9df942b4e 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -4,6 +4,8 @@ pub use self::basic_executor::*; mod wait_set_runner; pub use self::wait_set_runner::*; +use std::io::Write; + use crate::{ Node, NodeOptions, RclrsError, Context, ContextHandle, Waitable, GuardCondition, }; @@ -148,13 +150,20 @@ impl ExecutorCommands { let (mut sender, receiver) = oneshot::channel(); self.channel.add_async_task(Box::pin( async move { + dbg!(); + std::io::stdout().lock().flush().unwrap(); let cancellation = sender.cancellation(); let output = match select(cancellation, std::pin::pin!(f)).await { // The task was cancelled - Either::Left(_) => return, + // Either::Left(_) => return, // The task completed - Either::Right((output, _)) => output, + // Either::Right((output, _)) => output, + + Either::Left(_) => { dbg!(); return; } + Either::Right((output, _)) => { dbg!(); output } }; + dbg!(); + std::io::stdout().lock().flush().unwrap(); sender.send(output).ok(); } )); diff --git a/rclrs/src/executor/basic_executor.rs b/rclrs/src/executor/basic_executor.rs index 545d28b06..20bb19119 100644 --- a/rclrs/src/executor/basic_executor.rs +++ b/rclrs/src/executor/basic_executor.rs @@ -16,9 +16,11 @@ use std::{ use crate::{ executor::{ExecutorRuntime, ExecutorChannel, SpinConditions}, - WaitSet, Waitable, Context, WaitSetRunner, + Waitable, Context, WaitSetRunner, }; +use std::io::Write; + /// 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 /// @@ -77,7 +79,9 @@ impl ExecutorRuntime for BasicExecutorRuntime { wait_set_finished_clone.store(true, Ordering::Release); })); + let mut count = 0; while let Ok(task) = self.next_task(&wait_set_finished) { + dbg!(); // 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() { @@ -93,6 +97,10 @@ impl ExecutorRuntime for BasicExecutorRuntime { } } + // count += 1; + // if count > 20 { + // panic!("Done {count} iterations"); + // } } self.wait_set_runner = Some( @@ -161,6 +169,7 @@ impl BasicExecutorRuntime { fn process_spin_conditions(&self, conditions: &mut SpinConditions) { if let Some(promise) = conditions.options.until_promise_resolved.take() { let guard_condition = Arc::clone(&conditions.guard_condition); + let (sender, receiver) = oneshot::channel(); self.task_sender.add_async_task(Box::pin(async move { if let Err(err) = promise.await { // TODO(@mxgrey): We should change this to a log when logging @@ -172,8 +181,13 @@ impl BasicExecutorRuntime { ); } // TODO(@mxgrey): Log errors here when logging becomes available. + dbg!(); + std::io::stdout().lock().flush().unwrap(); guard_condition.trigger().ok(); + sender.send(()).ok(); })); + + conditions.options.until_promise_resolved = Some(receiver); } } @@ -239,10 +253,6 @@ struct Task { impl ArcWake for Task { fn wake_by_ref(arc_self: &Arc) { let cloned = Arc::clone(arc_self); - if let Err(err) = arc_self.task_sender.send(cloned) { - eprintln!( - "Unable to resume a task because sending it to the basic executor failed: {err}" - ); - } + arc_self.task_sender.send(cloned).ok(); } } diff --git a/rclrs/src/executor/wait_set_runner.rs b/rclrs/src/executor/wait_set_runner.rs index 002475aa4..6e099bfb7 100644 --- a/rclrs/src/executor/wait_set_runner.rs +++ b/rclrs/src/executor/wait_set_runner.rs @@ -3,10 +3,13 @@ use futures::channel::{ mpsc::{UnboundedSender, UnboundedReceiver, unbounded}, }; -use std::sync::atomic::Ordering; +use std::{ + sync::atomic::Ordering, + time::{Instant, Duration}, +}; use crate::{ - WaitSet, Context, SpinConditions, Promise, Waitable, + WaitSet, Context, SpinConditions, Promise, Waitable, RclrsError, RclReturnCode, }; /// This is a utility class that executors can use to easily run and manage @@ -64,6 +67,7 @@ impl WaitSetRunner { /// will be triggered after the user-provided promise is resolved. pub fn run_blocking(&mut self, mut conditions: SpinConditions) { let mut first_spin = true; + let t_stop_spinning = conditions.options.timeout.map(|dt| Instant::now() + dt); loop { // TODO(@mxgrey): SmallVec would be better suited here if we are // okay with adding that as a dependency. @@ -102,12 +106,29 @@ impl WaitSetRunner { break; } + let timeout = t_stop_spinning.map(|t| { + let timeout = t - Instant::now(); + if timeout < Duration::ZERO { + Duration::ZERO + } else { + timeout + } + }); + if let Err(err) = self.wait_set.wait( - conditions.options.timeout, + timeout, |executable| executable.execute(), ) { - // TODO(@mxgrey): Change this to a log when logging becomes available - eprintln!("Error while processing wait set: {err}"); + match err { + RclrsError::RclError { code: RclReturnCode::Timeout, .. } => { + // We have timed out, so we should stop waiting. + break; + } + err => { + // TODO(@mxgrey): Change this to a log when logging becomes available + eprintln!("Error while processing wait set: {err}"); + } + } } } } diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 8d8832cfb..f41b61700 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -30,8 +30,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::*; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 56e69677c..5648dafb5 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -21,7 +21,7 @@ use futures::{ channel::mpsc::{unbounded, UnboundedSender}, }; -use async_std::future::timeout; +// use async_std::future::timeout; use rosidl_runtime_rs::Message; @@ -33,6 +33,10 @@ use crate::{ ServiceAsyncCallback, ExecutorCommands, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; + +use std::io::Write; + + /// 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] @@ -449,22 +453,43 @@ impl Node { ) -> Promise<()> { let (listener, mut on_graph_change_receiver) = unbounded(); let promise = self.commands.query(async move { + dbg!(); + std::io::stdout().lock().flush().unwrap(); 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. + // 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; + // } + // } + + match on_graph_change_receiver.next().await { + Some(_) => { + dbg!(); + std::io::stdout().lock().flush().unwrap(); if condition() { + // Condition is met + dbg!(); + std::io::stdout().lock().flush().unwrap(); 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. + None => { + dbg!(); + std::io::stdout().lock().flush().unwrap(); + // Graph change sender is closed return; } } diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index e724a0b6b..d1f9510d3 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -319,10 +319,15 @@ mod tests { srv::rmw::*, }, Context, MandatoryParameter, Node, NodeOptions, ParameterRange, ParameterValue, RclrsError, - ReadOnlyParameter, QoSProfile, Executor, SpinOptions, Duration, + ReadOnlyParameter, QoSProfile, Executor, SpinOptions, }; use rosidl_runtime_rs::{seq, Sequence}; - use std::sync::{Arc, atomic::{AtomicBool, Ordering}}; + use std::{ + sync::{Arc, atomic::{AtomicBool, Ordering}}, + time::Duration, + }; + + use std::io::Write; struct TestNode { node: Arc, @@ -392,7 +397,7 @@ mod tests { // 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(10); + let timeout = Duration::from_secs(1); let initial_time = std::time::Instant::now(); let node = Arc::clone(&test.node); @@ -438,13 +443,18 @@ mod tests { } ); - executor.spin(SpinOptions::new().until_promise_resolved(promise)); + executor.spin( + SpinOptions::new() + .until_promise_resolved(promise) + .timeout(Duration::from_secs(1)) + ); Ok(()) } #[test] fn test_list_parameters_service() -> Result<(), RclrsError> { + dbg!(); let context = Context::default(); let (mut executor, _test, client_node) = construct_test_nodes(&context, "list"); let list_client = client_node.create_client::( @@ -452,11 +462,17 @@ mod tests { QoSProfile::services_default(), )?; + dbg!(); + std::io::stdout().lock().flush().unwrap(); + // return Ok(()); executor.spin( SpinOptions::default() .until_promise_resolved(list_client.notify_on_service_ready()) + .timeout(Duration::from_secs(2)) ); + dbg!(); + std::io::stdout().lock().flush().unwrap(); // List all parameters let callback_ran = Arc::new(AtomicBool::new(false)); let callback_ran_inner = Arc::clone(&callback_ran); @@ -468,6 +484,8 @@ mod tests { .call_then( &request, move |response: ListParameters_Response| { + dbg!(); + std::io::stdout().lock().flush().unwrap(); // use_sim_time + all the manually defined ones let names = response.result.names; assert_eq!(names.len(), 5); @@ -485,11 +503,18 @@ mod tests { ) .unwrap(); + dbg!(); + std::io::stdout().lock().flush().unwrap(); executor.spin( SpinOptions::default() .until_promise_resolved(promise) + .timeout(Duration::from_secs(5)) ); assert!(callback_ran.load(Ordering::Acquire)); + dbg!(); + std::io::stdout().lock().flush().unwrap(); + + return Ok(()); // Limit depth, namespaced parameter is not returned let callback_ran = Arc::new(AtomicBool::new(false)); diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index b32f1306f..c6780bc10 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -4,7 +4,7 @@ use std::{ sync::{Arc, Mutex, MutexGuard}, }; -use futures::channel::mpsc::{unbounded, UnboundedSender, TrySendError}; +use futures::channel::mpsc::{unbounded, UnboundedSender}; use crate::{ error::ToResult, @@ -81,9 +81,10 @@ where pub fn set_callback( &self, callback: impl ServiceCallback, - ) -> Result<(), TrySendError>> { + ) { let callback = callback.into_service_callback(); - self.action.unbounded_send(ServiceAction::SetCallback(callback)) + // TODO(@mxgrey): Log any errors here when logging becomes available + self.action.unbounded_send(ServiceAction::SetCallback(callback)).ok(); } /// Set the callback of this service, replacing the callback that was @@ -93,9 +94,10 @@ where pub fn set_async_callback( &self, callback: impl ServiceAsyncCallback, - ) -> Result<(), TrySendError>> { + ) { let callback = callback.into_service_async_callback(); - self.action.unbounded_send(ServiceAction::SetCallback(callback)) + // TODO(@mxgrey): Log any errors here when logging becomes available. + self.action.unbounded_send(ServiceAction::SetCallback(callback)).ok(); } /// Used by [`Node`][crate::Node] to create a new service @@ -115,7 +117,7 @@ where Arc::clone(commands.get_guard_condition()), )?; - commands.run(service_task( + let _ = commands.run(service_task( callback, receiver, Arc::clone(&service.handle), diff --git a/rclrs/src/service/service_task.rs b/rclrs/src/service/service_task.rs index 9bb45be07..adadba01d 100644 --- a/rclrs/src/service/service_task.rs +++ b/rclrs/src/service/service_task.rs @@ -23,14 +23,18 @@ pub(super) async fn service_task( handle: Arc, commands: Arc, ) { + dbg!(); while let Some(action) = receiver.next().await { match action { ServiceAction::SetCallback(new_callback) => { + dbg!(); callback = new_callback; } ServiceAction::Execute => { - if let Err(_) = callback.execute(&handle, &commands) { + dbg!(); + if let Err(err) = callback.execute(&handle, &commands) { // TODO(@mxgrey): Log the error here once logging is implemented + eprintln!("Error while executing a service callback: {err}"); } } } diff --git a/rclrs/src/subscription/subscription_async_callback.rs b/rclrs/src/subscription/subscription_async_callback.rs index f75c2e34e..1a22585f7 100644 --- a/rclrs/src/subscription/subscription_async_callback.rs +++ b/rclrs/src/subscription/subscription_async_callback.rs @@ -221,7 +221,7 @@ mod tests { } - async fn test_loaned_with_info(_msg: ReadOnlyLoanedMessage) { + async fn test_loaned_with_info(_msg: ReadOnlyLoanedMessage, _info: MessageInfo) { } } diff --git a/rclrs/src/subscription/subscription_callback.rs b/rclrs/src/subscription/subscription_callback.rs index 5e2aec292..c9f9d0024 100644 --- a/rclrs/src/subscription/subscription_callback.rs +++ b/rclrs/src/subscription/subscription_callback.rs @@ -244,7 +244,7 @@ mod tests { } - fn test_loaned_with_info(_msg: ReadOnlyLoanedMessage) { + fn test_loaned_with_info(_msg: ReadOnlyLoanedMessage, _info: MessageInfo) { } } diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index c64c6606f..9c1c3d69d 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -236,6 +236,7 @@ struct WaitSetHandle { #[cfg(test)] mod tests { use crate::*; + use std::time::Duration; #[test] fn traits() { @@ -249,7 +250,7 @@ mod tests { fn guard_condition_in_wait_set_readies() -> Result<(), RclrsError> { let mut executor = Context::new([])?.create_basic_executor(); - executor.commands().get_guard_condition().trigger(); + executor.commands().get_guard_condition().trigger().unwrap(); let start = std::time::Instant::now(); // This should stop spinning right away because the guard condition was From 8f0f192f387bac40fd8e0c14da4094af57b0a4be Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 14 Oct 2024 13:55:04 +0800 Subject: [PATCH 17/48] Experimenting with only taking from services and clients in the same thread as the wait set Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 137 ++++++++++++++++++---- rclrs/src/client/client_task.rs | 3 + rclrs/src/service.rs | 71 ++++------- rclrs/src/service/any_service_callback.rs | 46 ++++++-- 4 files changed, 178 insertions(+), 79 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index 502d2b0ca..45d0a649f 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -1,18 +1,16 @@ use std::{ - boxed::Box, ffi::CString, sync::{Arc, Mutex, MutexGuard}, + collections::HashMap, }; -use futures::channel::mpsc::{UnboundedSender, unbounded}; - use rosidl_runtime_rs::Message; use crate::{ error::ToResult, rcl_bindings::*, - MessageCow, Node, RclrsError, Promise, ENTITY_LIFECYCLE_MUTEX, - ExecutorCommands, Executable, QoSProfile, Waitable, WaitableLifecycle, + MessageCow, Node, RclrsError, RclReturnCode, Promise, ENTITY_LIFECYCLE_MUTEX, + Executable, QoSProfile, Waitable, WaitableLifecycle, ExecutableHandle, ExecutableKind, ServiceInfo, }; @@ -25,9 +23,6 @@ pub use client_callback::*; mod client_output; pub use client_output::*; -mod client_task; -use client_task::*; - /// Main class responsible for sending requests to a ROS service. /// /// The only available way to instantiate clients is via [`Node::create_client`][1], this is to @@ -40,7 +35,7 @@ where T: rosidl_runtime_rs::Service, { handle: Arc, - action: UnboundedSender>, + board: Arc>>, #[allow(unused)] lifecycle: WaitableLifecycle, } @@ -86,10 +81,9 @@ where } .ok()?; + println!("vvvvvvvvv Sent client request {sequence_number} vvvvvvvvvvvv"); // TODO(@mxgrey): Log errors here when logging becomes available. - self.action.unbounded_send( - ClientAction::NewRequest { sequence_number, sender } - ).ok(); + self.board.lock().unwrap().new_request(sequence_number, sender); Ok(promise) } @@ -231,14 +225,12 @@ where }); let commands = node.commands(); - - let (action, receiver) = unbounded(); - let _ = commands.run(client_task(receiver, Arc::clone(&handle))); + let board = Arc::new(Mutex::new(ClientRequestBoard::new())); let (waitable, lifecycle) = Waitable::new( Box::new(ClientExecutable { handle: Arc::clone(&handle), - action: action.clone(), + board: Arc::clone(&board), }), Some(Arc::clone(&commands.get_guard_condition())), ); @@ -246,15 +238,18 @@ where Ok(Arc::new(Self { handle, - action, + board, lifecycle, })) } } -struct ClientExecutable { +struct ClientExecutable +where + T: rosidl_runtime_rs::Service, +{ handle: Arc, - action: UnboundedSender> + board: Arc>> } impl Executable for ClientExecutable @@ -262,8 +257,7 @@ where T: rosidl_runtime_rs::Service, { fn execute(&mut self) -> Result<(), RclrsError> { - self.action.unbounded_send(ClientAction::TakeResponse).ok(); - Ok(()) + self.board.lock().unwrap().execute(&self.handle) } fn handle(&self) -> ExecutableHandle { @@ -275,6 +269,107 @@ where } } +type SequenceNumber = i64; + +/// This is used internally to monitor the state of active requests, as well as +/// responses that have arrived without a known request. +struct ClientRequestBoard +where + T: rosidl_runtime_rs::Service, +{ + // This stores all active requests that have not received a response yet + active_requests: HashMap>, + // This holds responses that came in when no active request matched the + // sequence number. This could happen if take_response is triggered before + // the new_request for the same sequence number. That is extremely unlikely + // to ever happen but is theoretically possible on systems that may exhibit + // very strange CPU scheduling patterns, so we should account for it. + loose_responses: HashMap, +} + +impl ClientRequestBoard +where + T: rosidl_runtime_rs::Service, +{ + fn new() -> Self { + Self { + active_requests: Default::default(), + loose_responses: Default::default(), + } + } + + fn new_request( + &mut self, + sequence_number: SequenceNumber, + sender: AnyClientOutputSender, + ) { + if let Some((response, info)) = self.loose_responses.remove(&sequence_number) { + // Weirdly the response for this request already arrived, so we'll + // send it off immediately. + sender.send_response(response, info); + } else { + self.active_requests.insert(sequence_number, sender); + } + } + + fn execute(&mut self, handle: &Arc) -> Result<(), RclrsError> { + match self.take_response(handle) { + Ok((response, info)) => { + let seq = info.request_id.sequence_number; + if let Some(sender) = self.active_requests.remove(&seq) { + dbg!(); + println!("Received response for {info:?}"); + // The active request is available, so send this response off + sender.send_response(response, info); + } else { + dbg!(); + println!("Received loose response for {info:?}"); + // 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 + dbg!(); + println!("Spurious wakeup for client"); + } + err => { + dbg!(); + // TODO(@mxgrey): Log the error here once logging is available + eprintln!("Error while taking a response for a client: {err}"); + } + } + } + } + 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`. diff --git a/rclrs/src/client/client_task.rs b/rclrs/src/client/client_task.rs index a5b2c3cd3..ba811188a 100644 --- a/rclrs/src/client/client_task.rs +++ b/rclrs/src/client/client_task.rs @@ -54,10 +54,12 @@ pub(super) async fn client_task( let seq = info.request_id.sequence_number; if let Some(sender) = active_requests.remove(&seq) { dbg!(); + println!("Received response for {info:?}"); // The active request is available, so send this response off sender.send_response(response, info); } else { dbg!(); + println!("Received loose response for {info:?}"); // Weirdly there isn't an active request for this, so save // it in the loose responses map. loose_responses.insert(seq, (response, info)); @@ -68,6 +70,7 @@ pub(super) async fn client_task( RclrsError::RclError { code: RclReturnCode::ClientTakeFailed, .. } => { // This is okay, it means a spurious wakeup happened dbg!(); + println!("Spurious wakeup for client"); } err => { dbg!(); diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index c6780bc10..d3e8c855c 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -4,12 +4,10 @@ use std::{ sync::{Arc, Mutex, MutexGuard}, }; -use futures::channel::mpsc::{unbounded, UnboundedSender}; - use crate::{ error::ToResult, rcl_bindings::*, - NodeHandle, RclrsError, Waitable, WaitableLifecycle, GuardCondition, QoSProfile, + NodeHandle, RclrsError, Waitable, WaitableLifecycle, QoSProfile, Executable, ExecutableKind, ExecutableHandle, ENTITY_LIFECYCLE_MUTEX, ExecutorCommands, }; @@ -25,9 +23,6 @@ pub use service_callback::*; mod service_info; pub use service_info::*; -mod service_task; -use service_task::*; - /// Struct for responding to requests sent by ROS service clients. /// /// The only way to instantiate a service is via [`Node::create_service()`][1] @@ -45,11 +40,8 @@ where { /// This handle is used to access the data that rcl holds for this service. handle: Arc, - /// This allows us to replace the callback in the service task. - /// - /// Holding onto this sender will keep the service task alive. Once this - /// sender is dropped, the service task will end itself. - action: UnboundedSender>, + /// 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. lifecycle: WaitableLifecycle, @@ -84,7 +76,7 @@ where ) { let callback = callback.into_service_callback(); // TODO(@mxgrey): Log any errors here when logging becomes available - self.action.unbounded_send(ServiceAction::SetCallback(callback)).ok(); + *self.callback.lock().unwrap() = callback; } /// Set the callback of this service, replacing the callback that was @@ -96,8 +88,7 @@ where callback: impl ServiceAsyncCallback, ) { let callback = callback.into_service_async_callback(); - // TODO(@mxgrey): Log any errors here when logging becomes available. - self.action.unbounded_send(ServiceAction::SetCallback(callback)).ok(); + *self.callback.lock().unwrap() = callback; } /// Used by [`Node`][crate::Node] to create a new service @@ -108,35 +99,7 @@ where node_handle: &Arc, commands: &Arc, ) -> Result, RclrsError> { - let (sender, receiver) = unbounded(); - let (service, waitable) = Self::new( - topic, - qos, - sender, - Arc::clone(&node_handle), - Arc::clone(commands.get_guard_condition()), - )?; - - let _ = commands.run(service_task( - callback, - receiver, - Arc::clone(&service.handle), - Arc::clone(commands), - )); - - commands.add_to_wait_set(waitable); - - Ok(service) - } - - /// Instantiate the service. - fn new( - topic: &str, - qos: QoSProfile, - action: UnboundedSender>, - node_handle: Arc, - guard_condition: Arc, - ) -> Result<(Arc, Waitable), RclrsError> { + 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() @@ -174,26 +137,29 @@ where let handle = Arc::new(ServiceHandle { rcl_service: Mutex::new(rcl_service), - node_handle, + node_handle: Arc::clone(&node_handle), }); - let (waiter, lifecycle) = Waitable::new( + let (waitable, lifecycle) = Waitable::new( Box::new(ServiceExecutable { handle: Arc::clone(&handle), - action: action.clone(), + callback: Arc::clone(&callback), + commands: Arc::clone(&commands), }), - Some(guard_condition), + Some(Arc::clone(commands.get_guard_condition())), ); - let service = Arc::new(Self { handle, action, lifecycle }); + let service = Arc::new(Self { handle, callback, lifecycle }); + commands.add_to_wait_set(waitable); - Ok((service, waiter)) + Ok(service) } } struct ServiceExecutable { handle: Arc, - action: UnboundedSender>, + callback: Arc>>, + commands: Arc, } impl Executable for ServiceExecutable @@ -201,7 +167,10 @@ where T: rosidl_runtime_rs::Service, { fn execute(&mut self) -> Result<(), RclrsError> { - self.action.unbounded_send(ServiceAction::Execute).ok(); + if let Err(err) = self.callback.lock().unwrap().execute(&self.handle, &self.commands) { + // TODO(@mxgrey): Log the error here once logging is implemented + eprintln!("Error while executing a service callback: {err}"); + } Ok(()) } diff --git a/rclrs/src/service/any_service_callback.rs b/rclrs/src/service/any_service_callback.rs index 411f2b271..1682b703f 100644 --- a/rclrs/src/service/any_service_callback.rs +++ b/rclrs/src/service/any_service_callback.rs @@ -34,27 +34,46 @@ impl AnyServiceCallback { commands: &Arc, ) -> Result<(), RclrsError> { let mut evaluate = || { + dbg!(); match self { AnyServiceCallback::OnlyRequest(cb) => { + dbg!(); let (msg, mut rmw_request_id) = Self::take_request(handle)?; let handle = Arc::clone(&handle); let response = cb(msg); - commands.run(async move { + dbg!(); + let _ = commands.run(async move { // TODO(@mxgrey): Log any errors here when logging is available - Self::send_response(&handle, &mut rmw_request_id, response.await).ok(); + dbg!(); + println!("Sending service response: {rmw_request_id:?}"); + if let Err(err) = Self::send_response(&handle, &mut rmw_request_id, response.await) { + // TODO(@mxgrey): Use logging instead when it becomes available + eprintln!("Error while sending service response for {rmw_request_id:?}: {err}"); + } else { + println!("No error while sending {rmw_request_id:?}"); + } }); } AnyServiceCallback::WithId(cb) => { + dbg!(); let (msg, mut rmw_request_id) = Self::take_request(handle)?; let request_id = RequestId::from_rmw_request_id(&rmw_request_id); let handle = Arc::clone(&handle); let response = cb(msg, request_id); - commands.run(async move { - // TODO(@mxgrey): Log any errors here when logging is available - Self::send_response(&handle, &mut rmw_request_id, response.await).ok(); + dbg!(); + let _ = commands.run(async move { + dbg!(); + println!("Sending service response: {rmw_request_id:?}"); + if let Err(err) = Self::send_response(&handle, &mut rmw_request_id, response.await) { + // TODO(@mxgrey): Use logging instead when it becomes available + eprintln!("Error while sending service response for {rmw_request_id:?}: {err}"); + } else { + println!("No error while sending {rmw_request_id:?}"); + } }); } AnyServiceCallback::WithInfo(cb) => { + dbg!(); let (msg, rmw_service_info) = Self::take_request_with_info(handle)?; let mut rmw_request_id = rmw_request_id_t { writer_guid: rmw_service_info.request_id.writer_guid, @@ -63,9 +82,17 @@ impl AnyServiceCallback { let service_info = ServiceInfo::from_rmw_service_info(&rmw_service_info); let handle = Arc::clone(&handle); let response = cb(msg, service_info); - commands.run(async move { + dbg!(); + let _ = commands.run(async move { // TODO(@mxgrey): Log any errors here when logging is available - Self::send_response(&handle, &mut rmw_request_id, response.await).ok(); + dbg!(); + println!("Sending service response: {rmw_request_id:?}"); + if let Err(err) = Self::send_response(&handle, &mut rmw_request_id, response.await) { + // TODO(@mxgrey): Use logging instead when it becomes available + eprintln!("Error while sending service response for {rmw_request_id:?}: {err}"); + } else { + println!("No error while sending {rmw_request_id:?}"); + } }); } } @@ -73,6 +100,7 @@ impl AnyServiceCallback { Ok(()) }; + dbg!(); match evaluate() { Err(RclrsError::RclError { code: RclReturnCode::ServiceTakeFailed, @@ -80,6 +108,8 @@ impl AnyServiceCallback { }) => { // Spurious wakeup - this may happen even when a waitlist indicated that this // subscription was ready, so it shouldn't be an error. + dbg!(); + println!("Spurious wakeup for service request"); Ok(()) } other => other, @@ -122,6 +152,7 @@ impl AnyServiceCallback { ) } .ok()?; + println!("^^^^^^^^^^ service request arrived: {request_id_out:?} ^^^^^^^^^^^^^^"); Ok((T::Request::from_rmw_message(request_out), request_id_out)) } @@ -140,6 +171,7 @@ impl AnyServiceCallback { ) } .ok()?; + println!("^^^^^^^^^^^^ service request arrived: {service_info_out:?} ^^^^^^^^^^^^^"); Ok((T::Request::from_rmw_message(request_out), service_info_out)) } From c99f410b9b6c8bb576378fc99de6e5bfc1413cae Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 14 Oct 2024 19:25:54 +0800 Subject: [PATCH 18/48] Migrate subscription to use shared callback instead of an async task Signed-off-by: Michael X. Grey --- rclrs/src/parameter/service.rs | 10 +++-- rclrs/src/subscription.rs | 71 ++++++++-------------------------- rclrs/src/wait.rs | 52 +++++++++++++++---------- 3 files changed, 55 insertions(+), 78 deletions(-) diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index d1f9510d3..6bad4ff68 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -514,8 +514,6 @@ mod tests { dbg!(); std::io::stdout().lock().flush().unwrap(); - return Ok(()); - // Limit depth, namespaced parameter is not returned let callback_ran = Arc::new(AtomicBool::new(false)); let callback_ran_inner = Arc::clone(&callback_ran); @@ -793,6 +791,12 @@ mod tests { 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, @@ -801,7 +805,7 @@ mod tests { // Setting the undeclared parameter is now allowed assert!(response.results[0].successful); assert_eq!( - test.node.use_undeclared_parameters().get("undeclared_bool"), + test_node.use_undeclared_parameters().get("undeclared_bool"), Some(ParameterValue::Bool(true)) ); callback_ran_inner.store(true, Ordering::Release); diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 2b61ae603..2c1e8c0ce 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -5,14 +5,12 @@ use std::{ use rosidl_runtime_rs::{Message, RmwMessage}; -use futures::channel::mpsc::{unbounded, UnboundedSender, TrySendError}; - use crate::{ error::ToResult, qos::QoSProfile, rcl_bindings::*, ExecutorCommands, NodeHandle, RclrsError, Waitable, Executable, ExecutableHandle, - ExecutableKind, GuardCondition, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, + ExecutableKind, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, }; mod any_subscription_callback; @@ -30,9 +28,6 @@ pub use message_info::*; mod readonly_loaned_message; pub use readonly_loaned_message::*; -mod subscription_task; -use subscription_task::*; - /// Struct for receiving messages of type `T`. /// /// The only way to instantiate a subscription is via [`Node::create_subscription()`][2] @@ -59,7 +54,7 @@ where /// /// Holding onto this sender will keep the subscription task alive. Once /// this sender is dropped, the subscription task will end itself. - action: UnboundedSender>, + callback: Arc>>, /// Holding onto this keeps the waiter for this subscription alive in the /// wait set of the executor. lifecycle: WaitableLifecycle, @@ -91,9 +86,9 @@ where pub fn set_callback( &self, callback: impl SubscriptionCallback, - ) -> Result<(), TrySendError>> { + ) { let callback = callback.into_subscription_callback(); - self.action.unbounded_send(SubscriptionAction::SetCallback(callback)) + *self.callback.lock().unwrap() = callback; } /// Set the callback of this subscription, replacing the callback that was @@ -103,9 +98,9 @@ where pub fn set_async_callback( &self, callback: impl SubscriptionAsyncCallback, - ) -> Result<(), TrySendError>> { + ) { let callback = callback.into_subscription_async_callback(); - self.action.unbounded_send(SubscriptionAction::SetCallback(callback)) + *self.callback.lock().unwrap() = callback; } /// Used by [`Node`][crate::Node] to create a new subscription. @@ -116,40 +111,8 @@ where node_handle: &Arc, commands: &Arc, ) -> Result, RclrsError> { - let (sender, receiver) = unbounded(); - let (subscription, waiter) = Self::new( - topic, - qos, - sender, - Arc::clone(&node_handle), - Arc::clone(commands.get_guard_condition()), - )?; + let callback = Arc::new(Mutex::new(callback)); - commands.run(subscription_task( - callback, - receiver, - Arc::clone(&subscription.handle), - Arc::clone(&commands), - )); - - commands.add_to_wait_set(waiter); - - Ok(subscription) - } - - /// Instantiate the Subscription. - fn new( - topic: &str, - qos: QoSProfile, - action: UnboundedSender>, - node_handle: Arc, - guard_condition: Arc, - ) -> Result<(Arc, Waitable), RclrsError> - // 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, - { // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_subscription = unsafe { rcl_get_zero_initialized_subscription() }; let type_support = @@ -186,26 +149,27 @@ where let handle = Arc::new(SubscriptionHandle { rcl_subscription: Mutex::new(rcl_subscription), - node_handle, + node_handle: Arc::clone(node_handle), }); - let (waiter, lifecycle) = Waitable::new( + let (waitable, lifecycle) = Waitable::new( Box::new(SubscriptionExecutable { handle: Arc::clone(&handle), - action: action.clone(), + callback: Arc::clone(&callback), + commands: Arc::clone(commands), }), - Some(guard_condition), + Some(Arc::clone(commands.get_guard_condition())), ); + commands.add_to_wait_set(waitable); - let subscription = Arc::new(Self { handle, action, lifecycle }); - - Ok((subscription, waiter)) + Ok(Arc::new(Self { handle, callback, lifecycle })) } } struct SubscriptionExecutable { handle: Arc, - action: UnboundedSender>, + callback: Arc>>, + commands: Arc, } impl Executable for SubscriptionExecutable @@ -213,8 +177,7 @@ where T: Message, { fn execute(&mut self) -> Result<(), RclrsError> { - self.action.unbounded_send(SubscriptionAction::Execute).ok(); - Ok(()) + self.callback.lock().unwrap().execute(&self.handle, &self.commands) } fn kind(&self) -> crate::ExecutableKind { diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 9c1c3d69d..a1bdc8352 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -31,8 +31,8 @@ pub use waitable::*; /// A struct for waiting on subscriptions and other waitable entities to become ready. pub struct WaitSet { - entities: HashMap>, - pub(crate) handle: WaitSetHandle, + primitives: HashMap>, + handle: WaitSetHandle, } // SAFETY: While the rcl_wait_set_t does have some interior mutability (because it has @@ -53,8 +53,8 @@ impl WaitSet { context_handle: Arc::clone(&context.handle), }; - let mut wait_set = Self { entities: HashMap::new(), handle }; - wait_set.register_rcl_entities()?; + let mut wait_set = Self { primitives: HashMap::new(), handle }; + wait_set.register_rcl_primitives()?; Ok(wait_set) } @@ -68,10 +68,10 @@ impl WaitSet { return Err(RclrsError::AlreadyAddedToWaitSet); } let kind = entity.executable.kind(); - self.entities.entry(kind).or_default().push(entity); + self.primitives.entry(kind).or_default().push(entity); } self.resize_rcl_containers()?; - self.register_rcl_entities()?; + self.register_rcl_primitives()?; Ok(()) } @@ -80,13 +80,8 @@ impl WaitSet { /// This effectively resets the wait set to the state it was in after being created by /// [`WaitSet::new`]. pub fn clear(&mut self) { - self.entities.clear(); - // This cannot fail – the rcl_wait_set_clear function only checks that the input handle is - // valid, which it always is in our case. Hence, only debug_assert instead of returning - // Result. - // SAFETY: No preconditions for this function (besides passing in a valid wait set). - let ret = unsafe { rcl_wait_set_clear(&mut self.handle.rcl_wait_set) }; - debug_assert_eq!(ret, 0); + self.primitives.clear(); + self.rcl_clear(); } /// Blocks until the wait set is ready, or until the timeout has been exceeded. @@ -147,13 +142,13 @@ impl WaitSet { } // Remove any waitables that are no longer being used - for waiter in self.entities.values_mut() { - waiter.retain(|w| w.in_use()); + for waitable in self.primitives.values_mut() { + waitable.retain(|w| w.in_use()); } // For the remaining entities, check if they were activated and then run // the callback for those that were. - for waiter in self.entities.values_mut().flat_map(|v| v) { + for waiter in self.primitives.values_mut().flat_map(|v| v) { if waiter.is_ready(&self.handle.rcl_wait_set) { f(&mut *waiter.executable)?; } @@ -168,8 +163,11 @@ impl WaitSet { // Note that self.clear() will not change the allocated size of each rcl // entity container, so we do not need to resize before re-registering // the rcl entities. - self.clear(); - self.register_rcl_entities(); + self.rcl_clear(); + if let Err(err) = self.register_rcl_primitives() { + // TODO(@mxgrey): Log this error when logging is available + eprintln!("Error while registering rcl primitives: {err}"); + } Ok(()) } @@ -177,7 +175,7 @@ impl WaitSet { /// Get a count of the different kinds of entities in the wait set. pub fn count(&self) -> WaitableCount { let mut c = WaitableCount::new(); - for (kind, collection) in &self.entities { + for (kind, collection) in &self.primitives { c.add(*kind, collection.len()); } c @@ -191,6 +189,18 @@ impl WaitSet { Ok(()) } + /// Clear only the rcl_wait_set. This is done so that we can safely repopulate + /// it to perform another wait. This does not effect the entities that we + /// consider to still be in the wait set. + fn rcl_clear(&mut self) { + // This cannot fail – the rcl_wait_set_clear function only checks that the input handle is + // valid, which it always is in our case. Hence, only debug_assert instead of returning + // Result. + // SAFETY: No preconditions for this function (besides passing in a valid wait set). + let ret = unsafe { rcl_wait_set_clear(&mut self.handle.rcl_wait_set) }; + debug_assert_eq!(ret, 0); + } + /// Registers all the waitable entities with the rcl wait set. /// /// # Errors @@ -199,8 +209,8 @@ impl WaitSet { /// then there is a bug in rclrs. /// /// [1]: crate::RclReturnCode - fn register_rcl_entities(&mut self) -> Result<(), RclrsError> { - for entity in self.entities.values_mut().flat_map(|c| c) { + fn register_rcl_primitives(&mut self) -> Result<(), RclrsError> { + for entity in self.primitives.values_mut().flat_map(|c| c) { entity.add_to_wait_set(&mut self.handle.rcl_wait_set)?; } Ok(()) From 26b41f86c89190946d36f020e3b7218bea35e9b8 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 14 Oct 2024 19:30:04 +0800 Subject: [PATCH 19/48] Remove task modules that are no longer used Signed-off-by: Michael X. Grey --- rclrs/src/client/client_task.rs | 120 -------------------- rclrs/src/service/service_task.rs | 42 ------- rclrs/src/subscription/subscription_task.rs | 38 ------- 3 files changed, 200 deletions(-) delete mode 100644 rclrs/src/client/client_task.rs delete mode 100644 rclrs/src/service/service_task.rs delete mode 100644 rclrs/src/subscription/subscription_task.rs diff --git a/rclrs/src/client/client_task.rs b/rclrs/src/client/client_task.rs deleted file mode 100644 index ba811188a..000000000 --- a/rclrs/src/client/client_task.rs +++ /dev/null @@ -1,120 +0,0 @@ -use rosidl_runtime_rs::{Message, Service}; - -use futures::{ - channel::mpsc::UnboundedReceiver, - StreamExt, -}; - -use std::{ - sync::Arc, - collections::HashMap, -}; - -use std::io::Write; - -use crate::{ - error::ToResult, - rcl_bindings::*, - client::ClientHandle, - AnyClientOutputSender, ServiceInfo, RclrsError, RclReturnCode, -}; - -pub enum ClientAction { - TakeResponse, - NewRequest{ - sequence_number: SequenceNumber, - sender: AnyClientOutputSender, - }, -} - -pub(super) type SequenceNumber = i64; - -pub(super) async fn client_task( - mut receiver: UnboundedReceiver>, - handle: Arc, -) { - dbg!(); - std::io::stdout().lock().flush().unwrap(); - // This stores all active requests that have not received a response yet - let mut active_requests: HashMap> = HashMap::new(); - - // This holds responses that came in when no active request matched the - // sequence number. This could happen if the TakeResponse arrives before the - // NewRequest for the same sequence number. That is extremely unlikely to - // ever happen but is theoretically possible, so we should account for it. - let mut loose_responses: HashMap = HashMap::new(); - - while let Some(action) = receiver.next().await { - match action { - ClientAction::TakeResponse => { - dbg!(); - std::io::stdout().lock().flush().unwrap(); - match take_response::(&handle) { - Ok((response, info)) => { - let seq = info.request_id.sequence_number; - if let Some(sender) = active_requests.remove(&seq) { - dbg!(); - println!("Received response for {info:?}"); - // The active request is available, so send this response off - sender.send_response(response, info); - } else { - dbg!(); - println!("Received loose response for {info:?}"); - // Weirdly there isn't an active request for this, so save - // it in the loose responses map. - loose_responses.insert(seq, (response, info)); - } - } - Err(err) => { - match err { - RclrsError::RclError { code: RclReturnCode::ClientTakeFailed, .. } => { - // This is okay, it means a spurious wakeup happened - dbg!(); - println!("Spurious wakeup for client"); - } - err => { - dbg!(); - // TODO(@mxgrey): Log the error here once logging is available - eprintln!("Error while taking a response for a client: {err}"); - } - } - } - } - } - ClientAction::NewRequest { sequence_number, sender } => { - dbg!(); - std::io::stdout().lock().flush().unwrap(); - if let Some((response, info)) = loose_responses.remove(&sequence_number) { - // The response for this request already arrive, so we'll - // send it off immediately. - dbg!(); - sender.send_response(response, info); - } else { - dbg!(); - active_requests.insert(sequence_number, sender); - } - } - } - } -} - -fn take_response( - handle: &Arc, -) -> Result<(Response, rmw_service_info_t), RclrsError> { - let mut service_info_out = ServiceInfo::zero_initialized_rmw(); - let mut response_out = Response::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 Response::RmwMsg as *mut _, - ) - } - .ok() - .map(|_| ( - Response::from_rmw_message(response_out), - service_info_out, - )) -} diff --git a/rclrs/src/service/service_task.rs b/rclrs/src/service/service_task.rs deleted file mode 100644 index adadba01d..000000000 --- a/rclrs/src/service/service_task.rs +++ /dev/null @@ -1,42 +0,0 @@ -use rosidl_runtime_rs::Service; - -use crate::{ - service::ServiceHandle, - AnyServiceCallback, ExecutorCommands, -}; - -use futures::{ - channel::mpsc::UnboundedReceiver, - StreamExt, -}; - -use std::sync::Arc; - -pub(super) enum ServiceAction { - Execute, - SetCallback(AnyServiceCallback), -} - -pub(super) async fn service_task( - mut callback: AnyServiceCallback, - mut receiver: UnboundedReceiver>, - handle: Arc, - commands: Arc, -) { - dbg!(); - while let Some(action) = receiver.next().await { - match action { - ServiceAction::SetCallback(new_callback) => { - dbg!(); - callback = new_callback; - } - ServiceAction::Execute => { - dbg!(); - if let Err(err) = callback.execute(&handle, &commands) { - // TODO(@mxgrey): Log the error here once logging is implemented - eprintln!("Error while executing a service callback: {err}"); - } - } - } - } -} diff --git a/rclrs/src/subscription/subscription_task.rs b/rclrs/src/subscription/subscription_task.rs deleted file mode 100644 index 8e0210a93..000000000 --- a/rclrs/src/subscription/subscription_task.rs +++ /dev/null @@ -1,38 +0,0 @@ -use rosidl_runtime_rs::Message; - -use crate::{ - subscription::SubscriptionHandle, - AnySubscriptionCallback, ExecutorCommands, -}; - -use futures::{ - channel::mpsc::UnboundedReceiver, - StreamExt, -}; - -use std::sync::Arc; - -pub(super) enum SubscriptionAction { - Execute, - SetCallback(AnySubscriptionCallback), -} - -pub(super) async fn subscription_task( - mut callback: AnySubscriptionCallback, - mut receiver: UnboundedReceiver>, - handle: Arc, - commands: Arc, -) { - while let Some(action) = receiver.next().await { - match action { - SubscriptionAction::SetCallback(new_callback) => { - callback = new_callback; - } - SubscriptionAction::Execute => { - if let Err(_) = callback.execute(&handle, &commands) { - // TODO(@mxgrey): Log the error here once logging is implemented - } - } - } - } -} From 500b0831c86075f42b31d3a8175c0a14201c6633 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 14 Oct 2024 19:31:20 +0800 Subject: [PATCH 20/48] Rename wait module to wait_set Signed-off-by: Michael X. Grey --- rclrs/src/lib.rs | 4 ++-- rclrs/src/{wait.rs => wait_set.rs} | 0 rclrs/src/{wait => wait_set}/guard_condition.rs | 0 rclrs/src/{wait => wait_set}/waitable.rs | 0 4 files changed, 2 insertions(+), 2 deletions(-) rename rclrs/src/{wait.rs => wait_set.rs} (100%) rename rclrs/src/{wait => wait_set}/guard_condition.rs (100%) rename rclrs/src/{wait => wait_set}/waitable.rs (100%) diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index f41b61700..d1070a0fc 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -20,7 +20,7 @@ mod subscription; mod time; mod time_source; mod vendor; -mod wait; +mod wait_set; #[cfg(test)] mod test_helpers; @@ -45,4 +45,4 @@ pub use service::*; pub use subscription::*; pub use time::*; use time_source::*; -pub use wait::*; +pub use wait_set::*; diff --git a/rclrs/src/wait.rs b/rclrs/src/wait_set.rs similarity index 100% rename from rclrs/src/wait.rs rename to rclrs/src/wait_set.rs diff --git a/rclrs/src/wait/guard_condition.rs b/rclrs/src/wait_set/guard_condition.rs similarity index 100% rename from rclrs/src/wait/guard_condition.rs rename to rclrs/src/wait_set/guard_condition.rs diff --git a/rclrs/src/wait/waitable.rs b/rclrs/src/wait_set/waitable.rs similarity index 100% rename from rclrs/src/wait/waitable.rs rename to rclrs/src/wait_set/waitable.rs From ee15de5b39319b0387638aae608912f96e98036e Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 14 Oct 2024 19:33:03 +0800 Subject: [PATCH 21/48] Move the wait_set_runner to the wait_set module Signed-off-by: Michael X. Grey --- rclrs/src/executor.rs | 3 --- rclrs/src/wait_set.rs | 3 +++ rclrs/src/{executor => wait_set}/wait_set_runner.rs | 0 3 files changed, 3 insertions(+), 3 deletions(-) rename rclrs/src/{executor => wait_set}/wait_set_runner.rs (100%) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 9df942b4e..d87da633f 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,9 +1,6 @@ mod basic_executor; pub use self::basic_executor::*; -mod wait_set_runner; -pub use self::wait_set_runner::*; - use std::io::Write; use crate::{ diff --git a/rclrs/src/wait_set.rs b/rclrs/src/wait_set.rs index a1bdc8352..d70e311cd 100644 --- a/rclrs/src/wait_set.rs +++ b/rclrs/src/wait_set.rs @@ -29,6 +29,9 @@ pub use guard_condition::*; mod waitable; pub use waitable::*; +mod wait_set_runner; +pub use wait_set_runner::*; + /// A struct for waiting on subscriptions and other waitable entities to become ready. pub struct WaitSet { primitives: HashMap>, diff --git a/rclrs/src/executor/wait_set_runner.rs b/rclrs/src/wait_set/wait_set_runner.rs similarity index 100% rename from rclrs/src/executor/wait_set_runner.rs rename to rclrs/src/wait_set/wait_set_runner.rs From d395f5f8834fdfbcfdb5175d46057951a1ffc0c9 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 14 Oct 2024 19:39:11 +0800 Subject: [PATCH 22/48] Remove unnecessary debug outputs Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 8 --- rclrs/src/executor.rs | 13 +---- rclrs/src/executor/basic_executor.rs | 13 +---- rclrs/src/node.rs | 62 +++++++++-------------- rclrs/src/parameter/service.rs | 14 ----- rclrs/src/service/any_service_callback.rs | 26 ---------- 6 files changed, 28 insertions(+), 108 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index 45d0a649f..266958e6c 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -81,7 +81,6 @@ where } .ok()?; - println!("vvvvvvvvv Sent client request {sequence_number} vvvvvvvvvvvv"); // TODO(@mxgrey): Log errors here when logging becomes available. self.board.lock().unwrap().new_request(sequence_number, sender); @@ -317,13 +316,9 @@ where Ok((response, info)) => { let seq = info.request_id.sequence_number; if let Some(sender) = self.active_requests.remove(&seq) { - dbg!(); - println!("Received response for {info:?}"); // The active request is available, so send this response off sender.send_response(response, info); } else { - dbg!(); - println!("Received loose response for {info:?}"); // Weirdly there isn't an active request for this, so save // it in the loose responses map. self.loose_responses.insert(seq, (response, info)); @@ -333,11 +328,8 @@ where match err { RclrsError::RclError { code: RclReturnCode::ClientTakeFailed, .. } => { // This is okay, it means a spurious wakeup happened - dbg!(); - println!("Spurious wakeup for client"); } err => { - dbg!(); // TODO(@mxgrey): Log the error here once logging is available eprintln!("Error while taking a response for a client: {err}"); } diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index d87da633f..92b3d3dcd 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,8 +1,6 @@ mod basic_executor; pub use self::basic_executor::*; -use std::io::Write; - use crate::{ Node, NodeOptions, RclrsError, Context, ContextHandle, Waitable, GuardCondition, }; @@ -147,20 +145,13 @@ impl ExecutorCommands { let (mut sender, receiver) = oneshot::channel(); self.channel.add_async_task(Box::pin( async move { - dbg!(); - std::io::stdout().lock().flush().unwrap(); let cancellation = sender.cancellation(); let output = match select(cancellation, std::pin::pin!(f)).await { // The task was cancelled - // Either::Left(_) => return, + Either::Left(_) => return, // The task completed - // Either::Right((output, _)) => output, - - Either::Left(_) => { dbg!(); return; } - Either::Right((output, _)) => { dbg!(); output } + Either::Right((output, _)) => output, }; - dbg!(); - std::io::stdout().lock().flush().unwrap(); sender.send(output).ok(); } )); diff --git a/rclrs/src/executor/basic_executor.rs b/rclrs/src/executor/basic_executor.rs index 20bb19119..89480381b 100644 --- a/rclrs/src/executor/basic_executor.rs +++ b/rclrs/src/executor/basic_executor.rs @@ -19,8 +19,6 @@ use crate::{ Waitable, Context, WaitSetRunner, }; -use std::io::Write; - /// 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 /// @@ -79,9 +77,7 @@ impl ExecutorRuntime for BasicExecutorRuntime { wait_set_finished_clone.store(true, Ordering::Release); })); - let mut count = 0; while let Ok(task) = self.next_task(&wait_set_finished) { - dbg!(); // 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() { @@ -96,11 +92,6 @@ impl ExecutorRuntime for BasicExecutorRuntime { *future_slot = Some(future); } } - - // count += 1; - // if count > 20 { - // panic!("Done {count} iterations"); - // } } self.wait_set_runner = Some( @@ -181,8 +172,6 @@ impl BasicExecutorRuntime { ); } // TODO(@mxgrey): Log errors here when logging becomes available. - dbg!(); - std::io::stdout().lock().flush().unwrap(); guard_condition.trigger().ok(); sender.send(()).ok(); })); @@ -202,7 +191,7 @@ impl BasicExecutorRuntime { } } -pub struct BasicExecutorChannel { +struct BasicExecutorChannel { task_sender: TaskSender, waitable_sender: UnboundedSender, } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 5648dafb5..4e7e04a73 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -21,7 +21,7 @@ use futures::{ channel::mpsc::{unbounded, UnboundedSender}, }; -// use async_std::future::timeout; +use async_std::future::timeout; use rosidl_runtime_rs::Message; @@ -33,10 +33,6 @@ use crate::{ ServiceAsyncCallback, ExecutorCommands, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; - -use std::io::Write; - - /// 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] @@ -453,46 +449,38 @@ impl Node { ) -> Promise<()> { let (listener, mut on_graph_change_receiver) = unbounded(); let promise = self.commands.query(async move { - dbg!(); - std::io::stdout().lock().flush().unwrap(); 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; - // } - // } - - match on_graph_change_receiver.next().await { - Some(_) => { - dbg!(); - std::io::stdout().lock().flush().unwrap(); + 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() { - // Condition is met - dbg!(); - std::io::stdout().lock().flush().unwrap(); return; } } - None => { - dbg!(); - std::io::stdout().lock().flush().unwrap(); - // Graph change sender is closed + 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; } } + + // match on_graph_change_receiver.next().await { + // Some(_) => { + // if condition() { + // // Condition is met + // return; + // } + // } + // None => { + // // Graph change sender is closed + // return; + // } + // } } }); diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 6bad4ff68..535e65b9a 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -327,8 +327,6 @@ mod tests { time::Duration, }; - use std::io::Write; - struct TestNode { node: Arc, bool_param: MandatoryParameter, @@ -454,7 +452,6 @@ mod tests { #[test] fn test_list_parameters_service() -> Result<(), RclrsError> { - dbg!(); let context = Context::default(); let (mut executor, _test, client_node) = construct_test_nodes(&context, "list"); let list_client = client_node.create_client::( @@ -462,8 +459,6 @@ mod tests { QoSProfile::services_default(), )?; - dbg!(); - std::io::stdout().lock().flush().unwrap(); // return Ok(()); executor.spin( SpinOptions::default() @@ -471,8 +466,6 @@ mod tests { .timeout(Duration::from_secs(2)) ); - dbg!(); - std::io::stdout().lock().flush().unwrap(); // List all parameters let callback_ran = Arc::new(AtomicBool::new(false)); let callback_ran_inner = Arc::clone(&callback_ran); @@ -484,8 +477,6 @@ mod tests { .call_then( &request, move |response: ListParameters_Response| { - dbg!(); - std::io::stdout().lock().flush().unwrap(); // use_sim_time + all the manually defined ones let names = response.result.names; assert_eq!(names.len(), 5); @@ -503,16 +494,12 @@ mod tests { ) .unwrap(); - dbg!(); - std::io::stdout().lock().flush().unwrap(); executor.spin( SpinOptions::default() .until_promise_resolved(promise) .timeout(Duration::from_secs(5)) ); assert!(callback_ran.load(Ordering::Acquire)); - dbg!(); - std::io::stdout().lock().flush().unwrap(); // Limit depth, namespaced parameter is not returned let callback_ran = Arc::new(AtomicBool::new(false)); @@ -579,7 +566,6 @@ mod tests { &request, move |response: ListParameters_Response| { 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"); diff --git a/rclrs/src/service/any_service_callback.rs b/rclrs/src/service/any_service_callback.rs index 1682b703f..988c9c372 100644 --- a/rclrs/src/service/any_service_callback.rs +++ b/rclrs/src/service/any_service_callback.rs @@ -34,46 +34,31 @@ impl AnyServiceCallback { commands: &Arc, ) -> Result<(), RclrsError> { let mut evaluate = || { - dbg!(); match self { AnyServiceCallback::OnlyRequest(cb) => { - dbg!(); let (msg, mut rmw_request_id) = Self::take_request(handle)?; let handle = Arc::clone(&handle); let response = cb(msg); - dbg!(); let _ = commands.run(async move { - // TODO(@mxgrey): Log any errors here when logging is available - dbg!(); - println!("Sending service response: {rmw_request_id:?}"); if let Err(err) = Self::send_response(&handle, &mut rmw_request_id, response.await) { // TODO(@mxgrey): Use logging instead when it becomes available eprintln!("Error while sending service response for {rmw_request_id:?}: {err}"); - } else { - println!("No error while sending {rmw_request_id:?}"); } }); } AnyServiceCallback::WithId(cb) => { - dbg!(); let (msg, mut rmw_request_id) = Self::take_request(handle)?; let request_id = RequestId::from_rmw_request_id(&rmw_request_id); let handle = Arc::clone(&handle); let response = cb(msg, request_id); - dbg!(); let _ = commands.run(async move { - dbg!(); - println!("Sending service response: {rmw_request_id:?}"); if let Err(err) = Self::send_response(&handle, &mut rmw_request_id, response.await) { // TODO(@mxgrey): Use logging instead when it becomes available eprintln!("Error while sending service response for {rmw_request_id:?}: {err}"); - } else { - println!("No error while sending {rmw_request_id:?}"); } }); } AnyServiceCallback::WithInfo(cb) => { - dbg!(); let (msg, rmw_service_info) = Self::take_request_with_info(handle)?; let mut rmw_request_id = rmw_request_id_t { writer_guid: rmw_service_info.request_id.writer_guid, @@ -82,16 +67,10 @@ impl AnyServiceCallback { let service_info = ServiceInfo::from_rmw_service_info(&rmw_service_info); let handle = Arc::clone(&handle); let response = cb(msg, service_info); - dbg!(); let _ = commands.run(async move { - // TODO(@mxgrey): Log any errors here when logging is available - dbg!(); - println!("Sending service response: {rmw_request_id:?}"); if let Err(err) = Self::send_response(&handle, &mut rmw_request_id, response.await) { // TODO(@mxgrey): Use logging instead when it becomes available eprintln!("Error while sending service response for {rmw_request_id:?}: {err}"); - } else { - println!("No error while sending {rmw_request_id:?}"); } }); } @@ -100,7 +79,6 @@ impl AnyServiceCallback { Ok(()) }; - dbg!(); match evaluate() { Err(RclrsError::RclError { code: RclReturnCode::ServiceTakeFailed, @@ -108,8 +86,6 @@ impl AnyServiceCallback { }) => { // Spurious wakeup - this may happen even when a waitlist indicated that this // subscription was ready, so it shouldn't be an error. - dbg!(); - println!("Spurious wakeup for service request"); Ok(()) } other => other, @@ -152,7 +128,6 @@ impl AnyServiceCallback { ) } .ok()?; - println!("^^^^^^^^^^ service request arrived: {request_id_out:?} ^^^^^^^^^^^^^^"); Ok((T::Request::from_rmw_message(request_out), request_id_out)) } @@ -171,7 +146,6 @@ impl AnyServiceCallback { ) } .ok()?; - println!("^^^^^^^^^^^^ service request arrived: {service_info_out:?} ^^^^^^^^^^^^^"); Ok((T::Request::from_rmw_message(request_out), service_info_out)) } From 5c5959216ce8da9b3b2e8c256d8014430ed52722 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 14 Oct 2024 19:57:59 +0800 Subject: [PATCH 23/48] Big cleanup Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 14 +- rclrs/src/client/client_async_callback.rs | 3 + rclrs/src/client/client_callback.rs | 1 + rclrs/src/client/client_output.rs | 17 +- rclrs/src/executor/basic_executor.rs | 4 +- rclrs/src/service.rs | 13 +- rclrs/src/subscription.rs | 15 +- .../subscription/any_subscription_callback.rs | 12 +- rclrs/src/wait_set.rs | 11 +- rclrs/src/wait_set/guard_condition.rs | 27 ++- rclrs/src/wait_set/rcl_primitive.rs | 51 ++++ rclrs/src/wait_set/waitable.rs | 229 ++++++++---------- 12 files changed, 228 insertions(+), 169 deletions(-) create mode 100644 rclrs/src/wait_set/rcl_primitive.rs diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index 266958e6c..33189905c 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -10,8 +10,8 @@ use crate::{ error::ToResult, rcl_bindings::*, MessageCow, Node, RclrsError, RclReturnCode, Promise, ENTITY_LIFECYCLE_MUTEX, - Executable, QoSProfile, Waitable, WaitableLifecycle, - ExecutableHandle, ExecutableKind, ServiceInfo, + RclPrimitive, QoSProfile, Waitable, WaitableLifecycle, + RclPrimitiveHandle, RclPrimitiveKind, ServiceInfo, }; mod client_async_callback; @@ -251,7 +251,7 @@ where board: Arc>> } -impl Executable for ClientExecutable +impl RclPrimitive for ClientExecutable where T: rosidl_runtime_rs::Service, { @@ -259,12 +259,12 @@ where self.board.lock().unwrap().execute(&self.handle) } - fn handle(&self) -> ExecutableHandle { - ExecutableHandle::Client(self.handle.lock()) + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::Client(self.handle.lock()) } - fn kind(&self) -> ExecutableKind { - ExecutableKind::Client + fn kind(&self) -> RclPrimitiveKind { + RclPrimitiveKind::Client } } diff --git a/rclrs/src/client/client_async_callback.rs b/rclrs/src/client/client_async_callback.rs index b97c8472d..e01765f28 100644 --- a/rclrs/src/client/client_async_callback.rs +++ b/rclrs/src/client/client_async_callback.rs @@ -13,7 +13,10 @@ 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; } diff --git a/rclrs/src/client/client_callback.rs b/rclrs/src/client/client_callback.rs index d34fb6980..037075677 100644 --- a/rclrs/src/client/client_callback.rs +++ b/rclrs/src/client/client_callback.rs @@ -11,6 +11,7 @@ pub trait ClientCallback: Send + 'static where T: Service, { + /// Trigger the callback to run fn run_client_callback(self, response: T::Response, info: ServiceInfo); } diff --git a/rclrs/src/client/client_output.rs b/rclrs/src/client/client_output.rs index e0f7b8c44..3309c1944 100644 --- a/rclrs/src/client/client_output.rs +++ b/rclrs/src/client/client_output.rs @@ -13,13 +13,15 @@ use crate::{ /// /// 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::RequestOnly(sender), receiver) + (AnyClientOutputSender::ResponseOnly(sender), receiver) } } @@ -39,8 +41,11 @@ impl ClientOutput for (Response, ServiceInfo) { /// Can send any kind of response for a client call. pub enum AnyClientOutputSender { - RequestOnly(Sender), + /// 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)>), } @@ -51,17 +56,17 @@ impl AnyClientOutputSender { service_info: rmw_service_info_t, ) { match self { - Self::RequestOnly(sender) => { - sender.send(response); + Self::ResponseOnly(sender) => { + let _ = sender.send(response); } Self::WithId(sender) => { - sender.send(( + let _ = sender.send(( response, RequestId::from_rmw_request_id(&service_info.request_id), )); } Self::WithServiceInfo(sender) => { - sender.send(( + let _ = sender.send(( response, ServiceInfo::from_rmw_service_info(&service_info), )); diff --git a/rclrs/src/executor/basic_executor.rs b/rclrs/src/executor/basic_executor.rs index 89480381b..87b4530d0 100644 --- a/rclrs/src/executor/basic_executor.rs +++ b/rclrs/src/executor/basic_executor.rs @@ -1,17 +1,15 @@ use futures::{ - future::{BoxFuture, FutureExt}, + future::BoxFuture, task::{waker_ref, ArcWake}, channel::{oneshot, mpsc::UnboundedSender}, }; use std::{ - future::Future, sync::{ mpsc::{Sender, Receiver, channel}, atomic::{AtomicBool, Ordering}, Arc, Mutex, }, task::Context as TaskContext, - time::Duration, }; use crate::{ diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index d3e8c855c..aa990336e 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -8,7 +8,7 @@ use crate::{ error::ToResult, rcl_bindings::*, NodeHandle, RclrsError, Waitable, WaitableLifecycle, QoSProfile, - Executable, ExecutableKind, ExecutableHandle, ENTITY_LIFECYCLE_MUTEX, ExecutorCommands, + RclPrimitive, RclPrimitiveKind, RclPrimitiveHandle, ENTITY_LIFECYCLE_MUTEX, ExecutorCommands, }; mod any_service_callback; @@ -44,6 +44,7 @@ where callback: Arc>>, /// Holding onto this keeps the waiter for this service alive in the wait /// set of the executor. + #[allow(unused)] lifecycle: WaitableLifecycle, } @@ -162,7 +163,7 @@ struct ServiceExecutable { commands: Arc, } -impl Executable for ServiceExecutable +impl RclPrimitive for ServiceExecutable where T: rosidl_runtime_rs::Service, { @@ -174,12 +175,12 @@ where Ok(()) } - fn kind(&self) -> crate::ExecutableKind { - ExecutableKind::Service + fn kind(&self) -> crate::RclPrimitiveKind { + RclPrimitiveKind::Service } - fn handle(&self) -> ExecutableHandle { - ExecutableHandle::Service(self.handle.lock()) + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::Service(self.handle.lock()) } } diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 2c1e8c0ce..ce501ff44 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -9,8 +9,8 @@ use crate::{ error::ToResult, qos::QoSProfile, rcl_bindings::*, - ExecutorCommands, NodeHandle, RclrsError, Waitable, Executable, ExecutableHandle, - ExecutableKind, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, + ExecutorCommands, NodeHandle, RclrsError, Waitable, RclPrimitive, RclPrimitiveHandle, + RclPrimitiveKind, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, }; mod any_subscription_callback; @@ -57,6 +57,7 @@ where callback: Arc>>, /// Holding onto this keeps the waiter for this subscription alive in the /// wait set of the executor. + #[allow(unused)] lifecycle: WaitableLifecycle, } @@ -172,7 +173,7 @@ struct SubscriptionExecutable { commands: Arc, } -impl Executable for SubscriptionExecutable +impl RclPrimitive for SubscriptionExecutable where T: Message, { @@ -180,12 +181,12 @@ where self.callback.lock().unwrap().execute(&self.handle, &self.commands) } - fn kind(&self) -> crate::ExecutableKind { - ExecutableKind::Subscription + fn kind(&self) -> crate::RclPrimitiveKind { + RclPrimitiveKind::Subscription } - fn handle(&self) -> ExecutableHandle { - ExecutableHandle::Subscription(self.handle.lock()) + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::Subscription(self.handle.lock()) } } diff --git a/rclrs/src/subscription/any_subscription_callback.rs b/rclrs/src/subscription/any_subscription_callback.rs index 437070912..9a6c97502 100644 --- a/rclrs/src/subscription/any_subscription_callback.rs +++ b/rclrs/src/subscription/any_subscription_callback.rs @@ -50,27 +50,27 @@ impl AnySubscriptionCallback { match self { AnySubscriptionCallback::Regular(cb) => { let (msg, _) = Self::take(handle)?; - commands.run(cb(msg)); + let _ = commands.run(cb(msg)); } AnySubscriptionCallback::RegularWithMessageInfo(cb) => { let (msg, msg_info) = Self::take(handle)?; - commands.run(cb(msg, msg_info)); + let _ = commands.run(cb(msg, msg_info)); } AnySubscriptionCallback::Boxed(cb) => { let (msg, _) = Self::take_boxed(handle)?; - commands.run(cb(msg)); + let _ = commands.run(cb(msg)); } AnySubscriptionCallback::BoxedWithMessageInfo(cb) => { let (msg, msg_info) = Self::take_boxed(handle)?; - commands.run(cb(msg, msg_info)); + let _ = commands.run(cb(msg, msg_info)); } AnySubscriptionCallback::Loaned(cb) => { let (msg, _) = Self::take_loaned(handle)?; - commands.run(cb(msg)); + let _ = commands.run(cb(msg)); } AnySubscriptionCallback::LoanedWithMessageInfo(cb) => { let (msg, msg_info) = Self::take_loaned(handle)?; - commands.run(cb(msg, msg_info)); + let _ = commands.run(cb(msg, msg_info)); } } Ok(()) diff --git a/rclrs/src/wait_set.rs b/rclrs/src/wait_set.rs index d70e311cd..fba917b26 100644 --- a/rclrs/src/wait_set.rs +++ b/rclrs/src/wait_set.rs @@ -26,6 +26,9 @@ use crate::{ mod guard_condition; pub use guard_condition::*; +mod rcl_primitive; +pub use rcl_primitive::*; + mod waitable; pub use waitable::*; @@ -34,7 +37,7 @@ pub use wait_set_runner::*; /// A struct for waiting on subscriptions and other waitable entities to become ready. pub struct WaitSet { - primitives: HashMap>, + primitives: HashMap>, handle: WaitSetHandle, } @@ -70,7 +73,7 @@ impl WaitSet { if entity.in_wait_set() { return Err(RclrsError::AlreadyAddedToWaitSet); } - let kind = entity.executable.kind(); + let kind = entity.primitive.kind(); self.primitives.entry(kind).or_default().push(entity); } self.resize_rcl_containers()?; @@ -116,7 +119,7 @@ impl WaitSet { pub fn wait( &mut self, timeout: Option, - mut f: impl FnMut(&mut dyn Executable) -> Result<(), RclrsError>, + mut f: impl FnMut(&mut dyn RclPrimitive) -> Result<(), RclrsError>, ) -> Result<(), RclrsError> { let timeout_ns = match timeout.map(|d| d.as_nanos()) { None => -1, @@ -153,7 +156,7 @@ impl WaitSet { // the callback for those that were. for waiter in self.primitives.values_mut().flat_map(|v| v) { if waiter.is_ready(&self.handle.rcl_wait_set) { - f(&mut *waiter.executable)?; + f(&mut *waiter.primitive)?; } } diff --git a/rclrs/src/wait_set/guard_condition.rs b/rclrs/src/wait_set/guard_condition.rs index 6c887c9ab..a2dbc2154 100644 --- a/rclrs/src/wait_set/guard_condition.rs +++ b/rclrs/src/wait_set/guard_condition.rs @@ -5,8 +5,8 @@ use std::{ use crate::{ rcl_bindings::*, - ContextHandle, RclrsError, ToResult, WaitableLifecycle, Executable, - Waitable, ExecutableKind, ExecutableHandle, + ContextHandle, RclrsError, ToResult, WaitableLifecycle, RclPrimitive, + Waitable, RclPrimitiveKind, RclPrimitiveHandle, }; /// A waitable entity used for waking up a wait set manually. @@ -135,14 +135,25 @@ struct GuardConditionHandle { /// from rcl and either have static lifetimes or lifetimes that depend on /// something else. pub enum InnerGuardConditionHandle { + /// This variant means the guard condition was created and owned by rclrs. + /// Its memory is managed by us. Owned(rcl_guard_condition_t), + /// This variant means the guard condition was created and owned by rcl. + /// The owner object represents something that the lifecycle of the guard + /// condition depends on, such as the rcl_node that created it. Unowned { + /// This is the unowned guard condition pointer. We must not deallocate + /// it. handle: *const rcl_guard_condition_t, + /// This somehow holds a shared reference to the owner of the guard + /// condition. We need to hold onto this to ensure the guard condition + /// remains valid. owner: Box, }, } impl InnerGuardConditionHandle { + /// Get the handle if it is owned by rclrs pub fn owned(&self) -> Option<&rcl_guard_condition_t> { match self { Self::Owned(handle) => Some(handle), @@ -150,6 +161,7 @@ impl InnerGuardConditionHandle { } } + /// Get the handle if it is owned by rclrs pub fn owned_mut(&mut self) -> Option<&mut rcl_guard_condition_t> { match self { Self::Owned(handle) => Some(handle), @@ -157,6 +169,7 @@ impl InnerGuardConditionHandle { } } + /// Apply a function to the handle pub fn use_handle(&self, f: impl FnOnce(&rcl_guard_condition_t) -> Out) -> Out { match self { Self::Owned(handle) => f(handle), @@ -191,7 +204,7 @@ struct GuardConditionExecutable { callback: Option>, } -impl Executable for GuardConditionExecutable { +impl RclPrimitive for GuardConditionExecutable { fn execute(&mut self) -> Result<(), RclrsError> { if let Some(callback) = &mut self.callback { callback(); @@ -199,12 +212,12 @@ impl Executable for GuardConditionExecutable { Ok(()) } - fn kind(&self) -> ExecutableKind { - ExecutableKind::GuardCondition + fn kind(&self) -> RclPrimitiveKind { + RclPrimitiveKind::GuardCondition } - fn handle(&self) -> ExecutableHandle { - ExecutableHandle::GuardCondition( + fn handle(&self) -> RclPrimitiveHandle { + RclPrimitiveHandle::GuardCondition( self.handle.rcl_guard_condition.lock().unwrap() ) } diff --git a/rclrs/src/wait_set/rcl_primitive.rs b/rclrs/src/wait_set/rcl_primitive.rs new file mode 100644 index 000000000..3fbb172d0 --- /dev/null +++ b/rclrs/src/wait_set/rcl_primitive.rs @@ -0,0 +1,51 @@ +use std::sync::MutexGuard; + +use crate::{ + rcl_bindings::*, + RclrsError, InnerGuardConditionHandle, +}; + +/// This provides the public API for executing a waitable item. +pub trait RclPrimitive: Send + Sync { + /// Trigger this primitive to run. + fn execute(&mut self) -> Result<(), RclrsError>; + + /// Indicate what kind of primitive this is. + fn kind(&self) -> RclPrimitiveKind; + + /// Provide the handle for this primitive + fn handle(&self) -> RclPrimitiveHandle; +} + +/// Enum to describe the kind of an executable. +#[derive(Clone, Copy, Hash, PartialEq, Eq, PartialOrd, Ord)] +pub enum RclPrimitiveKind { + /// Subscription + Subscription, + /// Guard Condition + GuardCondition, + /// Timer + Timer, + /// Client + Client, + /// Service + Service, + /// Event + Event, +} + +/// Used by the wait set to obtain the handle of a primitive. +pub enum RclPrimitiveHandle<'a> { + /// Handle for a subscription + Subscription(MutexGuard<'a, rcl_subscription_t>), + /// Handle for a guard condition + GuardCondition(MutexGuard<'a, InnerGuardConditionHandle>), + /// Handle for a timer + Timer(MutexGuard<'a, rcl_timer_t>), + /// Handle for a client + Client(MutexGuard<'a, rcl_client_t>), + /// Handle for a service + Service(MutexGuard<'a, rcl_service_t>), + /// Handle for an event + Event(MutexGuard<'a, rcl_event_t>), +} diff --git a/rclrs/src/wait_set/waitable.rs b/rclrs/src/wait_set/waitable.rs index f7ba71219..a216a5f7e 100644 --- a/rclrs/src/wait_set/waitable.rs +++ b/rclrs/src/wait_set/waitable.rs @@ -1,132 +1,34 @@ use std::sync::{ atomic::{AtomicBool, Ordering}, - Arc, MutexGuard, + Arc, }; use crate::{ error::ToResult, rcl_bindings::*, - RclrsError, GuardCondition, InnerGuardConditionHandle, + RclrsError, GuardCondition, RclPrimitiveKind, RclPrimitive, RclPrimitiveHandle, }; -/// Enum to describe the kind of an executable. -#[derive(Clone, Copy, Hash, PartialEq, Eq, PartialOrd, Ord)] -pub enum ExecutableKind { - Subscription, - GuardCondition, - Timer, - Client, - Service, - Event, -} - -/// Used by the wait set to obtain the handle of an executable. -pub enum ExecutableHandle<'a> { - Subscription(MutexGuard<'a, rcl_subscription_t>), - GuardCondition(MutexGuard<'a, InnerGuardConditionHandle>), - Timer(MutexGuard<'a, rcl_timer_t>), - Client(MutexGuard<'a, rcl_client_t>), - Service(MutexGuard<'a, rcl_service_t>), - Event(MutexGuard<'a, rcl_event_t>), -} - -#[derive(Default, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] -pub struct WaitableCount { - pub subscriptions: usize, - pub guard_conditions: usize, - pub timers: usize, - pub clients: usize, - pub services: usize, - pub events: usize, -} - -impl WaitableCount { - pub fn new() -> Self { - Self::default() - } - - pub(super) fn add(&mut self, kind: ExecutableKind, count: usize) { - match kind { - ExecutableKind::Subscription => self.subscriptions += count, - ExecutableKind::GuardCondition => self.guard_conditions += count, - ExecutableKind::Timer => self.timers += count, - ExecutableKind::Client => self.clients += count, - ExecutableKind::Service => self.services += count, - ExecutableKind::Event => self.events += count, - } - } - - pub(super) unsafe fn initialize( - &self, - rcl_context: &mut rcl_context_s, - ) -> Result { - unsafe { - // SAFETY: Getting a zero-initialized value is always safe - let mut rcl_wait_set = rcl_get_zero_initialized_wait_set(); - // SAFETY: We're passing in a zero-initialized wait set and a valid context. - // There are no other preconditions. - rcl_wait_set_init( - &mut rcl_wait_set, - self.subscriptions, - self.guard_conditions, - self.timers, - self.clients, - self.services, - self.events, - &mut *rcl_context, - rcutils_get_default_allocator(), - ) - .ok()?; - Ok(rcl_wait_set) - } - } - - pub(super) unsafe fn resize( - &self, - rcl_wait_set: &mut rcl_wait_set_t, - ) -> Result<(), RclrsError> { - unsafe { - rcl_wait_set_resize( - rcl_wait_set, - self.subscriptions, - self.guard_conditions, - self.timers, - self.clients, - self.services, - self.events, - ) - } - .ok() - } -} - -/// This provides the public API for executing a waitable item. -pub trait Executable { - /// Trigger this executable to run. - fn execute(&mut self) -> Result<(), RclrsError>; - - /// Indicate what kind of executable this is. - fn kind(&self) -> ExecutableKind; - - /// Provide the handle for this executable - fn handle(&self) -> ExecutableHandle; -} - +/// This struct manages the presence of an rcl primitive inside the wait set. +/// It will keep track of where the primitive is within the wait set as well as +/// automatically remove the primitive from the wait set once it isn't being +/// used anymore. #[must_use = "If you do not give the Waiter to a WaitSet then it will never be useful"] pub struct Waitable { - pub(super) executable: Box, + pub(super) primitive: Box, in_use: Arc, index_in_wait_set: Option, } impl Waitable { + /// Create a new waitable. pub fn new( - waitable: Box, + primitive: Box, guard_condition: Option>, ) -> (Self, WaitableLifecycle) { let in_use = Arc::new(AtomicBool::new(true)); let waiter = Self { - executable: waitable, + primitive, in_use: Arc::clone(&in_use), index_in_wait_set: None, }; @@ -149,13 +51,13 @@ impl Waitable { // SAFETY: Each field in the wait set is an array of points. // The dereferencing that we do is equivalent to obtaining the // element of the array at the index-th position. - match self.executable.kind() { - ExecutableKind::Subscription => wait_set.subscriptions.add(index).is_null(), - ExecutableKind::GuardCondition => wait_set.guard_conditions.add(index).is_null(), - ExecutableKind::Service => wait_set.services.add(index).is_null(), - ExecutableKind::Client => wait_set.clients.add(index).is_null(), - ExecutableKind::Timer => wait_set.timers.add(index).is_null(), - ExecutableKind::Event => wait_set.events.add(index).is_null(), + match self.primitive.kind() { + RclPrimitiveKind::Subscription => wait_set.subscriptions.add(index).is_null(), + RclPrimitiveKind::GuardCondition => wait_set.guard_conditions.add(index).is_null(), + RclPrimitiveKind::Service => wait_set.services.add(index).is_null(), + RclPrimitiveKind::Client => wait_set.clients.add(index).is_null(), + RclPrimitiveKind::Timer => wait_set.timers.add(index).is_null(), + RclPrimitiveKind::Event => wait_set.events.add(index).is_null(), } }; !ptr_is_null @@ -171,25 +73,25 @@ impl Waitable { unsafe { // SAFETY: The Executable is responsible for maintaining the lifecycle // of the handle, so it is guaranteed to be valid here. - match self.executable.handle() { - ExecutableHandle::Subscription(handle) => { + match self.primitive.handle() { + RclPrimitiveHandle::Subscription(handle) => { rcl_wait_set_add_subscription(wait_set, &*handle, &mut index) } - ExecutableHandle::GuardCondition(handle) => { + RclPrimitiveHandle::GuardCondition(handle) => { handle.use_handle(|handle| { rcl_wait_set_add_guard_condition(wait_set, &*handle, &mut index) }) } - ExecutableHandle::Service(handle) => { + RclPrimitiveHandle::Service(handle) => { rcl_wait_set_add_service(wait_set, &*handle, &mut index) } - ExecutableHandle::Client(handle) => { + RclPrimitiveHandle::Client(handle) => { rcl_wait_set_add_client(wait_set, &*handle, &mut index) } - ExecutableHandle::Timer(handle) => { + RclPrimitiveHandle::Timer(handle) => { rcl_wait_set_add_timer(wait_set, &*handle, &mut index) } - ExecutableHandle::Event(handle) => { + RclPrimitiveHandle::Event(handle) => { rcl_wait_set_add_event(wait_set, &*handle, &mut index) } } @@ -201,6 +103,9 @@ impl Waitable { } } +/// This is used internally to track whether an rcl primitive is still being +/// used. When this gets dropped, the rcl primitive will automatically be +/// removed from the wait set. #[must_use = "If you do not hold onto the WaiterLifecycle, then its Waiter will be immediately dropped"] pub struct WaitableLifecycle { in_use: Arc, @@ -211,7 +116,85 @@ impl Drop for WaitableLifecycle { fn drop(&mut self) { self.in_use.store(false, Ordering::Release); if let Some(guard_condition) = &self.guard_condition { - guard_condition.trigger(); + guard_condition.trigger().ok(); + } + } +} + +/// Count the number of rcl primitives in the wait set. +#[derive(Default, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] +pub struct WaitableCount { + /// How many subscriptions are present + pub subscriptions: usize, + /// How many guard conditions are present + pub guard_conditions: usize, + /// How many timers are present + pub timers: usize, + /// How many clients are present + pub clients: usize, + /// How many services are present + pub services: usize, + /// How many events are present + pub events: usize, +} + +impl WaitableCount { + /// Begin a new count with everything starting out at zero. + pub fn new() -> Self { + Self::default() + } + + pub(super) fn add(&mut self, kind: RclPrimitiveKind, count: usize) { + match kind { + RclPrimitiveKind::Subscription => self.subscriptions += count, + RclPrimitiveKind::GuardCondition => self.guard_conditions += count, + RclPrimitiveKind::Timer => self.timers += count, + RclPrimitiveKind::Client => self.clients += count, + RclPrimitiveKind::Service => self.services += count, + RclPrimitiveKind::Event => self.events += count, } } + + pub(super) unsafe fn initialize( + &self, + rcl_context: &mut rcl_context_s, + ) -> Result { + unsafe { + // SAFETY: Getting a zero-initialized value is always safe + let mut rcl_wait_set = rcl_get_zero_initialized_wait_set(); + // SAFETY: We're passing in a zero-initialized wait set and a valid context. + // There are no other preconditions. + rcl_wait_set_init( + &mut rcl_wait_set, + self.subscriptions, + self.guard_conditions, + self.timers, + self.clients, + self.services, + self.events, + &mut *rcl_context, + rcutils_get_default_allocator(), + ) + .ok()?; + Ok(rcl_wait_set) + } + } + + pub(super) unsafe fn resize( + &self, + rcl_wait_set: &mut rcl_wait_set_t, + ) -> Result<(), RclrsError> { + unsafe { + rcl_wait_set_resize( + rcl_wait_set, + self.subscriptions, + self.guard_conditions, + self.timers, + self.clients, + self.services, + self.events, + ) + } + .ok() + } } From 0138e3386bf3e5d4abfe63335d54ea76cb2fa3db Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 14 Oct 2024 20:27:04 +0800 Subject: [PATCH 24/48] Update doctests Signed-off-by: Michael X. Grey --- rclrs/src/context.rs | 2 +- rclrs/src/node.rs | 48 +++++------ .../src/node/{options.rs => node_options.rs} | 80 +++++++++---------- 3 files changed, 64 insertions(+), 66 deletions(-) rename rclrs/src/node/{options.rs => node_options.rs} (87%) diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 1b4d4f5d3..fcca0e789 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -104,7 +104,7 @@ impl Context { /// /// # Example /// ``` - /// use rclrs::{Context, InitOptions}; + /// # use rclrs::{Context, InitOptions}; /// let context = Context::new_with_options([], InitOptions::new().with_domain_id(Some(5))).unwrap(); /// assert_eq!(context.domain_id(), 5); /// ```` diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 4e7e04a73..fcc3a057c 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,5 +1,5 @@ -mod options; -pub use options::*; +mod node_options; +pub use node_options::*; mod graph; pub use graph::*; @@ -128,13 +128,13 @@ impl Node { /// ``` /// # use rclrs::{Context, 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)?.create_basic_executor(); + /// let node_r = executor_r.create_node("my_node")?; /// assert_eq!(node_r.name(), "your_node"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -149,18 +149,18 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError}; + /// # use rclrs::{Context, RclrsError, NodeOptions}; /// // 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( + /// NodeOptions::new("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)?.create_basic_executor(); + /// let node_r = executor_r.create_node("my_node")?; /// assert_eq!(node_r.namespace(), "/your_namespace"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -175,12 +175,12 @@ impl Node { /// /// # 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, NodeOptions}; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node( + /// NodeOptions::new("my_node") + /// .namespace("/my/namespace") + /// )?; /// assert_eq!(node.fully_qualified_name(), "/my/namespace/my_node"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -353,8 +353,8 @@ impl Node { /// # 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>(()) @@ -382,8 +382,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), diff --git a/rclrs/src/node/options.rs b/rclrs/src/node/node_options.rs similarity index 87% rename from rclrs/src/node/options.rs rename to rclrs/src/node/node_options.rs index a93eecb3e..8d391f84f 100644 --- a/rclrs/src/node/options.rs +++ b/rclrs/src/node/node_options.rs @@ -28,19 +28,19 @@ use crate::{ /// /// # Example /// ``` -/// # use rclrs::{Context, NodeBuilder, Node, RclrsError}; -/// let context = Context::new([])?; +/// # use rclrs::{Context, NodeOptions, Node, RclrsError}; +/// let executor = Context::default().create_basic_executor(); /// // Building a node in a single expression -/// let node = NodeBuilder::new(&context, "foo_node").namespace("/bar").build()?; +/// let node = executor.create_node(NodeOptions::new("foo_node").namespace("/bar"))?; /// 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()?; +/// // Building a node via NodeOptions +/// let node = executor.create_node(NodeOptions::new("bar_node"))?; /// 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()?; +/// let mut options = NodeOptions::new("goose"); +/// options = options.namespace("/duck/duck"); +/// let node = executor.create_node(options)?; /// assert_eq!(node.fully_qualified_name(), "/duck/duck/goose"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -75,17 +75,15 @@ impl NodeOptions { /// /// # Example /// ``` - /// # use rclrs::{Context, NodeOptions}; - /// let context = Context::new(); + /// # use rclrs::{Context, NodeOptions, RclrsError, RclReturnCode}; + /// let executor = Context::default().create_basic_executor(); /// // This is a valid node name - /// assert!(NodeOptions::new("my_node").is_valid()); + /// assert!(executor.create_node(NodeOptions::new("my_node")).is_ok()); /// // This is another valid node name (although not a good one) - /// assert!(NodeBuilder::new(&context, "_______").build().is_ok()); + /// assert!(executor.create_node(NodeOptions::new("_______")).is_ok()); /// // This is an invalid node name /// assert!(matches!( - /// NodeBuilder::new(&context, "röböt") - /// .build() - /// .unwrap_err(), + /// executor.create_node(NodeOptions::new("röböt")).unwrap_err(), /// RclrsError::RclError { code: RclReturnCode::NodeInvalidName, .. } /// )); /// # Ok::<(), RclrsError>(()) @@ -128,25 +126,25 @@ impl NodeOptions { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, RclrsError, RclReturnCode}; - /// let context = Context::new([])?; + /// # use rclrs::{Context, Node, NodeOptions, 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 = NodeOptions::new("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( + /// NodeOptions::new("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( + /// NodeOptions::new("my_node") /// .namespace("foo") - /// .build()? - /// .namespace(), + /// )?.namespace(), /// "/foo" /// ); /// # Ok::<(), RclrsError>(()) @@ -167,21 +165,21 @@ impl NodeOptions { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, NodeBuilder, RclrsError}; + /// # use rclrs::{Context, Node, NodeOptions, 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)?.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( + /// NodeOptions::new("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( + /// NodeOptions::new("my_other_node") + /// .use_global_arguments(true) + /// )?; /// assert_eq!(node_with_global_args.name(), "your_node"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -202,18 +200,18 @@ impl NodeOptions { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, NodeBuilder, RclrsError}; + /// # use rclrs::{Context, Node, NodeOptions, 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)?.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( + /// NodeOptions::new("my_node") + /// .arguments(node_args) + /// )?; /// assert_eq!(node.name(), "node_args_node"); /// # Ok::<(), RclrsError>(()) /// ``` From 22d2c843b8117e8e9a36737d0977516c7a8b8930 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 14 Oct 2024 20:40:13 +0800 Subject: [PATCH 25/48] Remove old comments Signed-off-by: Michael X. Grey --- rclrs/src/node.rs | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index fcc3a057c..3614ef34b 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -468,19 +468,6 @@ impl Node { return; } } - - // match on_graph_change_receiver.next().await { - // Some(_) => { - // if condition() { - // // Condition is met - // return; - // } - // } - // None => { - // // Graph change sender is closed - // return; - // } - // } } }); From 29fe10ef7f9bd33fcd0fdb795f562e5a19e0aa0d Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 16 Nov 2024 16:05:28 +0000 Subject: [PATCH 26/48] Migrate Context, Executor, and Node creation to new API Signed-off-by: Michael X. Grey --- rclrs/src/context.rs | 66 ++++--- rclrs/src/error.rs | 21 +++ rclrs/src/executor.rs | 123 ++++++++----- rclrs/src/lib.rs | 65 ------- rclrs/src/node.rs | 162 ++++++++++-------- rclrs/src/node/graph.rs | 17 +- .../src/node/{builder.rs => node_options.rs} | 153 +++++++++-------- rclrs/src/parameter.rs | 114 +++++++----- rclrs/src/parameter/service.rs | 106 +++++++----- rclrs/src/parameter/value.rs | 15 +- rclrs/src/test_helpers/graph_helpers.rs | 19 +- rclrs/src/time_source.rs | 35 ++-- rclrs/src/wait.rs | 2 +- rclrs/src/wait/guard_condition.rs | 6 +- 14 files changed, 509 insertions(+), 395 deletions(-) rename rclrs/src/node/{builder.rs => node_options.rs} (75%) diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 90c8fbd3c..b218fc24b 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -6,7 +6,7 @@ use std::{ vec::Vec, }; -use crate::{rcl_bindings::*, RclrsError, ToResult}; +use crate::{rcl_bindings::*, RclrsError, ToResult, Executor}; /// This is locked whenever initializing or dropping any middleware entity /// because we have found issues in RCL and some RMW implementations that @@ -70,34 +70,41 @@ pub(crate) struct ContextHandle { pub(crate) rcl_context: Mutex, } +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 { @@ -150,6 +157,23 @@ 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 a basic executor that comes built into rclrs. + pub fn create_basic_executor(&self) -> Executor { + Executor::new(Arc::clone(&self.handle)) + } + /// 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]. @@ -250,14 +274,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..515a5b95c 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -352,3 +352,24 @@ impl ToResult for rcl_ret_t { to_rclrs_result(*self) } } + +/// A helper trait to disregard timeouts as not an error. +pub trait RclrsErrorFilter { + /// If the result was a timeout error, change it to `Ok(())`. + fn timeout_ok(self) -> Result<(), RclrsError>; +} + +impl RclrsErrorFilter for Result<(), RclrsError> { + fn timeout_ok(self) -> Result<(), RclrsError> { + match self { + Ok(()) => Ok(()), + Err(err) => { + if matches!(err, RclrsError::RclError { code: RclReturnCode::Timeout, .. }) { + return Ok(()); + } + + Err(err) + } + } + } +} diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 37c43a68e..63951afef 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,48 +1,61 @@ -use crate::{rcl_bindings::rcl_context_is_valid, Node, RclReturnCode, RclrsError, WaitSet}; +use crate::{ + rcl_bindings::rcl_context_is_valid, + Node, RclrsError, WaitSet, ContextHandle, NodeOptions, WeakNode, +}; use std::{ - sync::{Arc, Mutex, Weak}, + sync::{Arc, Mutex}, time::Duration, }; /// Single-threaded executor implementation. -pub struct SingleThreadedExecutor { - nodes_mtx: Mutex>>, +pub struct Executor { + context: Arc, + nodes_mtx: Mutex>, } -impl Default for SingleThreadedExecutor { - fn default() -> Self { - Self::new() +impl Executor { + /// Create a [`Node`] that will run on this Executor. + pub fn create_node( + &self, + options: impl Into, + ) -> Result { + let options: NodeOptions = options.into(); + let node = options.build(&self.context)?; + self.nodes_mtx.lock().unwrap().push(node.downgrade()); + Ok(node) } -} -impl SingleThreadedExecutor { - /// Creates a new executor. - pub fn new() -> Self { - SingleThreadedExecutor { - nodes_mtx: Mutex::new(Vec::new()), - } - } + /// Spin the Executor. The current thread will be blocked until the Executor + /// stops spinning. + /// + /// [`SpinOptions`] can be used to automatically stop the spinning when + /// certain conditions are met. Use `SpinOptions::default()` to allow the + /// Executor to keep spinning indefinitely. + pub fn spin(&mut self, options: SpinOptions) -> Result<(), RclrsError> { + loop { + if self.nodes_mtx.lock().unwrap().is_empty() { + // Nothing to spin for, so just quit here + return Ok(()); + } - /// 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(()) - } + self.spin_once(options.timeout)?; - /// 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(()) + if options.only_next_available_work { + // We were only suppposed to spin once, so quit here + return Ok(()); + } + + std::thread::yield_now(); + } } /// Polls the nodes for new messages and executes the corresponding callbacks. /// /// This function additionally checks that the context is still valid. - pub fn spin_once(&self, timeout: Option) -> Result<(), RclrsError> { + fn spin_once(&self, timeout: Option) -> Result<(), RclrsError> { for node in { self.nodes_mtx.lock().unwrap() } .iter() - .filter_map(Weak::upgrade) + .filter_map(WeakNode::upgrade) .filter(|node| unsafe { rcl_context_is_valid(&*node.handle.context_handle.rcl_context.lock().unwrap()) }) @@ -66,19 +79,51 @@ impl SingleThreadedExecutor { Ok(()) } - /// 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, - } + /// Used by [`Context`] to create the `Executor`. Users cannot call this + /// function. + pub(crate) fn new(context: Arc) -> Self { + Self { + context, + nodes_mtx: Mutex::new(Vec::new()), } + } +} - Ok(()) +/// 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, + /// Stop waiting after this duration of time has passed. Use `Some(0)` to not + /// wait any amount of time. Use `None` to wait an infinite amount of time. + pub timeout: Option, +} + +impl SpinOptions { + /// Use default spin options. + pub fn new() -> Self { + Self::default() + } + + /// Behave like spin_once in rclcpp and rclpy. + pub fn spin_once() -> Self { + Self { + only_next_available_work: true, + ..Default::default() + } + } + + /// Stop spinning once this durtion of time is reached. + pub fn timeout(mut self, timeout: Duration) -> Self { + self.timeout = Some(timeout); + self } } diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 4924b36cb..f41b61700 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -30,8 +30,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::*; @@ -48,66 +46,3 @@ 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) -} diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 97684d6bc..d91a2c859 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,4 +1,4 @@ -mod builder; +mod node_options; mod graph; use std::{ cmp::PartialEq, @@ -11,9 +11,9 @@ use std::{ use rosidl_runtime_rs::Message; -pub use self::{builder::*, graph::*}; +pub use self::{graph::*, node_options::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, + rcl_bindings::*, Client, ClientBase, Clock, ContextHandle, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, TimeSource, ENTITY_LIFECYCLE_MUTEX, @@ -58,13 +58,9 @@ unsafe impl Send for rcl_node_t {} /// [2]: https://docs.ros.org/en/rolling/How-To-Guides/Node-arguments.html /// [3]: crate::NodeBuilder::new /// [4]: crate::NodeBuilder::namespace +#[derive(Clone)] pub struct Node { - pub(crate) clients_mtx: Mutex>>, - pub(crate) guard_conditions_mtx: Mutex>>, - pub(crate) services_mtx: Mutex>>, - pub(crate) subscriptions_mtx: Mutex>>, - time_source: TimeSource, - parameter: ParameterInterface, + pub(crate) primitives: Arc, pub(crate) handle: Arc, } @@ -78,6 +74,18 @@ pub(crate) struct NodeHandle { pub(crate) context_handle: Arc, } +/// This struct manages the primitives that are associated with a [`Node`]. +/// Storing them here allows the inner state of the `Node` to be shared across +/// clones. +pub(crate) struct NodePrimitives { + pub(crate) clients_mtx: Mutex>>, + pub(crate) guard_conditions_mtx: Mutex>>, + pub(crate) services_mtx: Mutex>>, + pub(crate) subscriptions_mtx: Mutex>>, + time_source: TimeSource, + parameter: ParameterInterface, +} + impl Drop for NodeHandle { fn drop(&mut self) { let _context_lock = self.context_handle.rcl_context.lock().unwrap(); @@ -106,17 +114,9 @@ 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() - } - /// Returns the clock associated with this node. pub fn get_clock(&self) -> Clock { - self.time_source.get_clock() + self.primitives.time_source.get_clock() } /// Returns the name of the node. @@ -126,15 +126,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>(()) /// ``` @@ -149,18 +149,18 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError}; + /// # use rclrs::{Context, InitOptions, RclrsError, NodeOptions}; /// // 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( + /// NodeOptions::new("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>(()) /// ``` @@ -175,12 +175,12 @@ impl Node { /// /// # 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, NodeOptions}; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node( + /// NodeOptions::new("my_node") + /// .namespace("/my/namespace") + /// )?; /// assert_eq!(node.fully_qualified_name(), "/my/namespace/my_node"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -206,7 +206,7 @@ impl Node { 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); + { self.primitives.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); Ok(client) } @@ -224,7 +224,7 @@ impl Node { Arc::clone(&self.handle.context_handle), None, )); - { self.guard_conditions_mtx.lock().unwrap() } + { self.primitives.guard_conditions_mtx.lock().unwrap() } .push(Arc::downgrade(&guard_condition) as Weak); guard_condition } @@ -246,7 +246,7 @@ impl Node { Arc::clone(&self.handle.context_handle), Some(Box::new(callback) as Box), )); - { self.guard_conditions_mtx.lock().unwrap() } + { self.primitives.guard_conditions_mtx.lock().unwrap() } .push(Arc::downgrade(&guard_condition) as Weak); guard_condition } @@ -285,7 +285,7 @@ impl Node { topic, callback, )?); - { self.services_mtx.lock().unwrap() } + { self.primitives.services_mtx.lock().unwrap() } .push(Arc::downgrade(&service) as Weak); Ok(service) } @@ -309,7 +309,7 @@ impl Node { qos, callback, )?); - { self.subscriptions_mtx.lock() } + { self.primitives.subscriptions_mtx.lock() } .unwrap() .push(Arc::downgrade(&subscription) as Weak); Ok(subscription) @@ -317,28 +317,28 @@ impl Node { /// Returns the subscriptions that have not been dropped yet. pub(crate) fn live_subscriptions(&self) -> Vec> { - { self.subscriptions_mtx.lock().unwrap() } + { self.primitives.subscriptions_mtx.lock().unwrap() } .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_clients(&self) -> Vec> { - { self.clients_mtx.lock().unwrap() } + { self.primitives.clients_mtx.lock().unwrap() } .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_guard_conditions(&self) -> Vec> { - { self.guard_conditions_mtx.lock().unwrap() } + { self.primitives.guard_conditions_mtx.lock().unwrap() } .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_services(&self) -> Vec> { - { self.services_mtx.lock().unwrap() } + { self.primitives.services_mtx.lock().unwrap() } .iter() .filter_map(Weak::upgrade) .collect() @@ -347,24 +347,24 @@ impl Node { /// 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 [`NodeOptions`] into [`Context::new`][2] or [`Context::from_env`][3]. /// /// [1]: https://docs.ros.org/en/rolling/Concepts/About-Domain-ID.html + /// [2]: crate::Context::new + /// [3]: 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; @@ -385,8 +385,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), @@ -409,36 +409,50 @@ impl Node { &'a self, name: impl Into>, ) -> ParameterBuilder<'a, T> { - self.parameter.declare(name.into()) + self.primitives.parameter.declare(name.into()) } /// Enables usage of undeclared parameters for this node. /// /// Returns a [`Parameters`] struct that can be used to get and set all parameters. pub fn use_undeclared_parameters(&self) -> Parameters { - self.parameter.allow_undeclared(); + self.primitives.parameter.allow_undeclared(); Parameters { - interface: &self.parameter, + interface: &self.primitives.parameter, } } - /// Creates a [`NodeBuilder`][1] with the given name. - /// - /// Convenience function equivalent to [`NodeBuilder::new()`][2]. - /// - /// [1]: crate::NodeBuilder - /// [2]: crate::NodeBuilder::new - /// - /// # 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) + /// Get a `WeakNode` for this `Node`. + pub fn downgrade(&self) -> WeakNode { + WeakNode { + primitives: Arc::downgrade(&self.primitives), + handle: Arc::downgrade(&self.handle), + } + } +} + +/// This gives weak access to a [`Node`]. +/// +/// Holding onto this will not keep the `Node` alive, but calling [`Self::upgrade`] +/// on it will give you access to the `Node` if it is still alive. +#[derive(Default)] +pub struct WeakNode { + primitives: Weak, + handle: Weak, +} + +impl WeakNode { + /// Create a `WeakNode` that is not associated with any `Node`. + pub fn new() -> WeakNode { + Self::default() + } + + /// Get the [`Node`] that this `WeakNode` refers to if it's available. + pub fn upgrade(&self) -> Option { + Some(Node { + primitives: self.primitives.upgrade()?, + handle: self.handle.upgrade()?, + }) } } diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 343fa61d3..106fac72c 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -482,11 +482,12 @@ 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(); // Test that the graph has no publishers let names_and_topics = node .get_publisher_names_and_types_by_node(node_name, "") @@ -543,9 +544,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(); @@ -559,9 +560,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/builder.rs b/rclrs/src/node/node_options.rs similarity index 75% rename from rclrs/src/node/builder.rs rename to rclrs/src/node/node_options.rs index 011d5d2f3..801385bcf 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/node_options.rs @@ -4,7 +4,8 @@ use std::{ }; use crate::{ - rcl_bindings::*, ClockType, Context, ContextHandle, Node, NodeHandle, ParameterInterface, + rcl_bindings::*, + ClockType, Node, NodeHandle, ParameterInterface, NodePrimitives, ContextHandle, QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, }; @@ -24,27 +25,26 @@ use crate::{ /// /// # Example /// ``` -/// # use rclrs::{Context, NodeBuilder, Node, RclrsError}; -/// let context = Context::new([])?; +/// # use rclrs::{Context, NodeOptions, Node, RclrsError}; +/// let executor = Context::default().create_basic_executor(); /// // Building a node in a single expression -/// let node = NodeBuilder::new(&context, "foo_node").namespace("/bar").build()?; +/// let node = executor.create_node(NodeOptions::new("foo_node").namespace("/bar"))?; /// 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()?; +/// // Building a node via NodeOptions +/// let node = executor.create_node(NodeOptions::new("bar_node"))?; /// 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()?; +/// let mut options = NodeOptions::new("goose"); +/// options = options.namespace("/duck/duck"); +/// let node = executor.create_node(options)?; /// assert_eq!(node.fully_qualified_name(), "/duck/duck/goose"); /// # Ok::<(), RclrsError>(()) /// ``` /// /// [1]: crate::Node /// [2]: crate::Node::builder -pub struct NodeBuilder { - context: Arc, +pub struct NodeOptions { name: String, namespace: String, use_global_arguments: bool, @@ -55,7 +55,7 @@ pub struct NodeBuilder { clock_qos: QoSProfile, } -impl NodeBuilder { +impl NodeOptions { /// Creates a builder for a node with the given name. /// /// See the [`Node` docs][1] for general information on node names. @@ -72,17 +72,15 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, NodeBuilder, RclrsError, RclReturnCode}; - /// let context = Context::new([])?; + /// # use rclrs::{Context, NodeOptions, RclrsError, RclReturnCode}; + /// let executor = Context::default().create_basic_executor(); /// // This is a valid node name - /// assert!(NodeBuilder::new(&context, "my_node").build().is_ok()); + /// assert!(executor.create_node(NodeOptions::new("my_node")).is_ok()); /// // This is another valid node name (although not a good one) - /// assert!(NodeBuilder::new(&context, "_______").build().is_ok()); + /// assert!(executor.create_node(NodeOptions::new("_______")).is_ok()); /// // This is an invalid node name /// assert!(matches!( - /// NodeBuilder::new(&context, "röböt") - /// .build() - /// .unwrap_err(), + /// executor.create_node(NodeOptions::new("röböt")).unwrap_err(), /// RclrsError::RclError { code: RclReturnCode::NodeInvalidName, .. } /// )); /// # Ok::<(), RclrsError>(()) @@ -91,9 +89,8 @@ impl NodeBuilder { /// [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), + pub fn new(name: impl ToString) -> NodeOptions { + NodeOptions { name: name.to_string(), namespace: "/".to_string(), use_global_arguments: true, @@ -126,25 +123,25 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, RclrsError, RclReturnCode}; - /// let context = Context::new([])?; + /// # use rclrs::{Context, Node, NodeOptions, 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 = NodeOptions::new("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( + /// NodeOptions::new("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( + /// NodeOptions::new("my_node") /// .namespace("foo") - /// .build()? - /// .namespace(), + /// )?.namespace(), /// "/foo" /// ); /// # Ok::<(), RclrsError>(()) @@ -154,7 +151,7 @@ impl NodeBuilder { /// [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 { + pub fn namespace(mut self, namespace: impl ToString) -> Self { self.namespace = namespace.to_string(); self } @@ -165,21 +162,21 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, NodeBuilder, RclrsError}; + /// # use rclrs::{Context, InitOptions, Node, NodeOptions, 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( + /// NodeOptions::new("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( + /// NodeOptions::new("my_other_node") + /// .use_global_arguments(true) + /// )?; /// assert_eq!(node_with_global_args.name(), "your_node"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -200,26 +197,29 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, NodeBuilder, RclrsError}; + /// # use rclrs::{Context, InitOptions, Node, NodeOptions, 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( + /// NodeOptions::new("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(); + pub fn arguments(mut self, arguments: Args) -> Self + where + Args::Item: ToString, + { + self.arguments = arguments.into_iter().map(|item| item.to_string()).collect(); self } @@ -257,12 +257,12 @@ impl NodeBuilder { /// Builds the node instance. /// - /// Node name and namespace validation is performed in this method. - /// - /// For example usage, see the [`NodeBuilder`][1] docs. - /// - /// [1]: crate::NodeBuilder - pub fn build(&self) -> Result, RclrsError> { + /// Only used internally. Downstream users should call + /// [`Executor::create_node`]. + pub(crate) fn build( + self, + context: &Arc, + ) -> Result { let node_name = CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul { err, @@ -274,7 +274,7 @@ impl NodeBuilder { s: self.namespace.clone(), })?; let rcl_node_options = self.create_rcl_node_options()?; - let rcl_context = &mut *self.context.rcl_context.lock().unwrap(); + let rcl_context = &mut *context.rcl_context.lock().unwrap(); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_node = unsafe { rcl_get_zero_initialized_node() }; @@ -298,7 +298,7 @@ impl NodeBuilder { let handle = Arc::new(NodeHandle { rcl_node: Mutex::new(rcl_node), - context_handle: Arc::clone(&self.context), + context_handle: Arc::clone(&context), }); let parameter = { let rcl_node = handle.rcl_node.lock().unwrap(); @@ -308,21 +308,26 @@ impl NodeBuilder { &rcl_context.global_arguments, )? }; - let node = Arc::new(Node { + + let node = Node { handle, - clients_mtx: Mutex::new(vec![]), - guard_conditions_mtx: Mutex::new(vec![]), - services_mtx: Mutex::new(vec![]), - subscriptions_mtx: Mutex::new(vec![]), - time_source: TimeSource::builder(self.clock_type) - .clock_qos(self.clock_qos) - .build(), - parameter, - }); - node.time_source.attach_node(&node); + primitives: Arc::new(NodePrimitives { + clients_mtx: Mutex::default(), + guard_conditions_mtx: Mutex::default(), + services_mtx: Mutex::default(), + subscriptions_mtx: Mutex::default(), + time_source: TimeSource::builder(self.clock_type) + .clock_qos(self.clock_qos) + .build(), + parameter, + }), + }; + node.primitives.time_source.attach_node(&node); + if self.start_parameter_services { - node.parameter.create_services(&node)?; + node.primitives.parameter.create_services(&node)?; } + Ok(node) } @@ -367,6 +372,12 @@ impl NodeBuilder { } } +impl From for NodeOptions { + fn from(name: T) -> Self { + NodeOptions::new(name) + } +} + impl Drop for rcl_node_options_t { fn drop(&mut self) { // SAFETY: Do not finish this struct except here. diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index c7153ce01..5be93adab 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -871,18 +871,23 @@ 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, "param_test_node").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("param_test_node").unwrap(); // Declaring a parameter with a different type than what was overridden should return an // error @@ -928,19 +933,24 @@ 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, "param_test_node").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("param_test_node").unwrap(); let overridden_int = node .declare_parameter("declared_int") @@ -1084,13 +1094,18 @@ 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, "param_test_node").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("param_test_node").unwrap(); // If a parameter was set as an override and as an undeclared parameter, the undeclared // value should get priority node.use_undeclared_parameters() @@ -1106,13 +1121,18 @@ 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, "param_test_node").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("param_test_node").unwrap(); { // Setting a parameter with an override let param = node @@ -1157,8 +1177,10 @@ mod tests { #[test] fn test_parameter_ranges() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node("param_test_node") + .unwrap(); // Setting invalid ranges should fail let range = ParameterRange { lower: Some(10), @@ -1285,8 +1307,10 @@ mod tests { #[test] fn test_readonly_parameters() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node("param_test_node") + .unwrap(); let param = node .declare_parameter("int_param") .default(100) @@ -1312,8 +1336,10 @@ mod tests { #[test] fn test_preexisting_value_error() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node("param_test_node") + .unwrap(); node.use_undeclared_parameters() .set("int_param", 100) .unwrap(); @@ -1365,8 +1391,10 @@ mod tests { #[test] fn test_optional_parameter_apis() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node("param_test_node") + .unwrap(); node.declare_parameter::("int_param") .optional() .unwrap(); diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 7c8ffe62d..11df07f28 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -312,23 +312,26 @@ mod tests { }, srv::rmw::*, }, - Context, MandatoryParameter, Node, NodeBuilder, ParameterRange, ParameterValue, RclrsError, - ReadOnlyParameter, + Context, MandatoryParameter, Node, ParameterRange, ParameterValue, RclrsError, + ReadOnlyParameter, NodeOptions, Executor, SpinOptions, RclrsErrorFilter, }; use rosidl_runtime_rs::{seq, Sequence}; - use std::sync::{Arc, RwLock}; + use std::{ + sync::{Arc, RwLock}, + 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<(), ()> + async fn try_until_timeout(mut f: F) -> Result<(), ()> where - F: FnOnce() -> bool + Copy, + F: FnMut() -> bool, { let mut retry_count = 0; while !f() { @@ -341,11 +344,13 @@ mod tests { Ok(()) } - fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { - let node = NodeBuilder::new(context, "node") + 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) - .build() - .unwrap(); + ) + .unwrap(); let range = ParameterRange { lower: Some(0), upper: Some(100), @@ -375,12 +380,14 @@ mod tests { .mandatory() .unwrap(); - let client = NodeBuilder::new(context, "client") + let client = executor.create_node( + NodeOptions::new("client") .namespace(ns) - .build() - .unwrap(); + ) + .unwrap(); ( + executor, TestNode { node, bool_param, @@ -394,8 +401,7 @@ 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 (_, node, _client) = construct_test_nodes("names_types"); std::thread::sleep(std::time::Duration::from_millis(100)); @@ -429,8 +435,7 @@ mod tests { #[tokio::test] async fn test_list_parameters_service() -> Result<(), RclrsError> { - let context = Context::new([]).unwrap(); - let (node, client) = construct_test_nodes(&context, "list"); + let (mut executor, _test, client) = construct_test_nodes("list"); let list_client = client.create_client::("/list/node/list_parameters")?; try_until_timeout(|| list_client.service_is_ready().unwrap()) @@ -441,9 +446,14 @@ mod tests { 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(); + try_until_timeout(move || { + executor.spin( + SpinOptions::spin_once() + .timeout(Duration::ZERO) + ) + .timeout_ok() + .unwrap(); + *inner_done.read().unwrap() }) .await @@ -542,7 +552,6 @@ mod tests { 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"); @@ -564,14 +573,14 @@ mod tests { #[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 (mut executor, test, client) = construct_test_nodes("get_set"); let get_client = client.create_client::("/get_set/node/get_parameters")?; let set_client = client.create_client::("/get_set/node/set_parameters")?; let set_atomically_client = client .create_client::("/get_set/node/set_parameters_atomically")?; try_until_timeout(|| { + println!(" >> testing services"); get_client.service_is_ready().unwrap() && set_client.service_is_ready().unwrap() && set_atomically_client.service_is_ready().unwrap() @@ -581,18 +590,26 @@ mod tests { 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(); + try_until_timeout(move || { + println!(" -- spin"); + executor.spin( + SpinOptions::spin_once() + .timeout(Duration::ZERO) + ) + .timeout_ok() + .unwrap(); + *inner_done.read().unwrap() }) .await .unwrap(); }); + let _hold_node = test.node.clone(); + let _hold_client = client.clone(); + let res = tokio::task::spawn(async move { // Get an existing parameter let request = GetParameters_Request { @@ -636,7 +653,10 @@ mod tests { }, ) .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) + try_until_timeout(|| { + println!("checking client"); + *client_finished.read().unwrap() + }) .await .unwrap(); @@ -711,7 +731,7 @@ mod tests { 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()); + assert!(test.bool_param.get()); set_client .async_send_request_with_callback( &request, @@ -721,14 +741,14 @@ mod tests { // 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()); + 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!(node.dynamic_param.get(), ParameterValue::Bool(true)); + assert_eq!(test.dynamic_param.get(), ParameterValue::Bool(true)); // Setting a value out of range assert!(!response.results[4].successful); // Setting an invalid type @@ -743,7 +763,7 @@ mod tests { .unwrap(); // Set the node to use undeclared parameters and try to set one - node.node.use_undeclared_parameters(); + test.node.use_undeclared_parameters(); let request = SetParameters_Request { parameters: seq![undeclared_bool], }; @@ -758,7 +778,7 @@ mod tests { // Setting the undeclared parameter is now allowed assert!(response.results[0].successful); assert_eq!( - node.node.use_undeclared_parameters().get("undeclared_bool"), + test.node.use_undeclared_parameters().get("undeclared_bool"), Some(ParameterValue::Bool(true)) ); }, @@ -783,7 +803,10 @@ mod tests { }, ) .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) + try_until_timeout(|| { + println!("checking client finished"); + *client_finished.read().unwrap() + }) .await .unwrap(); *done.write().unwrap() = true; @@ -797,8 +820,7 @@ mod tests { #[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 (mut executor, _test, client) = construct_test_nodes("describe"); let describe_client = client.create_client::("/describe/node/describe_parameters")?; let get_types_client = @@ -814,11 +836,15 @@ mod tests { 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(); + try_until_timeout(move || { + executor.spin( + SpinOptions::spin_once() + .timeout(Duration::ZERO) + ) + .timeout_ok() + .unwrap(); + *inner_done.read().unwrap() }) .await diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index 82fe31ebb..3a20ae24b 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, RclrsError, ToResult, InitOptions}; // 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/test_helpers/graph_helpers.rs b/rclrs/src/test_helpers/graph_helpers.rs index 1e9b581ae..ce3e82f67 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -1,19 +1,20 @@ -use crate::{Context, Node, NodeBuilder, RclrsError}; -use std::sync::Arc; +use crate::{Context, Node, RclrsError, NodeOptions}; 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") + node1: executor.create_node( + NodeOptions::new("graph_test_node_1") .namespace(namespace) - .build()?, - node2: NodeBuilder::new(&context, "graph_test_node_2") + )?, + node2: executor.create_node( + NodeOptions::new("graph_test_node_2") .namespace(namespace) - .build()?, + )?, }) } diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index a6b600800..a5dcad50f 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -1,15 +1,15 @@ use crate::{ clock::{Clock, ClockSource, ClockType}, vendor::rosgraph_msgs::msg::Clock as ClockMsg, - Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, + Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, WeakNode, }; -use std::sync::{Arc, Mutex, RwLock, Weak}; +use std::sync::{Arc, Mutex, RwLock}; /// Time source for a node that drives the attached clock. /// 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, @@ -60,7 +60,7 @@ impl TimeSourceBuilder { ClockType::SteadyTime => Clock::steady(), }; TimeSource { - node: Mutex::new(Weak::new()), + node: Mutex::new(WeakNode::new()), clock: RwLock::new(clock), clock_source: Arc::new(Mutex::new(None)), requested_clock_type: self.clock_type, @@ -85,7 +85,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 @@ -93,7 +93,7 @@ impl TimeSource { .default(false) .read_only() .unwrap(); - *self.node.lock().unwrap() = Arc::downgrade(node); + *self.node.lock().unwrap() = node.downgrade(); self.set_ros_time_enable(param.get()); *self.use_sim_time.lock().unwrap() = Some(param); } @@ -145,24 +145,29 @@ impl TimeSource { #[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(), "test_node").unwrap(); + let node = Context::default().create_basic_executor().create_node("test_node").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, "test_node").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("test_node").unwrap(); // Default sim time value should be 0 (no message received) assert_eq!(node.get_clock().now().nsec, 0); } diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 243c9d857..c0e0c659d 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -427,7 +427,7 @@ mod tests { #[test] fn guard_condition_in_wait_set_readies() -> Result<(), RclrsError> { - let context = Context::new([])?; + let context = Context::default(); let guard_condition = Arc::new(GuardCondition::new(&context)); diff --git a/rclrs/src/wait/guard_condition.rs b/rclrs/src/wait/guard_condition.rs index 92a6acd00..d9d8b62d9 100644 --- a/rclrs/src/wait/guard_condition.rs +++ b/rclrs/src/wait/guard_condition.rs @@ -16,7 +16,7 @@ use crate::{rcl_bindings::*, Context, ContextHandle, RclrsError, ToResult}; /// # use rclrs::{Context, GuardCondition, WaitSet, RclrsError}; /// # use std::sync::{Arc, atomic::Ordering}; /// -/// let context = Context::new([])?; +/// let context = Context::default(); /// /// let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false)); /// let atomic_bool_for_closure = Arc::clone(&atomic_bool); @@ -162,7 +162,7 @@ mod tests { #[test] fn test_guard_condition() -> Result<(), RclrsError> { - let context = Context::new([])?; + let context = Context::default(); let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false)); let atomic_bool_for_closure = Arc::clone(&atomic_bool); @@ -180,7 +180,7 @@ mod tests { #[test] fn test_guard_condition_wait() -> Result<(), RclrsError> { - let context = Context::new([])?; + let context = Context::default(); let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false)); let atomic_bool_for_closure = Arc::clone(&atomic_bool); From 1b5c187a761ae9a9de92392e593c45e6fd0e9554 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 16 Nov 2024 16:34:30 +0000 Subject: [PATCH 27/48] Update examples Signed-off-by: Michael X. Grey --- examples/message_demo/src/message_demo.rs | 10 +++--- .../src/minimal_client.rs | 8 ++--- .../src/minimal_client_async.rs | 8 ++--- .../src/minimal_service.rs | 8 ++--- .../minimal_pub_sub/src/minimal_publisher.rs | 7 ++-- .../minimal_pub_sub/src/minimal_subscriber.rs | 9 +++--- .../minimal_pub_sub/src/minimal_two_nodes.rs | 32 +++++++------------ .../src/zero_copy_publisher.rs | 7 ++-- .../src/zero_copy_subscriber.rs | 8 ++--- examples/rust_pubsub/src/simple_publisher.rs | 27 ++++++++-------- examples/rust_pubsub/src/simple_subscriber.rs | 17 +++++----- 11 files changed, 60 insertions(+), 81 deletions(-) diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index 3e2bf53b2..bc3b7c028 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,8 +138,8 @@ 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", @@ -166,10 +166,10 @@ fn demonstrate_pubsub() -> Result<(), Error> { )?; 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..baeff971d 100644 --- a/examples/minimal_client_service/src/minimal_client.rs +++ b/examples/minimal_client_service/src/minimal_client.rs @@ -1,11 +1,9 @@ -use std::env; - use anyhow::{Error, Result}; 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_client")?; + let node = executor.create_node("minimal_client")?; let client = node.create_client::("add_two_ints")?; @@ -30,5 +28,5 @@ fn main() -> Result<(), Error> { std::thread::sleep(std::time::Duration::from_millis(500)); println!("Waiting for response"); - rclrs::spin(node).map_err(|err| err.into()) + executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) } diff --git a/examples/minimal_client_service/src/minimal_client_async.rs b/examples/minimal_client_service/src/minimal_client_async.rs index 0eeb87f4d..f011569de 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}; #[tokio::main] async 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_client")?; + let node = executor.create_node("minimal_client")?; let client = node.create_client::("add_two_ints")?; @@ -22,7 +20,7 @@ async fn main() -> Result<(), Error> { println!("Waiting for response"); - let rclrs_spin = tokio::task::spawn_blocking(move || rclrs::spin(node)); + let rclrs_spin = tokio::task::spawn_blocking(move || executor.spin(rclrs::SpinOptions::default())); let response = future.await?; println!( diff --git a/examples/minimal_client_service/src/minimal_service.rs b/examples/minimal_client_service/src/minimal_service.rs index b4149c817..f38a49f21 100644 --- a/examples/minimal_client_service/src/minimal_service.rs +++ b/examples/minimal_client_service/src/minimal_service.rs @@ -1,5 +1,3 @@ -use std::env; - use anyhow::{Error, Result}; fn handle_service( @@ -13,13 +11,13 @@ fn handle_service( } 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_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(rclrs::SpinOptions::default()).map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/minimal_publisher.rs b/examples/minimal_pub_sub/src/minimal_publisher.rs index 720086917..7ca95cf97 100644 --- a/examples/minimal_pub_sub/src/minimal_publisher.rs +++ b/examples/minimal_pub_sub/src/minimal_publisher.rs @@ -1,11 +1,10 @@ -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)?; diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index ebc5fc194..2d375bd7d 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -1,11 +1,10 @@ -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 mut executor = context.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_subscriber")?; + let node = executor.create_node("minimal_subscriber")?; let mut num_messages: usize = 0; @@ -19,5 +18,5 @@ fn main() -> Result<(), Error> { }, )?; - rclrs::spin(node).map_err(|err| err.into()) + executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index fb03574a2..f829afc52 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -1,23 +1,19 @@ -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, + 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, @@ -50,11 +46,11 @@ 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)?; @@ -71,11 +67,5 @@ 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..3f904f673 100644 --- a/examples/minimal_pub_sub/src/zero_copy_publisher.rs +++ b/examples/minimal_pub_sub/src/zero_copy_publisher.rs @@ -1,11 +1,10 @@ -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)?; diff --git a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs index 9551dba0e..cfb569ab2 100644 --- a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -1,11 +1,9 @@ -use std::env; - use anyhow::{Error, Result}; 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; @@ -19,5 +17,5 @@ fn main() -> Result<(), Error> { }, )?; - rclrs::spin(node).map_err(|err| err.into()) + executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) } diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index 98d0e0f74..edacd0247 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -1,36 +1,37 @@ -use rclrs::{create_node, Context, Node, Publisher, RclrsError, QOS_PROFILE_DEFAULT}; +use rclrs::{Context, Executor, Publisher, RclrsError, SpinOptions, QOS_PROFILE_DEFAULT}; use std::{sync::Arc, thread, time::Duration}; use std_msgs::msg::String as StringMsg; -struct SimplePublisherNode { - node: Arc, - _publisher: Arc>, + +struct SimplePublisher { + publisher: Arc>, } -impl SimplePublisherNode { - fn new(context: &Context) -> Result { - let node = create_node(context, "simple_publisher").unwrap(); - let _publisher = node + +impl SimplePublisher { + fn new(executor: &Executor) -> Result { + let node = executor.create_node("simple_publisher").unwrap(); + let publisher = node .create_publisher("publish_hello", QOS_PROFILE_DEFAULT) .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 mut executor = Context::default_from_env().unwrap().create_basic_executor(); + let publisher = Arc::new(SimplePublisher::new(&executor).unwrap()); let publisher_other_thread = Arc::clone(&publisher); let mut count: i32 = 0; thread::spawn(move || loop { thread::sleep(Duration::from_millis(1000)); count = publisher_other_thread.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..2ba2c4e7a 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -1,19 +1,19 @@ -use rclrs::{create_node, Context, Node, RclrsError, Subscription, QOS_PROFILE_DEFAULT}; +use rclrs::{Context, Executor, RclrsError, SpinOptions, Subscription, QOS_PROFILE_DEFAULT}; use std::{ - env, sync::{Arc, Mutex}, thread, time::Duration, }; use std_msgs::msg::String as StringMsg; + pub struct SimpleSubscriptionNode { - node: Arc, _subscriber: Arc>, 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 @@ -26,7 +26,6 @@ impl SimpleSubscriptionNode { ) .unwrap(); Ok(Self { - node, _subscriber, data, }) @@ -41,12 +40,12 @@ impl SimpleSubscriptionNode { } } fn main() -> Result<(), RclrsError> { - let context = Context::new(env::args()).unwrap(); - let subscription = Arc::new(SimpleSubscriptionNode::new(&context).unwrap()); + let mut executor = Context::default_from_env().unwrap().create_basic_executor(); + let subscription = Arc::new(SimpleSubscriptionNode::new(&executor).unwrap()); let subscription_other_thread = Arc::clone(&subscription); thread::spawn(move || loop { thread::sleep(Duration::from_millis(1000)); subscription_other_thread.data_callback().unwrap() }); - rclrs::spin(subscription.node.clone()) + executor.spin(SpinOptions::default()) } From 1ec9f10acfcaffac0da6237d8aa6c6696332de57 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 19 Nov 2024 16:41:58 +0800 Subject: [PATCH 28/48] Fix documentation Signed-off-by: Michael X. Grey --- rclrs/src/context.rs | 10 ++++----- rclrs/src/node.rs | 38 ++++++++++++++++++---------------- rclrs/src/node/node_options.rs | 14 ++++++------- rclrs/src/publisher.rs | 4 ++-- rclrs/src/subscription.rs | 5 ++--- 5 files changed, 35 insertions(+), 36 deletions(-) diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index b218fc24b..5d50d1d8d 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -83,13 +83,13 @@ impl Context { /// Creates a new context. /// /// * `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. + /// 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. /// /// * `options` - Additional options that your application can use to override - /// settings that would otherwise be determined by the environment. + /// settings that would otherwise be determined by the environment. /// /// Creating a context will fail if `args` contains invalid ROS arguments. /// diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index d91a2c859..f23a17d6d 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -28,9 +28,13 @@ unsafe impl Send for rcl_node_t {} /// 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,16 +52,19 @@ 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 +/// [3]: Node::namespace +/// [4]: Node::name +/// [5]: crate::NodeOptions::new +/// [6]: crate::NodeOptions::namespace +/// [7]: crate::Executor::create_node #[derive(Clone)] pub struct Node { pub(crate) primitives: Arc, @@ -213,12 +220,11 @@ impl Node { /// Creates a [`GuardCondition`][1] with no callback. /// /// A weak pointer to the `GuardCondition` is stored within this node. - /// When this node is added to a wait set (e.g. when calling `spin_once`[2] - /// with this node as an argument), the guard condition can be used to - /// interrupt the wait. + /// When this node is added to a wait set (e.g. when its executor is [spinning][2]), + /// the guard condition can be used to interrupt the wait. /// /// [1]: crate::GuardCondition - /// [2]: crate::spin_once + /// [2]: crate::Executor::spin pub fn create_guard_condition(&self) -> Arc { let guard_condition = Arc::new(GuardCondition::new_with_context_handle( Arc::clone(&self.handle.context_handle), @@ -232,12 +238,11 @@ impl Node { /// Creates a [`GuardCondition`][1] with a callback. /// /// A weak pointer to the `GuardCondition` is stored within this node. - /// When this node is added to a wait set (e.g. when calling `spin_once`[2] - /// with this node as an argument), the guard condition can be used to - /// interrupt the wait. + /// When this node is added to a wait set (e.g. when its executor is [spinning][2]), + /// the guard condition can be used to interrupt the wait. /// /// [1]: crate::GuardCondition - /// [2]: crate::spin_once + /// [2]: crate::Executor::spin pub fn create_guard_condition_with_callback(&mut self, callback: F) -> Arc where F: Fn() + Send + Sync + 'static, @@ -254,7 +259,6 @@ impl Node { /// Creates a [`Publisher`][1]. /// /// [1]: crate::Publisher - // TODO: make publisher's lifetime depend on node's lifetime pub fn create_publisher( &self, topic: &str, @@ -270,7 +274,6 @@ impl Node { /// Creates a [`Service`][1]. /// /// [1]: crate::Service - // TODO: make service's lifetime depend on node's lifetime pub fn create_service( &self, topic: &str, @@ -293,7 +296,6 @@ impl Node { /// Creates a [`Subscription`][1]. /// /// [1]: crate::Subscription - // TODO: make subscription's lifetime depend on node's lifetime pub fn create_subscription( &self, topic: &str, diff --git a/rclrs/src/node/node_options.rs b/rclrs/src/node/node_options.rs index 801385bcf..e4172719d 100644 --- a/rclrs/src/node/node_options.rs +++ b/rclrs/src/node/node_options.rs @@ -9,10 +9,9 @@ use crate::{ QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, }; -/// A builder for creating a [`Node`][1]. +/// A set of options 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: "/"` @@ -43,7 +42,6 @@ use crate::{ /// ``` /// /// [1]: crate::Node -/// [2]: crate::Node::builder pub struct NodeOptions { name: String, namespace: String, @@ -68,7 +66,7 @@ impl NodeOptions { /// - 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]. + /// Note that node name validation is delayed until [`Executor::create_node`][3]. /// /// # Example /// ``` @@ -88,7 +86,7 @@ impl NodeOptions { /// /// [1]: crate::Node#naming /// [2]: https://docs.ros2.org/latest/api/rmw/validate__node__name_8h.html#a5690a285aed9735f89ef11950b6e39e3 - /// [3]: NodeBuilder::build + /// [3]: crate::Executor::create_node pub fn new(name: impl ToString) -> NodeOptions { NodeOptions { name: name.to_string(), @@ -119,7 +117,7 @@ impl NodeOptions { /// - 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 /// ``` @@ -150,7 +148,7 @@ impl NodeOptions { /// [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 + /// [4]: crate::Executor::create_node pub fn namespace(mut self, namespace: impl ToString) -> Self { self.namespace = namespace.to_string(); self @@ -298,7 +296,7 @@ impl NodeOptions { let handle = Arc::new(NodeHandle { rcl_node: Mutex::new(rcl_node), - context_handle: Arc::clone(&context), + context_handle: Arc::clone(context), }); let parameter = { let rcl_node = handle.rcl_node.lock().unwrap(); diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 2935ca322..125361d0c 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -50,9 +50,9 @@ impl Drop for PublisherHandle { /// The underlying RMW will decide on the concrete delivery mechanism (network stack, shared /// memory, or intraprocess). /// -/// Sending messages does not require calling [`spin`][1] on the publisher's node. +/// Sending messages does not require the node's executor to [spin][2]. /// -/// [1]: crate::spin +/// [2]: crate::Executor::spin pub struct Publisher where T: Message, diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index fbd518c21..645f9019c 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -66,7 +66,7 @@ pub trait SubscriptionBase: Send + Sync { /// /// There can be multiple subscriptions for the same topic, in different nodes or the same node. /// -/// Receiving messages requires calling [`spin_once`][1] or [`spin`][2] on the subscription's node. +/// Receiving messages requires the node's executor to [spin][2]. /// /// When a subscription is created, it may take some time to get "matched" with a corresponding /// publisher. @@ -74,8 +74,7 @@ pub trait SubscriptionBase: Send + Sync { /// 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 +/// [2]: crate::Executor::spin /// [3]: crate::Node::create_subscription /// [4]: crate::Node pub struct Subscription From 0874d8d4bf822075d076294c14b0b4e46416b7d2 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 15:50:51 +0800 Subject: [PATCH 29/48] Fix formatting Signed-off-by: Michael X. Grey --- rclrs/src/context.rs | 5 +-- rclrs/src/error.rs | 8 +++- rclrs/src/executor.rs | 9 ++-- rclrs/src/node.rs | 13 +++--- rclrs/src/node/graph.rs | 3 +- rclrs/src/node/node_options.rs | 11 ++--- rclrs/src/parameter/service.rs | 58 ++++++++++--------------- rclrs/src/parameter/value.rs | 2 +- rclrs/src/test_helpers/graph_helpers.rs | 12 ++--- rclrs/src/time_source.rs | 9 ++-- 10 files changed, 58 insertions(+), 72 deletions(-) diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 5d50d1d8d..17894ca46 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -6,7 +6,7 @@ use std::{ vec::Vec, }; -use crate::{rcl_bindings::*, RclrsError, ToResult, Executor}; +use crate::{rcl_bindings::*, Executor, RclrsError, ToResult}; /// This is locked whenever initializing or dropping any middleware entity /// because we have found issues in RCL and some RMW implementations that @@ -74,8 +74,7 @@ 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") + Self::new([], InitOptions::default()).expect("Failed to instantiate a default context") } } diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index 515a5b95c..961e7d5e6 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -364,7 +364,13 @@ impl RclrsErrorFilter for Result<(), RclrsError> { match self { Ok(()) => Ok(()), Err(err) => { - if matches!(err, RclrsError::RclError { code: RclReturnCode::Timeout, .. }) { + if matches!( + err, + RclrsError::RclError { + code: RclReturnCode::Timeout, + .. + } + ) { return Ok(()); } diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 63951afef..3202bd34f 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,6 +1,6 @@ use crate::{ - rcl_bindings::rcl_context_is_valid, - Node, RclrsError, WaitSet, ContextHandle, NodeOptions, WeakNode, + rcl_bindings::rcl_context_is_valid, ContextHandle, Node, NodeOptions, RclrsError, WaitSet, + WeakNode, }; use std::{ sync::{Arc, Mutex}, @@ -15,10 +15,7 @@ pub struct Executor { impl Executor { /// Create a [`Node`] that will run on this Executor. - pub fn create_node( - &self, - options: impl Into, - ) -> Result { + pub fn create_node(&self, options: impl Into) -> Result { let options: NodeOptions = options.into(); let node = options.build(&self.context)?; self.nodes_mtx.lock().unwrap().push(node.downgrade()); diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index f23a17d6d..42cb3a226 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,5 +1,5 @@ -mod node_options; mod graph; +mod node_options; use std::{ cmp::PartialEq, ffi::CStr, @@ -13,10 +13,10 @@ use rosidl_runtime_rs::Message; pub use self::{graph::*, node_options::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, ContextHandle, GuardCondition, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - TimeSource, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, Client, ClientBase, Clock, ContextHandle, GuardCondition, ParameterBuilder, + ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, + ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, TimeSource, + ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -213,7 +213,8 @@ impl Node { T: rosidl_runtime_rs::Service, { let client = Arc::new(Client::::new(Arc::clone(&self.handle), topic)?); - { self.primitives.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); + { self.primitives.clients_mtx.lock().unwrap() } + .push(Arc::downgrade(&client) as Weak); Ok(client) } diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 106fac72c..385612155 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -482,8 +482,7 @@ mod tests { .map(|value: usize| if value != 99 { 99 } else { 98 }) .unwrap_or(99); - let executor = - Context::new([], InitOptions::new().with_domain_id(Some(domain_id))) + let executor = Context::new([], InitOptions::new().with_domain_id(Some(domain_id))) .unwrap() .create_basic_executor(); let node_name = "test_publisher_names_and_types"; diff --git a/rclrs/src/node/node_options.rs b/rclrs/src/node/node_options.rs index e4172719d..ae88182cb 100644 --- a/rclrs/src/node/node_options.rs +++ b/rclrs/src/node/node_options.rs @@ -4,9 +4,9 @@ use std::{ }; use crate::{ - rcl_bindings::*, - ClockType, Node, NodeHandle, ParameterInterface, NodePrimitives, ContextHandle, - QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, + rcl_bindings::*, ClockType, ContextHandle, Node, NodeHandle, NodePrimitives, + ParameterInterface, QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, + QOS_PROFILE_CLOCK, }; /// A set of options for creating a [`Node`][1]. @@ -257,10 +257,7 @@ impl NodeOptions { /// /// Only used internally. Downstream users should call /// [`Executor::create_node`]. - pub(crate) fn build( - self, - context: &Arc, - ) -> Result { + pub(crate) fn build(self, context: &Arc) -> Result { let node_name = CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul { err, diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 11df07f28..6a30a6833 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -312,8 +312,8 @@ mod tests { }, srv::rmw::*, }, - Context, MandatoryParameter, Node, ParameterRange, ParameterValue, RclrsError, - ReadOnlyParameter, NodeOptions, Executor, SpinOptions, RclrsErrorFilter, + Context, Executor, MandatoryParameter, Node, NodeOptions, ParameterRange, ParameterValue, + RclrsError, RclrsErrorFilter, ReadOnlyParameter, SpinOptions, }; use rosidl_runtime_rs::{seq, Sequence}; use std::{ @@ -346,11 +346,9 @@ mod tests { 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 node = executor + .create_node(NodeOptions::new("node").namespace(ns)) + .unwrap(); let range = ParameterRange { lower: Some(0), upper: Some(100), @@ -380,11 +378,9 @@ mod tests { .mandatory() .unwrap(); - let client = executor.create_node( - NodeOptions::new("client") - .namespace(ns) - ) - .unwrap(); + let client = executor + .create_node(NodeOptions::new("client").namespace(ns)) + .unwrap(); ( executor, @@ -447,12 +443,10 @@ mod tests { let inner_done = done.clone(); let rclrs_spin = tokio::task::spawn(async move { try_until_timeout(move || { - executor.spin( - SpinOptions::spin_once() - .timeout(Duration::ZERO) - ) - .timeout_ok() - .unwrap(); + executor + .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) + .timeout_ok() + .unwrap(); *inner_done.read().unwrap() }) @@ -594,12 +588,10 @@ mod tests { let rclrs_spin = tokio::task::spawn(async move { try_until_timeout(move || { println!(" -- spin"); - executor.spin( - SpinOptions::spin_once() - .timeout(Duration::ZERO) - ) - .timeout_ok() - .unwrap(); + executor + .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) + .timeout_ok() + .unwrap(); *inner_done.read().unwrap() }) @@ -657,8 +649,8 @@ mod tests { println!("checking client"); *client_finished.read().unwrap() }) - .await - .unwrap(); + .await + .unwrap(); // Set a mix of existing, non existing, dynamic and out of range parameters let bool_parameter = RmwParameter { @@ -807,8 +799,8 @@ mod tests { println!("checking client finished"); *client_finished.read().unwrap() }) - .await - .unwrap(); + .await + .unwrap(); *done.write().unwrap() = true; }); @@ -838,12 +830,10 @@ mod tests { let inner_done = done.clone(); let rclrs_spin = tokio::task::spawn(async move { try_until_timeout(move || { - executor.spin( - SpinOptions::spin_once() - .timeout(Duration::ZERO) - ) - .timeout_ok() - .unwrap(); + executor + .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) + .timeout_ok() + .unwrap(); *inner_done.read().unwrap() }) diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index 3a20ae24b..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, InitOptions}; + use crate::{Context, InitOptions, RclrsError, ToResult}; // TODO(luca) tests for all from / to ParameterVariant functions diff --git a/rclrs/src/test_helpers/graph_helpers.rs b/rclrs/src/test_helpers/graph_helpers.rs index ce3e82f67..8596cebd1 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -1,4 +1,4 @@ -use crate::{Context, Node, RclrsError, NodeOptions}; +use crate::{Context, Node, NodeOptions, RclrsError}; pub(crate) struct TestGraph { pub node1: Node, @@ -8,13 +8,7 @@ pub(crate) struct TestGraph { pub(crate) fn construct_test_graph(namespace: &str) -> Result { let executor = Context::default().create_basic_executor(); Ok(TestGraph { - node1: executor.create_node( - NodeOptions::new("graph_test_node_1") - .namespace(namespace) - )?, - node2: executor.create_node( - NodeOptions::new("graph_test_node_2") - .namespace(namespace) - )?, + node1: executor.create_node(NodeOptions::new("graph_test_node_1").namespace(namespace))?, + node2: executor.create_node(NodeOptions::new("graph_test_node_2").namespace(namespace))?, }) } diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index a5dcad50f..7cb4010c7 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -1,7 +1,7 @@ use crate::{ clock::{Clock, ClockSource, ClockType}, vendor::rosgraph_msgs::msg::Clock as ClockMsg, - Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, WeakNode, + Node, QoSProfile, ReadOnlyParameter, Subscription, WeakNode, QOS_PROFILE_CLOCK, }; use std::sync::{Arc, Mutex, RwLock}; @@ -149,7 +149,10 @@ mod tests { #[test] fn time_source_default_clock() { - let node = Context::default().create_basic_executor().create_node("test_node").unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node("test_node") + .unwrap(); // Default clock should be above 0 (use_sim_time is default false) assert!(node.get_clock().now().nsec > 0); } @@ -162,7 +165,7 @@ mod tests { String::from("-p"), String::from("use_sim_time:=true"), ], - InitOptions::default() + InitOptions::default(), ) .unwrap() .create_basic_executor(); From 433a3485a329def63e3a3699b99b8566f1906f64 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 17:22:36 +0800 Subject: [PATCH 30/48] Migrate to SubscriptionOptions Signed-off-by: Michael X. Grey --- examples/message_demo/src/message_demo.rs | 2 - .../minimal_pub_sub/src/minimal_subscriber.rs | 1 - .../minimal_pub_sub/src/minimal_two_nodes.rs | 1 - .../src/zero_copy_subscriber.rs | 1 - examples/rust_pubsub/src/simple_subscriber.rs | 3 +- rclrs/src/node.rs | 68 +++-- rclrs/src/node/primitive_options.rs | 244 ++++++++++++++++++ rclrs/src/qos.rs | 52 +++- rclrs/src/subscription.rs | 51 ++-- rclrs/src/time_source.rs | 21 +- 10 files changed, 384 insertions(+), 60 deletions(-) create mode 100644 rclrs/src/node/primitive_options.rs diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index 3e2bf53b2..a31ee8007 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -153,13 +153,11 @@ fn demonstrate_pubsub() -> Result<(), Error> { 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!") }, diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index ebc5fc194..5d730a8e6 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -11,7 +11,6 @@ fn main() -> Result<(), Error> { let _subscription = node.create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, move |msg: std_msgs::msg::String| { num_messages += 1; println!("I heard: '{}'", msg.data); diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index fb03574a2..e70bba87a 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -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); }, diff --git a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs index 9551dba0e..bed0a47c0 100644 --- a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -11,7 +11,6 @@ fn main() -> Result<(), Error> { let _subscription = node.create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, move |msg: rclrs::ReadOnlyLoanedMessage<'_, std_msgs::msg::UInt32>| { num_messages += 1; println!("I heard: '{}'", msg.data); diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index a0d02bb4c..1ba670f13 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -1,4 +1,4 @@ -use rclrs::{create_node, Context, Node, RclrsError, Subscription, QOS_PROFILE_DEFAULT}; +use rclrs::{create_node, Context, Node, RclrsError, Subscription}; use std::{ env, sync::{Arc, Mutex}, @@ -19,7 +19,6 @@ impl SimpleSubscriptionNode { let _subscriber = node .create_subscription::( "publish_hello", - QOS_PROFILE_DEFAULT, move |msg: StringMsg| { *data_mut.lock().unwrap() = Some(msg); }, diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 97684d6bc..e41f8a18f 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,5 +1,6 @@ mod builder; mod graph; +mod primitive_options; use std::{ cmp::PartialEq, ffi::CStr, @@ -11,12 +12,12 @@ use std::{ use rosidl_runtime_rs::Message; -pub use self::{builder::*, graph::*}; +pub use self::{builder::*, graph::*, primitive_options::*}; use crate::{ rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - TimeSource, ENTITY_LIFECYCLE_MUTEX, + SubscriptionOptions, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -292,12 +293,48 @@ impl Node { /// Creates a [`Subscription`][1]. /// - /// [1]: crate::Subscription - // TODO: make subscription's lifetime depend on node's lifetime - pub fn create_subscription( + /// + /// Pass in only the topic name for the `options` argument to use all default subscription options: + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let subscription = node.create_subscription( + /// "my_subscription", + /// |_msg: test_msgs::msg::Empty| { + /// println!("Received message!"); + /// }, + /// ); + /// ``` + /// + /// Take advantage of [`IntoPrimitiveOptions`] to easily build up the + /// subscription options: + /// + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let subscription = node.create_subscription( + /// "my_subscription" + /// .keep_last(100) + /// .transient_local(), + /// |_msg: test_msgs::msg::Empty| { + /// println!("Received message!"); + /// }, + /// ); + /// + /// let reliable_subscription = node.create_subscription( + /// "my_reliable_subscription" + /// .reliable(), + /// |_msg: test_msgs::msg::Empty| { + /// println!("Received message!"); + /// }, + /// ); + /// ``` + /// + pub fn create_subscription<'a, T, Args>( &self, - topic: &str, - qos: QoSProfile, + options: impl Into>, callback: impl SubscriptionCallback, ) -> Result>, RclrsError> where @@ -305,8 +342,7 @@ impl Node { { let subscription = Arc::new(Subscription::::new( Arc::clone(&self.handle), - topic, - qos, + options, callback, )?); { self.subscriptions_mtx.lock() } @@ -461,8 +497,7 @@ pub(crate) unsafe fn call_string_getter_with_rcl_node( #[cfg(test)] mod tests { - use super::*; - use crate::test_helpers::*; + use crate::{test_helpers::*, *}; #[test] fn traits() { @@ -472,25 +507,20 @@ mod tests { #[test] fn test_topic_names_and_types() -> Result<(), RclrsError> { - use crate::QOS_PROFILE_SYSTEM_DEFAULT; use test_msgs::msg; let graph = construct_test_graph("test_topics_graph")?; let _node_1_defaults_subscription = graph.node1.create_subscription::( "graph_test_topic_3", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::Defaults| {}, )?; - let _node_2_empty_subscription = graph.node2.create_subscription::( - "graph_test_topic_1", - QOS_PROFILE_SYSTEM_DEFAULT, - |_msg: msg::Empty| {}, - )?; + let _node_2_empty_subscription = graph + .node2 + .create_subscription::("graph_test_topic_1", |_msg: msg::Empty| {})?; let _node_2_basic_types_subscription = graph.node2.create_subscription::( "graph_test_topic_2", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::BasicTypes| {}, )?; diff --git a/rclrs/src/node/primitive_options.rs b/rclrs/src/node/primitive_options.rs new file mode 100644 index 000000000..7a8aa3eb8 --- /dev/null +++ b/rclrs/src/node/primitive_options.rs @@ -0,0 +1,244 @@ +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_duration`] 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)) + } +} + +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/qos.rs b/rclrs/src/qos.rs index b26f01ef8..83eb797d7 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, } } @@ -251,7 +251,7 @@ impl QoSProfile { /// 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 } @@ -260,6 +260,38 @@ impl QoSProfile { 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/subscription.rs b/rclrs/src/subscription.rs index fbd518c21..0494f71c9 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -10,7 +10,7 @@ use crate::{ error::{RclReturnCode, ToResult}, qos::QoSProfile, rcl_bindings::*, - NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + IntoPrimitiveOptions, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; mod callback; @@ -93,10 +93,9 @@ where T: Message, { /// Creates a new subscription. - pub(crate) fn new( + pub(crate) fn new<'a, Args>( node_handle: Arc, - topic: &str, - qos: QoSProfile, + options: impl Into>, callback: impl SubscriptionCallback, ) -> Result // This uses pub(crate) visibility to avoid instantiating this struct outside @@ -104,6 +103,7 @@ where where T: Message, { + let SubscriptionOptions { topic, qos } = options.into(); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_subscription = unsafe { rcl_get_zero_initialized_subscription() }; let type_support = @@ -114,8 +114,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 +132,7 @@ where &*rcl_node, type_support, topic_c_string.as_ptr(), - &subscription_options, + &rcl_subscription_options, ) .ok()?; } @@ -263,6 +263,31 @@ where } } +/// `SubscriptionOptions` are used by [`Node::create_subscription`] to initialize +/// a [`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, T: IntoPrimitiveOptions<'a>> From for SubscriptionOptions<'a> { + fn from(value: T) -> Self { + let options = value.into_primitive_options(); + // Topics will use the QOS_PROFILE_DEFAULT by default, which is designed + // to roughly match the ROS 1 default topic behavior. + let mut qos = QoSProfile::topics_default(); + options.apply(&mut qos); + Self { + topic: options.name, + qos, + } + } +} + impl SubscriptionBase for Subscription where T: Message, @@ -332,27 +357,23 @@ mod tests { #[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/time_source.rs b/rclrs/src/time_source.rs index a6b600800..a79398af4 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -1,7 +1,7 @@ use crate::{ clock::{Clock, ClockSource, ClockType}, vendor::rosgraph_msgs::msg::Clock as ClockMsg, - Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, + IntoPrimitiveOptions, Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, }; use std::sync::{Arc, Mutex, RwLock, Weak}; @@ -131,14 +131,17 @@ 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() } } From f12f874e8339a6678d09b1e04f286211692523ff Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 17:54:27 +0800 Subject: [PATCH 31/48] Migrate to PublisherOptions Signed-off-by: Michael X. Grey --- examples/message_demo/src/message_demo.rs | 10 +---- .../minimal_pub_sub/src/minimal_publisher.rs | 3 +- .../minimal_pub_sub/src/minimal_two_nodes.rs | 3 +- .../src/zero_copy_publisher.rs | 3 +- examples/rust_pubsub/src/simple_publisher.rs | 6 +-- rclrs/src/node.rs | 43 +++++++++++++++---- rclrs/src/publisher.rs | 39 +++++++++++++---- 7 files changed, 72 insertions(+), 35 deletions(-) diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index a31ee8007..cd42dfd94 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -141,14 +141,8 @@ fn demonstrate_pubsub() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; let node = rclrs::create_node(&context, "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::( diff --git a/examples/minimal_pub_sub/src/minimal_publisher.rs b/examples/minimal_pub_sub/src/minimal_publisher.rs index 720086917..f4cf30d0a 100644 --- a/examples/minimal_pub_sub/src/minimal_publisher.rs +++ b/examples/minimal_pub_sub/src/minimal_publisher.rs @@ -7,8 +7,7 @@ fn main() -> Result<(), Error> { let node = rclrs::create_node(&context, "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_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index e70bba87a..247db325c 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -55,8 +55,7 @@ fn main() -> Result<(), Error> { let subscriber_node_one = MinimalSubscriber::new("minimal_subscriber_one", "topic")?; let subscriber_node_two = MinimalSubscriber::new("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(); diff --git a/examples/minimal_pub_sub/src/zero_copy_publisher.rs b/examples/minimal_pub_sub/src/zero_copy_publisher.rs index 5e73b5de7..2cccd1682 100644 --- a/examples/minimal_pub_sub/src/zero_copy_publisher.rs +++ b/examples/minimal_pub_sub/src/zero_copy_publisher.rs @@ -7,8 +7,7 @@ fn main() -> Result<(), Error> { let node = rclrs::create_node(&context, "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/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index 98d0e0f74..6807ab1ef 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -1,4 +1,4 @@ -use rclrs::{create_node, Context, Node, Publisher, RclrsError, QOS_PROFILE_DEFAULT}; +use rclrs::{create_node, Context, Node, Publisher, RclrsError}; use std::{sync::Arc, thread, time::Duration}; use std_msgs::msg::String as StringMsg; struct SimplePublisherNode { @@ -8,9 +8,7 @@ struct SimplePublisherNode { 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) - .unwrap(); + let _publisher = node.create_publisher("publish_hello").unwrap(); Ok(Self { node, _publisher }) } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index e41f8a18f..8c7d9434e 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -15,7 +15,7 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*, primitive_options::*}; use crate::{ rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, + ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, PublisherOptions, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, SubscriptionOptions, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; @@ -252,19 +252,46 @@ impl Node { guard_condition } - /// Creates a [`Publisher`][1]. + /// Pass in only the topic name for the `options` argument to use all default publisher options: + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let publisher = node.create_publisher::( + /// "my_topic" + /// ) + /// .unwrap(); + /// ``` + /// + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the + /// publisher options: /// - /// [1]: crate::Publisher - // TODO: make publisher's lifetime depend on node's lifetime - pub fn create_publisher( + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let publisher = node.create_publisher::( + /// "my_topic" + /// .keep_last(100) + /// .transient_local() + /// ) + /// .unwrap(); + /// + /// let reliable_publisher = node.create_publisher::( + /// "my_topic" + /// .reliable() + /// ) + /// .unwrap(); + /// ``` + /// + pub fn create_publisher<'a, T>( &self, - topic: &str, - qos: QoSProfile, + options: impl Into>, ) -> Result>, RclrsError> where T: Message, { - let publisher = Arc::new(Publisher::::new(Arc::clone(&self.handle), topic, qos)?); + let publisher = Arc::new(Publisher::::new(Arc::clone(&self.handle), options)?); Ok(publisher) } diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 2935ca322..7cff6df6c 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, + NodeHandle, ENTITY_LIFECYCLE_MUTEX, IntoPrimitiveOptions, }; mod loaned_message; @@ -78,14 +78,14 @@ where /// Creates a new `Publisher`. /// /// Node and namespace changes are always applied _before_ topic remapping. - pub(crate) fn new( + pub(crate) fn new<'a>( node_handle: Arc, - topic: &str, - qos: QoSProfile, + options: impl Into>, ) -> Result 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 = @@ -231,6 +231,28 @@ where } } +/// `PublisherOptions` are used by [`Node::create_publisher`][1] to initialize +/// a [`Publisher`]. +/// +/// [1]: crate::NodeState::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, T: IntoPrimitiveOptions<'a>> From for PublisherOptions<'a> { + fn from(value: T) -> Self { + let options = value.into_primitive_options(); + let mut qos = QoSProfile::topics_default(); + options.apply(&mut qos); + Self { topic: options.name, qos } + } +} + /// Convenience trait for [`Publisher::publish`]. pub trait MessageCow<'a, T: Message> { /// Wrap the owned or borrowed message in a `Cow`. @@ -262,7 +284,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"; @@ -270,16 +292,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, + "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)); From 2c32e207b7b44561cf805c2741b0e9eeb60298af Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 18:03:19 +0800 Subject: [PATCH 32/48] Migrate to ServiceOptions Signed-off-by: Michael X. Grey --- rclrs/src/node.rs | 56 ++++++++++++++++++++++++++++------ rclrs/src/parameter/service.rs | 21 ++++++++----- rclrs/src/service.rs | 34 +++++++++++++++++---- 3 files changed, 89 insertions(+), 22 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 8c7d9434e..0807d1a54 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -16,7 +16,7 @@ pub use self::{builder::*, graph::*, primitive_options::*}; use crate::{ rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, PublisherOptions, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, + RclrsError, Service, ServiceBase, ServiceOptions, Subscription, SubscriptionBase, SubscriptionCallback, SubscriptionOptions, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; @@ -297,11 +297,49 @@ impl Node { /// Creates a [`Service`][1]. /// + /// Pass in only the service name for the `options` argument to use all default service options: + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let service = node.create_service::( + /// "my_service", + /// |_info, _request| { + /// println!("Received request!"); + /// test_msgs::srv::Empty_Response::default() + /// }, + /// ); + /// ``` + /// + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the + /// service options: + /// + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let service = node.create_service::( + /// "my_service" + /// .keep_all() + /// .transient_local(), + /// |_info, _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 is best not to change the reliability + /// setting unless you know what you are doing. + /// /// [1]: crate::Service - // TODO: make service's lifetime depend on node's lifetime - pub fn create_service( + /// [2]: crate::QoSReliabilityPolicy::Reliable + pub fn create_service<'a, T, F>( &self, - topic: &str, + options: impl Into>, callback: F, ) -> Result>, RclrsError> where @@ -310,7 +348,7 @@ impl Node { { let service = Arc::new(Service::::new( Arc::clone(&self.handle), - topic, + options, callback, )?); { self.services_mtx.lock().unwrap() } @@ -327,14 +365,14 @@ impl Node { /// # let context = Context::new([]).unwrap(); /// # let node = create_node(&context, "my_node").unwrap(); /// let subscription = node.create_subscription( - /// "my_subscription", + /// "my_topic", /// |_msg: test_msgs::msg::Empty| { /// println!("Received message!"); /// }, /// ); /// ``` /// - /// Take advantage of [`IntoPrimitiveOptions`] to easily build up the + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the /// subscription options: /// /// ``` @@ -342,7 +380,7 @@ impl Node { /// # let context = Context::new([]).unwrap(); /// # let node = create_node(&context, "my_node").unwrap(); /// let subscription = node.create_subscription( - /// "my_subscription" + /// "my_topic" /// .keep_last(100) /// .transient_local(), /// |_msg: test_msgs::msg::Empty| { @@ -351,7 +389,7 @@ impl Node { /// ); /// /// let reliable_subscription = node.create_subscription( - /// "my_reliable_subscription" + /// "my_reliable_topic" /// .reliable(), /// |_msg: test_msgs::msg::Empty| { /// println!("Received message!"); diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 7c8ffe62d..b4f79f77f 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -9,7 +9,8 @@ use rosidl_runtime_rs::Sequence; use super::ParameterMap; use crate::{ parameter::{DeclaredValue, ParameterKind, ParameterStorage}, - rmw_request_id_t, Node, RclrsError, Service, + rmw_request_id_t, Node, RclrsError, Service, IntoPrimitiveOptions, + QoSProfile, }; // The variables only exist to keep a strong reference to the services and are technically unused. @@ -247,7 +248,8 @@ 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"), + (fqn.clone() + "/describe_parameters") + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: DescribeParameters_Request| { let map = map.lock().unwrap(); describe_parameters(req, &map) @@ -255,7 +257,8 @@ impl ParameterService { )?; let map = parameter_map.clone(); let get_parameter_types_service = node.create_service( - &(fqn.clone() + "/get_parameter_types"), + (fqn.clone() + "/get_parameter_types") + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { let map = map.lock().unwrap(); get_parameter_types(req, &map) @@ -263,7 +266,8 @@ impl ParameterService { )?; let map = parameter_map.clone(); let get_parameters_service = node.create_service( - &(fqn.clone() + "/get_parameters"), + (fqn.clone() + "/get_parameters") + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: GetParameters_Request| { let map = map.lock().unwrap(); get_parameters(req, &map) @@ -271,7 +275,8 @@ impl ParameterService { )?; let map = parameter_map.clone(); let list_parameters_service = node.create_service( - &(fqn.clone() + "/list_parameters"), + (fqn.clone() + "/list_parameters") + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: ListParameters_Request| { let map = map.lock().unwrap(); list_parameters(req, &map) @@ -279,14 +284,16 @@ impl ParameterService { )?; let map = parameter_map.clone(); let set_parameters_service = node.create_service( - &(fqn.clone() + "/set_parameters"), + (fqn.clone() + "/set_parameters") + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, 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"), + (fqn.clone() + "/set_parameters_atomically") + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { let mut map = parameter_map.lock().unwrap(); set_parameters_atomically(req, &mut map) diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index ac43e51a8..f25727a29 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -9,7 +9,7 @@ use rosidl_runtime_rs::Message; use crate::{ error::{RclReturnCode, ToResult}, rcl_bindings::*, - MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + IntoPrimitiveOptions, MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, QoSProfile, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -80,9 +80,9 @@ where T: rosidl_runtime_rs::Service, { /// Creates a new service. - pub(crate) fn new( + pub(crate) fn new<'a, F>( node_handle: Arc, - topic: &str, + options: impl Into>, callback: F, ) -> Result // This uses pub(crate) visibility to avoid instantiating this struct outside @@ -91,17 +91,19 @@ where T: rosidl_runtime_rs::Service, F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send, { + let ServiceOptions { name, qos } = options.into(); // 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(); @@ -181,6 +183,26 @@ where } } +/// `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 for the service. + pub qos: QoSProfile, +} + +impl<'a, T: IntoPrimitiveOptions<'a>> From for ServiceOptions<'a> { + fn from(value: T) -> Self { + let options = value.into_primitive_options(); + let mut qos = QoSProfile::services_default(); + options.apply(&mut qos); + Self { name: options.name, qos } + } +} + impl ServiceBase for Service where T: rosidl_runtime_rs::Service, From 6c61c9c2f2a468926184a15843c6f412486d65d3 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 19:58:21 +0800 Subject: [PATCH 33/48] Migrate to ClientOptions Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 45 +++++++++++++++++++++++++----- rclrs/src/node.rs | 51 ++++++++++++++++++++++++++++------ rclrs/src/parameter/service.rs | 20 +++++-------- rclrs/src/publisher.rs | 13 +++++---- rclrs/src/service.rs | 7 +++-- 5 files changed, 100 insertions(+), 36 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index b308f1de2..428a0245f 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -11,7 +11,7 @@ use rosidl_runtime_rs::Message; use crate::{ error::{RclReturnCode, ToResult}, rcl_bindings::*, - MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + IntoPrimitiveOptions, MessageCow, NodeHandle, QoSProfile, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -83,23 +83,29 @@ where T: rosidl_runtime_rs::Service, { /// Creates a new client. - pub(crate) fn new(node_handle: Arc, topic: &str) -> Result + pub(crate) fn new<'a>( + node_handle: Arc, + options: impl Into>, + ) -> Result // 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(); @@ -275,6 +281,31 @@ where } } +/// `ClientOptions` are used by [`Node::create_client`][1] to initialize a +/// [`Client`] for a service. +/// +/// [1]: crate::NodeState::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, T: IntoPrimitiveOptions<'a>> From for ClientOptions<'a> { + fn from(value: T) -> Self { + let options = value.into_primitive_options(); + let mut qos = QoSProfile::services_default(); + options.apply(&mut qos); + Self { + service_name: options.name, + qos, + } + } +} + impl ClientBase for Client where T: rosidl_runtime_rs::Service, diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 0807d1a54..ad5dbda77 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -14,10 +14,11 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*, primitive_options::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, PublisherOptions, - RclrsError, Service, ServiceBase, ServiceOptions, Subscription, SubscriptionBase, SubscriptionCallback, - SubscriptionOptions, TimeSource, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, Client, ClientBase, ClientOptions, Clock, Context, ContextHandle, + GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, + PublisherOptions, RclrsError, Service, ServiceBase, ServiceOptions, Subscription, + SubscriptionBase, SubscriptionCallback, SubscriptionOptions, TimeSource, + ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -200,13 +201,45 @@ impl Node { /// 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 context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "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 context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "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][2], so it's best not to change the reliability + /// setting unless you know what you are doing. + pub fn create_client<'a, T>( + &self, + options: impl Into>, + ) -> Result>, RclrsError> where T: rosidl_runtime_rs::Service, { - let client = Arc::new(Client::::new(Arc::clone(&self.handle), topic)?); + let client = Arc::new(Client::::new(Arc::clone(&self.handle), options)?); { self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); Ok(client) } @@ -332,7 +365,7 @@ impl Node { /// 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 is best not to change the reliability + /// 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 diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index b4f79f77f..dbba78502 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -9,8 +9,7 @@ use rosidl_runtime_rs::Sequence; use super::ParameterMap; use crate::{ parameter::{DeclaredValue, ParameterKind, ParameterStorage}, - rmw_request_id_t, Node, RclrsError, Service, IntoPrimitiveOptions, - QoSProfile, + rmw_request_id_t, IntoPrimitiveOptions, Node, QoSProfile, RclrsError, Service, }; // The variables only exist to keep a strong reference to the services and are technically unused. @@ -248,8 +247,7 @@ 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") - .qos(QoSProfile::parameter_services_default()), + (fqn.clone() + "/describe_parameters").qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: DescribeParameters_Request| { let map = map.lock().unwrap(); describe_parameters(req, &map) @@ -257,8 +255,7 @@ impl ParameterService { )?; let map = parameter_map.clone(); let get_parameter_types_service = node.create_service( - (fqn.clone() + "/get_parameter_types") - .qos(QoSProfile::parameter_services_default()), + (fqn.clone() + "/get_parameter_types").qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { let map = map.lock().unwrap(); get_parameter_types(req, &map) @@ -266,8 +263,7 @@ impl ParameterService { )?; let map = parameter_map.clone(); let get_parameters_service = node.create_service( - (fqn.clone() + "/get_parameters") - .qos(QoSProfile::parameter_services_default()), + (fqn.clone() + "/get_parameters").qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: GetParameters_Request| { let map = map.lock().unwrap(); get_parameters(req, &map) @@ -275,8 +271,7 @@ impl ParameterService { )?; let map = parameter_map.clone(); let list_parameters_service = node.create_service( - (fqn.clone() + "/list_parameters") - .qos(QoSProfile::parameter_services_default()), + (fqn.clone() + "/list_parameters").qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: ListParameters_Request| { let map = map.lock().unwrap(); list_parameters(req, &map) @@ -284,8 +279,7 @@ impl ParameterService { )?; let map = parameter_map.clone(); let set_parameters_service = node.create_service( - (fqn.clone() + "/set_parameters") - .qos(QoSProfile::parameter_services_default()), + (fqn.clone() + "/set_parameters").qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: SetParameters_Request| { let mut map = map.lock().unwrap(); set_parameters(req, &mut map) @@ -293,7 +287,7 @@ impl ParameterService { )?; let set_parameters_atomically_service = node.create_service( (fqn.clone() + "/set_parameters_atomically") - .qos(QoSProfile::parameter_services_default()), + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { let mut map = parameter_map.lock().unwrap(); set_parameters_atomically(req, &mut map) diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 7cff6df6c..c37c446e0 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, + IntoPrimitiveOptions, NodeHandle, ENTITY_LIFECYCLE_MUTEX, }; mod loaned_message; @@ -249,7 +249,10 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From for PublisherOptions<'a> { let options = value.into_primitive_options(); let mut qos = QoSProfile::topics_default(); options.apply(&mut qos); - Self { topic: options.name, qos } + Self { + topic: options.name, + qos, + } } } @@ -294,9 +297,9 @@ mod tests { .node1 .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" - )?; + 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 diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index f25727a29..eb810ad1a 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -9,7 +9,7 @@ use rosidl_runtime_rs::Message; use crate::{ error::{RclReturnCode, ToResult}, rcl_bindings::*, - IntoPrimitiveOptions, MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, QoSProfile, + IntoPrimitiveOptions, MessageCow, NodeHandle, QoSProfile, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -199,7 +199,10 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From for ServiceOptions<'a> { let options = value.into_primitive_options(); let mut qos = QoSProfile::services_default(); options.apply(&mut qos); - Self { name: options.name, qos } + Self { + name: options.name, + qos, + } } } From da0536152c26203d8cf5fd050a5680718a218e7b Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 20:05:01 +0800 Subject: [PATCH 34/48] Enable direct creation of the _Options for all primitive types Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 21 ++++++++++++++------- rclrs/src/publisher.rs | 21 ++++++++++++++------- rclrs/src/service.rs | 23 +++++++++++++++-------- rclrs/src/subscription.rs | 23 ++++++++++++++--------- 4 files changed, 57 insertions(+), 31 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index 428a0245f..05282c35c 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -294,18 +294,25 @@ pub struct ClientOptions<'a> { pub qos: QoSProfile, } -impl<'a, T: IntoPrimitiveOptions<'a>> From for ClientOptions<'a> { - fn from(value: T) -> Self { - let options = value.into_primitive_options(); - let mut qos = QoSProfile::services_default(); - options.apply(&mut qos); +impl<'a> ClientOptions<'a> { + /// Initialize a new [`ClientOptions`] with default settings. + pub fn new(service_name: &'a str) -> Self { Self { - service_name: options.name, - qos, + service_name, + qos: QoSProfile::services_default(), } } } +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 + } +} + impl ClientBase for Client where T: rosidl_runtime_rs::Service, diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index c37c446e0..bc299f14e 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -244,18 +244,25 @@ pub struct PublisherOptions<'a> { pub qos: QoSProfile, } -impl<'a, T: IntoPrimitiveOptions<'a>> From for PublisherOptions<'a> { - fn from(value: T) -> Self { - let options = value.into_primitive_options(); - let mut qos = QoSProfile::topics_default(); - options.apply(&mut qos); +impl<'a> PublisherOptions<'a> { + /// Initialize a new [`PublisherOptions`] with default settings. + pub fn new(topic: &'a str) -> Self { Self { - topic: options.name, - qos, + 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 [`Publisher::publish`]. pub trait MessageCow<'a, T: Message> { /// Wrap the owned or borrowed message in a `Cow`. diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index eb810ad1a..1ff18c5b3 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -190,22 +190,29 @@ where pub struct ServiceOptions<'a> { /// The name for the service pub name: &'a str, - /// The quality of service for the service. + /// The quality of service profile for the service. pub qos: QoSProfile, } -impl<'a, T: IntoPrimitiveOptions<'a>> From for ServiceOptions<'a> { - fn from(value: T) -> Self { - let options = value.into_primitive_options(); - let mut qos = QoSProfile::services_default(); - options.apply(&mut qos); +impl<'a> ServiceOptions<'a> { + /// Initialize a new [`ServiceOptions`] with default settings. + pub fn new(name: &'a str) -> Self { Self { - name: options.name, - qos, + 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 + } +} + impl ServiceBase for Service where T: rosidl_runtime_rs::Service, diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 0494f71c9..cfc77cfc5 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -274,20 +274,25 @@ pub struct SubscriptionOptions<'a> { pub qos: QoSProfile, } -impl<'a, T: IntoPrimitiveOptions<'a>> From for SubscriptionOptions<'a> { - fn from(value: T) -> Self { - let options = value.into_primitive_options(); - // Topics will use the QOS_PROFILE_DEFAULT by default, which is designed - // to roughly match the ROS 1 default topic behavior. - let mut qos = QoSProfile::topics_default(); - options.apply(&mut qos); +impl<'a> SubscriptionOptions<'a> { + /// Initialize a new [`SubscriptionOptions`] with default settings. + pub fn new(topic: &'a str) -> Self { Self { - topic: options.name, - qos, + 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 + } +} + impl SubscriptionBase for Subscription where T: Message, From bf3d01acec2a936bdb4168e66b04de6a93d7224f Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 20:19:31 +0800 Subject: [PATCH 35/48] Migrate Node to shared state pattern Signed-off-by: Michael X. Grey --- .../minimal_pub_sub/src/minimal_two_nodes.rs | 2 +- examples/rust_pubsub/src/simple_publisher.rs | 2 +- examples/rust_pubsub/src/simple_subscriber.rs | 2 +- rclrs/src/client.rs | 2 +- rclrs/src/executor.rs | 10 +++--- rclrs/src/lib.rs | 18 +++++------ rclrs/src/node.rs | 31 +++++++++++++------ rclrs/src/node/builder.rs | 29 ++++++++--------- rclrs/src/node/graph.rs | 10 +++--- rclrs/src/parameter.rs | 2 +- rclrs/src/parameter/service.rs | 4 +-- rclrs/src/service.rs | 2 +- rclrs/src/subscription.rs | 2 +- rclrs/src/test_helpers/graph_helpers.rs | 5 ++- rclrs/src/time_source.rs | 6 ++-- 15 files changed, 70 insertions(+), 57 deletions(-) diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index fb03574a2..837a63fad 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -10,7 +10,7 @@ use anyhow::{Error, Result}; struct MinimalSubscriber { num_messages: AtomicU32, - node: Arc, + node: rclrs::Node, subscription: Mutex>>>, } diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index 98d0e0f74..d1ee9427f 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -2,7 +2,7 @@ use rclrs::{create_node, Context, Node, Publisher, RclrsError, QOS_PROFILE_DEFAU use std::{sync::Arc, thread, time::Duration}; use std_msgs::msg::String as StringMsg; struct SimplePublisherNode { - node: Arc, + node: Node, _publisher: Arc>, } impl SimplePublisherNode { diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index a0d02bb4c..01f8a6567 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -7,7 +7,7 @@ use std::{ }; use std_msgs::msg::String as StringMsg; pub struct SimpleSubscriptionNode { - node: Arc, + node: Node, _subscriber: Arc>, data: Arc>>, } diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index b308f1de2..e2ede79ef 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -67,7 +67,7 @@ type RequestId = i64; /// 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. /// -/// [1]: crate::Node::create_client +/// [1]: crate::NodeState::create_client /// [2]: crate::Node pub struct Client where diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 37c43a68e..1b77dfbce 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,4 +1,6 @@ -use crate::{rcl_bindings::rcl_context_is_valid, Node, RclReturnCode, RclrsError, WaitSet}; +use crate::{ + rcl_bindings::rcl_context_is_valid, Node, NodeState, RclReturnCode, RclrsError, WaitSet, +}; use std::{ sync::{Arc, Mutex, Weak}, time::Duration, @@ -6,7 +8,7 @@ use std::{ /// Single-threaded executor implementation. pub struct SingleThreadedExecutor { - nodes_mtx: Mutex>>, + nodes_mtx: Mutex>>, } impl Default for SingleThreadedExecutor { @@ -24,13 +26,13 @@ impl SingleThreadedExecutor { } /// Add a node to the executor. - pub fn add_node(&self, node: &Arc) -> Result<(), RclrsError> { + pub fn add_node(&self, node: &Node) -> Result<(), RclrsError> { { self.nodes_mtx.lock().unwrap() }.push(Arc::downgrade(node)); Ok(()) } /// Remove a node from the executor. - pub fn remove_node(&self, node: Arc) -> Result<(), RclrsError> { + pub fn remove_node(&self, node: Node) -> Result<(), RclrsError> { { self.nodes_mtx.lock().unwrap() } .retain(|n| !n.upgrade().map(|n| Arc::ptr_eq(&n, &node)).unwrap_or(false)); Ok(()) diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 4924b36cb..281dc3788 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -30,7 +30,7 @@ mod rcl_bindings; #[cfg(feature = "dyn_msg")] pub mod dynamic_message; -use std::{sync::Arc, time::Duration}; +use std::time::Duration; pub use arguments::*; pub use client::*; @@ -59,14 +59,14 @@ pub use wait::*; /// This can usually be ignored. /// /// [1]: crate::RclReturnCode -pub fn spin_once(node: Arc, timeout: Option) -> Result<(), RclrsError> { +pub fn spin_once(node: Node, 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> { +pub fn spin(node: Node) -> Result<(), RclrsError> { let executor = SingleThreadedExecutor::new(); executor.add_node(&node)?; executor.spin() @@ -77,7 +77,7 @@ pub fn spin(node: Arc) -> Result<(), RclrsError> { /// Convenience function equivalent to [`Node::new`][1]. /// Please see that function's documentation. /// -/// [1]: crate::Node::new +/// [1]: crate::NodeState::new /// /// # Example /// ``` @@ -87,17 +87,17 @@ pub fn spin(node: Arc) -> Result<(), RclrsError> { /// assert!(node.is_ok()); /// # Ok::<(), RclrsError>(()) /// ``` -pub fn create_node(context: &Context, node_name: &str) -> Result, RclrsError> { - Node::new(context, node_name) +pub fn create_node(context: &Context, node_name: &str) -> Result { + NodeState::new(context, node_name) } /// Creates a [`NodeBuilder`]. /// -/// Convenience function equivalent to [`NodeBuilder::new()`][1] and [`Node::builder()`][2]. +/// Convenience function equivalent to [`NodeBuilder::new()`][1] and [`NodeState::builder()`][2]. /// Please see that function's documentation. /// /// [1]: crate::NodeBuilder::new -/// [2]: crate::Node::builder +/// [2]: crate::NodeState::builder /// /// # Example /// ``` @@ -109,5 +109,5 @@ pub fn create_node(context: &Context, node_name: &str) -> Result, Rclr /// # Ok::<(), RclrsError>(()) /// ``` pub fn create_node_builder(context: &Context, node_name: &str) -> NodeBuilder { - Node::builder(context, node_name) + NodeState::builder(context, node_name) } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 97684d6bc..100d9f127 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -48,7 +48,7 @@ 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. +/// [`NodeState::namespace()`] and [`NodeState::name()`] functions for examples. /// /// ## Rules for valid names /// The rules for valid node names and node namespaces are explained in @@ -58,7 +58,18 @@ unsafe impl Send for rcl_node_t {} /// [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 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 { pub(crate) clients_mtx: Mutex>>, pub(crate) guard_conditions_mtx: Mutex>>, pub(crate) services_mtx: Mutex>>, @@ -89,15 +100,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()) @@ -105,12 +116,12 @@ impl fmt::Debug for Node { } } -impl Node { +impl NodeState { /// 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> { + pub fn new(context: &Context, node_name: &str) -> Result { Self::builder(context, node_name).build() } @@ -171,7 +182,7 @@ 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 /// ``` @@ -431,9 +442,9 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, RclrsError}; + /// # use rclrs::{Context, NodeState, RclrsError}; /// let context = Context::new([])?; - /// let node = Node::builder(&context, "my_node").build()?; + /// let node = NodeState::builder(&context, "my_node").build()?; /// assert_eq!(node.name(), "my_node"); /// # Ok::<(), RclrsError>(()) /// ``` diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 011d5d2f3..eee3a4bec 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -4,14 +4,15 @@ use std::{ }; use crate::{ - rcl_bindings::*, ClockType, Context, ContextHandle, Node, NodeHandle, ParameterInterface, - QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, + rcl_bindings::*, ClockType, Context, ContextHandle, 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]. +/// This struct instance can be created via [`NodeState::builder()`][2]. /// /// The default values for optional fields are: /// - `namespace: "/"` @@ -24,17 +25,17 @@ use crate::{ /// /// # Example /// ``` -/// # use rclrs::{Context, NodeBuilder, Node, RclrsError}; +/// # use rclrs::{Context, NodeBuilder, NodeState, 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()?; +/// // Building a node via NodeState::builder() +/// let node = NodeState::builder(&context, "bar_node").build()?; /// assert_eq!(node.name(), "bar_node"); /// // Building a node step-by-step -/// let mut builder = Node::builder(&context, "goose"); +/// let mut builder = NodeState::builder(&context, "goose"); /// builder = builder.namespace("/duck/duck"); /// let node = builder.build()?; /// assert_eq!(node.fully_qualified_name(), "/duck/duck/goose"); @@ -42,7 +43,7 @@ use crate::{ /// ``` /// /// [1]: crate::Node -/// [2]: crate::Node::builder +/// [2]: crate::NodeState::builder pub struct NodeBuilder { context: Arc, name: String, @@ -126,14 +127,14 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, RclrsError, RclReturnCode}; + /// # use rclrs::{Context, NodeState, RclrsError, RclReturnCode}; /// let context = Context::new([])?; /// // This is a valid namespace - /// let builder_ok_ns = Node::builder(&context, "my_node").namespace("/some/nested/namespace"); + /// let builder_ok_ns = NodeState::builder(&context, "my_node").namespace("/some/nested/namespace"); /// assert!(builder_ok_ns.build().is_ok()); /// // This is an invalid namespace /// assert!(matches!( - /// Node::builder(&context, "my_node") + /// NodeState::builder(&context, "my_node") /// .namespace("/10_percent_luck/20_percent_skill") /// .build() /// .unwrap_err(), @@ -141,7 +142,7 @@ impl NodeBuilder { /// )); /// // A missing forward slash at the beginning is automatically added /// assert_eq!( - /// Node::builder(&context, "my_node") + /// NodeState::builder(&context, "my_node") /// .namespace("foo") /// .build()? /// .namespace(), @@ -262,7 +263,7 @@ impl NodeBuilder { /// For example usage, see the [`NodeBuilder`][1] docs. /// /// [1]: crate::NodeBuilder - pub fn build(&self) -> Result, RclrsError> { + pub fn build(&self) -> Result { let node_name = CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul { err, @@ -308,7 +309,7 @@ impl NodeBuilder { &rcl_context.global_arguments, )? }; - let node = Arc::new(Node { + let node = Arc::new(NodeState { handle, clients_mtx: Mutex::new(vec![]), guard_conditions_mtx: Mutex::new(vec![]), diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 343fa61d3..8dc40e3ec 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, @@ -486,7 +486,7 @@ mod tests { Context::new_with_options([], InitOptions::new().with_domain_id(Some(domain_id))) .unwrap(); let node_name = "test_publisher_names_and_types"; - let node = Node::new(&context, node_name).unwrap(); + let node = NodeState::new(&context, node_name).unwrap(); // Test that the graph has no publishers let names_and_topics = node .get_publisher_names_and_types_by_node(node_name, "") @@ -545,7 +545,7 @@ mod tests { fn test_node_names() { let context = Context::new([]).unwrap(); let node_name = "test_node_names"; - let node = Node::new(&context, node_name).unwrap(); + let node = NodeState::new(&context, node_name).unwrap(); let names_and_namespaces = node.get_node_names().unwrap(); @@ -561,7 +561,7 @@ mod tests { fn test_node_names_with_enclaves() { let context = Context::new([]).unwrap(); let node_name = "test_node_names_with_enclaves"; - let node = Node::new(&context, node_name).unwrap(); + let node = NodeState::new(&context, node_name).unwrap(); let names_and_namespaces = node.get_node_names_with_enclaves().unwrap(); diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index c7153ce01..26d22725e 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -82,7 +82,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, diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 7c8ffe62d..996be8538 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -319,7 +319,7 @@ mod tests { use std::sync::{Arc, RwLock}; struct TestNode { - node: Arc, + node: Node, bool_param: MandatoryParameter, _ns_param: MandatoryParameter, _read_only_param: ReadOnlyParameter, @@ -341,7 +341,7 @@ mod tests { Ok(()) } - fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { + fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Node) { let node = NodeBuilder::new(context, "node") .namespace(ns) .build() diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index ac43e51a8..47579c862 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -64,7 +64,7 @@ type ServiceCallback = /// 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. /// -/// [1]: crate::Node::create_service +/// [1]: crate::NodeState::create_service /// [2]: crate::Node pub struct Service where diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index fbd518c21..395d29426 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -76,7 +76,7 @@ pub trait SubscriptionBase: Send + Sync { /// /// [1]: crate::spin_once /// [2]: crate::spin -/// [3]: crate::Node::create_subscription +/// [3]: crate::NodeState::create_subscription /// [4]: crate::Node pub struct Subscription where diff --git a/rclrs/src/test_helpers/graph_helpers.rs b/rclrs/src/test_helpers/graph_helpers.rs index 1e9b581ae..2ee3d3bb0 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -1,9 +1,8 @@ use crate::{Context, Node, NodeBuilder, RclrsError}; -use std::sync::Arc; 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 { diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index a6b600800..4237230ee 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -1,7 +1,7 @@ use crate::{ clock::{Clock, ClockSource, ClockType}, vendor::rosgraph_msgs::msg::Clock as ClockMsg, - Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, + Node, NodeState, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, }; use std::sync::{Arc, Mutex, RwLock, Weak}; @@ -9,7 +9,7 @@ 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, @@ -85,7 +85,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 From bbb5333f094e3ef102844b8e8db540ff67820ded Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 23:00:56 +0800 Subject: [PATCH 36/48] Migrate primitives to state pattern Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 25 +++++++++--- rclrs/src/node.rs | 32 +++++++-------- rclrs/src/parameter/service.rs | 12 +++--- rclrs/src/publisher.rs | 33 ++++++++++++---- rclrs/src/publisher/loaned_message.rs | 6 +-- rclrs/src/service.rs | 33 ++++++++++++---- rclrs/src/subscription.rs | 39 ++++++++++++------- .../subscription/readonly_loaned_message.rs | 6 +-- rclrs/src/time_source.rs | 4 +- 9 files changed, 125 insertions(+), 65 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index e2ede79ef..b7ac77521 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -64,12 +64,25 @@ type RequestId = i64; /// Main class responsible for sending requests to a ROS service. /// -/// 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. +/// 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::Node -pub struct Client +/// [2]: crate::spin +pub type Client = Arc>; + +/// The inner state of a [`Client`]. +/// +/// 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. +/// +/// The public API of the [`Client`] type is implemented via `ClientState`. +/// +/// [1]: std::sync::Weak +pub struct ClientState where T: rosidl_runtime_rs::Service, { @@ -78,7 +91,7 @@ where futures: Arc>>>, } -impl Client +impl ClientState where T: rosidl_runtime_rs::Service, { @@ -275,7 +288,7 @@ where } } -impl ClientBase for Client +impl ClientBase for ClientState where T: rosidl_runtime_rs::Service, { diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 100d9f127..118cfd01e 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -13,10 +13,10 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - TimeSource, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, Client, ClientBase, ClientState, Clock, Context, ContextHandle, + GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, + PublisherState, QoSProfile, RclrsError, Service, ServiceBase, ServiceState, Subscription, + SubscriptionBase, SubscriptionCallback, SubscriptionState, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -212,11 +212,11 @@ impl NodeState { /// /// [1]: crate::Client // TODO: make client's lifetime depend on node's lifetime - pub fn create_client(&self, topic: &str) -> Result>, RclrsError> + pub fn create_client(&self, topic: &str) -> Result, RclrsError> where T: rosidl_runtime_rs::Service, { - let client = Arc::new(Client::::new(Arc::clone(&self.handle), topic)?); + let client = Arc::new(ClientState::::new(Arc::clone(&self.handle), topic)?); { self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); Ok(client) } @@ -270,11 +270,15 @@ impl NodeState { &self, topic: &str, qos: QoSProfile, - ) -> Result>, RclrsError> + ) -> Result, RclrsError> where T: Message, { - let publisher = Arc::new(Publisher::::new(Arc::clone(&self.handle), topic, qos)?); + let publisher = Arc::new(PublisherState::::new( + Arc::clone(&self.handle), + topic, + qos, + )?); Ok(publisher) } @@ -282,16 +286,12 @@ impl NodeState { /// /// [1]: crate::Service // TODO: make service's lifetime depend on node's lifetime - pub fn create_service( - &self, - topic: &str, - callback: F, - ) -> Result>, RclrsError> + pub fn create_service(&self, topic: &str, callback: F) -> Result, RclrsError> where T: rosidl_runtime_rs::Service, F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send, { - let service = Arc::new(Service::::new( + let service = Arc::new(ServiceState::::new( Arc::clone(&self.handle), topic, callback, @@ -310,11 +310,11 @@ impl NodeState { topic: &str, qos: QoSProfile, callback: impl SubscriptionCallback, - ) -> Result>, RclrsError> + ) -> Result, RclrsError> where T: Message, { - let subscription = Arc::new(Subscription::::new( + let subscription = Arc::new(SubscriptionState::::new( Arc::clone(&self.handle), topic, qos, diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 996be8538..efe90278f 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -16,17 +16,17 @@ use crate::{ // 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( diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 2935ca322..33746c1bc 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -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 calling [`spin`][2] on the publisher's node. +/// +/// [1]: crate::NodeState::create_publisher +/// [2]: crate::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,12 +83,12 @@ 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, { @@ -179,7 +196,7 @@ where } } -impl Publisher +impl PublisherState where T: RmwMessage, { @@ -231,7 +248,7 @@ where } } -/// Convenience trait for [`Publisher::publish`]. +/// 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>; diff --git a/rclrs/src/publisher/loaned_message.rs b/rclrs/src/publisher/loaned_message.rs index c03fc300f..66ad1046a 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<'a, T> Deref for LoanedMessage<'a, T> diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index 47579c862..8c02ec4e4 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -59,14 +59,33 @@ pub trait ServiceBase: Send + Sync { type ServiceCallback = Box Response + 'static + Send>; -/// Main class responsible for responding to requests sent by ROS clients. +/// Provide a service that can respond to requests sent by ROS service clients. /// -/// 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. +/// Create a service using [`Node::create_service`][1]. +/// +/// 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][2]. /// /// [1]: crate::NodeState::create_service -/// [2]: crate::Node -pub struct Service +/// [2]: crate::spin +pub type Service = Arc>; + +/// The inner state of a [`Service`]. +/// +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Service`] in a non-owning way. It is +/// generally recommended to manage the `ServiceState` inside of an [`Arc`], +/// and [`Service`] is provided as a convenience alias for that. +/// +/// The public API of the [`Service`] type is implemented via `ServiceState`. +/// +/// [1]: std::sync::Weak +pub struct ServiceState where T: rosidl_runtime_rs::Service, { @@ -75,7 +94,7 @@ where pub callback: Mutex>, } -impl Service +impl ServiceState where T: rosidl_runtime_rs::Service, { @@ -181,7 +200,7 @@ where } } -impl ServiceBase for Service +impl ServiceBase for ServiceState where T: rosidl_runtime_rs::Service, { diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 395d29426..8580b499d 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -64,21 +64,32 @@ pub trait SubscriptionBase: Send + Sync { /// Struct for receiving messages of type `T`. /// +/// Create a subscription using [`Node::create_subscription`][1]. +/// /// 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 the node's executor to [spin][2]. /// /// 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 +/// [1]: crate::NodeState::create_subscription /// [2]: crate::spin -/// [3]: crate::NodeState::create_subscription -/// [4]: crate::Node -pub struct Subscription +pub type Subscription = Arc>; + +/// The inner state of a [`Subscription`]. +/// +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Subscription`] in a non-owning way. It is +/// generally recommended to manage the `SubscriptionState` inside of an [`Arc`], +/// and [`Subscription`] is provided as a convenience alias for that. +/// +/// The public API of the [`Subscription`] type is implemented via `SubscriptionState`. +/// +/// [1]: std::sync::Weak +pub struct SubscriptionState where T: Message, { @@ -88,7 +99,7 @@ where message: PhantomData, } -impl Subscription +impl SubscriptionState where T: Message, { @@ -233,11 +244,11 @@ where /// When there is no new message, this will return a /// [`SubscriptionTakeFailed`][1]. /// - /// This is the counterpart to [`Publisher::borrow_loaned_message()`][2]. See its documentation + /// This is the counterpart to [`PublisherState::borrow_loaned_message()`][2]. See its documentation /// for more information. /// /// [1]: crate::RclrsError - /// [2]: crate::Publisher::borrow_loaned_message + /// [2]: crate::PublisherState::borrow_loaned_message pub fn take_loaned(&self) -> Result<(ReadOnlyLoanedMessage<'_, T>, MessageInfo), RclrsError> { let mut msg_ptr = std::ptr::null_mut(); let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; @@ -263,7 +274,7 @@ where } } -impl SubscriptionBase for Subscription +impl SubscriptionBase for SubscriptionState where T: Message, { @@ -326,8 +337,8 @@ mod tests { #[test] fn traits() { - assert_send::>(); - assert_sync::>(); + assert_send::>(); + assert_sync::>(); } #[test] diff --git a/rclrs/src/subscription/readonly_loaned_message.rs b/rclrs/src/subscription/readonly_loaned_message.rs index df4ad6a5b..d287cc1b8 100644 --- a/rclrs/src/subscription/readonly_loaned_message.rs +++ b/rclrs/src/subscription/readonly_loaned_message.rs @@ -2,7 +2,7 @@ use std::ops::Deref; use rosidl_runtime_rs::Message; -use crate::{rcl_bindings::*, Subscription, ToResult}; +use crate::{rcl_bindings::*, SubscriptionState, ToResult}; /// A message that is owned by the middleware, loaned out for reading. /// @@ -10,7 +10,7 @@ use crate::{rcl_bindings::*, Subscription, ToResult}; /// message, it's the same as `&T`, and otherwise it's the corresponding RMW-native /// message. /// -/// This type is returned by [`Subscription::take_loaned()`] and may be used in +/// This type is returned by [`SubscriptionState::take_loaned()`] and may be used in /// subscription callbacks. /// /// The loan is returned by dropping the `ReadOnlyLoanedMessage`. @@ -19,7 +19,7 @@ where T: Message, { pub(super) msg_ptr: *const T::RmwMsg, - pub(super) subscription: &'a Subscription, + pub(super) subscription: &'a SubscriptionState, } impl<'a, T> Deref for ReadOnlyLoanedMessage<'a, T> diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index 4237230ee..747c696ce 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -14,7 +14,7 @@ pub(crate) struct TimeSource { 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 @@ -122,7 +122,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 From 4c2a67b0472db677ae1a840b8bb755ebcc62dc14 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 23:17:57 +0800 Subject: [PATCH 37/48] Fix example formatting Signed-off-by: Michael X. Grey --- .../minimal_client_service/src/minimal_client.rs | 4 +++- .../src/minimal_client_async.rs | 3 ++- .../src/minimal_service.rs | 4 +++- .../minimal_pub_sub/src/minimal_subscriber.rs | 4 +++- .../minimal_pub_sub/src/minimal_two_nodes.rs | 16 ++++++++++++---- .../minimal_pub_sub/src/zero_copy_subscriber.rs | 4 +++- examples/rust_pubsub/src/simple_subscriber.rs | 5 +---- 7 files changed, 27 insertions(+), 13 deletions(-) diff --git a/examples/minimal_client_service/src/minimal_client.rs b/examples/minimal_client_service/src/minimal_client.rs index baeff971d..49f18e242 100644 --- a/examples/minimal_client_service/src/minimal_client.rs +++ b/examples/minimal_client_service/src/minimal_client.rs @@ -28,5 +28,7 @@ fn main() -> Result<(), Error> { std::thread::sleep(std::time::Duration::from_millis(500)); println!("Waiting for response"); - executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) + executor + .spin(rclrs::SpinOptions::default()) + .map_err(|err| err.into()) } diff --git a/examples/minimal_client_service/src/minimal_client_async.rs b/examples/minimal_client_service/src/minimal_client_async.rs index f011569de..8babe04e7 100644 --- a/examples/minimal_client_service/src/minimal_client_async.rs +++ b/examples/minimal_client_service/src/minimal_client_async.rs @@ -20,7 +20,8 @@ async fn main() -> Result<(), Error> { println!("Waiting for response"); - let rclrs_spin = tokio::task::spawn_blocking(move || executor.spin(rclrs::SpinOptions::default())); + let rclrs_spin = + tokio::task::spawn_blocking(move || executor.spin(rclrs::SpinOptions::default())); let response = future.await?; println!( diff --git a/examples/minimal_client_service/src/minimal_service.rs b/examples/minimal_client_service/src/minimal_service.rs index f38a49f21..84d154fec 100644 --- a/examples/minimal_client_service/src/minimal_service.rs +++ b/examples/minimal_client_service/src/minimal_service.rs @@ -19,5 +19,7 @@ fn main() -> Result<(), Error> { .create_service::("add_two_ints", handle_service)?; println!("Starting server"); - executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) + executor + .spin(rclrs::SpinOptions::default()) + .map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index 2d375bd7d..2f0820660 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -18,5 +18,7 @@ fn main() -> Result<(), Error> { }, )?; - executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) + executor + .spin(rclrs::SpinOptions::default()) + .map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index f829afc52..b894b1912 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -12,7 +12,11 @@ struct MinimalSubscriber { } impl MinimalSubscriber { - pub fn new(executor: &rclrs::Executor, name: &str, topic: &str) -> Result, rclrs::RclrsError> { + 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(), @@ -49,8 +53,10 @@ fn main() -> Result<(), Error> { 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(&executor, "minimal_subscriber_one", "topic")?; - let _subscriber_node_two = MinimalSubscriber::new(&executor, "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)?; @@ -67,5 +73,7 @@ fn main() -> Result<(), Error> { } }); - executor.spin(rclrs::SpinOptions::default()).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_subscriber.rs b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs index cfb569ab2..7b9f538e9 100644 --- a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -17,5 +17,7 @@ fn main() -> Result<(), Error> { }, )?; - executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) + executor + .spin(rclrs::SpinOptions::default()) + .map_err(|err| err.into()) } diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index 2ba2c4e7a..4df161221 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -25,10 +25,7 @@ impl SimpleSubscriptionNode { }, ) .unwrap(); - Ok(Self { - _subscriber, - data, - }) + Ok(Self { _subscriber, data }) } fn data_callback(&self) -> Result<(), RclrsError> { if let Some(data) = self.data.lock().unwrap().as_ref() { From daebaa888bbe615af7fa10d51a3787af4ecc3ac2 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 23:19:06 +0800 Subject: [PATCH 38/48] Fix example formatting Signed-off-by: Michael X. Grey --- examples/message_demo/src/message_demo.rs | 6 ++++-- examples/rust_pubsub/src/simple_subscriber.rs | 9 +++------ 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index cd42dfd94..bed233c9e 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -141,8 +141,10 @@ fn demonstrate_pubsub() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; let node = rclrs::create_node(&context, "message_demo")?; - let idiomatic_publisher = node.create_publisher::("topic")?; - let direct_publisher = node.create_publisher::("topic")?; + let idiomatic_publisher = + node.create_publisher::("topic")?; + let direct_publisher = + node.create_publisher::("topic")?; let _idiomatic_subscription = node .create_subscription::( diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index 1ba670f13..488f53c00 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -17,12 +17,9 @@ impl SimpleSubscriptionNode { let data: Arc>> = Arc::new(Mutex::new(None)); let data_mut: Arc>> = Arc::clone(&data); let _subscriber = node - .create_subscription::( - "publish_hello", - move |msg: StringMsg| { - *data_mut.lock().unwrap() = Some(msg); - }, - ) + .create_subscription::("publish_hello", move |msg: StringMsg| { + *data_mut.lock().unwrap() = Some(msg); + }) .unwrap(); Ok(Self { node, From 2cff0b7599e3494044d290e00796d3f3bd72719c Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 23:25:52 +0800 Subject: [PATCH 39/48] Fix examples Signed-off-by: Michael X. Grey --- examples/minimal_pub_sub/src/minimal_two_nodes.rs | 2 +- examples/rust_pubsub/src/simple_publisher.rs | 2 +- examples/rust_pubsub/src/simple_subscriber.rs | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index 837a63fad..8712509bf 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -11,7 +11,7 @@ use anyhow::{Error, Result}; struct MinimalSubscriber { num_messages: AtomicU32, node: rclrs::Node, - subscription: Mutex>>>, + subscription: Mutex>>, } impl MinimalSubscriber { diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index d1ee9427f..359dfc16f 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -3,7 +3,7 @@ use std::{sync::Arc, thread, time::Duration}; use std_msgs::msg::String as StringMsg; struct SimplePublisherNode { node: Node, - _publisher: Arc>, + _publisher: Publisher, } impl SimplePublisherNode { fn new(context: &Context) -> Result { diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index 01f8a6567..f98ff8c58 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -8,7 +8,7 @@ use std::{ use std_msgs::msg::String as StringMsg; pub struct SimpleSubscriptionNode { node: Node, - _subscriber: Arc>, + _subscriber: Subscription, data: Arc>>, } impl SimpleSubscriptionNode { From 126aaca87f0447a03430b20988b1b2ebbfd447ae Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 21 Nov 2024 00:10:27 +0800 Subject: [PATCH 40/48] Fix docs Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 2 +- rclrs/src/node.rs | 8 +++++--- rclrs/src/publisher.rs | 2 +- rclrs/src/subscription.rs | 4 +++- 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index 05282c35c..b679402a0 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -284,7 +284,7 @@ where /// `ClientOptions` are used by [`Node::create_client`][1] to initialize a /// [`Client`] for a service. /// -/// [1]: crate::NodeState::create_client +/// [1]: crate::Node::create_client #[derive(Debug, Clone)] #[non_exhaustive] pub struct ClientOptions<'a> { diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index ad5dbda77..1e47a70f6 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -199,7 +199,7 @@ impl Node { unsafe { call_string_getter_with_rcl_node(&rcl_node, getter) } } - /// Creates a [`Client`][1]. + /// Creates a [`Client`]. /// /// Pass in only the service name for the `options` argument to use all default client options: /// ``` @@ -230,8 +230,10 @@ impl Node { /// 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][2], so it's best not to change the reliability + /// 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, options: impl Into>, @@ -389,7 +391,7 @@ impl Node { Ok(service) } - /// Creates a [`Subscription`][1]. + /// Creates a [`Subscription`]. /// /// /// Pass in only the topic name for the `options` argument to use all default subscription options: diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index bc299f14e..cb43c1746 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -234,7 +234,7 @@ where /// `PublisherOptions` are used by [`Node::create_publisher`][1] to initialize /// a [`Publisher`]. /// -/// [1]: crate::NodeState::create_publisher +/// [1]: crate::Node::create_publisher #[derive(Debug, Clone)] #[non_exhaustive] pub struct PublisherOptions<'a> { diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index cfc77cfc5..f99d4924d 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -263,8 +263,10 @@ where } } -/// `SubscriptionOptions` are used by [`Node::create_subscription`] to initialize +/// `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> { From 012ff2e245699666c94d55bfb5dac561487ccff4 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 21 Nov 2024 13:14:34 +0800 Subject: [PATCH 41/48] Make deadline, liveliness_lease, and lifespan all symmetric Signed-off-by: Michael X. Grey --- rclrs/src/node/primitive_options.rs | 2 +- rclrs/src/qos.rs | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rclrs/src/node/primitive_options.rs b/rclrs/src/node/primitive_options.rs index 7a8aa3eb8..5682873cf 100644 --- a/rclrs/src/node/primitive_options.rs +++ b/rclrs/src/node/primitive_options.rs @@ -37,7 +37,7 @@ pub struct PrimitiveOptions<'a> { pub lifespan: Option, /// Override the default [`QoSProfile::liveliness`] for the primitive. pub liveliness: Option, - /// Override the default [`QoSProfile::liveliness_lease_duration`] for the primitive. + /// 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, diff --git a/rclrs/src/qos.rs b/rclrs/src/qos.rs index 83eb797d7..699576964 100644 --- a/rclrs/src/qos.rs +++ b/rclrs/src/qos.rs @@ -244,7 +244,7 @@ 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 } @@ -256,7 +256,7 @@ impl QoSProfile { } /// 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 } From f33e8d5f24ef82a4b9f6bffaabd5e5466ff023b1 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 21 Nov 2024 13:17:33 +0800 Subject: [PATCH 42/48] Add an API to the primitive options builder for avoiding ROS namespace conventions Signed-off-by: Michael X. Grey --- rclrs/src/node/primitive_options.rs | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rclrs/src/node/primitive_options.rs b/rclrs/src/node/primitive_options.rs index 5682873cf..0299c70f0 100644 --- a/rclrs/src/node/primitive_options.rs +++ b/rclrs/src/node/primitive_options.rs @@ -171,6 +171,15 @@ pub trait IntoPrimitiveOptions<'a>: Sized { 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> { From 98b8ea2461217116326e6a5db43aa4b6a08c9c01 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 21 Nov 2024 13:51:28 +0800 Subject: [PATCH 43/48] Retrigger CI Signed-off-by: Michael X. Grey From 605950695c62eed54f02b03ea9f4f6a21f0e28a5 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 23 Nov 2024 17:03:50 +0800 Subject: [PATCH 44/48] Implicitly cast &str to NodeOptions Signed-off-by: Michael X. Grey --- rclrs/src/executor.rs | 9 +- rclrs/src/node.rs | 8 +- rclrs/src/node/node_options.rs | 298 ++++++++++++++---------- rclrs/src/parameter/service.rs | 4 +- rclrs/src/test_helpers/graph_helpers.rs | 6 +- 5 files changed, 184 insertions(+), 141 deletions(-) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 3202bd34f..64c3baee6 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,5 +1,5 @@ use crate::{ - rcl_bindings::rcl_context_is_valid, ContextHandle, Node, NodeOptions, RclrsError, WaitSet, + rcl_bindings::rcl_context_is_valid, ContextHandle, IntoNodeOptions, Node, RclrsError, WaitSet, WeakNode, }; use std::{ @@ -15,8 +15,11 @@ pub struct Executor { impl Executor { /// Create a [`Node`] that will run on this Executor. - pub fn create_node(&self, options: impl Into) -> Result { - let options: NodeOptions = options.into(); + pub fn create_node<'a>( + &'a self, + options: impl IntoNodeOptions<'a>, + ) -> Result { + let options = options.into_node_options(); let node = options.build(&self.context)?; self.nodes_mtx.lock().unwrap().push(node.downgrade()); Ok(node) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 42cb3a226..117a05da1 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -156,11 +156,11 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, InitOptions, RclrsError, NodeOptions}; + /// # use rclrs::{Context, InitOptions, RclrsError, IntoNodeOptions}; /// // Without remapping /// let executor = Context::default().create_basic_executor(); /// let node = executor.create_node( - /// NodeOptions::new("my_node") + /// "my_node" /// .namespace("/my/namespace") /// )?; /// assert_eq!(node.namespace(), "/my/namespace"); @@ -182,10 +182,10 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError, NodeOptions}; + /// # use rclrs::{Context, RclrsError, IntoNodeOptions}; /// let executor = Context::default().create_basic_executor(); /// let node = executor.create_node( - /// NodeOptions::new("my_node") + /// "my_node" /// .namespace("/my/namespace") /// )?; /// assert_eq!(node.fully_qualified_name(), "/my/namespace/my_node"); diff --git a/rclrs/src/node/node_options.rs b/rclrs/src/node/node_options.rs index ae88182cb..467bbed8f 100644 --- a/rclrs/src/node/node_options.rs +++ b/rclrs/src/node/node_options.rs @@ -1,4 +1,5 @@ use std::{ + borrow::Borrow, ffi::CString, sync::{Arc, Mutex}, }; @@ -9,96 +10,13 @@ use crate::{ QOS_PROFILE_CLOCK, }; -/// A set of options for creating a [`Node`][1]. -/// -/// The builder pattern 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` +/// This trait helps to build [`NodeOptions`] which can be passed into +/// [`Executor::create_node`][1]. /// -/// # Example -/// ``` -/// # use rclrs::{Context, NodeOptions, Node, RclrsError}; -/// let executor = Context::default().create_basic_executor(); -/// // Building a node in a single expression -/// let node = executor.create_node(NodeOptions::new("foo_node").namespace("/bar"))?; -/// assert_eq!(node.name(), "foo_node"); -/// assert_eq!(node.namespace(), "/bar"); -/// // Building a node via NodeOptions -/// let node = executor.create_node(NodeOptions::new("bar_node"))?; -/// assert_eq!(node.name(), "bar_node"); -/// // Building a node step-by-step -/// let mut options = NodeOptions::new("goose"); -/// options = options.namespace("/duck/duck"); -/// let node = executor.create_node(options)?; -/// assert_eq!(node.fully_qualified_name(), "/duck/duck/goose"); -/// # Ok::<(), RclrsError>(()) -/// ``` -/// -/// [1]: crate::Node -pub struct NodeOptions { - name: String, - namespace: String, - use_global_arguments: bool, - arguments: Vec, - enable_rosout: bool, - start_parameter_services: bool, - clock_type: ClockType, - clock_qos: QoSProfile, -} - -impl NodeOptions { - /// 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 [`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: impl ToString) -> NodeOptions { - NodeOptions { - 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. /// @@ -121,15 +39,15 @@ impl NodeOptions { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, NodeOptions, RclrsError, RclReturnCode}; + /// # use rclrs::{Context, Node, IntoNodeOptions, RclrsError, RclReturnCode}; /// let executor = Context::default().create_basic_executor(); /// // This is a valid namespace - /// let options_ok_ns = NodeOptions::new("my_node").namespace("/some/nested/namespace"); + /// 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!( /// executor.create_node( - /// NodeOptions::new("my_node") + /// "my_node" /// .namespace("/10_percent_luck/20_percent_skill") /// ).unwrap_err(), /// RclrsError::RclError { code: RclReturnCode::NodeInvalidNamespace, .. } @@ -137,7 +55,7 @@ impl NodeOptions { /// // A missing forward slash at the beginning is automatically added /// assert_eq!( /// executor.create_node( - /// NodeOptions::new("my_node") + /// "my_node" /// .namespace("foo") /// )?.namespace(), /// "/foo" @@ -149,9 +67,10 @@ impl NodeOptions { /// [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]: crate::Executor::create_node - pub fn namespace(mut self, namespace: impl ToString) -> Self { - self.namespace = namespace.to_string(); - self + 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. @@ -160,19 +79,19 @@ impl NodeOptions { /// /// # Example /// ``` - /// # use rclrs::{Context, InitOptions, Node, NodeOptions, RclrsError}; + /// # use rclrs::{Context, InitOptions, Node, IntoNodeOptions, RclrsError}; /// let context_args = ["--ros-args", "--remap", "__node:=your_node"] /// .map(String::from); /// let executor = Context::new(context_args, InitOptions::default())?.create_basic_executor(); /// // Ignore the global arguments: /// let node_without_global_args = executor.create_node( - /// NodeOptions::new("my_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 = executor.create_node( - /// NodeOptions::new("my_other_node") + /// "my_other_node" /// .use_global_arguments(true) /// )?; /// assert_eq!(node_with_global_args.name(), "your_node"); @@ -180,9 +99,10 @@ impl NodeOptions { /// ``` /// /// [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. @@ -195,7 +115,7 @@ impl NodeOptions { /// /// # Example /// ``` - /// # use rclrs::{Context, InitOptions, Node, NodeOptions, 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); @@ -204,7 +124,7 @@ impl NodeOptions { /// let node_args = ["--ros-args", "--remap", "my_node:__node:=node_args_node"] /// .map(String::from); /// let node = executor.create_node( - /// NodeOptions::new("my_node") + /// "my_node" /// .arguments(node_args) /// )?; /// assert_eq!(node.name(), "node_args_node"); @@ -213,12 +133,13 @@ impl NodeOptions { /// /// [1]: crate::Context::new /// [2]: https://design.ros2.org/articles/ros_command_line_arguments.html - pub fn arguments(mut self, arguments: Args) -> Self + fn arguments(self, arguments: Args) -> NodeOptions<'a> where Args::Item: ToString, { - self.arguments = arguments.into_iter().map(|item| item.to_string()).collect(); - self + 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. @@ -227,30 +148,138 @@ impl NodeOptions { /// 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 + } +} + +/// 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. + /// + /// 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 [`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. @@ -258,15 +287,14 @@ impl NodeOptions { /// Only used internally. Downstream users should call /// [`Executor::create_node`]. pub(crate) fn build(self, context: &Arc) -> Result { - let node_name = - CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul { - err, - s: self.name.clone(), - })?; + 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 *context.rcl_context.lock().unwrap(); @@ -367,9 +395,21 @@ impl NodeOptions { } } -impl From for NodeOptions { - fn from(name: T) -> Self { - NodeOptions::new(name) +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) } } diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 6a30a6833..a4b65c7e0 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -312,8 +312,8 @@ mod tests { }, srv::rmw::*, }, - Context, Executor, MandatoryParameter, Node, NodeOptions, ParameterRange, ParameterValue, - RclrsError, RclrsErrorFilter, ReadOnlyParameter, SpinOptions, + Context, Executor, IntoNodeOptions, MandatoryParameter, Node, NodeOptions, ParameterRange, + ParameterValue, RclrsError, RclrsErrorFilter, ReadOnlyParameter, SpinOptions, }; use rosidl_runtime_rs::{seq, Sequence}; use std::{ diff --git a/rclrs/src/test_helpers/graph_helpers.rs b/rclrs/src/test_helpers/graph_helpers.rs index 8596cebd1..f61f5db96 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -1,4 +1,4 @@ -use crate::{Context, Node, NodeOptions, RclrsError}; +use crate::{Context, IntoNodeOptions, Node, RclrsError}; pub(crate) struct TestGraph { pub node1: Node, @@ -8,7 +8,7 @@ pub(crate) struct TestGraph { pub(crate) fn construct_test_graph(namespace: &str) -> Result { let executor = Context::default().create_basic_executor(); Ok(TestGraph { - node1: executor.create_node(NodeOptions::new("graph_test_node_1").namespace(namespace))?, - node2: executor.create_node(NodeOptions::new("graph_test_node_2").namespace(namespace))?, + node1: executor.create_node("graph_test_node_1".namespace(namespace))?, + node2: executor.create_node("graph_test_node_2".namespace(namespace))?, }) } From 259fcb374e05624a6cb6061bbe1d10f87bb1f7ed Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 23 Nov 2024 17:06:25 +0800 Subject: [PATCH 45/48] Remove debug outputs Signed-off-by: Michael X. Grey --- rclrs/src/parameter/service.rs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index a4b65c7e0..8fe5742ff 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -574,7 +574,6 @@ mod tests { .create_client::("/get_set/node/set_parameters_atomically")?; try_until_timeout(|| { - println!(" >> testing services"); get_client.service_is_ready().unwrap() && set_client.service_is_ready().unwrap() && set_atomically_client.service_is_ready().unwrap() @@ -587,7 +586,6 @@ mod tests { let inner_done = done.clone(); let rclrs_spin = tokio::task::spawn(async move { try_until_timeout(move || { - println!(" -- spin"); executor .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) .timeout_ok() @@ -646,7 +644,6 @@ mod tests { ) .unwrap(); try_until_timeout(|| { - println!("checking client"); *client_finished.read().unwrap() }) .await @@ -796,7 +793,6 @@ mod tests { ) .unwrap(); try_until_timeout(|| { - println!("checking client finished"); *client_finished.read().unwrap() }) .await From e1ceb70cf6bc0585910d40f7eeaa34425eab5994 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 23 Nov 2024 17:11:59 +0800 Subject: [PATCH 46/48] Fix formatting Signed-off-by: Michael X. Grey --- rclrs/src/parameter/service.rs | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 8fe5742ff..770489e7e 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -643,11 +643,9 @@ mod tests { }, ) .unwrap(); - try_until_timeout(|| { - *client_finished.read().unwrap() - }) - .await - .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 { @@ -792,11 +790,9 @@ mod tests { }, ) .unwrap(); - try_until_timeout(|| { - *client_finished.read().unwrap() - }) - .await - .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); *done.write().unwrap() = true; }); From 1f1d826abe9895400e2a2c85c880c221fe84f0be Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 9 Dec 2024 17:23:51 +0800 Subject: [PATCH 47/48] Refining merge Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 2 +- rclrs/src/executor.rs | 25 ++++--- rclrs/src/executor/basic_executor.rs | 23 ++++--- rclrs/src/node.rs | 8 +-- rclrs/src/parameter/service.rs | 90 +++++++++++++++++-------- rclrs/src/service.rs | 4 +- rclrs/src/test_helpers/graph_helpers.rs | 1 - rclrs/src/wait_set.rs | 4 +- rclrs/src/wait_set/wait_set_runner.rs | 38 +++-------- 9 files changed, 107 insertions(+), 88 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index 72de8cfe1..15baf57f6 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -1,7 +1,7 @@ use std::{ collections::HashMap, ffi::CString, - sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, + sync::{Arc, Mutex, MutexGuard}, }; use rosidl_runtime_rs::Message; diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 07b1c90bb..af2cfe632 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -46,9 +46,9 @@ impl Executor { /// [`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) { + pub fn spin(&mut self, options: SpinOptions) -> Result<(), RclrsError> { let conditions = self.make_spin_conditions(options); - self.runtime.spin(conditions); + self.runtime.spin(conditions) } /// Spin the Executor as an async task. This does not block the current thread. @@ -60,7 +60,7 @@ impl Executor { /// 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 { + pub async fn spin_async(self, options: SpinOptions) -> (Self, Result<(), RclrsError>) { let conditions = self.make_spin_conditions(options); let Self { context, @@ -68,12 +68,15 @@ impl Executor { runtime, } = self; - let runtime = runtime.spin_async(conditions).await; - Self { - context, - commands, - runtime, - } + let (runtime, result) = runtime.spin_async(conditions).await; + ( + Self { + context, + commands, + runtime, + }, + result, + ) } /// Creates a new executor using the provided runtime. Users of rclrs should @@ -231,7 +234,7 @@ pub trait ExecutorRuntime: Send { /// Tell the runtime to spin while blocking any further execution until the /// spinning is complete. - fn spin(&mut self, conditions: SpinConditions); + fn spin(&mut self, conditions: SpinConditions) -> Result<(), RclrsError>; /// Tell the runtime to spin asynchronously, not blocking the current /// thread. The runtime instance will be consumed by this function, but it @@ -240,7 +243,7 @@ pub trait ExecutorRuntime: Send { fn spin_async( self: Box, conditions: SpinConditions, - ) -> BoxFuture<'static, Box>; + ) -> BoxFuture<'static, (Box, Result<(), RclrsError>)>; } /// A bundle of optional conditions that a user may want to impose on how long diff --git a/rclrs/src/executor/basic_executor.rs b/rclrs/src/executor/basic_executor.rs index a314ff804..1ee2c0ad7 100644 --- a/rclrs/src/executor/basic_executor.rs +++ b/rclrs/src/executor/basic_executor.rs @@ -14,7 +14,7 @@ use std::{ use crate::{ executor::{ExecutorChannel, ExecutorRuntime, SpinConditions}, - Context, WaitSetRunner, Waitable, + Context, RclrsError, WaitSetRunner, Waitable, }; /// The implementation of this runtime is based off of the async Rust reference book: @@ -40,7 +40,7 @@ pub struct BasicExecutorRuntime { } impl ExecutorRuntime for BasicExecutorRuntime { - fn spin(&mut self, mut conditions: SpinConditions) { + fn spin(&mut self, mut conditions: SpinConditions) -> Result<(), RclrsError> { self.process_spin_conditions(&mut conditions); let wait_set_runner = self.wait_set_runner.take().expect( @@ -92,19 +92,20 @@ impl ExecutorRuntime for BasicExecutorRuntime { } } - self.wait_set_runner = Some( - wait_set_receiver.recv().expect( - "Basic executor failed to receive the WaitSetRunner at the end of its spinning. \ - This is a critical bug in rclrs. \ - Please report this bug to the maintainers of rclrs by providing a minimum reproduction of the problem." - ) + let (runner, result) = wait_set_receiver.recv().expect( + "Basic executor failed to receive the WaitSetRunner at the end of its spinning. \ + This is a critical bug in rclrs. \ + Please report this bug to the maintainers of rclrs by providing a minimum reproduction of the problem." ); + + self.wait_set_runner = Some(runner); + result } fn spin_async( mut self: Box, conditions: SpinConditions, - ) -> BoxFuture<'static, Box> { + ) -> BoxFuture<'static, (Box, Result<(), RclrsError>)> { let (sender, receiver) = oneshot::channel(); // Create a thread to run the executor. We should not run the executor // as an async task because it blocks its current thread while running. @@ -117,8 +118,8 @@ impl ExecutorRuntime for BasicExecutorRuntime { // executor. But that would probably require us to introduce a new // dependency such as tokio. std::thread::spawn(move || { - self.spin(conditions); - sender.send(self as Box).ok(); + let result = self.spin(conditions); + sender.send((self as Box, result)).ok(); }); Box::pin(async move { diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index f544575d6..8be556458 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -31,10 +31,10 @@ use rosidl_runtime_rs::Message; use crate::{ rcl_bindings::*, Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands, LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Promise, - Publisher, PublisherOptions, PublisherState, QoSProfile, RclrsError, Service, - ServiceAsyncCallback, ServiceCallback, ServiceOptions, ServiceState, Subscription, - SubscriptionAsyncCallback, SubscriptionCallback, SubscriptionOptions, SubscriptionState, - TimeSource, ToLogParams, ENTITY_LIFECYCLE_MUTEX, + Publisher, PublisherOptions, PublisherState, RclrsError, Service, ServiceAsyncCallback, + ServiceCallback, ServiceOptions, ServiceState, Subscription, SubscriptionAsyncCallback, + SubscriptionCallback, SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, + ENTITY_LIFECYCLE_MUTEX, }; /// A processing unit that can communicate with other nodes. diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 3bffa5cf7..304f78f98 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -9,7 +9,7 @@ use rosidl_runtime_rs::Sequence; use super::ParameterMap; use crate::{ parameter::{DeclaredValue, ParameterKind, ParameterStorage}, - rmw_request_id_t, IntoPrimitiveOptions, Node, QoSProfile, RclrsError, Service, + IntoPrimitiveOptions, Node, QoSProfile, RclrsError, Service, }; // The variables only exist to keep a strong reference to the services and are technically unused. @@ -437,11 +437,13 @@ mod tests { !not_finished }); - executor.spin( - SpinOptions::new() - .until_promise_resolved(promise) - .timeout(Duration::from_secs(1)), - ); + executor + .spin( + SpinOptions::new() + .until_promise_resolved(promise) + .timeout(Duration::from_secs(1)), + ) + .unwrap(); Ok(()) } @@ -453,11 +455,13 @@ mod tests { 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)), - ); + executor + .spin( + SpinOptions::default() + .until_promise_resolved(list_client.notify_on_service_ready()) + .timeout(Duration::from_secs(2)), + ) + .unwrap(); // List all parameters let callback_ran = Arc::new(AtomicBool::new(false)); @@ -484,11 +488,13 @@ mod tests { }) .unwrap(); - executor.spin( - SpinOptions::default() - .until_promise_resolved(promise) - .timeout(Duration::from_secs(5)), - ); + executor + .spin( + SpinOptions::default() + .until_promise_resolved(promise) + .timeout(Duration::from_secs(5)), + ) + .unwrap(); assert!(callback_ran.load(Ordering::Acquire)); // Limit depth, namespaced parameter is not returned @@ -508,7 +514,9 @@ mod tests { }) .unwrap(); - executor.spin(SpinOptions::default().until_promise_resolved(promise)); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); assert!(callback_ran.load(Ordering::Acquire)); // Filter by prefix, just return the requested one with the right prefix @@ -529,7 +537,9 @@ mod tests { }) .unwrap(); - executor.spin(SpinOptions::default().until_promise_resolved(promise)); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); assert!(callback_ran.load(Ordering::Acquire)); // If prefix is equal to names, parameters should be returned @@ -550,7 +560,9 @@ mod tests { }) .unwrap(); - executor.spin(SpinOptions::default().until_promise_resolved(promise)); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); assert!(callback_ran.load(Ordering::Acquire)); Ok(()) @@ -578,7 +590,9 @@ mod tests { 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)); + executor + .spin(SpinOptions::default().until_promise_resolved(clients_ready)) + .unwrap(); // Get an existing parameter let callback_ran = Arc::new(AtomicBool::new(false)); @@ -596,7 +610,9 @@ mod tests { }) .unwrap(); - executor.spin(SpinOptions::default().until_promise_resolved(promise)); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); assert!(callback_ran.load(Ordering::Acquire)); // Getting both existing and non existing parameters, missing one should return @@ -617,7 +633,9 @@ mod tests { }) .unwrap(); - executor.spin(SpinOptions::default().until_promise_resolved(promise)); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); assert!(callback_ran.load(Ordering::Acquire)); // Set a mix of existing, non existing, dynamic and out of range parameters @@ -717,7 +735,9 @@ mod tests { }) .unwrap(); - executor.spin(SpinOptions::default().until_promise_resolved(promise)); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); assert!(callback_ran.load(Ordering::Acquire)); // Set the node to use undeclared parameters and try to set one @@ -746,7 +766,9 @@ mod tests { }) .unwrap(); - executor.spin(SpinOptions::default().until_promise_resolved(promise)); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); assert!(callback_ran.load(Ordering::Acquire)); // With set_parameters_atomically, if one fails all should fail @@ -765,7 +787,9 @@ mod tests { ) .unwrap(); - executor.spin(SpinOptions::default().until_promise_resolved(promise)); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); assert!(callback_ran.load(Ordering::Acquire)); Ok(()) @@ -789,7 +813,9 @@ mod tests { 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)); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); // Describe all parameters let request = DescribeParameters_Request { @@ -836,7 +862,9 @@ mod tests { }) .unwrap(); - executor.spin(SpinOptions::default().until_promise_resolved(promise)); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); assert!(callback_ran.load(Ordering::Acquire)); // If a describe parameters request is sent with a non existing parameter, an empty @@ -860,7 +888,9 @@ mod tests { }) .unwrap(); - executor.spin(SpinOptions::default().until_promise_resolved(promise)); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); assert!(callback_ran.load(Ordering::Acquire)); // Get all parameter types, including a non existing one that will be NOT_SET @@ -888,7 +918,9 @@ mod tests { }) .unwrap(); - executor.spin(SpinOptions::default().until_promise_resolved(promise)); + executor + .spin(SpinOptions::default().until_promise_resolved(promise)) + .unwrap(); assert!(callback_ran.load(Ordering::Acquire)); Ok(()) diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index 87d5a0fcd..9430c0383 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -1,11 +1,9 @@ use std::{ boxed::Box, ffi::{CStr, CString}, - sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, + sync::{Arc, Mutex, MutexGuard}, }; -use rosidl_runtime_rs::Message; - use crate::{ error::ToResult, rcl_bindings::*, ExecutorCommands, IntoPrimitiveOptions, NodeHandle, QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, Waitable, diff --git a/rclrs/src/test_helpers/graph_helpers.rs b/rclrs/src/test_helpers/graph_helpers.rs index 9aa93bf0c..f61f5db96 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -1,5 +1,4 @@ use crate::{Context, IntoNodeOptions, Node, RclrsError}; -use std::sync::Arc; pub(crate) struct TestGraph { pub node1: Node, diff --git a/rclrs/src/wait_set.rs b/rclrs/src/wait_set.rs index 54a335b81..5729aa98c 100644 --- a/rclrs/src/wait_set.rs +++ b/rclrs/src/wait_set.rs @@ -270,7 +270,9 @@ mod tests { let start = std::time::Instant::now(); // This should stop spinning right away because the guard condition was // already triggered. - executor.spin(SpinOptions::spin_once().timeout(Duration::from_secs(10))); + executor + .spin(SpinOptions::spin_once().timeout(Duration::from_secs(10))) + .unwrap(); // If it took more than a second to finish spinning then something is // probably wrong. diff --git a/rclrs/src/wait_set/wait_set_runner.rs b/rclrs/src/wait_set/wait_set_runner.rs index 1c7d633c9..f7ce8c71a 100644 --- a/rclrs/src/wait_set/wait_set_runner.rs +++ b/rclrs/src/wait_set/wait_set_runner.rs @@ -8,7 +8,7 @@ use std::{ time::{Duration, Instant}, }; -use crate::{Context, Promise, RclReturnCode, RclrsError, SpinConditions, WaitSet, Waitable}; +use crate::{Context, Promise, RclrsError, SpinConditions, WaitSet, Waitable}; /// This is a utility class that executors can use to easily run and manage /// their wait set. @@ -45,12 +45,12 @@ impl WaitSetRunner { /// the best practice is for your executor runtime to swap that out with a /// new promise which ensures that the [`SpinConditions::guard_condition`] /// will be triggered after the user-provided promise is resolved. - pub fn run(mut self, conditions: SpinConditions) -> Promise { + pub fn run(mut self, conditions: SpinConditions) -> Promise<(Self, Result<(), RclrsError>)> { let (sender, promise) = channel(); std::thread::spawn(move || { - self.run_blocking(conditions); + let result = self.run_blocking(conditions); // TODO(@mxgrey): Log any error here when logging becomes available - sender.send(self).ok(); + sender.send((self, result)).ok(); }); promise @@ -63,7 +63,7 @@ impl WaitSetRunner { /// the best practice is for your executor runtime to swap that out with a /// new promise which ensures that the [`SpinConditions::guard_condition`] /// will be triggered after the user-provided promise is resolved. - pub fn run_blocking(&mut self, mut conditions: SpinConditions) { + pub fn run_blocking(&mut self, mut conditions: SpinConditions) -> Result<(), RclrsError> { let mut first_spin = true; let t_stop_spinning = conditions.options.timeout.map(|dt| Instant::now() + dt); loop { @@ -81,7 +81,7 @@ impl WaitSetRunner { if conditions.options.only_next_available_work && !first_spin { // We've already completed a spin and were asked to only do one, // so break here - break; + return Ok(()); } first_spin = false; @@ -89,19 +89,19 @@ impl WaitSetRunner { let r = promise.try_recv(); if r.is_ok_and(|r| r.is_some()) || r.is_err() { // The promise has been resolved, so we should stop spinning. - break; + return Ok(()); } } if conditions.halt_spinning.load(Ordering::Acquire) { // The user has manually asked for the spinning to stop - break; + return Ok(()); } if !conditions.context.ok() { // The ROS context has switched to being invalid, so we should // stop spinning. - break; + return Ok(()); } let timeout = t_stop_spinning.map(|t| { @@ -113,24 +113,8 @@ impl WaitSetRunner { } }); - if let Err(err) = self - .wait_set - .wait(timeout, |executable| executable.execute()) - { - match err { - RclrsError::RclError { - code: RclReturnCode::Timeout, - .. - } => { - // We have timed out, so we should stop waiting. - break; - } - err => { - // TODO(@mxgrey): Change this to a log when logging becomes available - eprintln!("Error while processing wait set: {err}"); - } - } - } + self.wait_set + .wait(timeout, |executable| executable.execute())?; } } } From 6d3f7e4660cf6c82e2416139829331c9b1566b0c Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 9 Dec 2024 19:31:58 +0800 Subject: [PATCH 48/48] Update examples Signed-off-by: Michael X. Grey --- .../src/minimal_client.rs | 30 +++++++++---------- .../src/minimal_client_async.rs | 28 ++++++++--------- .../src/minimal_service.rs | 17 +++++++---- .../minimal_pub_sub/src/minimal_subscriber.rs | 18 ++++++----- .../src/zero_copy_subscriber.rs | 16 +++++----- examples/rust_pubsub/src/simple_publisher.rs | 6 ++-- examples/rust_pubsub/src/simple_subscriber.rs | 11 +++---- 7 files changed, 66 insertions(+), 60 deletions(-) diff --git a/examples/minimal_client_service/src/minimal_client.rs b/examples/minimal_client_service/src/minimal_client.rs index 49f18e242..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 anyhow::{Error, Result}; +use rclrs::{Context, SpinOptions, Promise}; fn main() -> Result<(), Error> { - let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); + let mut executor = Context::default_from_env()?.create_basic_executor(); 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"); - executor - .spin(rclrs::SpinOptions::default()) - .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 8babe04e7..b4bca1372 100644 --- a/examples/minimal_client_service/src/minimal_client_async.rs +++ b/examples/minimal_client_service/src/minimal_client_async.rs @@ -1,8 +1,8 @@ use anyhow::{Error, Result}; +use rclrs::{Context, SpinOptions}; -#[tokio::main] -async fn main() -> Result<(), Error> { - let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); +fn main() -> Result<(), Error> { + let mut executor = Context::default_from_env()?.create_basic_executor(); let node = executor.create_node("minimal_client")?; @@ -16,19 +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 || executor.spin(rclrs::SpinOptions::default())); - - let response = future.await?; - println!( - "Result of {} + {} is: {}", - request.a, request.b, response.sum - ); - - rclrs_spin.await.ok(); + executor.spin(SpinOptions::new().until_promise_resolved(promise))?; Ok(()) } diff --git a/examples/minimal_client_service/src/minimal_service.rs b/examples/minimal_client_service/src/minimal_service.rs index 84d154fec..f249940bf 100644 --- a/examples/minimal_client_service/src/minimal_service.rs +++ b/examples/minimal_client_service/src/minimal_service.rs @@ -1,17 +1,23 @@ 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 mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); + let mut executor = Context::default_from_env()?.create_basic_executor(); let node = executor.create_node("minimal_service")?; @@ -19,7 +25,6 @@ fn main() -> Result<(), Error> { .create_service::("add_two_ints", handle_service)?; println!("Starting server"); - executor - .spin(rclrs::SpinOptions::default()) - .map_err(|err| err.into()) + executor.spin(SpinOptions::default())?; + Ok(()) } diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index ebe7406ee..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 anyhow::{Error, Result}; +use std::sync::Mutex; +use rclrs::{Context, SpinOptions}; fn main() -> Result<(), Error> { - let context = rclrs::Context::default_from_env()?; + let context = Context::default_from_env()?; let mut executor = context.create_basic_executor(); 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", 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); }, )?; - executor - .spin(rclrs::SpinOptions::default()) - .map_err(|err| err.into()) + println!("Waiting for messages..."); + executor.spin(SpinOptions::default())?; + Ok(()) } diff --git a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs index b44752d65..4769e9f12 100644 --- a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -1,22 +1,24 @@ use anyhow::{Error, Result}; +use std::sync::Mutex; +use rclrs::ReadOnlyLoanedMessage; fn main() -> Result<(), Error> { let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); 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", - 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); }, )?; - executor - .spin(rclrs::SpinOptions::default()) - .map_err(|err| err.into()) + executor.spin(rclrs::SpinOptions::default())?; + Ok(()) } diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index b4283d073..8b5b467a7 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -1,5 +1,5 @@ -use rclrs::{Context, Executor, Publisher, RclrsError, SpinOptions, 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 { @@ -26,7 +26,7 @@ impl SimplePublisherNode { fn main() -> Result<(), RclrsError> { let mut executor = Context::default_from_env().unwrap().create_basic_executor(); - let node = Arc::new(SimplePublisher::new(&executor).unwrap()); + let node = SimplePublisherNode::new(&executor).unwrap(); let mut count: i32 = 0; thread::spawn(move || loop { thread::sleep(Duration::from_millis(1000)); diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index bae5f551b..5e11f5fda 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -1,4 +1,4 @@ -use rclrs::{Context, Executor, RclrsError, SpinOptions, Subscription, QOS_PROFILE_DEFAULT}; +use rclrs::{Context, Executor, RclrsError, SpinOptions, Subscription}; use std::{ sync::{Arc, Mutex}, thread, @@ -7,7 +7,8 @@ use std::{ use std_msgs::msg::String as StringMsg; pub struct SimpleSubscriptionNode { - _subscriber: Arc>, + #[allow(unused)] + subscriber: Subscription, data: Arc>>, } @@ -16,7 +17,7 @@ impl SimpleSubscriptionNode { 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", move |msg: StringMsg| { @@ -24,7 +25,7 @@ impl SimpleSubscriptionNode { }, ) .unwrap(); - Ok(Self { _subscriber, data }) + Ok(Self { subscriber, data }) } fn data_callback(&self) -> Result<(), RclrsError> { if let Some(data) = self.data.lock().unwrap().as_ref() { @@ -37,7 +38,7 @@ impl SimpleSubscriptionNode { } fn main() -> Result<(), RclrsError> { let mut executor = Context::default_from_env().unwrap().create_basic_executor(); - let node = Arc::new(SimpleSubscriptionNode::new(&executor).unwrap()); + let node = SimpleSubscriptionNode::new(&executor).unwrap(); thread::spawn(move || loop { thread::sleep(Duration::from_millis(1000)); node.data_callback().unwrap()