Skip to content

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

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

Open
wants to merge 7 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 @@ -8,8 +8,8 @@ use anyhow::{Error, Result};

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

impl MinimalSubscriber {
Expand Down
9 changes: 4 additions & 5 deletions examples/rust_pubsub/src/simple_publisher.rs
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
use rclrs::*;
use std::{sync::Arc, thread, time::Duration};
use std::{thread, time::Duration};
use std_msgs::msg::String as StringMsg;

struct SimplePublisher {
publisher: Arc<Publisher<StringMsg>>,
publisher: Publisher<StringMsg>,
}

impl SimplePublisher {
Expand All @@ -24,12 +24,11 @@ impl SimplePublisher {

fn main() -> Result<(), RclrsError> {
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 publisher = SimplePublisher::new(&executor).unwrap();
let mut count: i32 = 0;
thread::spawn(move || loop {
thread::sleep(Duration::from_millis(1000));
count = publisher_other_thread.publish_data(count).unwrap();
count = publisher.publish_data(count).unwrap();
});
executor.spin(SpinOptions::default()).first_error()
}
7 changes: 3 additions & 4 deletions examples/rust_pubsub/src/simple_subscriber.rs
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ use std::{
use std_msgs::msg::String as StringMsg;

pub struct SimpleSubscriptionNode {
_subscriber: Arc<Subscription<StringMsg>>,
_subscriber: Subscription<StringMsg>,
data: Arc<Mutex<Option<StringMsg>>>,
}

Expand All @@ -34,11 +34,10 @@ impl SimpleSubscriptionNode {
}
fn main() -> Result<(), RclrsError> {
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);
let subscription = SimpleSubscriptionNode::new(&executor).unwrap();
thread::spawn(move || loop {
thread::sleep(Duration::from_millis(1000));
subscription_other_thread.data_callback().unwrap()
subscription.data_callback().unwrap()
});
executor.spin(SpinOptions::default()).first_error()
}
31 changes: 22 additions & 9 deletions rclrs/src/client.rs
Original file line number Diff line number Diff line change
Expand Up @@ -65,12 +65,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 @@ -80,16 +93,16 @@ where
/// Ensure the parent node remains alive as long as the subscription is held.
/// This implementation will change in the future.
#[allow(unused)]
node: Arc<Node>,
node: Node,
}

impl<T> Client<T>
impl<T> ClientState<T>
where
T: rosidl_runtime_rs::Service,
{
/// Creates a new client.
pub(crate) fn new<'a>(
node: &Arc<Node>,
node: &Node,
options: impl Into<ClientOptions<'a>>,
) -> Result<Self, RclrsError>
// This uses pub(crate) visibility to avoid instantiating this struct outside
Expand Down Expand Up @@ -319,7 +332,7 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From<T> for ClientOptions<'a> {
}
}

impl<T> ClientBase for Client<T>
impl<T> ClientBase for ClientState<T>
where
T: rosidl_runtime_rs::Service,
{
Expand Down
8 changes: 4 additions & 4 deletions rclrs/src/executor.rs
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
use crate::{
rcl_bindings::rcl_context_is_valid, Context, ContextHandle, IntoNodeOptions, Node, RclrsError,
WaitSet,
rcl_bindings::rcl_context_is_valid, Context, ContextHandle, IntoNodeOptions, Node, NodeState,
RclrsError, WaitSet,
};
use std::{
sync::{Arc, Mutex, Weak},
Expand All @@ -10,15 +10,15 @@ use std::{
/// Single-threaded executor implementation.
pub struct Executor {
context: Arc<ContextHandle>,
nodes_mtx: Mutex<Vec<Weak<Node>>>,
nodes_mtx: Mutex<Vec<Weak<NodeState>>>,
}

impl Executor {
/// Create a [`Node`] that will run on this Executor.
pub fn create_node<'a>(
&'a self,
options: impl IntoNodeOptions<'a>,
) -> Result<Arc<Node>, RclrsError> {
) -> Result<Node, RclrsError> {
let options = options.into_node_options();
let node = options.build(&self.context)?;
self.nodes_mtx.lock().unwrap().push(Arc::downgrade(&node));
Expand Down
52 changes: 31 additions & 21 deletions rclrs/src/node.rs
Original file line number Diff line number Diff line change
Expand Up @@ -19,11 +19,11 @@ use std::{
use rosidl_runtime_rs::Message;

use crate::{
rcl_bindings::*, Client, ClientBase, ClientOptions, Clock, ContextHandle, GuardCondition,
LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters,
Publisher, PublisherOptions, RclrsError, Service, ServiceBase, ServiceOptions, Subscription,
SubscriptionBase, SubscriptionCallback, SubscriptionOptions, TimeSource, ToLogParams,
ENTITY_LIFECYCLE_MUTEX,
rcl_bindings::*, Client, ClientBase, ClientOptions, ClientState, Clock, ContextHandle,
GuardCondition, LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant,
Parameters, Publisher, PublisherOptions, PublisherState, RclrsError, Service, ServiceBase,
ServiceOptions, ServiceState, Subscription, SubscriptionBase, SubscriptionCallback,
SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams, ENTITY_LIFECYCLE_MUTEX,
};

// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
Expand Down Expand Up @@ -59,7 +59,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()`][3] and [`Node::name()`][4] 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 @@ -72,7 +72,17 @@ unsafe impl Send for rcl_node_t {}
/// [5]: crate::NodeOptions::new
/// [6]: crate::NodeOptions::namespace
/// [7]: crate::Executor::create_node
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`]
/// 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 @@ -128,23 +138,23 @@ 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 {
/// Returns the clock associated with this node.
pub fn get_clock(&self) -> Clock {
self.time_source.get_clock()
Expand Down Expand Up @@ -202,7 +212,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 @@ -266,11 +276,11 @@ impl Node {
pub fn create_client<'a, T>(
self: &Arc<Self>,
options: impl Into<ClientOptions<'a>>,
) -> Result<Arc<Client<T>>, RclrsError>
) -> Result<Client<T>, RclrsError>
where
T: rosidl_runtime_rs::Service,
{
let client = Arc::new(Client::<T>::new(self, options)?);
let client = Arc::new(ClientState::<T>::new(self, options)?);
{ self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak<dyn ClientBase>);
Ok(client)
}
Expand Down Expand Up @@ -349,11 +359,11 @@ impl Node {
pub fn create_publisher<'a, T>(
&self,
options: impl Into<PublisherOptions<'a>>,
) -> Result<Arc<Publisher<T>>, RclrsError>
) -> Result<Publisher<T>, RclrsError>
where
T: Message,
{
let publisher = Arc::new(Publisher::<T>::new(Arc::clone(&self.handle), options)?);
let publisher = Arc::new(PublisherState::<T>::new(Arc::clone(&self.handle), options)?);
Ok(publisher)
}

Expand Down Expand Up @@ -403,12 +413,12 @@ impl Node {
self: &Arc<Self>,
options: impl Into<ServiceOptions<'a>>,
callback: F,
) -> Result<Arc<Service<T>>, RclrsError>
) -> 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(self, options, callback)?);
let service = Arc::new(ServiceState::<T>::new(self, options, callback)?);
{ self.services_mtx.lock().unwrap() }
.push(Arc::downgrade(&service) as Weak<dyn ServiceBase>);
Ok(service)
Expand Down Expand Up @@ -459,11 +469,11 @@ impl Node {
self: &Arc<Self>,
options: impl Into<SubscriptionOptions<'a>>,
callback: impl SubscriptionCallback<T, Args>,
) -> Result<Arc<Subscription<T>>, RclrsError>
) -> Result<Subscription<T>, RclrsError>
where
T: Message,
{
let subscription = Arc::new(Subscription::<T>::new(self, options, callback)?);
let subscription = Arc::new(SubscriptionState::<T>::new(self, options, callback)?);
{ self.subscriptions_mtx.lock() }
.unwrap()
.push(Arc::downgrade(&subscription) as Weak<dyn SubscriptionBase>);
Expand Down Expand Up @@ -583,7 +593,7 @@ impl Node {
}
}

impl<'a> ToLogParams<'a> for &'a Node {
impl<'a> ToLogParams<'a> for &'a NodeState {
fn to_log_params(self) -> LogParams<'a> {
self.logger().to_log_params()
}
Expand Down
4 changes: 2 additions & 2 deletions rclrs/src/node/graph.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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,
Expand Down
9 changes: 5 additions & 4 deletions rclrs/src/node/node_options.rs
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,9 @@ use std::{
};

use crate::{
rcl_bindings::*, ClockType, ContextHandle, Logger, Node, NodeHandle, ParameterInterface,
QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK,
rcl_bindings::*, ClockType, ContextHandle, Logger, Node, NodeHandle, NodeState,
ParameterInterface, QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX,
QOS_PROFILE_CLOCK,
};

/// This trait helps to build [`NodeOptions`] which can be passed into
Expand Down Expand Up @@ -285,7 +286,7 @@ impl<'a> NodeOptions<'a> {
///
/// Only used internally. Downstream users should call
/// [`Executor::create_node`].
pub(crate) fn build(self, context: &Arc<ContextHandle>) -> Result<Arc<Node>, RclrsError> {
pub(crate) fn build(self, context: &Arc<ContextHandle>) -> Result<Node, RclrsError> {
let node_name = CString::new(self.name).map_err(|err| RclrsError::StringContainsNul {
err,
s: self.name.to_owned(),
Expand Down Expand Up @@ -355,7 +356,7 @@ impl<'a> NodeOptions<'a> {
}
};

let node = Arc::new(Node {
let node = Arc::new(NodeState {
clients_mtx: Mutex::default(),
guard_conditions_mtx: Mutex::default(),
services_mtx: Mutex::default(),
Expand Down
4 changes: 2 additions & 2 deletions rclrs/src/parameter.rs
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,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<str>,
Expand Down Expand Up @@ -789,7 +789,7 @@ impl ParameterInterface {
}
}

pub(crate) fn create_services(&self, node: &Arc<Node>) -> Result<(), RclrsError> {
pub(crate) fn create_services(&self, node: &Node) -> Result<(), RclrsError> {
*self.services.lock().unwrap() =
Some(ParameterService::new(node, self.parameter_map.clone())?);
Ok(())
Expand Down
Loading