From bf3d01acec2a936bdb4168e66b04de6a93d7224f Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 20:19:31 +0800 Subject: [PATCH 1/3] Migrate Node to shared state pattern Signed-off-by: Michael X. Grey --- .../minimal_pub_sub/src/minimal_two_nodes.rs | 2 +- examples/rust_pubsub/src/simple_publisher.rs | 2 +- examples/rust_pubsub/src/simple_subscriber.rs | 2 +- rclrs/src/client.rs | 2 +- rclrs/src/executor.rs | 10 +++--- rclrs/src/lib.rs | 18 +++++------ rclrs/src/node.rs | 31 +++++++++++++------ rclrs/src/node/builder.rs | 29 ++++++++--------- rclrs/src/node/graph.rs | 10 +++--- rclrs/src/parameter.rs | 2 +- rclrs/src/parameter/service.rs | 4 +-- rclrs/src/service.rs | 2 +- rclrs/src/subscription.rs | 2 +- rclrs/src/test_helpers/graph_helpers.rs | 5 ++- rclrs/src/time_source.rs | 6 ++-- 15 files changed, 70 insertions(+), 57 deletions(-) diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index fb03574a2..837a63fad 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -10,7 +10,7 @@ use anyhow::{Error, Result}; struct MinimalSubscriber { num_messages: AtomicU32, - node: Arc, + node: rclrs::Node, subscription: Mutex>>>, } diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index 98d0e0f74..d1ee9427f 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -2,7 +2,7 @@ use rclrs::{create_node, Context, Node, Publisher, RclrsError, QOS_PROFILE_DEFAU use std::{sync::Arc, thread, time::Duration}; use std_msgs::msg::String as StringMsg; struct SimplePublisherNode { - node: Arc, + node: Node, _publisher: Arc>, } impl SimplePublisherNode { diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index a0d02bb4c..01f8a6567 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -7,7 +7,7 @@ use std::{ }; use std_msgs::msg::String as StringMsg; pub struct SimpleSubscriptionNode { - node: Arc, + node: Node, _subscriber: Arc>, data: Arc>>, } diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index b308f1de2..e2ede79ef 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -67,7 +67,7 @@ type RequestId = i64; /// The only available way to instantiate clients is via [`Node::create_client`][1], this is to /// ensure that [`Node`][2]s can track all the clients that have been created. /// -/// [1]: crate::Node::create_client +/// [1]: crate::NodeState::create_client /// [2]: crate::Node pub struct Client where diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 37c43a68e..1b77dfbce 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,4 +1,6 @@ -use crate::{rcl_bindings::rcl_context_is_valid, Node, RclReturnCode, RclrsError, WaitSet}; +use crate::{ + rcl_bindings::rcl_context_is_valid, Node, NodeState, RclReturnCode, RclrsError, WaitSet, +}; use std::{ sync::{Arc, Mutex, Weak}, time::Duration, @@ -6,7 +8,7 @@ use std::{ /// Single-threaded executor implementation. pub struct SingleThreadedExecutor { - nodes_mtx: Mutex>>, + nodes_mtx: Mutex>>, } impl Default for SingleThreadedExecutor { @@ -24,13 +26,13 @@ impl SingleThreadedExecutor { } /// Add a node to the executor. - pub fn add_node(&self, node: &Arc) -> Result<(), RclrsError> { + pub fn add_node(&self, node: &Node) -> Result<(), RclrsError> { { self.nodes_mtx.lock().unwrap() }.push(Arc::downgrade(node)); Ok(()) } /// Remove a node from the executor. - pub fn remove_node(&self, node: Arc) -> Result<(), RclrsError> { + pub fn remove_node(&self, node: Node) -> Result<(), RclrsError> { { self.nodes_mtx.lock().unwrap() } .retain(|n| !n.upgrade().map(|n| Arc::ptr_eq(&n, &node)).unwrap_or(false)); Ok(()) diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 4924b36cb..281dc3788 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -30,7 +30,7 @@ mod rcl_bindings; #[cfg(feature = "dyn_msg")] pub mod dynamic_message; -use std::{sync::Arc, time::Duration}; +use std::time::Duration; pub use arguments::*; pub use client::*; @@ -59,14 +59,14 @@ pub use wait::*; /// This can usually be ignored. /// /// [1]: crate::RclReturnCode -pub fn spin_once(node: Arc, timeout: Option) -> Result<(), RclrsError> { +pub fn spin_once(node: Node, timeout: Option) -> Result<(), RclrsError> { let executor = SingleThreadedExecutor::new(); executor.add_node(&node)?; executor.spin_once(timeout) } /// Convenience function for calling [`spin_once`] in a loop. -pub fn spin(node: Arc) -> Result<(), RclrsError> { +pub fn spin(node: Node) -> Result<(), RclrsError> { let executor = SingleThreadedExecutor::new(); executor.add_node(&node)?; executor.spin() @@ -77,7 +77,7 @@ pub fn spin(node: Arc) -> Result<(), RclrsError> { /// Convenience function equivalent to [`Node::new`][1]. /// Please see that function's documentation. /// -/// [1]: crate::Node::new +/// [1]: crate::NodeState::new /// /// # Example /// ``` @@ -87,17 +87,17 @@ pub fn spin(node: Arc) -> Result<(), RclrsError> { /// assert!(node.is_ok()); /// # Ok::<(), RclrsError>(()) /// ``` -pub fn create_node(context: &Context, node_name: &str) -> Result, RclrsError> { - Node::new(context, node_name) +pub fn create_node(context: &Context, node_name: &str) -> Result { + NodeState::new(context, node_name) } /// Creates a [`NodeBuilder`]. /// -/// Convenience function equivalent to [`NodeBuilder::new()`][1] and [`Node::builder()`][2]. +/// Convenience function equivalent to [`NodeBuilder::new()`][1] and [`NodeState::builder()`][2]. /// Please see that function's documentation. /// /// [1]: crate::NodeBuilder::new -/// [2]: crate::Node::builder +/// [2]: crate::NodeState::builder /// /// # Example /// ``` @@ -109,5 +109,5 @@ pub fn create_node(context: &Context, node_name: &str) -> Result, Rclr /// # Ok::<(), RclrsError>(()) /// ``` pub fn create_node_builder(context: &Context, node_name: &str) -> NodeBuilder { - Node::builder(context, node_name) + NodeState::builder(context, node_name) } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 97684d6bc..100d9f127 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -48,7 +48,7 @@ unsafe impl Send for rcl_node_t {} /// In that sense, the parameters to the node creation functions are only the _default_ namespace and /// name. /// See also the [official tutorial][1] on the command line arguments for ROS nodes, and the -/// [`Node::namespace()`] and [`Node::name()`] functions for examples. +/// [`NodeState::namespace()`] and [`NodeState::name()`] functions for examples. /// /// ## Rules for valid names /// The rules for valid node names and node namespaces are explained in @@ -58,7 +58,18 @@ unsafe impl Send for rcl_node_t {} /// [2]: https://docs.ros.org/en/rolling/How-To-Guides/Node-arguments.html /// [3]: crate::NodeBuilder::new /// [4]: crate::NodeBuilder::namespace -pub struct Node { +pub type Node = Arc; + +/// The inner state of a [`Node`]. +/// +/// This is public so that you can choose to put it inside a [`Weak`] if you +/// want to be able to refer to a [`Node`] in a non-owning way. It is generally +/// recommended to manage the [`NodeState`] inside of an [`Arc`], and [`Node`] +/// recommended to manage the `NodeState` inside of an [`Arc`], and [`Node`] +/// is provided as convenience alias for that. +/// +/// The public API of the [`Node`] type is implemented via `NodeState`. +pub struct NodeState { pub(crate) clients_mtx: Mutex>>, pub(crate) guard_conditions_mtx: Mutex>>, pub(crate) services_mtx: Mutex>>, @@ -89,15 +100,15 @@ impl Drop for NodeHandle { } } -impl Eq for Node {} +impl Eq for NodeState {} -impl PartialEq for Node { +impl PartialEq for NodeState { fn eq(&self, other: &Self) -> bool { Arc::ptr_eq(&self.handle, &other.handle) } } -impl fmt::Debug for Node { +impl fmt::Debug for NodeState { fn fmt(&self, f: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> { f.debug_struct("Node") .field("fully_qualified_name", &self.fully_qualified_name()) @@ -105,12 +116,12 @@ impl fmt::Debug for Node { } } -impl Node { +impl NodeState { /// Creates a new node in the empty namespace. /// /// See [`NodeBuilder::new()`] for documentation. #[allow(clippy::new_ret_no_self)] - pub fn new(context: &Context, node_name: &str) -> Result, RclrsError> { + pub fn new(context: &Context, node_name: &str) -> Result { Self::builder(context, node_name).build() } @@ -171,7 +182,7 @@ impl Node { /// Returns the fully qualified name of the node. /// /// The fully qualified name of the node is the node namespace combined with the node name. - /// It is subject to the remappings shown in [`Node::name()`] and [`Node::namespace()`]. + /// It is subject to the remappings shown in [`NodeState::name()`] and [`NodeState::namespace()`]. /// /// # Example /// ``` @@ -431,9 +442,9 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, RclrsError}; + /// # use rclrs::{Context, NodeState, RclrsError}; /// let context = Context::new([])?; - /// let node = Node::builder(&context, "my_node").build()?; + /// let node = NodeState::builder(&context, "my_node").build()?; /// assert_eq!(node.name(), "my_node"); /// # Ok::<(), RclrsError>(()) /// ``` diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 011d5d2f3..eee3a4bec 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -4,14 +4,15 @@ use std::{ }; use crate::{ - rcl_bindings::*, ClockType, Context, ContextHandle, Node, NodeHandle, ParameterInterface, - QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, + rcl_bindings::*, ClockType, Context, ContextHandle, Node, NodeHandle, NodeState, + ParameterInterface, QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, + QOS_PROFILE_CLOCK, }; /// A builder for creating a [`Node`][1]. /// /// The builder pattern allows selectively setting some fields, and leaving all others at their default values. -/// This struct instance can be created via [`Node::builder()`][2]. +/// This struct instance can be created via [`NodeState::builder()`][2]. /// /// The default values for optional fields are: /// - `namespace: "/"` @@ -24,17 +25,17 @@ use crate::{ /// /// # Example /// ``` -/// # use rclrs::{Context, NodeBuilder, Node, RclrsError}; +/// # use rclrs::{Context, NodeBuilder, NodeState, RclrsError}; /// let context = Context::new([])?; /// // Building a node in a single expression /// let node = NodeBuilder::new(&context, "foo_node").namespace("/bar").build()?; /// assert_eq!(node.name(), "foo_node"); /// assert_eq!(node.namespace(), "/bar"); -/// // Building a node via Node::builder() -/// let node = Node::builder(&context, "bar_node").build()?; +/// // Building a node via NodeState::builder() +/// let node = NodeState::builder(&context, "bar_node").build()?; /// assert_eq!(node.name(), "bar_node"); /// // Building a node step-by-step -/// let mut builder = Node::builder(&context, "goose"); +/// let mut builder = NodeState::builder(&context, "goose"); /// builder = builder.namespace("/duck/duck"); /// let node = builder.build()?; /// assert_eq!(node.fully_qualified_name(), "/duck/duck/goose"); @@ -42,7 +43,7 @@ use crate::{ /// ``` /// /// [1]: crate::Node -/// [2]: crate::Node::builder +/// [2]: crate::NodeState::builder pub struct NodeBuilder { context: Arc, name: String, @@ -126,14 +127,14 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, RclrsError, RclReturnCode}; + /// # use rclrs::{Context, NodeState, RclrsError, RclReturnCode}; /// let context = Context::new([])?; /// // This is a valid namespace - /// let builder_ok_ns = Node::builder(&context, "my_node").namespace("/some/nested/namespace"); + /// let builder_ok_ns = NodeState::builder(&context, "my_node").namespace("/some/nested/namespace"); /// assert!(builder_ok_ns.build().is_ok()); /// // This is an invalid namespace /// assert!(matches!( - /// Node::builder(&context, "my_node") + /// NodeState::builder(&context, "my_node") /// .namespace("/10_percent_luck/20_percent_skill") /// .build() /// .unwrap_err(), @@ -141,7 +142,7 @@ impl NodeBuilder { /// )); /// // A missing forward slash at the beginning is automatically added /// assert_eq!( - /// Node::builder(&context, "my_node") + /// NodeState::builder(&context, "my_node") /// .namespace("foo") /// .build()? /// .namespace(), @@ -262,7 +263,7 @@ impl NodeBuilder { /// For example usage, see the [`NodeBuilder`][1] docs. /// /// [1]: crate::NodeBuilder - pub fn build(&self) -> Result, RclrsError> { + pub fn build(&self) -> Result { let node_name = CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul { err, @@ -308,7 +309,7 @@ impl NodeBuilder { &rcl_context.global_arguments, )? }; - let node = Arc::new(Node { + let node = Arc::new(NodeState { handle, clients_mtx: Mutex::new(vec![]), guard_conditions_mtx: Mutex::new(vec![]), diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 343fa61d3..8dc40e3ec 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -3,7 +3,7 @@ use std::{ ffi::{CStr, CString}, }; -use crate::{rcl_bindings::*, Node, RclrsError, ToResult}; +use crate::{rcl_bindings::*, NodeState, RclrsError, ToResult}; impl Drop for rmw_names_and_types_t { fn drop(&mut self) { @@ -57,7 +57,7 @@ pub struct TopicEndpointInfo { pub topic_type: String, } -impl Node { +impl NodeState { /// Returns a list of topic names and types for publishers associated with a node. pub fn get_publisher_names_and_types_by_node( &self, @@ -486,7 +486,7 @@ mod tests { Context::new_with_options([], InitOptions::new().with_domain_id(Some(domain_id))) .unwrap(); let node_name = "test_publisher_names_and_types"; - let node = Node::new(&context, node_name).unwrap(); + let node = NodeState::new(&context, node_name).unwrap(); // Test that the graph has no publishers let names_and_topics = node .get_publisher_names_and_types_by_node(node_name, "") @@ -545,7 +545,7 @@ mod tests { fn test_node_names() { let context = Context::new([]).unwrap(); let node_name = "test_node_names"; - let node = Node::new(&context, node_name).unwrap(); + let node = NodeState::new(&context, node_name).unwrap(); let names_and_namespaces = node.get_node_names().unwrap(); @@ -561,7 +561,7 @@ mod tests { fn test_node_names_with_enclaves() { let context = Context::new([]).unwrap(); let node_name = "test_node_names_with_enclaves"; - let node = Node::new(&context, node_name).unwrap(); + let node = NodeState::new(&context, node_name).unwrap(); let names_and_namespaces = node.get_node_names_with_enclaves().unwrap(); diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index c7153ce01..26d22725e 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -82,7 +82,7 @@ enum DeclaredValue { } /// Builder used to declare a parameter. Obtain this by calling -/// [`crate::Node::declare_parameter`]. +/// [`crate::NodeState::declare_parameter`]. #[must_use] pub struct ParameterBuilder<'a, T: ParameterVariant> { name: Arc, diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 7c8ffe62d..996be8538 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -319,7 +319,7 @@ mod tests { use std::sync::{Arc, RwLock}; struct TestNode { - node: Arc, + node: Node, bool_param: MandatoryParameter, _ns_param: MandatoryParameter, _read_only_param: ReadOnlyParameter, @@ -341,7 +341,7 @@ mod tests { Ok(()) } - fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { + fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Node) { let node = NodeBuilder::new(context, "node") .namespace(ns) .build() diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index ac43e51a8..47579c862 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -64,7 +64,7 @@ type ServiceCallback = /// The only available way to instantiate services is via [`Node::create_service()`][1], this is to /// ensure that [`Node`][2]s can track all the services that have been created. /// -/// [1]: crate::Node::create_service +/// [1]: crate::NodeState::create_service /// [2]: crate::Node pub struct Service where diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index fbd518c21..395d29426 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -76,7 +76,7 @@ pub trait SubscriptionBase: Send + Sync { /// /// [1]: crate::spin_once /// [2]: crate::spin -/// [3]: crate::Node::create_subscription +/// [3]: crate::NodeState::create_subscription /// [4]: crate::Node pub struct Subscription where diff --git a/rclrs/src/test_helpers/graph_helpers.rs b/rclrs/src/test_helpers/graph_helpers.rs index 1e9b581ae..2ee3d3bb0 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -1,9 +1,8 @@ use crate::{Context, Node, NodeBuilder, RclrsError}; -use std::sync::Arc; pub(crate) struct TestGraph { - pub node1: Arc, - pub node2: Arc, + pub node1: Node, + pub node2: Node, } pub(crate) fn construct_test_graph(namespace: &str) -> Result { diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index a6b600800..4237230ee 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -1,7 +1,7 @@ use crate::{ clock::{Clock, ClockSource, ClockType}, vendor::rosgraph_msgs::msg::Clock as ClockMsg, - Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, + Node, NodeState, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, }; use std::sync::{Arc, Mutex, RwLock, Weak}; @@ -9,7 +9,7 @@ use std::sync::{Arc, Mutex, RwLock, Weak}; /// If the node's `use_sim_time` parameter is set to `true`, the `TimeSource` will subscribe /// to the `/clock` topic and drive the attached clock pub(crate) struct TimeSource { - node: Mutex>, + node: Mutex>, clock: RwLock, clock_source: Arc>>, requested_clock_type: ClockType, @@ -85,7 +85,7 @@ impl TimeSource { /// Attaches the given node to to the `TimeSource`, using its interface to read the /// `use_sim_time` parameter and create the clock subscription. - pub(crate) fn attach_node(&self, node: &Arc) { + pub(crate) fn attach_node(&self, node: &Node) { // TODO(luca) Make this parameter editable and register a parameter callback // that calls set_ros_time(bool) once parameter callbacks are implemented. let param = node From bbb5333f094e3ef102844b8e8db540ff67820ded Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 23:00:56 +0800 Subject: [PATCH 2/3] Migrate primitives to state pattern Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 25 +++++++++--- rclrs/src/node.rs | 32 +++++++-------- rclrs/src/parameter/service.rs | 12 +++--- rclrs/src/publisher.rs | 33 ++++++++++++---- rclrs/src/publisher/loaned_message.rs | 6 +-- rclrs/src/service.rs | 33 ++++++++++++---- rclrs/src/subscription.rs | 39 ++++++++++++------- .../subscription/readonly_loaned_message.rs | 6 +-- rclrs/src/time_source.rs | 4 +- 9 files changed, 125 insertions(+), 65 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index e2ede79ef..b7ac77521 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -64,12 +64,25 @@ type RequestId = i64; /// Main class responsible for sending requests to a ROS service. /// -/// The only available way to instantiate clients is via [`Node::create_client`][1], this is to -/// ensure that [`Node`][2]s can track all the clients that have been created. +/// Create a client using [`Node::create_client`][1]. +/// +/// Receiving responses requires the node's executor to [spin][2]. /// /// [1]: crate::NodeState::create_client -/// [2]: crate::Node -pub struct Client +/// [2]: crate::spin +pub type Client = Arc>; + +/// The inner state of a [`Client`]. +/// +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Client`] in a non-owning way. It is +/// generally recommended to manage the `ClientState` inside of an [`Arc`], +/// and [`Client`] is provided as a convenience alias for that. +/// +/// The public API of the [`Client`] type is implemented via `ClientState`. +/// +/// [1]: std::sync::Weak +pub struct ClientState where T: rosidl_runtime_rs::Service, { @@ -78,7 +91,7 @@ where futures: Arc>>>, } -impl Client +impl ClientState where T: rosidl_runtime_rs::Service, { @@ -275,7 +288,7 @@ where } } -impl ClientBase for Client +impl ClientBase for ClientState where T: rosidl_runtime_rs::Service, { diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 100d9f127..118cfd01e 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -13,10 +13,10 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - TimeSource, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, Client, ClientBase, ClientState, Clock, Context, ContextHandle, + GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, + PublisherState, QoSProfile, RclrsError, Service, ServiceBase, ServiceState, Subscription, + SubscriptionBase, SubscriptionCallback, SubscriptionState, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -212,11 +212,11 @@ impl NodeState { /// /// [1]: crate::Client // TODO: make client's lifetime depend on node's lifetime - pub fn create_client(&self, topic: &str) -> Result>, RclrsError> + pub fn create_client(&self, topic: &str) -> Result, RclrsError> where T: rosidl_runtime_rs::Service, { - let client = Arc::new(Client::::new(Arc::clone(&self.handle), topic)?); + let client = Arc::new(ClientState::::new(Arc::clone(&self.handle), topic)?); { self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); Ok(client) } @@ -270,11 +270,15 @@ impl NodeState { &self, topic: &str, qos: QoSProfile, - ) -> Result>, RclrsError> + ) -> Result, RclrsError> where T: Message, { - let publisher = Arc::new(Publisher::::new(Arc::clone(&self.handle), topic, qos)?); + let publisher = Arc::new(PublisherState::::new( + Arc::clone(&self.handle), + topic, + qos, + )?); Ok(publisher) } @@ -282,16 +286,12 @@ impl NodeState { /// /// [1]: crate::Service // TODO: make service's lifetime depend on node's lifetime - pub fn create_service( - &self, - topic: &str, - callback: F, - ) -> Result>, RclrsError> + pub fn create_service(&self, topic: &str, callback: F) -> Result, RclrsError> where T: rosidl_runtime_rs::Service, F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send, { - let service = Arc::new(Service::::new( + let service = Arc::new(ServiceState::::new( Arc::clone(&self.handle), topic, callback, @@ -310,11 +310,11 @@ impl NodeState { topic: &str, qos: QoSProfile, callback: impl SubscriptionCallback, - ) -> Result>, RclrsError> + ) -> Result, RclrsError> where T: Message, { - let subscription = Arc::new(Subscription::::new( + let subscription = Arc::new(SubscriptionState::::new( Arc::clone(&self.handle), topic, qos, diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 996be8538..efe90278f 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -16,17 +16,17 @@ use crate::{ // What is used is the Weak that is stored in the node, and is upgraded when spinning. pub struct ParameterService { #[allow(dead_code)] - describe_parameters_service: Arc>, + describe_parameters_service: Service, #[allow(dead_code)] - get_parameter_types_service: Arc>, + get_parameter_types_service: Service, #[allow(dead_code)] - get_parameters_service: Arc>, + get_parameters_service: Service, #[allow(dead_code)] - list_parameters_service: Arc>, + list_parameters_service: Service, #[allow(dead_code)] - set_parameters_service: Arc>, + set_parameters_service: Service, #[allow(dead_code)] - set_parameters_atomically_service: Arc>, + set_parameters_atomically_service: Service, } fn describe_parameters( diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 2935ca322..33746c1bc 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -45,15 +45,32 @@ impl Drop for PublisherHandle { /// Struct for sending messages of type `T`. /// +/// Create a publisher using [`Node::create_publisher`][1]. +/// /// Multiple publishers can be created for the same topic, in different nodes or the same node. +/// A clone of a `Publisher` will refer to the same publisher instance as the original. +/// The underlying instance is tied to [`PublisherState`] which implements the [`Publisher`] API. /// /// The underlying RMW will decide on the concrete delivery mechanism (network stack, shared /// memory, or intraprocess). /// -/// Sending messages does not require calling [`spin`][1] on the publisher's node. +/// Sending messages does not require calling [`spin`][2] on the publisher's node. +/// +/// [1]: crate::NodeState::create_publisher +/// [2]: crate::spin +pub type Publisher = Arc>; + +/// The inner state of a [`Publisher`]. +/// +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Publisher`] in a non-owning way. It is +/// generally recommended to manage the `PublisherState` inside of an [`Arc`], +/// and [`Publisher`] is provided as a convenience alias for that. +/// +/// The public API of the [`Publisher`] type is implemented via `PublisherState`. /// -/// [1]: crate::spin -pub struct Publisher +/// [1]: std::sync::Weak +pub struct PublisherState where T: Message, { @@ -66,12 +83,12 @@ where // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread // they are running in. Therefore, this type can be safely sent to another thread. -unsafe impl Send for Publisher where T: Message {} +unsafe impl Send for PublisherState where T: Message {} // SAFETY: The type_support_ptr prevents the default Sync impl. // rosidl_message_type_support_t is a read-only type without interior mutability. -unsafe impl Sync for Publisher where T: Message {} +unsafe impl Sync for PublisherState where T: Message {} -impl Publisher +impl PublisherState where T: Message, { @@ -179,7 +196,7 @@ where } } -impl Publisher +impl PublisherState where T: RmwMessage, { @@ -231,7 +248,7 @@ where } } -/// Convenience trait for [`Publisher::publish`]. +/// Convenience trait for [`PublisherState::publish`]. pub trait MessageCow<'a, T: Message> { /// Wrap the owned or borrowed message in a `Cow`. fn into_cow(self) -> Cow<'a, T>; diff --git a/rclrs/src/publisher/loaned_message.rs b/rclrs/src/publisher/loaned_message.rs index c03fc300f..66ad1046a 100644 --- a/rclrs/src/publisher/loaned_message.rs +++ b/rclrs/src/publisher/loaned_message.rs @@ -2,13 +2,13 @@ use std::ops::{Deref, DerefMut}; use rosidl_runtime_rs::RmwMessage; -use crate::{rcl_bindings::*, Publisher, RclrsError, ToResult}; +use crate::{rcl_bindings::*, PublisherState, RclrsError, ToResult}; /// A message that is owned by the middleware, loaned for publishing. /// /// It dereferences to a `&mut T`. /// -/// This type is returned by [`Publisher::borrow_loaned_message()`], see the documentation of +/// This type is returned by [`PublisherState::borrow_loaned_message()`], see the documentation of /// that function for more information. /// /// The loan is returned by dropping the message or [publishing it][1]. @@ -19,7 +19,7 @@ where T: RmwMessage, { pub(super) msg_ptr: *mut T, - pub(super) publisher: &'a Publisher, + pub(super) publisher: &'a PublisherState, } impl<'a, T> Deref for LoanedMessage<'a, T> diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index 47579c862..8c02ec4e4 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -59,14 +59,33 @@ pub trait ServiceBase: Send + Sync { type ServiceCallback = Box Response + 'static + Send>; -/// Main class responsible for responding to requests sent by ROS clients. +/// Provide a service that can respond to requests sent by ROS service clients. /// -/// The only available way to instantiate services is via [`Node::create_service()`][1], this is to -/// ensure that [`Node`][2]s can track all the services that have been created. +/// Create a service using [`Node::create_service`][1]. +/// +/// ROS only supports having one service provider for any given fully-qualified +/// service name. "Fully-qualified" means the namespace is also taken into account +/// for uniqueness. A clone of a `Service` will refer to the same service provider +/// instance as the original. The underlying instance is tied to [`ServiceState`] +/// which implements the [`Service`] API. +/// +/// Responding to requests requires the node's executor to [spin][2]. /// /// [1]: crate::NodeState::create_service -/// [2]: crate::Node -pub struct Service +/// [2]: crate::spin +pub type Service = Arc>; + +/// The inner state of a [`Service`]. +/// +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Service`] in a non-owning way. It is +/// generally recommended to manage the `ServiceState` inside of an [`Arc`], +/// and [`Service`] is provided as a convenience alias for that. +/// +/// The public API of the [`Service`] type is implemented via `ServiceState`. +/// +/// [1]: std::sync::Weak +pub struct ServiceState where T: rosidl_runtime_rs::Service, { @@ -75,7 +94,7 @@ where pub callback: Mutex>, } -impl Service +impl ServiceState where T: rosidl_runtime_rs::Service, { @@ -181,7 +200,7 @@ where } } -impl ServiceBase for Service +impl ServiceBase for ServiceState where T: rosidl_runtime_rs::Service, { diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 395d29426..8580b499d 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -64,21 +64,32 @@ pub trait SubscriptionBase: Send + Sync { /// Struct for receiving messages of type `T`. /// +/// Create a subscription using [`Node::create_subscription`][1]. +/// /// There can be multiple subscriptions for the same topic, in different nodes or the same node. +/// A clone of a `Subscription` will refer to the same subscription instance as the original. +/// The underlying instance is tied to [`SubscriptionState`] which implements the [`Subscription`] API. /// -/// Receiving messages requires calling [`spin_once`][1] or [`spin`][2] on the subscription's node. +/// Receiving messages requires the node's executor to [spin][2]. /// /// When a subscription is created, it may take some time to get "matched" with a corresponding /// publisher. /// -/// The only available way to instantiate subscriptions is via [`Node::create_subscription()`][3], this -/// is to ensure that [`Node`][4]s can track all the subscriptions that have been created. -/// -/// [1]: crate::spin_once +/// [1]: crate::NodeState::create_subscription /// [2]: crate::spin -/// [3]: crate::NodeState::create_subscription -/// [4]: crate::Node -pub struct Subscription +pub type Subscription = Arc>; + +/// The inner state of a [`Subscription`]. +/// +/// This is public so that you can choose to create a [`Weak`][1] reference to it +/// if you want to be able to refer to a [`Subscription`] in a non-owning way. It is +/// generally recommended to manage the `SubscriptionState` inside of an [`Arc`], +/// and [`Subscription`] is provided as a convenience alias for that. +/// +/// The public API of the [`Subscription`] type is implemented via `SubscriptionState`. +/// +/// [1]: std::sync::Weak +pub struct SubscriptionState where T: Message, { @@ -88,7 +99,7 @@ where message: PhantomData, } -impl Subscription +impl SubscriptionState where T: Message, { @@ -233,11 +244,11 @@ where /// When there is no new message, this will return a /// [`SubscriptionTakeFailed`][1]. /// - /// This is the counterpart to [`Publisher::borrow_loaned_message()`][2]. See its documentation + /// This is the counterpart to [`PublisherState::borrow_loaned_message()`][2]. See its documentation /// for more information. /// /// [1]: crate::RclrsError - /// [2]: crate::Publisher::borrow_loaned_message + /// [2]: crate::PublisherState::borrow_loaned_message pub fn take_loaned(&self) -> Result<(ReadOnlyLoanedMessage<'_, T>, MessageInfo), RclrsError> { let mut msg_ptr = std::ptr::null_mut(); let mut message_info = unsafe { rmw_get_zero_initialized_message_info() }; @@ -263,7 +274,7 @@ where } } -impl SubscriptionBase for Subscription +impl SubscriptionBase for SubscriptionState where T: Message, { @@ -326,8 +337,8 @@ mod tests { #[test] fn traits() { - assert_send::>(); - assert_sync::>(); + assert_send::>(); + assert_sync::>(); } #[test] diff --git a/rclrs/src/subscription/readonly_loaned_message.rs b/rclrs/src/subscription/readonly_loaned_message.rs index df4ad6a5b..d287cc1b8 100644 --- a/rclrs/src/subscription/readonly_loaned_message.rs +++ b/rclrs/src/subscription/readonly_loaned_message.rs @@ -2,7 +2,7 @@ use std::ops::Deref; use rosidl_runtime_rs::Message; -use crate::{rcl_bindings::*, Subscription, ToResult}; +use crate::{rcl_bindings::*, SubscriptionState, ToResult}; /// A message that is owned by the middleware, loaned out for reading. /// @@ -10,7 +10,7 @@ use crate::{rcl_bindings::*, Subscription, ToResult}; /// message, it's the same as `&T`, and otherwise it's the corresponding RMW-native /// message. /// -/// This type is returned by [`Subscription::take_loaned()`] and may be used in +/// This type is returned by [`SubscriptionState::take_loaned()`] and may be used in /// subscription callbacks. /// /// The loan is returned by dropping the `ReadOnlyLoanedMessage`. @@ -19,7 +19,7 @@ where T: Message, { pub(super) msg_ptr: *const T::RmwMsg, - pub(super) subscription: &'a Subscription, + pub(super) subscription: &'a SubscriptionState, } impl<'a, T> Deref for ReadOnlyLoanedMessage<'a, T> diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index 4237230ee..747c696ce 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -14,7 +14,7 @@ pub(crate) struct TimeSource { clock_source: Arc>>, requested_clock_type: ClockType, clock_qos: QoSProfile, - clock_subscription: Mutex>>>, + clock_subscription: Mutex>>, last_received_time: Arc>>, // TODO(luca) Make this parameter editable when we have parameter callbacks implemented and can // safely change clock type at runtime @@ -122,7 +122,7 @@ impl TimeSource { clock.set_ros_time_override(nanoseconds); } - fn create_clock_sub(&self) -> Arc> { + fn create_clock_sub(&self) -> Subscription { let clock = self.clock_source.clone(); let last_received_time = self.last_received_time.clone(); // Safe to unwrap since the function will only fail if invalid arguments are provided From 2cff0b7599e3494044d290e00796d3f3bd72719c Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 23:25:52 +0800 Subject: [PATCH 3/3] Fix examples Signed-off-by: Michael X. Grey --- examples/minimal_pub_sub/src/minimal_two_nodes.rs | 2 +- examples/rust_pubsub/src/simple_publisher.rs | 2 +- examples/rust_pubsub/src/simple_subscriber.rs | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index 837a63fad..8712509bf 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -11,7 +11,7 @@ use anyhow::{Error, Result}; struct MinimalSubscriber { num_messages: AtomicU32, node: rclrs::Node, - subscription: Mutex>>>, + subscription: Mutex>>, } impl MinimalSubscriber { diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index d1ee9427f..359dfc16f 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -3,7 +3,7 @@ use std::{sync::Arc, thread, time::Duration}; use std_msgs::msg::String as StringMsg; struct SimplePublisherNode { node: Node, - _publisher: Arc>, + _publisher: Publisher, } impl SimplePublisherNode { fn new(context: &Context) -> Result { diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index 01f8a6567..f98ff8c58 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -8,7 +8,7 @@ use std::{ use std_msgs::msg::String as StringMsg; pub struct SimpleSubscriptionNode { node: Node, - _subscriber: Arc>, + _subscriber: Subscription, data: Arc>>, } impl SimpleSubscriptionNode {