Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Shared state pattern (spin-off of #427) #430

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions examples/minimal_pub_sub/src/minimal_two_nodes.rs
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,8 @@ use anyhow::{Error, Result};

struct MinimalSubscriber {
num_messages: AtomicU32,
node: Arc<rclrs::Node>,
subscription: Mutex<Option<Arc<rclrs::Subscription<std_msgs::msg::String>>>>,
node: rclrs::Node,
subscription: Mutex<Option<rclrs::Subscription<std_msgs::msg::String>>>,
}

impl MinimalSubscriber {
Expand Down
4 changes: 2 additions & 2 deletions examples/rust_pubsub/src/simple_publisher.rs
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,8 @@ 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>,
_publisher: Arc<Publisher<StringMsg>>,
node: Node,
_publisher: Publisher<StringMsg>,
}
impl SimplePublisherNode {
fn new(context: &Context) -> Result<Self, RclrsError> {
Expand Down
4 changes: 2 additions & 2 deletions examples/rust_pubsub/src/simple_subscriber.rs
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@ use std::{
};
use std_msgs::msg::String as StringMsg;
pub struct SimpleSubscriptionNode {
node: Arc<Node>,
_subscriber: Arc<Subscription<StringMsg>>,
node: Node,
_subscriber: Subscription<StringMsg>,
data: Arc<Mutex<Option<StringMsg>>>,
}
impl SimpleSubscriptionNode {
Expand Down
27 changes: 20 additions & 7 deletions rclrs/src/client.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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].
///
/// [1]: crate::Node::create_client
/// [2]: crate::Node
pub struct Client<T>
/// Receiving responses requires the node's executor to [spin][2].
///
/// [1]: crate::NodeState::create_client
/// [2]: crate::spin
pub type Client<T> = Arc<ClientState<T>>;

/// 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<T>
where
T: rosidl_runtime_rs::Service,
{
Expand All @@ -78,7 +91,7 @@ where
futures: Arc<Mutex<HashMap<RequestId, oneshot::Sender<T::Response>>>>,
}

impl<T> Client<T>
impl<T> ClientState<T>
where
T: rosidl_runtime_rs::Service,
{
Expand Down Expand Up @@ -275,7 +288,7 @@ where
}
}

impl<T> ClientBase for Client<T>
impl<T> ClientBase for ClientState<T>
where
T: rosidl_runtime_rs::Service,
{
Expand Down
10 changes: 6 additions & 4 deletions rclrs/src/executor.rs
Original file line number Diff line number Diff line change
@@ -1,12 +1,14 @@
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,
};

/// Single-threaded executor implementation.
pub struct SingleThreadedExecutor {
nodes_mtx: Mutex<Vec<Weak<Node>>>,
nodes_mtx: Mutex<Vec<Weak<NodeState>>>,
}

impl Default for SingleThreadedExecutor {
Expand All @@ -24,13 +26,13 @@ impl SingleThreadedExecutor {
}

/// Add a node to the executor.
pub fn add_node(&self, node: &Arc<Node>) -> 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<Node>) -> 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(())
Expand Down
18 changes: 9 additions & 9 deletions rclrs/src/lib.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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::*;
Expand Down Expand Up @@ -59,14 +59,14 @@ pub use wait::*;
/// This can usually be ignored.
///
/// [1]: crate::RclReturnCode
pub fn spin_once(node: Arc<Node>, timeout: Option<Duration>) -> Result<(), RclrsError> {
pub fn spin_once(node: Node, timeout: Option<Duration>) -> 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<Node>) -> Result<(), RclrsError> {
pub fn spin(node: Node) -> Result<(), RclrsError> {
let executor = SingleThreadedExecutor::new();
executor.add_node(&node)?;
executor.spin()
Expand All @@ -77,7 +77,7 @@ pub fn spin(node: Arc<Node>) -> Result<(), RclrsError> {
/// Convenience function equivalent to [`Node::new`][1].
/// Please see that function's documentation.
///
/// [1]: crate::Node::new
/// [1]: crate::NodeState::new
///
/// # Example
/// ```
Expand All @@ -87,17 +87,17 @@ pub fn spin(node: Arc<Node>) -> Result<(), RclrsError> {
/// assert!(node.is_ok());
/// # Ok::<(), RclrsError>(())
/// ```
pub fn create_node(context: &Context, node_name: &str) -> Result<Arc<Node>, RclrsError> {
Node::new(context, node_name)
pub fn create_node(context: &Context, node_name: &str) -> Result<Node, RclrsError> {
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
/// ```
Expand All @@ -109,5 +109,5 @@ pub fn create_node(context: &Context, node_name: &str) -> Result<Arc<Node>, Rclr
/// # Ok::<(), RclrsError>(())
/// ```
pub fn create_node_builder(context: &Context, node_name: &str) -> NodeBuilder {
Node::builder(context, node_name)
NodeState::builder(context, node_name)
}
63 changes: 37 additions & 26 deletions rclrs/src/node.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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<NodeState>;

/// 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<Vec<Weak<dyn ClientBase>>>,
pub(crate) guard_conditions_mtx: Mutex<Vec<Weak<GuardCondition>>>,
pub(crate) services_mtx: Mutex<Vec<Weak<dyn ServiceBase>>>,
Expand Down Expand Up @@ -89,28 +100,28 @@ 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())
.finish()
}
}

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<Arc<Node>, RclrsError> {
pub fn new(context: &Context, node_name: &str) -> Result<Node, RclrsError> {
Self::builder(context, node_name).build()
}

Expand Down Expand Up @@ -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
/// ```
Expand Down Expand Up @@ -201,11 +212,11 @@ impl Node {
///
/// [1]: crate::Client
// TODO: make client's lifetime depend on node's lifetime
pub fn create_client<T>(&self, topic: &str) -> Result<Arc<Client<T>>, RclrsError>
pub fn create_client<T>(&self, topic: &str) -> Result<Client<T>, RclrsError>
where
T: rosidl_runtime_rs::Service,
{
let client = Arc::new(Client::<T>::new(Arc::clone(&self.handle), topic)?);
let client = Arc::new(ClientState::<T>::new(Arc::clone(&self.handle), topic)?);
{ self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak<dyn ClientBase>);
Ok(client)
}
Expand Down Expand Up @@ -259,28 +270,28 @@ impl Node {
&self,
topic: &str,
qos: QoSProfile,
) -> Result<Arc<Publisher<T>>, RclrsError>
) -> Result<Publisher<T>, RclrsError>
where
T: Message,
{
let publisher = Arc::new(Publisher::<T>::new(Arc::clone(&self.handle), topic, qos)?);
let publisher = Arc::new(PublisherState::<T>::new(
Arc::clone(&self.handle),
topic,
qos,
)?);
Ok(publisher)
}

/// Creates a [`Service`][1].
///
/// [1]: crate::Service
// TODO: make service's lifetime depend on node's lifetime
pub fn create_service<T, F>(
&self,
topic: &str,
callback: F,
) -> Result<Arc<Service<T>>, RclrsError>
pub fn create_service<T, F>(&self, topic: &str, callback: F) -> Result<Service<T>, RclrsError>
where
T: rosidl_runtime_rs::Service,
F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send,
{
let service = Arc::new(Service::<T>::new(
let service = Arc::new(ServiceState::<T>::new(
Arc::clone(&self.handle),
topic,
callback,
Expand All @@ -299,11 +310,11 @@ impl Node {
topic: &str,
qos: QoSProfile,
callback: impl SubscriptionCallback<T, Args>,
) -> Result<Arc<Subscription<T>>, RclrsError>
) -> Result<Subscription<T>, RclrsError>
where
T: Message,
{
let subscription = Arc::new(Subscription::<T>::new(
let subscription = Arc::new(SubscriptionState::<T>::new(
Arc::clone(&self.handle),
topic,
qos,
Expand Down Expand Up @@ -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>(())
/// ```
Expand Down
Loading
Loading