From 29fe10ef7f9bd33fcd0fdb795f562e5a19e0aa0d Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 16 Nov 2024 16:05:28 +0000 Subject: [PATCH 01/17] 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 02/17] 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 03/17] 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 04/17] 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 4c2a67b0472db677ae1a840b8bb755ebcc62dc14 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 23:17:57 +0800 Subject: [PATCH 05/17] 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 605950695c62eed54f02b03ea9f4f6a21f0e28a5 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 23 Nov 2024 17:03:50 +0800 Subject: [PATCH 06/17] 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 07/17] 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 08/17] 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 b465f6f74eba71270e843d690e44f81ac78ca873 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 16 Dec 2024 23:03:05 +0800 Subject: [PATCH 09/17] Return a Vec of errors from spinning Signed-off-by: Michael X. Grey --- rclrs/src/error.rs | 97 ++++++++++++++++++++++++++++++---- rclrs/src/executor.rs | 10 ++-- rclrs/src/parameter/service.rs | 3 ++ 3 files changed, 96 insertions(+), 14 deletions(-) diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index 961e7d5e6..527a4d3a6 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -34,6 +34,33 @@ pub enum RclrsError { AlreadyAddedToWaitSet, } +impl RclrsError { + /// Returns true if the error was due to a timeout, otherwise returns false. + pub fn is_timeout(&self) -> bool { + matches!( + self, + RclrsError::RclError { + code: RclReturnCode::Timeout, + .. + } + ) + } + + /// Returns true if the error was because a subscription, service, or client + /// take failed, otherwise returns false. + pub fn is_take_failed(&self) -> bool { + matches!( + self, + RclrsError::RclError { + code: RclReturnCode::SubscriptionTakeFailed + | RclReturnCode::ServiceTakeFailed + | RclReturnCode::ClientTakeFailed, + .. + } + ) + } +} + impl Display for RclrsError { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { match self { @@ -355,27 +382,77 @@ impl ToResult for rcl_ret_t { /// A helper trait to disregard timeouts as not an error. pub trait RclrsErrorFilter { + /// Get the first error available, or Ok(()) if there are no errors. + fn first_error(self) -> Result<(), RclrsError>; + /// If the result was a timeout error, change it to `Ok(())`. - fn timeout_ok(self) -> Result<(), RclrsError>; + fn timeout_ok(self) -> Self; + + /// If a subscription, service, or client take failed, change the result + /// to be `Ok(())`. + fn take_failed_ok(self) -> Self; + + /// Some error types just indicate an early termination but do not indicate + /// that anything in the system has misbehaved. This filters out anything + /// that is part of the normal operation of rcl. + fn ignore_non_errors(self) -> Self + where + Self: Sized, + { + self.timeout_ok().take_failed_ok() + } } impl RclrsErrorFilter for Result<(), RclrsError> { + fn first_error(self) -> Result<(), RclrsError> { + self + } + fn timeout_ok(self) -> Result<(), RclrsError> { match self { Ok(()) => Ok(()), Err(err) => { - if matches!( - err, - RclrsError::RclError { - code: RclReturnCode::Timeout, - .. - } - ) { - return Ok(()); + if err.is_timeout() { + Ok(()) + } else { + Err(err) } + } + } + } - Err(err) + fn take_failed_ok(self) -> Result<(), RclrsError> { + match self { + Err(err) => { + if err.is_take_failed() { + // Spurious wakeup - this may happen even when a waitset indicated that + // work was ready, so we won't report it as an error + Ok(()) + } else { + Err(err) + } } + other => other, + } + } +} + +impl RclrsErrorFilter for Vec { + fn first_error(mut self) -> Result<(), RclrsError> { + if self.is_empty() { + return Ok(()); } + + Err(self.remove(0)) + } + + fn timeout_ok(mut self) -> Self { + self.retain(|err| !err.is_timeout()); + self + } + + fn take_failed_ok(mut self) -> Self { + self.retain(|err| !err.is_take_failed()); + self } } diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 83117a84a..c6482dae0 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -30,18 +30,20 @@ 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) -> Result<(), RclrsError> { + pub fn spin(&mut self, options: SpinOptions) -> Vec { loop { if self.nodes_mtx.lock().unwrap().is_empty() { // Nothing to spin for, so just quit here - return Ok(()); + return Vec::new(); } - self.spin_once(options.timeout)?; + if let Err(err) = self.spin_once(options.timeout) { + return vec![err]; + } if options.only_next_available_work { // We were only suppposed to spin once, so quit here - return Ok(()); + return Vec::new(); } std::thread::yield_now(); diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 2dd88b0fa..585c4719b 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -446,6 +446,7 @@ mod tests { executor .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) .timeout_ok() + .first_error() .unwrap(); *inner_done.read().unwrap() @@ -589,6 +590,7 @@ mod tests { executor .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) .timeout_ok() + .first_error() .unwrap(); *inner_done.read().unwrap() @@ -825,6 +827,7 @@ mod tests { executor .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) .timeout_ok() + .first_error() .unwrap(); *inner_done.read().unwrap() From 9a04993e75110bd6534cf20a117786d53a830966 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 16 Dec 2024 23:16:40 +0800 Subject: [PATCH 10/17] update examples Signed-off-by: Michael X. Grey --- examples/message_demo/src/message_demo.rs | 6 ++++-- examples/minimal_client_service/src/minimal_client.rs | 2 ++ examples/minimal_client_service/src/minimal_service.rs | 2 ++ examples/minimal_pub_sub/src/minimal_subscriber.rs | 2 ++ examples/minimal_pub_sub/src/minimal_two_nodes.rs | 2 ++ examples/minimal_pub_sub/src/zero_copy_subscriber.rs | 2 ++ examples/rust_pubsub/src/simple_publisher.rs | 4 ++-- examples/rust_pubsub/src/simple_subscriber.rs | 4 ++-- 8 files changed, 18 insertions(+), 6 deletions(-) diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index bc3b7c028..9bc9ef493 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -3,6 +3,8 @@ use std::convert::TryInto; use anyhow::{Error, Result}; use rosidl_runtime_rs::{seq, BoundedSequence, Message, Sequence}; +use rclrs::RclrsErrorFilter; + fn check_default_values() { let msg = rclrs_example_msgs::msg::rmw::VariousTypes::default(); assert!(msg.bool_member); @@ -166,10 +168,10 @@ fn demonstrate_pubsub() -> Result<(), Error> { )?; println!("Sending idiomatic message."); idiomatic_publisher.publish(rclrs_example_msgs::msg::VariousTypes::default())?; - executor.spin(rclrs::SpinOptions::spin_once())?; + executor.spin(rclrs::SpinOptions::spin_once()).first_error()?; println!("Sending RMW-native message."); direct_publisher.publish(rclrs_example_msgs::msg::rmw::VariousTypes::default())?; - executor.spin(rclrs::SpinOptions::spin_once())?; + executor.spin(rclrs::SpinOptions::spin_once()).first_error()?; Ok(()) } diff --git a/examples/minimal_client_service/src/minimal_client.rs b/examples/minimal_client_service/src/minimal_client.rs index 49f18e242..870a87ad4 100644 --- a/examples/minimal_client_service/src/minimal_client.rs +++ b/examples/minimal_client_service/src/minimal_client.rs @@ -1,4 +1,5 @@ use anyhow::{Error, Result}; +use rclrs::RclrsErrorFilter; fn main() -> Result<(), Error> { let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); @@ -30,5 +31,6 @@ fn main() -> Result<(), Error> { println!("Waiting for response"); executor .spin(rclrs::SpinOptions::default()) + .first_error() .map_err(|err| err.into()) } diff --git a/examples/minimal_client_service/src/minimal_service.rs b/examples/minimal_client_service/src/minimal_service.rs index 84d154fec..aa7962540 100644 --- a/examples/minimal_client_service/src/minimal_service.rs +++ b/examples/minimal_client_service/src/minimal_service.rs @@ -1,4 +1,5 @@ use anyhow::{Error, Result}; +use rclrs::RclrsErrorFilter; fn handle_service( _request_header: &rclrs::rmw_request_id_t, @@ -21,5 +22,6 @@ fn main() -> Result<(), Error> { println!("Starting server"); executor .spin(rclrs::SpinOptions::default()) + .first_error() .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 2f0820660..0e1f5986f 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -1,4 +1,5 @@ use anyhow::{Error, Result}; +use rclrs::RclrsErrorFilter; fn main() -> Result<(), Error> { let context = rclrs::Context::default_from_env()?; @@ -20,5 +21,6 @@ fn main() -> Result<(), Error> { executor .spin(rclrs::SpinOptions::default()) + .first_error() .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 235692a22..9d2f6b534 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -2,6 +2,7 @@ use std::sync::{ atomic::{AtomicU32, Ordering}, Arc, Mutex, }; +use rclrs::RclrsErrorFilter; use anyhow::{Error, Result}; @@ -75,5 +76,6 @@ fn main() -> Result<(), Error> { executor .spin(rclrs::SpinOptions::default()) + .first_error() .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 7b9f538e9..ced7b80c2 100644 --- a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -1,4 +1,5 @@ use anyhow::{Error, Result}; +use rclrs::RclrsErrorFilter; fn main() -> Result<(), Error> { let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); @@ -19,5 +20,6 @@ fn main() -> Result<(), Error> { executor .spin(rclrs::SpinOptions::default()) + .first_error() .map_err(|err| err.into()) } diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index edacd0247..66fb2234f 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -1,4 +1,4 @@ -use rclrs::{Context, Executor, Publisher, RclrsError, SpinOptions, QOS_PROFILE_DEFAULT}; +use rclrs::{Context, Executor, Publisher, RclrsError, SpinOptions, QOS_PROFILE_DEFAULT, RclrsErrorFilter}; use std::{sync::Arc, thread, time::Duration}; use std_msgs::msg::String as StringMsg; @@ -33,5 +33,5 @@ fn main() -> Result<(), RclrsError> { thread::sleep(Duration::from_millis(1000)); count = publisher_other_thread.publish_data(count).unwrap(); }); - executor.spin(SpinOptions::default()) + executor.spin(SpinOptions::default()).first_error() } diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index 4df161221..227d48c7f 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, QOS_PROFILE_DEFAULT, RclrsErrorFilter}; use std::{ sync::{Arc, Mutex}, thread, @@ -44,5 +44,5 @@ fn main() -> Result<(), RclrsError> { thread::sleep(Duration::from_millis(1000)); subscription_other_thread.data_callback().unwrap() }); - executor.spin(SpinOptions::default()) + executor.spin(SpinOptions::default()).first_error() } From 4de816a5dce37ef402998a479c0b3173b30a8c60 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 16 Dec 2024 23:29:20 +0800 Subject: [PATCH 11/17] Fix example formatting Signed-off-by: Michael X. Grey --- examples/message_demo/src/message_demo.rs | 8 ++++++-- examples/minimal_pub_sub/src/minimal_two_nodes.rs | 2 +- examples/rust_pubsub/src/simple_publisher.rs | 4 +++- examples/rust_pubsub/src/simple_subscriber.rs | 4 +++- 4 files changed, 13 insertions(+), 5 deletions(-) diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index 9bc9ef493..35eb6c5f7 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -168,10 +168,14 @@ fn demonstrate_pubsub() -> Result<(), Error> { )?; println!("Sending idiomatic message."); idiomatic_publisher.publish(rclrs_example_msgs::msg::VariousTypes::default())?; - executor.spin(rclrs::SpinOptions::spin_once()).first_error()?; + executor + .spin(rclrs::SpinOptions::spin_once()) + .first_error()?; println!("Sending RMW-native message."); direct_publisher.publish(rclrs_example_msgs::msg::rmw::VariousTypes::default())?; - executor.spin(rclrs::SpinOptions::spin_once()).first_error()?; + executor + .spin(rclrs::SpinOptions::spin_once()) + .first_error()?; Ok(()) } diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index 9d2f6b534..e13ca567f 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -1,8 +1,8 @@ +use rclrs::RclrsErrorFilter; use std::sync::{ atomic::{AtomicU32, Ordering}, Arc, Mutex, }; -use rclrs::RclrsErrorFilter; use anyhow::{Error, Result}; diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index 66fb2234f..6177d7bc2 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -1,4 +1,6 @@ -use rclrs::{Context, Executor, Publisher, RclrsError, SpinOptions, QOS_PROFILE_DEFAULT, RclrsErrorFilter}; +use rclrs::{ + Context, Executor, Publisher, RclrsError, RclrsErrorFilter, SpinOptions, QOS_PROFILE_DEFAULT, +}; use std::{sync::Arc, thread, time::Duration}; use std_msgs::msg::String as StringMsg; diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index 227d48c7f..71fd0af92 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -1,4 +1,6 @@ -use rclrs::{Context, Executor, RclrsError, SpinOptions, Subscription, QOS_PROFILE_DEFAULT, RclrsErrorFilter}; +use rclrs::{ + Context, Executor, RclrsError, RclrsErrorFilter, SpinOptions, Subscription, QOS_PROFILE_DEFAULT, +}; use std::{ sync::{Arc, Mutex}, thread, From a75cd24455d0722a0c3b98c16cdba40f15a90e11 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 14 Jan 2025 12:52:11 +0800 Subject: [PATCH 12/17] Introduce CreateBasicExecutor trait Signed-off-by: Michael X. Grey --- examples/message_demo/src/message_demo.rs | 16 +++++++-------- .../src/minimal_client.rs | 6 +++--- .../src/minimal_client_async.rs | 5 +++-- .../src/minimal_service.rs | 6 +++--- .../minimal_pub_sub/src/minimal_publisher.rs | 5 +++-- .../minimal_pub_sub/src/minimal_subscriber.rs | 8 ++++---- .../minimal_pub_sub/src/minimal_two_nodes.rs | 20 +++++++++---------- .../src/zero_copy_publisher.rs | 5 +++-- .../src/zero_copy_subscriber.rs | 10 +++++----- examples/rust_pubsub/src/simple_publisher.rs | 4 +--- examples/rust_pubsub/src/simple_subscriber.rs | 4 +--- rclrs/src/context.rs | 7 +------ rclrs/src/executor.rs | 15 +++++++++++++- rclrs/src/logging.rs | 2 +- rclrs/src/node.rs | 10 +++++----- rclrs/src/node/graph.rs | 2 +- rclrs/src/node/node_options.rs | 10 +++++----- rclrs/src/parameter.rs | 2 +- rclrs/src/parameter/service.rs | 3 +-- rclrs/src/test_helpers/graph_helpers.rs | 2 +- rclrs/src/time_source.rs | 2 +- 21 files changed, 75 insertions(+), 69 deletions(-) diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index 35eb6c5f7..e012bca80 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -3,7 +3,7 @@ use std::convert::TryInto; use anyhow::{Error, Result}; use rosidl_runtime_rs::{seq, BoundedSequence, Message, Sequence}; -use rclrs::RclrsErrorFilter; +use rclrs::*; fn check_default_values() { let msg = rclrs_example_msgs::msg::rmw::VariousTypes::default(); @@ -140,28 +140,28 @@ fn demonstrate_sequences() { fn demonstrate_pubsub() -> Result<(), Error> { println!("================== Interoperability demo =================="); // Demonstrate interoperability between idiomatic and RMW-native message types - 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("message_demo")?; let idiomatic_publisher = node.create_publisher::( "topic", - rclrs::QOS_PROFILE_DEFAULT, + QOS_PROFILE_DEFAULT, )?; let direct_publisher = node.create_publisher::( "topic", - rclrs::QOS_PROFILE_DEFAULT, + QOS_PROFILE_DEFAULT, )?; let _idiomatic_subscription = node .create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, + 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, + QOS_PROFILE_DEFAULT, move |_msg: rclrs_example_msgs::msg::rmw::VariousTypes| { println!("Got RMW-native message!") }, @@ -169,12 +169,12 @@ fn demonstrate_pubsub() -> Result<(), Error> { println!("Sending idiomatic message."); idiomatic_publisher.publish(rclrs_example_msgs::msg::VariousTypes::default())?; executor - .spin(rclrs::SpinOptions::spin_once()) + .spin(SpinOptions::spin_once()) .first_error()?; println!("Sending RMW-native message."); direct_publisher.publish(rclrs_example_msgs::msg::rmw::VariousTypes::default())?; executor - .spin(rclrs::SpinOptions::spin_once()) + .spin(SpinOptions::spin_once()) .first_error()?; Ok(()) diff --git a/examples/minimal_client_service/src/minimal_client.rs b/examples/minimal_client_service/src/minimal_client.rs index 870a87ad4..a8651b4a5 100644 --- a/examples/minimal_client_service/src/minimal_client.rs +++ b/examples/minimal_client_service/src/minimal_client.rs @@ -1,8 +1,8 @@ use anyhow::{Error, Result}; -use rclrs::RclrsErrorFilter; +use rclrs::*; 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")?; @@ -30,7 +30,7 @@ fn main() -> Result<(), Error> { println!("Waiting for response"); executor - .spin(rclrs::SpinOptions::default()) + .spin(SpinOptions::default()) .first_error() .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 8babe04e7..d8dfa2aa5 100644 --- a/examples/minimal_client_service/src/minimal_client_async.rs +++ b/examples/minimal_client_service/src/minimal_client_async.rs @@ -1,8 +1,9 @@ use anyhow::{Error, Result}; +use rclrs::*; #[tokio::main] async 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")?; @@ -21,7 +22,7 @@ async fn main() -> Result<(), Error> { println!("Waiting for response"); let rclrs_spin = - tokio::task::spawn_blocking(move || executor.spin(rclrs::SpinOptions::default())); + tokio::task::spawn_blocking(move || executor.spin(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 aa7962540..0fe681dbf 100644 --- a/examples/minimal_client_service/src/minimal_service.rs +++ b/examples/minimal_client_service/src/minimal_service.rs @@ -1,5 +1,5 @@ use anyhow::{Error, Result}; -use rclrs::RclrsErrorFilter; +use rclrs::*; fn handle_service( _request_header: &rclrs::rmw_request_id_t, @@ -12,7 +12,7 @@ fn handle_service( } 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")?; @@ -21,7 +21,7 @@ fn main() -> Result<(), Error> { println!("Starting server"); executor - .spin(rclrs::SpinOptions::default()) + .spin(SpinOptions::default()) .first_error() .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 7ca95cf97..c21aeb0ad 100644 --- a/examples/minimal_pub_sub/src/minimal_publisher.rs +++ b/examples/minimal_pub_sub/src/minimal_publisher.rs @@ -1,13 +1,14 @@ use anyhow::{Error, Result}; +use rclrs::*; fn main() -> Result<(), Error> { - let context = rclrs::Context::default_from_env()?; + let context = Context::default_from_env()?; let executor = context.create_basic_executor(); let node = executor.create_node("minimal_publisher")?; let publisher = - node.create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + node.create_publisher::("topic", QOS_PROFILE_DEFAULT)?; let mut message = std_msgs::msg::String::default(); diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index 0e1f5986f..f835bb2ba 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -1,8 +1,8 @@ use anyhow::{Error, Result}; -use rclrs::RclrsErrorFilter; +use rclrs::*; 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")?; @@ -11,7 +11,7 @@ fn main() -> Result<(), Error> { let _subscription = node.create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, + QOS_PROFILE_DEFAULT, move |msg: std_msgs::msg::String| { num_messages += 1; println!("I heard: '{}'", msg.data); @@ -20,7 +20,7 @@ fn main() -> Result<(), Error> { )?; executor - .spin(rclrs::SpinOptions::default()) + .spin(SpinOptions::default()) .first_error() .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 e13ca567f..b5e24fd45 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -1,4 +1,4 @@ -use rclrs::RclrsErrorFilter; +use rclrs::*; use std::sync::{ atomic::{AtomicU32, Ordering}, Arc, Mutex, @@ -8,16 +8,16 @@ use anyhow::{Error, Result}; struct MinimalSubscriber { num_messages: AtomicU32, - node: Arc, - subscription: Mutex>>>, + node: Arc, + subscription: Mutex>>>, } impl MinimalSubscriber { pub fn new( - executor: &rclrs::Executor, + executor: &Executor, name: &str, topic: &str, - ) -> Result, rclrs::RclrsError> { + ) -> Result, RclrsError> { let node = executor.create_node(name)?; let minimal_subscriber = Arc::new(MinimalSubscriber { num_messages: 0.into(), @@ -30,7 +30,7 @@ impl MinimalSubscriber { .node .create_subscription::( topic, - rclrs::QOS_PROFILE_DEFAULT, + QOS_PROFILE_DEFAULT, move |msg: std_msgs::msg::String| { minimal_subscriber_aux.callback(msg); }, @@ -51,7 +51,7 @@ impl MinimalSubscriber { } 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 publisher_node = executor.create_node("minimal_publisher")?; let _subscriber_node_one = @@ -60,9 +60,9 @@ fn main() -> Result<(), Error> { MinimalSubscriber::new(&executor, "minimal_subscriber_two", "topic")?; let publisher = publisher_node - .create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + .create_publisher::("topic", QOS_PROFILE_DEFAULT)?; - std::thread::spawn(move || -> Result<(), rclrs::RclrsError> { + std::thread::spawn(move || -> Result<(), RclrsError> { let mut message = std_msgs::msg::String::default(); let mut publish_count: u32 = 1; loop { @@ -75,7 +75,7 @@ fn main() -> Result<(), Error> { }); executor - .spin(rclrs::SpinOptions::default()) + .spin(SpinOptions::default()) .first_error() .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 3f904f673..e950ab8d1 100644 --- a/examples/minimal_pub_sub/src/zero_copy_publisher.rs +++ b/examples/minimal_pub_sub/src/zero_copy_publisher.rs @@ -1,13 +1,14 @@ use anyhow::{Error, Result}; +use rclrs::*; fn main() -> Result<(), Error> { - let context = rclrs::Context::default_from_env()?; + let context = Context::default_from_env()?; let executor = context.create_basic_executor(); let node = executor.create_node("minimal_publisher")?; let publisher = - node.create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + node.create_publisher::("topic", QOS_PROFILE_DEFAULT)?; let mut publish_count: u32 = 1; diff --git a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs index ced7b80c2..47d87f11c 100644 --- a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -1,8 +1,8 @@ use anyhow::{Error, Result}; -use rclrs::RclrsErrorFilter; +use rclrs::*; 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_subscriber")?; @@ -10,8 +10,8 @@ fn main() -> Result<(), Error> { let _subscription = node.create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, - move |msg: rclrs::ReadOnlyLoanedMessage<'_, std_msgs::msg::UInt32>| { + QOS_PROFILE_DEFAULT, + move |msg: ReadOnlyLoanedMessage<'_, std_msgs::msg::UInt32>| { num_messages += 1; println!("I heard: '{}'", msg.data); println!("(Got {} messages so far)", num_messages); @@ -19,7 +19,7 @@ fn main() -> Result<(), Error> { )?; executor - .spin(rclrs::SpinOptions::default()) + .spin(SpinOptions::default()) .first_error() .map_err(|err| err.into()) } diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index 6177d7bc2..ae0739e19 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -1,6 +1,4 @@ -use rclrs::{ - Context, Executor, Publisher, RclrsError, RclrsErrorFilter, SpinOptions, QOS_PROFILE_DEFAULT, -}; +use rclrs::*; use std::{sync::Arc, thread, time::Duration}; use std_msgs::msg::String as StringMsg; diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index 71fd0af92..b0d26dea0 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -1,6 +1,4 @@ -use rclrs::{ - Context, Executor, RclrsError, RclrsErrorFilter, SpinOptions, Subscription, QOS_PROFILE_DEFAULT, -}; +use rclrs::*; use std::{ sync::{Arc, Mutex}, thread, diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 14356b5f0..1aeeaf104 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -6,7 +6,7 @@ use std::{ vec::Vec, }; -use crate::{rcl_bindings::*, Executor, LoggingLifecycle, RclrsError, ToResult}; +use crate::{rcl_bindings::*, LoggingLifecycle, RclrsError, ToResult}; /// This is locked whenever initializing or dropping any middleware entity /// because we have found issues in RCL and some RMW implementations that @@ -183,11 +183,6 @@ impl Context { 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]. diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index c6482dae0..437be1cc1 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,5 +1,6 @@ use crate::{ - rcl_bindings::rcl_context_is_valid, ContextHandle, IntoNodeOptions, Node, RclrsError, WaitSet, + rcl_bindings::rcl_context_is_valid, Context, ContextHandle, IntoNodeOptions, Node, RclrsError, + WaitSet, }; use std::{ sync::{Arc, Mutex, Weak}, @@ -128,3 +129,15 @@ impl SpinOptions { self } } + +/// This trait allows [`Context`] to create a basic executor. +pub trait CreateBasicExecutor { + /// Create a basic executor associated with this [`Context`]. + fn create_basic_executor(&self) -> Executor; +} + +impl CreateBasicExecutor for Context { + fn create_basic_executor(&self) -> Executor { + Executor::new(Arc::clone(&self.handle)) + } +} diff --git a/rclrs/src/logging.rs b/rclrs/src/logging.rs index 5d2b2ac19..cbc30114f 100644 --- a/rclrs/src/logging.rs +++ b/rclrs/src/logging.rs @@ -25,7 +25,7 @@ pub use logger::*; /// # Examples /// /// ``` -/// use rclrs::{log, ToLogParams}; +/// use rclrs::*; /// use std::sync::Mutex; /// use std::time::Duration; /// use std::env; diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index d5a75d6eb..0adefadd6 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -150,7 +150,7 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, InitOptions, RclrsError}; + /// # use rclrs::*; /// // Without remapping /// let executor = Context::default().create_basic_executor(); /// let node = executor.create_node("my_node")?; @@ -173,7 +173,7 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, InitOptions, RclrsError, IntoNodeOptions}; + /// # use rclrs::*; /// // Without remapping /// let executor = Context::default().create_basic_executor(); /// let node = executor.create_node( @@ -199,7 +199,7 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError, IntoNodeOptions}; + /// # use rclrs::*; /// let executor = Context::default().create_basic_executor(); /// let node = executor.create_node( /// "my_node" @@ -375,7 +375,7 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError}; + /// # use rclrs::*; /// // Set default ROS domain ID to 10 here /// std::env::set_var("ROS_DOMAIN_ID", "10"); /// let executor = Context::default().create_basic_executor(); @@ -403,7 +403,7 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, ParameterRange, RclrsError}; + /// # use rclrs::*; /// 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 diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 870b2b67a..b1535bf64 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -454,7 +454,7 @@ fn convert_names_and_types( #[cfg(test)] mod tests { use super::*; - use crate::{Context, InitOptions}; + use crate::*; #[test] fn test_graph_empty() { diff --git a/rclrs/src/node/node_options.rs b/rclrs/src/node/node_options.rs index 9207f512e..9f5a41581 100644 --- a/rclrs/src/node/node_options.rs +++ b/rclrs/src/node/node_options.rs @@ -38,7 +38,7 @@ pub trait IntoNodeOptions<'a>: Sized { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, IntoNodeOptions, RclrsError, RclReturnCode}; + /// # use rclrs::*; /// let executor = Context::default().create_basic_executor(); /// // This is a valid namespace /// let options_ok_ns = "my_node".namespace("/some/nested/namespace"); @@ -78,7 +78,7 @@ pub trait IntoNodeOptions<'a>: Sized { /// /// # Example /// ``` - /// # use rclrs::{Context, InitOptions, Node, IntoNodeOptions, RclrsError}; + /// # use rclrs::*; /// let context_args = ["--ros-args", "--remap", "__node:=your_node"] /// .map(String::from); /// let executor = Context::new(context_args, InitOptions::default())?.create_basic_executor(); @@ -114,7 +114,7 @@ pub trait IntoNodeOptions<'a>: Sized { /// /// # Example /// ``` - /// # use rclrs::{Context, InitOptions, IntoNodeOptions, Node, RclrsError}; + /// # use rclrs::*; /// // 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); @@ -194,7 +194,7 @@ pub trait IntoNodeOptions<'a>: Sized { /// /// # Example /// ``` -/// # use rclrs::{ClockType, Context, IntoNodeOptions, NodeOptions, Node, RclrsError}; +/// # use rclrs::*; /// let executor = Context::default().create_basic_executor(); /// /// // Building a node with default options @@ -251,7 +251,7 @@ impl<'a> NodeOptions<'a> { /// /// # Example /// ``` - /// # use rclrs::{Context, NodeOptions, RclrsError, RclReturnCode}; + /// # use rclrs::*; /// let executor = Context::default().create_basic_executor(); /// // This is a valid node name /// assert!(executor.create_node(NodeOptions::new("my_node")).is_ok()); diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index 44ca7365c..87ff192dc 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -874,7 +874,7 @@ impl ParameterInterface { #[cfg(test)] mod tests { use super::*; - use crate::{Context, InitOptions}; + use crate::*; #[test] fn test_parameter_override_errors() { diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 585c4719b..465275f7f 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -312,8 +312,7 @@ mod tests { }, srv::rmw::*, }, - 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 ece87372a..838ef49f9 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -1,4 +1,4 @@ -use crate::{Context, IntoNodeOptions, Node, RclrsError}; +use crate::*; use std::sync::Arc; pub(crate) struct TestGraph { diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index e6480c4cf..1b5f16500 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -145,7 +145,7 @@ impl TimeSource { #[cfg(test)] mod tests { - use crate::{Context, InitOptions}; + use crate::*; #[test] fn time_source_default_clock() { From 71c69a80285aa0dedb035f32530ca0d8f2065a67 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 14 Jan 2025 13:01:28 +0800 Subject: [PATCH 13/17] Fix formatting Signed-off-by: Michael X. Grey --- examples/message_demo/src/message_demo.rs | 14 ++++---------- .../src/minimal_client_async.rs | 3 +-- examples/minimal_pub_sub/src/minimal_publisher.rs | 3 +-- examples/minimal_pub_sub/src/minimal_two_nodes.rs | 10 +++------- 4 files changed, 9 insertions(+), 21 deletions(-) diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index e012bca80..b9ce68f38 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -143,10 +143,8 @@ fn demonstrate_pubsub() -> Result<(), Error> { let mut executor = Context::default_from_env()?.create_basic_executor(); let node = executor.create_node("message_demo")?; - let idiomatic_publisher = node.create_publisher::( - "topic", - QOS_PROFILE_DEFAULT, - )?; + let idiomatic_publisher = node + .create_publisher::("topic", QOS_PROFILE_DEFAULT)?; let direct_publisher = node.create_publisher::( "topic", QOS_PROFILE_DEFAULT, @@ -168,14 +166,10 @@ fn demonstrate_pubsub() -> Result<(), Error> { )?; println!("Sending idiomatic message."); idiomatic_publisher.publish(rclrs_example_msgs::msg::VariousTypes::default())?; - executor - .spin(SpinOptions::spin_once()) - .first_error()?; + executor.spin(SpinOptions::spin_once()).first_error()?; println!("Sending RMW-native message."); direct_publisher.publish(rclrs_example_msgs::msg::rmw::VariousTypes::default())?; - executor - .spin(SpinOptions::spin_once()) - .first_error()?; + executor.spin(SpinOptions::spin_once()).first_error()?; Ok(()) } diff --git a/examples/minimal_client_service/src/minimal_client_async.rs b/examples/minimal_client_service/src/minimal_client_async.rs index d8dfa2aa5..c31f2e26e 100644 --- a/examples/minimal_client_service/src/minimal_client_async.rs +++ b/examples/minimal_client_service/src/minimal_client_async.rs @@ -21,8 +21,7 @@ async fn main() -> Result<(), Error> { println!("Waiting for response"); - let rclrs_spin = - tokio::task::spawn_blocking(move || executor.spin(SpinOptions::default())); + let rclrs_spin = tokio::task::spawn_blocking(move || executor.spin(SpinOptions::default())); let response = future.await?; println!( diff --git a/examples/minimal_pub_sub/src/minimal_publisher.rs b/examples/minimal_pub_sub/src/minimal_publisher.rs index c21aeb0ad..e74f4e05c 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 = executor.create_node("minimal_publisher")?; - let publisher = - node.create_publisher::("topic", QOS_PROFILE_DEFAULT)?; + let publisher = node.create_publisher::("topic", QOS_PROFILE_DEFAULT)?; 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 b5e24fd45..72eb55129 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -13,11 +13,7 @@ struct MinimalSubscriber { } impl MinimalSubscriber { - pub fn new( - executor: &Executor, - name: &str, - topic: &str, - ) -> Result, RclrsError> { + pub fn new(executor: &Executor, name: &str, topic: &str) -> Result, RclrsError> { let node = executor.create_node(name)?; let minimal_subscriber = Arc::new(MinimalSubscriber { num_messages: 0.into(), @@ -59,8 +55,8 @@ fn main() -> Result<(), Error> { let _subscriber_node_two = MinimalSubscriber::new(&executor, "minimal_subscriber_two", "topic")?; - let publisher = publisher_node - .create_publisher::("topic", QOS_PROFILE_DEFAULT)?; + let publisher = + publisher_node.create_publisher::("topic", QOS_PROFILE_DEFAULT)?; std::thread::spawn(move || -> Result<(), RclrsError> { let mut message = std_msgs::msg::String::default(); From b14e56e5853e02933de3d8da13dcd56b748edfb3 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 4 Feb 2025 15:24:38 +0800 Subject: [PATCH 14/17] Add documentation for Context::default Signed-off-by: Michael X. Grey --- rclrs/src/context.rs | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 1aeeaf104..2e70b8f04 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -79,6 +79,13 @@ pub(crate) struct ContextHandle { } impl Default for Context { + /// This will produce a [`Context`] without providing any command line + /// arguments and using only the default [`InitOptions`]. This is always + /// guaranteed to produce a valid [`Context`] instance. + /// + /// This is **not** the same as the "default context" defined for `rclcpp` + /// which is a globally shared context instance. `rclrs` does not offer a + /// globally shared context instance. fn default() -> Self { // SAFETY: It should always be valid to instantiate a context with no // arguments, no parameters, no options, etc. From d0472147e83d91a14dd62c0a9ea6479409b9b2f0 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 4 Feb 2025 15:34:24 +0800 Subject: [PATCH 15/17] Automatically clear dead nodes from the executor Signed-off-by: Michael X. Grey --- rclrs/src/executor.rs | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 437be1cc1..29f3220ea 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -78,6 +78,9 @@ impl Executor { } } + // Clear out any nodes that have been dropped. + self.nodes_mtx.lock().unwrap().retain(|weak_node| weak_node.strong_count() > 0); + Ok(()) } From e7f8bac0b69cdefed15267473526c69fa161f125 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 4 Feb 2025 15:35:44 +0800 Subject: [PATCH 16/17] Update reference index Signed-off-by: Michael X. Grey --- rclrs/src/publisher.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 6a6ad30d7..e3f44922a 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 the node's executor to [spin][2]. +/// Sending messages does not require the node's executor to [spin][1]. /// -/// [2]: crate::Executor::spin +/// [1]: crate::Executor::spin pub struct Publisher where T: Message, From 5c37fa824ea6eaf31e87f309f9a3bfaa1b53cb3b Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 4 Feb 2025 15:43:41 +0800 Subject: [PATCH 17/17] Fix style Signed-off-by: Michael X. Grey --- rclrs/src/executor.rs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 29f3220ea..7ffb08c93 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -79,7 +79,10 @@ impl Executor { } // Clear out any nodes that have been dropped. - self.nodes_mtx.lock().unwrap().retain(|weak_node| weak_node.strong_count() > 0); + self.nodes_mtx + .lock() + .unwrap() + .retain(|weak_node| weak_node.strong_count() > 0); Ok(()) }