From bbb5333f094e3ef102844b8e8db540ff67820ded Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 23:00:56 +0800 Subject: [PATCH] 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