From 433a3485a329def63e3a3699b99b8566f1906f64 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 17:22:36 +0800 Subject: [PATCH 01/14] Migrate to SubscriptionOptions Signed-off-by: Michael X. Grey --- examples/message_demo/src/message_demo.rs | 2 - .../minimal_pub_sub/src/minimal_subscriber.rs | 1 - .../minimal_pub_sub/src/minimal_two_nodes.rs | 1 - .../src/zero_copy_subscriber.rs | 1 - examples/rust_pubsub/src/simple_subscriber.rs | 3 +- rclrs/src/node.rs | 68 +++-- rclrs/src/node/primitive_options.rs | 244 ++++++++++++++++++ rclrs/src/qos.rs | 52 +++- rclrs/src/subscription.rs | 51 ++-- rclrs/src/time_source.rs | 21 +- 10 files changed, 384 insertions(+), 60 deletions(-) create mode 100644 rclrs/src/node/primitive_options.rs diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index 3e2bf53b2..a31ee8007 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -153,13 +153,11 @@ fn demonstrate_pubsub() -> Result<(), Error> { let _idiomatic_subscription = node .create_subscription::( "topic", - rclrs::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, move |_msg: rclrs_example_msgs::msg::rmw::VariousTypes| { println!("Got RMW-native message!") }, diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index ebc5fc194..5d730a8e6 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -11,7 +11,6 @@ fn main() -> Result<(), Error> { let _subscription = node.create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, move |msg: std_msgs::msg::String| { num_messages += 1; println!("I heard: '{}'", msg.data); diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index fb03574a2..e70bba87a 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -29,7 +29,6 @@ impl MinimalSubscriber { .node .create_subscription::( topic, - rclrs::QOS_PROFILE_DEFAULT, move |msg: std_msgs::msg::String| { minimal_subscriber_aux.callback(msg); }, diff --git a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs index 9551dba0e..bed0a47c0 100644 --- a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -11,7 +11,6 @@ fn main() -> Result<(), Error> { let _subscription = node.create_subscription::( "topic", - rclrs::QOS_PROFILE_DEFAULT, move |msg: rclrs::ReadOnlyLoanedMessage<'_, std_msgs::msg::UInt32>| { num_messages += 1; println!("I heard: '{}'", msg.data); diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index a0d02bb4c..1ba670f13 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -1,4 +1,4 @@ -use rclrs::{create_node, Context, Node, RclrsError, Subscription, QOS_PROFILE_DEFAULT}; +use rclrs::{create_node, Context, Node, RclrsError, Subscription}; use std::{ env, sync::{Arc, Mutex}, @@ -19,7 +19,6 @@ impl SimpleSubscriptionNode { let _subscriber = node .create_subscription::( "publish_hello", - QOS_PROFILE_DEFAULT, move |msg: StringMsg| { *data_mut.lock().unwrap() = Some(msg); }, diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 97684d6bc..e41f8a18f 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,5 +1,6 @@ mod builder; mod graph; +mod primitive_options; use std::{ cmp::PartialEq, ffi::CStr, @@ -11,12 +12,12 @@ use std::{ use rosidl_runtime_rs::Message; -pub use self::{builder::*, graph::*}; +pub use self::{builder::*, graph::*, primitive_options::*}; 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, + SubscriptionOptions, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -292,12 +293,48 @@ impl Node { /// Creates a [`Subscription`][1]. /// - /// [1]: crate::Subscription - // TODO: make subscription's lifetime depend on node's lifetime - pub fn create_subscription( + /// + /// Pass in only the topic name for the `options` argument to use all default subscription options: + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let subscription = node.create_subscription( + /// "my_subscription", + /// |_msg: test_msgs::msg::Empty| { + /// println!("Received message!"); + /// }, + /// ); + /// ``` + /// + /// Take advantage of [`IntoPrimitiveOptions`] to easily build up the + /// subscription options: + /// + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let subscription = node.create_subscription( + /// "my_subscription" + /// .keep_last(100) + /// .transient_local(), + /// |_msg: test_msgs::msg::Empty| { + /// println!("Received message!"); + /// }, + /// ); + /// + /// let reliable_subscription = node.create_subscription( + /// "my_reliable_subscription" + /// .reliable(), + /// |_msg: test_msgs::msg::Empty| { + /// println!("Received message!"); + /// }, + /// ); + /// ``` + /// + pub fn create_subscription<'a, T, Args>( &self, - topic: &str, - qos: QoSProfile, + options: impl Into>, callback: impl SubscriptionCallback, ) -> Result>, RclrsError> where @@ -305,8 +342,7 @@ impl Node { { let subscription = Arc::new(Subscription::::new( Arc::clone(&self.handle), - topic, - qos, + options, callback, )?); { self.subscriptions_mtx.lock() } @@ -461,8 +497,7 @@ pub(crate) unsafe fn call_string_getter_with_rcl_node( #[cfg(test)] mod tests { - use super::*; - use crate::test_helpers::*; + use crate::{test_helpers::*, *}; #[test] fn traits() { @@ -472,25 +507,20 @@ mod tests { #[test] fn test_topic_names_and_types() -> Result<(), RclrsError> { - use crate::QOS_PROFILE_SYSTEM_DEFAULT; use test_msgs::msg; let graph = construct_test_graph("test_topics_graph")?; let _node_1_defaults_subscription = graph.node1.create_subscription::( "graph_test_topic_3", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::Defaults| {}, )?; - let _node_2_empty_subscription = graph.node2.create_subscription::( - "graph_test_topic_1", - QOS_PROFILE_SYSTEM_DEFAULT, - |_msg: msg::Empty| {}, - )?; + let _node_2_empty_subscription = graph + .node2 + .create_subscription::("graph_test_topic_1", |_msg: msg::Empty| {})?; let _node_2_basic_types_subscription = graph.node2.create_subscription::( "graph_test_topic_2", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::BasicTypes| {}, )?; diff --git a/rclrs/src/node/primitive_options.rs b/rclrs/src/node/primitive_options.rs new file mode 100644 index 000000000..7a8aa3eb8 --- /dev/null +++ b/rclrs/src/node/primitive_options.rs @@ -0,0 +1,244 @@ +use crate::{ + QoSDurabilityPolicy, QoSDuration, QoSHistoryPolicy, QoSLivelinessPolicy, QoSProfile, + QoSReliabilityPolicy, +}; + +use std::{borrow::Borrow, time::Duration}; + +/// `PrimitiveOptions` are the subset of options that are relevant across all +/// primitives (e.g. [`Subscription`][1], [`Publisher`][2], [`Client`][3], and +/// [`Service`][4]). +/// +/// Each different primitive type may have its own defaults for the overall +/// quality of service settings, and we cannot know what the default will be +/// until the `PrimitiveOptions` gets converted into the more specific set of +/// options. Therefore we store each quality of service field separately so that +/// we will only override the settings that the user explicitly asked for, and +/// the rest will be determined by the default settings for each primitive. +/// +/// [1]: crate::Subscription +/// [2]: crate::Publisher +/// [3]: crate::Client +/// [4]: crate::Service +#[derive(Debug, Clone, Copy)] +#[non_exhaustive] +pub struct PrimitiveOptions<'a> { + /// The name that will be used for the primitive + pub name: &'a str, + /// Override the default [`QoSProfile::history`] for the primitive. + pub history: Option, + /// Override the default [`QoSProfile::reliability`] for the primitive. + pub reliability: Option, + /// Override the default [`QoSProfile::durability`] for the primitive. + pub durability: Option, + /// Override the default [`QoSProfile::deadline`] for the primitive. + pub deadline: Option, + /// Override the default [`QoSProfile::lifespan`] for the primitive. + pub lifespan: Option, + /// Override the default [`QoSProfile::liveliness`] for the primitive. + pub liveliness: Option, + /// Override the default [`QoSProfile::liveliness_lease_duration`] for the primitive. + pub liveliness_lease: Option, + /// Override the default [`QoSProfile::avoid_ros_namespace_conventions`] for the primitive. + pub avoid_ros_namespace_conventions: Option, +} + +/// Trait to implicitly convert a compatible object into [`PrimitiveOptions`]. +pub trait IntoPrimitiveOptions<'a>: Sized { + /// Convert the object into [`PrimitiveOptions`] with default settings. + fn into_primitive_options(self) -> PrimitiveOptions<'a>; + + /// Override all the quality of service settings for the primitive. + fn qos(self, profile: QoSProfile) -> PrimitiveOptions<'a> { + self.into_primitive_options().history(profile.history) + } + + /// Use the default topics quality of service profile. + fn topics_qos(self) -> PrimitiveOptions<'a> { + self.qos(QoSProfile::topics_default()) + } + + /// Use the default sensor data quality of service profile. + fn sensor_data_qos(self) -> PrimitiveOptions<'a> { + self.qos(QoSProfile::sensor_data_default()) + } + + /// Use the default services quality of service profile. + fn services_qos(self) -> PrimitiveOptions<'a> { + self.qos(QoSProfile::services_default()) + } + + /// Use the system-defined default quality of service profile. This profile + /// is determined by the underlying RMW implementation, so you cannot rely + /// on this profile being consistent or appropriate for your needs. + fn system_qos(self) -> PrimitiveOptions<'a> { + self.qos(QoSProfile::system_default()) + } + + /// Override the default [`QoSProfile::history`] for the primitive. + fn history(self, history: QoSHistoryPolicy) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.history = Some(history); + options + } + + /// Keep the last `depth` messages for the primitive. + fn keep_last(self, depth: u32) -> PrimitiveOptions<'a> { + self.history(QoSHistoryPolicy::KeepLast { depth }) + } + + /// Keep all messages for the primitive. + fn keep_all(self) -> PrimitiveOptions<'a> { + self.history(QoSHistoryPolicy::KeepAll) + } + + /// Override the default [`QoSProfile::reliability`] for the primitive. + fn reliability(self, reliability: QoSReliabilityPolicy) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.reliability = Some(reliability); + options + } + + /// Set the primitive to have [reliable][QoSReliabilityPolicy::Reliable] communication. + fn reliable(self) -> PrimitiveOptions<'a> { + self.reliability(QoSReliabilityPolicy::Reliable) + } + + /// Set the primitive to have [best-effort][QoSReliabilityPolicy::BestEffort] communication. + fn best_effort(self) -> PrimitiveOptions<'a> { + self.reliability(QoSReliabilityPolicy::BestEffort) + } + + /// Override the default [`QoSProfile::durability`] for the primitive. + fn durability(self, durability: QoSDurabilityPolicy) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.durability = Some(durability); + options + } + + /// Set the primitive to have [volatile][QoSDurabilityPolicy::Volatile] durability. + fn volatile(self) -> PrimitiveOptions<'a> { + self.durability(QoSDurabilityPolicy::Volatile) + } + + /// Set the primitive to have [transient local][QoSDurabilityPolicy::TransientLocal] durability. + fn transient_local(self) -> PrimitiveOptions<'a> { + self.durability(QoSDurabilityPolicy::TransientLocal) + } + + /// Override the default [`QoSProfile::lifespan`] for the primitive. + fn lifespan(self, lifespan: QoSDuration) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.lifespan = Some(lifespan); + options + } + + /// Set a custom duration for the [lifespan][QoSProfile::lifespan] of the primitive. + fn lifespan_duration(self, duration: Duration) -> PrimitiveOptions<'a> { + self.lifespan(QoSDuration::Custom(duration)) + } + + /// Make the [lifespan][QoSProfile::lifespan] of the primitive infinite. + fn infinite_lifespan(self) -> PrimitiveOptions<'a> { + self.lifespan(QoSDuration::Infinite) + } + + /// Override the default [`QoSProfile::deadline`] for the primitive. + fn deadline(self, deadline: QoSDuration) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.deadline = Some(deadline); + options + } + + /// Set the [`QoSProfile::deadline`] to a custom finite value. + fn deadline_duration(self, duration: Duration) -> PrimitiveOptions<'a> { + self.deadline(QoSDuration::Custom(duration)) + } + + /// Do not use a deadline for liveliness for this primitive. + fn no_deadline(self) -> PrimitiveOptions<'a> { + self.deadline(QoSDuration::Infinite) + } + + /// Override the default [`QoSProfile::liveliness_lease`] for the primitive. + fn liveliness_lease(self, lease: QoSDuration) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.liveliness_lease = Some(lease); + options + } + + /// Set a custom duration for the [liveliness lease][QoSProfile::liveliness_lease]. + fn liveliness_lease_duration(self, duration: Duration) -> PrimitiveOptions<'a> { + self.liveliness_lease(QoSDuration::Custom(duration)) + } +} + +impl<'a> IntoPrimitiveOptions<'a> for PrimitiveOptions<'a> { + fn into_primitive_options(self) -> PrimitiveOptions<'a> { + self + } +} + +impl<'a> IntoPrimitiveOptions<'a> for &'a str { + fn into_primitive_options(self) -> PrimitiveOptions<'a> { + PrimitiveOptions::new(self) + } +} + +impl<'a, T: Borrow> IntoPrimitiveOptions<'a> for &'a T { + fn into_primitive_options(self) -> PrimitiveOptions<'a> { + self.borrow().into_primitive_options() + } +} + +impl<'a> PrimitiveOptions<'a> { + /// Begin building a new set of `PrimitiveOptions` with only the name set. + pub fn new(name: &'a str) -> Self { + Self { + name, + history: None, + reliability: None, + durability: None, + deadline: None, + lifespan: None, + liveliness: None, + liveliness_lease: None, + avoid_ros_namespace_conventions: None, + } + } + + /// Apply the user-specified options to a pre-initialized [`QoSProfile`]. + pub fn apply(&self, qos: &mut QoSProfile) { + if let Some(history) = self.history { + qos.history = history; + } + + if let Some(reliability) = self.reliability { + qos.reliability = reliability; + } + + if let Some(durability) = self.durability { + qos.durability = durability; + } + + if let Some(deadline) = self.deadline { + qos.deadline = deadline; + } + + if let Some(lifespan) = self.lifespan { + qos.lifespan = lifespan; + } + + if let Some(liveliness) = self.liveliness { + qos.liveliness = liveliness; + } + + if let Some(liveliness_lease) = self.liveliness_lease { + qos.liveliness_lease = liveliness_lease; + } + + if let Some(convention) = self.avoid_ros_namespace_conventions { + qos.avoid_ros_namespace_conventions = convention; + } + } +} diff --git a/rclrs/src/qos.rs b/rclrs/src/qos.rs index b26f01ef8..83eb797d7 100644 --- a/rclrs/src/qos.rs +++ b/rclrs/src/qos.rs @@ -166,7 +166,7 @@ pub struct QoSProfile { /// The time within which the RMW publisher must show that it is alive. /// /// If this is `Infinite`, liveliness is not enforced. - pub liveliness_lease_duration: QoSDuration, + pub liveliness_lease: QoSDuration, /// If true, any ROS specific namespacing conventions will be circumvented. /// /// In the case of DDS and topics, for example, this means the typical @@ -200,7 +200,7 @@ impl From for rmw_qos_profile_t { deadline: qos.deadline.into(), lifespan: qos.lifespan.into(), liveliness: qos.liveliness.into(), - liveliness_lease_duration: qos.liveliness_lease_duration.into(), + liveliness_lease_duration: qos.liveliness_lease.into(), avoid_ros_namespace_conventions: qos.avoid_ros_namespace_conventions, } } @@ -251,7 +251,7 @@ impl QoSProfile { /// Sets the QoS profile liveliness lease duration to the specified `Duration`. pub fn liveliness_lease_duration(mut self, lease_duration: Duration) -> Self { - self.liveliness_lease_duration = QoSDuration::Custom(lease_duration); + self.liveliness_lease = QoSDuration::Custom(lease_duration); self } @@ -260,6 +260,38 @@ impl QoSProfile { self.lifespan = QoSDuration::Custom(lifespan); self } + + /// Get the default QoS profile for ordinary topics. + pub fn topics_default() -> Self { + QOS_PROFILE_DEFAULT + } + + /// Get the default QoS profile for topics that transmit sensor data. + pub fn sensor_data_default() -> Self { + QOS_PROFILE_SENSOR_DATA + } + + /// Get the default QoS profile for services. + pub fn services_default() -> Self { + QOS_PROFILE_SERVICES_DEFAULT + } + + /// Get the default QoS profile for parameter services. + pub fn parameter_services_default() -> Self { + QOS_PROFILE_PARAMETERS + } + + /// Get the default QoS profile for parameter event topics. + pub fn parameter_events_default() -> Self { + QOS_PROFILE_PARAMETER_EVENTS + } + + /// Get the system-defined default quality of service profile. This profile + /// is determined by the underlying RMW implementation, so you cannot rely + /// on this profile being consistent or appropriate for your needs. + pub fn system_default() -> Self { + QOS_PROFILE_SYSTEM_DEFAULT + } } impl From for rmw_qos_history_policy_t { @@ -355,7 +387,7 @@ pub const QOS_PROFILE_SENSOR_DATA: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -370,7 +402,7 @@ pub const QOS_PROFILE_CLOCK: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -384,7 +416,7 @@ pub const QOS_PROFILE_PARAMETERS: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -398,7 +430,7 @@ pub const QOS_PROFILE_DEFAULT: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -412,7 +444,7 @@ pub const QOS_PROFILE_SERVICES_DEFAULT: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -426,7 +458,7 @@ pub const QOS_PROFILE_PARAMETER_EVENTS: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; @@ -440,6 +472,6 @@ pub const QOS_PROFILE_SYSTEM_DEFAULT: QoSProfile = QoSProfile { deadline: QoSDuration::SystemDefault, lifespan: QoSDuration::SystemDefault, liveliness: QoSLivelinessPolicy::SystemDefault, - liveliness_lease_duration: QoSDuration::SystemDefault, + liveliness_lease: QoSDuration::SystemDefault, avoid_ros_namespace_conventions: false, }; diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index fbd518c21..0494f71c9 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -10,7 +10,7 @@ use crate::{ error::{RclReturnCode, ToResult}, qos::QoSProfile, rcl_bindings::*, - NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + IntoPrimitiveOptions, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; mod callback; @@ -93,10 +93,9 @@ where T: Message, { /// Creates a new subscription. - pub(crate) fn new( + pub(crate) fn new<'a, Args>( node_handle: Arc, - topic: &str, - qos: QoSProfile, + options: impl Into>, callback: impl SubscriptionCallback, ) -> Result // This uses pub(crate) visibility to avoid instantiating this struct outside @@ -104,6 +103,7 @@ where where T: Message, { + let SubscriptionOptions { topic, qos } = options.into(); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_subscription = unsafe { rcl_get_zero_initialized_subscription() }; let type_support = @@ -114,8 +114,8 @@ where })?; // SAFETY: No preconditions for this function. - let mut subscription_options = unsafe { rcl_subscription_get_default_options() }; - subscription_options.qos = qos.into(); + let mut rcl_subscription_options = unsafe { rcl_subscription_get_default_options() }; + rcl_subscription_options.qos = qos.into(); { let rcl_node = node_handle.rcl_node.lock().unwrap(); @@ -132,7 +132,7 @@ where &*rcl_node, type_support, topic_c_string.as_ptr(), - &subscription_options, + &rcl_subscription_options, ) .ok()?; } @@ -263,6 +263,31 @@ where } } +/// `SubscriptionOptions` are used by [`Node::create_subscription`] to initialize +/// a [`Subscription`]. +#[derive(Debug, Clone)] +#[non_exhaustive] +pub struct SubscriptionOptions<'a> { + /// The topic name for the subscription. + pub topic: &'a str, + /// The quality of service settings for the subscription. + pub qos: QoSProfile, +} + +impl<'a, T: IntoPrimitiveOptions<'a>> From for SubscriptionOptions<'a> { + fn from(value: T) -> Self { + let options = value.into_primitive_options(); + // Topics will use the QOS_PROFILE_DEFAULT by default, which is designed + // to roughly match the ROS 1 default topic behavior. + let mut qos = QoSProfile::topics_default(); + options.apply(&mut qos); + Self { + topic: options.name, + qos, + } + } +} + impl SubscriptionBase for Subscription where T: Message, @@ -332,27 +357,23 @@ mod tests { #[test] fn test_subscriptions() -> Result<(), RclrsError> { - use crate::{TopicEndpointInfo, QOS_PROFILE_SYSTEM_DEFAULT}; + use crate::TopicEndpointInfo; let namespace = "/test_subscriptions_graph"; let graph = construct_test_graph(namespace)?; - let node_2_empty_subscription = graph.node2.create_subscription::( - "graph_test_topic_1", - QOS_PROFILE_SYSTEM_DEFAULT, - |_msg: msg::Empty| {}, - )?; + let node_2_empty_subscription = graph + .node2 + .create_subscription::("graph_test_topic_1", |_msg: msg::Empty| {})?; let topic1 = node_2_empty_subscription.topic_name(); let node_2_basic_types_subscription = graph.node2.create_subscription::( "graph_test_topic_2", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::BasicTypes| {}, )?; let topic2 = node_2_basic_types_subscription.topic_name(); let node_1_defaults_subscription = graph.node1.create_subscription::( "graph_test_topic_3", - QOS_PROFILE_SYSTEM_DEFAULT, |_msg: msg::Defaults| {}, )?; let topic3 = node_1_defaults_subscription.topic_name(); diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index a6b600800..a79398af4 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, + IntoPrimitiveOptions, Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, }; use std::sync::{Arc, Mutex, RwLock, Weak}; @@ -131,14 +131,17 @@ impl TimeSource { .unwrap() .upgrade() .unwrap() - .create_subscription::("/clock", self.clock_qos, move |msg: ClockMsg| { - let nanoseconds: i64 = - (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64; - *last_received_time.lock().unwrap() = Some(nanoseconds); - if let Some(clock) = clock.lock().unwrap().as_mut() { - Self::update_clock(clock, nanoseconds); - } - }) + .create_subscription::( + "/clock".qos(self.clock_qos), + move |msg: ClockMsg| { + let nanoseconds: i64 = + (msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64; + *last_received_time.lock().unwrap() = Some(nanoseconds); + if let Some(clock) = clock.lock().unwrap().as_mut() { + Self::update_clock(clock, nanoseconds); + } + }, + ) .unwrap() } } From f12f874e8339a6678d09b1e04f286211692523ff Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 17:54:27 +0800 Subject: [PATCH 02/14] Migrate to PublisherOptions Signed-off-by: Michael X. Grey --- examples/message_demo/src/message_demo.rs | 10 +---- .../minimal_pub_sub/src/minimal_publisher.rs | 3 +- .../minimal_pub_sub/src/minimal_two_nodes.rs | 3 +- .../src/zero_copy_publisher.rs | 3 +- examples/rust_pubsub/src/simple_publisher.rs | 6 +-- rclrs/src/node.rs | 43 +++++++++++++++---- rclrs/src/publisher.rs | 39 +++++++++++++---- 7 files changed, 72 insertions(+), 35 deletions(-) diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index a31ee8007..cd42dfd94 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -141,14 +141,8 @@ fn demonstrate_pubsub() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; let node = rclrs::create_node(&context, "message_demo")?; - let idiomatic_publisher = node.create_publisher::( - "topic", - rclrs::QOS_PROFILE_DEFAULT, - )?; - let direct_publisher = node.create_publisher::( - "topic", - rclrs::QOS_PROFILE_DEFAULT, - )?; + let idiomatic_publisher = node.create_publisher::("topic")?; + let direct_publisher = node.create_publisher::("topic")?; let _idiomatic_subscription = node .create_subscription::( diff --git a/examples/minimal_pub_sub/src/minimal_publisher.rs b/examples/minimal_pub_sub/src/minimal_publisher.rs index 720086917..f4cf30d0a 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 = rclrs::create_node(&context, "minimal_publisher")?; - let publisher = - node.create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + let publisher = node.create_publisher::("topic")?; 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 e70bba87a..247db325c 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -55,8 +55,7 @@ fn main() -> Result<(), Error> { let subscriber_node_one = MinimalSubscriber::new("minimal_subscriber_one", "topic")?; let subscriber_node_two = MinimalSubscriber::new("minimal_subscriber_two", "topic")?; - let publisher = publisher_node - .create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + let publisher = publisher_node.create_publisher::("topic")?; std::thread::spawn(move || -> Result<(), rclrs::RclrsError> { let mut message = std_msgs::msg::String::default(); diff --git a/examples/minimal_pub_sub/src/zero_copy_publisher.rs b/examples/minimal_pub_sub/src/zero_copy_publisher.rs index 5e73b5de7..2cccd1682 100644 --- a/examples/minimal_pub_sub/src/zero_copy_publisher.rs +++ b/examples/minimal_pub_sub/src/zero_copy_publisher.rs @@ -7,8 +7,7 @@ fn main() -> Result<(), Error> { let node = rclrs::create_node(&context, "minimal_publisher")?; - let publisher = - node.create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + let publisher = node.create_publisher::("topic")?; let mut publish_count: u32 = 1; diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index 98d0e0f74..6807ab1ef 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -1,4 +1,4 @@ -use rclrs::{create_node, Context, Node, Publisher, RclrsError, QOS_PROFILE_DEFAULT}; +use rclrs::{create_node, Context, Node, Publisher, RclrsError}; use std::{sync::Arc, thread, time::Duration}; use std_msgs::msg::String as StringMsg; struct SimplePublisherNode { @@ -8,9 +8,7 @@ struct SimplePublisherNode { impl SimplePublisherNode { fn new(context: &Context) -> Result { let node = create_node(context, "simple_publisher").unwrap(); - let _publisher = node - .create_publisher("publish_hello", QOS_PROFILE_DEFAULT) - .unwrap(); + let _publisher = node.create_publisher("publish_hello").unwrap(); Ok(Self { node, _publisher }) } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index e41f8a18f..8c7d9434e 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -15,7 +15,7 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*, primitive_options::*}; use crate::{ rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, + ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, PublisherOptions, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, SubscriptionOptions, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; @@ -252,19 +252,46 @@ impl Node { guard_condition } - /// Creates a [`Publisher`][1]. + /// Pass in only the topic name for the `options` argument to use all default publisher options: + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let publisher = node.create_publisher::( + /// "my_topic" + /// ) + /// .unwrap(); + /// ``` + /// + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the + /// publisher options: /// - /// [1]: crate::Publisher - // TODO: make publisher's lifetime depend on node's lifetime - pub fn create_publisher( + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let publisher = node.create_publisher::( + /// "my_topic" + /// .keep_last(100) + /// .transient_local() + /// ) + /// .unwrap(); + /// + /// let reliable_publisher = node.create_publisher::( + /// "my_topic" + /// .reliable() + /// ) + /// .unwrap(); + /// ``` + /// + pub fn create_publisher<'a, T>( &self, - topic: &str, - qos: QoSProfile, + options: impl Into>, ) -> Result>, RclrsError> where T: Message, { - let publisher = Arc::new(Publisher::::new(Arc::clone(&self.handle), topic, qos)?); + let publisher = Arc::new(Publisher::::new(Arc::clone(&self.handle), options)?); Ok(publisher) } diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 2935ca322..7cff6df6c 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -11,7 +11,7 @@ use crate::{ error::{RclrsError, ToResult}, qos::QoSProfile, rcl_bindings::*, - NodeHandle, ENTITY_LIFECYCLE_MUTEX, + NodeHandle, ENTITY_LIFECYCLE_MUTEX, IntoPrimitiveOptions, }; mod loaned_message; @@ -78,14 +78,14 @@ where /// Creates a new `Publisher`. /// /// Node and namespace changes are always applied _before_ topic remapping. - pub(crate) fn new( + pub(crate) fn new<'a>( node_handle: Arc, - topic: &str, - qos: QoSProfile, + options: impl Into>, ) -> Result where T: Message, { + let PublisherOptions { topic, qos } = options.into(); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_publisher = unsafe { rcl_get_zero_initialized_publisher() }; let type_support_ptr = @@ -231,6 +231,28 @@ where } } +/// `PublisherOptions` are used by [`Node::create_publisher`][1] to initialize +/// a [`Publisher`]. +/// +/// [1]: crate::NodeState::create_publisher +#[derive(Debug, Clone)] +#[non_exhaustive] +pub struct PublisherOptions<'a> { + /// The topic name for the publisher. + pub topic: &'a str, + /// The quality of service settings for the publisher. + pub qos: QoSProfile, +} + +impl<'a, T: IntoPrimitiveOptions<'a>> From for PublisherOptions<'a> { + fn from(value: T) -> Self { + let options = value.into_primitive_options(); + let mut qos = QoSProfile::topics_default(); + options.apply(&mut qos); + Self { topic: options.name, qos } + } +} + /// Convenience trait for [`Publisher::publish`]. pub trait MessageCow<'a, T: Message> { /// Wrap the owned or borrowed message in a `Cow`. @@ -262,7 +284,7 @@ mod tests { #[test] fn test_publishers() -> Result<(), RclrsError> { - use crate::{TopicEndpointInfo, QOS_PROFILE_SYSTEM_DEFAULT}; + use crate::TopicEndpointInfo; use test_msgs::msg; let namespace = "/test_publishers_graph"; @@ -270,16 +292,15 @@ mod tests { let node_1_empty_publisher = graph .node1 - .create_publisher::("graph_test_topic_1", QOS_PROFILE_SYSTEM_DEFAULT)?; + .create_publisher::("graph_test_topic_1")?; let topic1 = node_1_empty_publisher.topic_name(); let node_1_basic_types_publisher = graph.node1.create_publisher::( - "graph_test_topic_2", - QOS_PROFILE_SYSTEM_DEFAULT, + "graph_test_topic_2" )?; let topic2 = node_1_basic_types_publisher.topic_name(); let node_2_default_publisher = graph .node2 - .create_publisher::("graph_test_topic_3", QOS_PROFILE_SYSTEM_DEFAULT)?; + .create_publisher::("graph_test_topic_3")?; let topic3 = node_2_default_publisher.topic_name(); std::thread::sleep(std::time::Duration::from_millis(100)); From 2c32e207b7b44561cf805c2741b0e9eeb60298af Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 18:03:19 +0800 Subject: [PATCH 03/14] Migrate to ServiceOptions Signed-off-by: Michael X. Grey --- rclrs/src/node.rs | 56 ++++++++++++++++++++++++++++------ rclrs/src/parameter/service.rs | 21 ++++++++----- rclrs/src/service.rs | 34 +++++++++++++++++---- 3 files changed, 89 insertions(+), 22 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 8c7d9434e..0807d1a54 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -16,7 +16,7 @@ pub use self::{builder::*, graph::*, primitive_options::*}; use crate::{ rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, PublisherOptions, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, + RclrsError, Service, ServiceBase, ServiceOptions, Subscription, SubscriptionBase, SubscriptionCallback, SubscriptionOptions, TimeSource, ENTITY_LIFECYCLE_MUTEX, }; @@ -297,11 +297,49 @@ impl Node { /// Creates a [`Service`][1]. /// + /// Pass in only the service name for the `options` argument to use all default service options: + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let service = node.create_service::( + /// "my_service", + /// |_info, _request| { + /// println!("Received request!"); + /// test_msgs::srv::Empty_Response::default() + /// }, + /// ); + /// ``` + /// + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the + /// service options: + /// + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let service = node.create_service::( + /// "my_service" + /// .keep_all() + /// .transient_local(), + /// |_info, _request| { + /// println!("Received request!"); + /// test_msgs::srv::Empty_Response::default() + /// }, + /// ); + /// ``` + /// + /// Any quality of service options that you explicitly specify will override + /// the default service options. Any that you do not explicitly specify will + /// remain the default service options. Note that services are generally + /// expected to use [reliable][2], so is best not to change the reliability + /// setting unless you know what you are doing. + /// /// [1]: crate::Service - // TODO: make service's lifetime depend on node's lifetime - pub fn create_service( + /// [2]: crate::QoSReliabilityPolicy::Reliable + pub fn create_service<'a, T, F>( &self, - topic: &str, + options: impl Into>, callback: F, ) -> Result>, RclrsError> where @@ -310,7 +348,7 @@ impl Node { { let service = Arc::new(Service::::new( Arc::clone(&self.handle), - topic, + options, callback, )?); { self.services_mtx.lock().unwrap() } @@ -327,14 +365,14 @@ impl Node { /// # let context = Context::new([]).unwrap(); /// # let node = create_node(&context, "my_node").unwrap(); /// let subscription = node.create_subscription( - /// "my_subscription", + /// "my_topic", /// |_msg: test_msgs::msg::Empty| { /// println!("Received message!"); /// }, /// ); /// ``` /// - /// Take advantage of [`IntoPrimitiveOptions`] to easily build up the + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the /// subscription options: /// /// ``` @@ -342,7 +380,7 @@ impl Node { /// # let context = Context::new([]).unwrap(); /// # let node = create_node(&context, "my_node").unwrap(); /// let subscription = node.create_subscription( - /// "my_subscription" + /// "my_topic" /// .keep_last(100) /// .transient_local(), /// |_msg: test_msgs::msg::Empty| { @@ -351,7 +389,7 @@ impl Node { /// ); /// /// let reliable_subscription = node.create_subscription( - /// "my_reliable_subscription" + /// "my_reliable_topic" /// .reliable(), /// |_msg: test_msgs::msg::Empty| { /// println!("Received message!"); diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 7c8ffe62d..b4f79f77f 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -9,7 +9,8 @@ use rosidl_runtime_rs::Sequence; use super::ParameterMap; use crate::{ parameter::{DeclaredValue, ParameterKind, ParameterStorage}, - rmw_request_id_t, Node, RclrsError, Service, + rmw_request_id_t, Node, RclrsError, Service, IntoPrimitiveOptions, + QoSProfile, }; // The variables only exist to keep a strong reference to the services and are technically unused. @@ -247,7 +248,8 @@ impl ParameterService { // destruction is made for the parameter map. let map = parameter_map.clone(); let describe_parameters_service = node.create_service( - &(fqn.clone() + "/describe_parameters"), + (fqn.clone() + "/describe_parameters") + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: DescribeParameters_Request| { let map = map.lock().unwrap(); describe_parameters(req, &map) @@ -255,7 +257,8 @@ impl ParameterService { )?; let map = parameter_map.clone(); let get_parameter_types_service = node.create_service( - &(fqn.clone() + "/get_parameter_types"), + (fqn.clone() + "/get_parameter_types") + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { let map = map.lock().unwrap(); get_parameter_types(req, &map) @@ -263,7 +266,8 @@ impl ParameterService { )?; let map = parameter_map.clone(); let get_parameters_service = node.create_service( - &(fqn.clone() + "/get_parameters"), + (fqn.clone() + "/get_parameters") + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: GetParameters_Request| { let map = map.lock().unwrap(); get_parameters(req, &map) @@ -271,7 +275,8 @@ impl ParameterService { )?; let map = parameter_map.clone(); let list_parameters_service = node.create_service( - &(fqn.clone() + "/list_parameters"), + (fqn.clone() + "/list_parameters") + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: ListParameters_Request| { let map = map.lock().unwrap(); list_parameters(req, &map) @@ -279,14 +284,16 @@ impl ParameterService { )?; let map = parameter_map.clone(); let set_parameters_service = node.create_service( - &(fqn.clone() + "/set_parameters"), + (fqn.clone() + "/set_parameters") + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: SetParameters_Request| { let mut map = map.lock().unwrap(); set_parameters(req, &mut map) }, )?; let set_parameters_atomically_service = node.create_service( - &(fqn.clone() + "/set_parameters_atomically"), + (fqn.clone() + "/set_parameters_atomically") + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { let mut map = parameter_map.lock().unwrap(); set_parameters_atomically(req, &mut map) diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index ac43e51a8..f25727a29 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -9,7 +9,7 @@ use rosidl_runtime_rs::Message; use crate::{ error::{RclReturnCode, ToResult}, rcl_bindings::*, - MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + IntoPrimitiveOptions, MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, QoSProfile, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -80,9 +80,9 @@ where T: rosidl_runtime_rs::Service, { /// Creates a new service. - pub(crate) fn new( + pub(crate) fn new<'a, F>( node_handle: Arc, - topic: &str, + options: impl Into>, callback: F, ) -> Result // This uses pub(crate) visibility to avoid instantiating this struct outside @@ -91,17 +91,19 @@ where T: rosidl_runtime_rs::Service, F: Fn(&rmw_request_id_t, T::Request) -> T::Response + 'static + Send, { + let ServiceOptions { name, qos } = options.into(); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_service = unsafe { rcl_get_zero_initialized_service() }; let type_support = ::get_type_support() as *const rosidl_service_type_support_t; - let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { + let topic_c_string = CString::new(name).map_err(|err| RclrsError::StringContainsNul { err, - s: topic.into(), + s: name.into(), })?; // SAFETY: No preconditions for this function. - let service_options = unsafe { rcl_service_get_default_options() }; + let mut service_options = unsafe { rcl_service_get_default_options() }; + service_options.qos = qos.into(); { let rcl_node = node_handle.rcl_node.lock().unwrap(); @@ -181,6 +183,26 @@ where } } +/// `ServiceOptions are used by [`Node::create_service`][1] to initialize a +/// [`Service`] provider. +#[derive(Debug, Clone)] +#[non_exhaustive] +pub struct ServiceOptions<'a> { + /// The name for the service + pub name: &'a str, + /// The quality of service for the service. + pub qos: QoSProfile, +} + +impl<'a, T: IntoPrimitiveOptions<'a>> From for ServiceOptions<'a> { + fn from(value: T) -> Self { + let options = value.into_primitive_options(); + let mut qos = QoSProfile::services_default(); + options.apply(&mut qos); + Self { name: options.name, qos } + } +} + impl ServiceBase for Service where T: rosidl_runtime_rs::Service, From 6c61c9c2f2a468926184a15843c6f412486d65d3 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 19:58:21 +0800 Subject: [PATCH 04/14] Migrate to ClientOptions Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 45 +++++++++++++++++++++++++----- rclrs/src/node.rs | 51 ++++++++++++++++++++++++++++------ rclrs/src/parameter/service.rs | 20 +++++-------- rclrs/src/publisher.rs | 13 +++++---- rclrs/src/service.rs | 7 +++-- 5 files changed, 100 insertions(+), 36 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index b308f1de2..428a0245f 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -11,7 +11,7 @@ use rosidl_runtime_rs::Message; use crate::{ error::{RclReturnCode, ToResult}, rcl_bindings::*, - MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, + IntoPrimitiveOptions, MessageCow, NodeHandle, QoSProfile, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -83,23 +83,29 @@ where T: rosidl_runtime_rs::Service, { /// Creates a new client. - pub(crate) fn new(node_handle: Arc, topic: &str) -> Result + pub(crate) fn new<'a>( + node_handle: Arc, + options: impl Into>, + ) -> Result // This uses pub(crate) visibility to avoid instantiating this struct outside // [`Node::create_client`], see the struct's documentation for the rationale where T: rosidl_runtime_rs::Service, { + let ClientOptions { service_name, qos } = options.into(); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_client = unsafe { rcl_get_zero_initialized_client() }; let type_support = ::get_type_support() as *const rosidl_service_type_support_t; - let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul { - err, - s: topic.into(), - })?; + let topic_c_string = + CString::new(service_name).map_err(|err| RclrsError::StringContainsNul { + err, + s: service_name.into(), + })?; // SAFETY: No preconditions for this function. - let client_options = unsafe { rcl_client_get_default_options() }; + let mut client_options = unsafe { rcl_client_get_default_options() }; + client_options.qos = qos.into(); { let rcl_node = node_handle.rcl_node.lock().unwrap(); @@ -275,6 +281,31 @@ where } } +/// `ClientOptions` are used by [`Node::create_client`][1] to initialize a +/// [`Client`] for a service. +/// +/// [1]: crate::NodeState::create_client +#[derive(Debug, Clone)] +#[non_exhaustive] +pub struct ClientOptions<'a> { + /// The name of the service that this client will send requests to + pub service_name: &'a str, + /// The quality of the service profile for this client + pub qos: QoSProfile, +} + +impl<'a, T: IntoPrimitiveOptions<'a>> From for ClientOptions<'a> { + fn from(value: T) -> Self { + let options = value.into_primitive_options(); + let mut qos = QoSProfile::services_default(); + options.apply(&mut qos); + Self { + service_name: options.name, + qos, + } + } +} + impl ClientBase for Client where T: rosidl_runtime_rs::Service, diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 0807d1a54..ad5dbda77 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -14,10 +14,11 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*, primitive_options::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, PublisherOptions, - RclrsError, Service, ServiceBase, ServiceOptions, Subscription, SubscriptionBase, SubscriptionCallback, - SubscriptionOptions, TimeSource, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, Client, ClientBase, ClientOptions, Clock, Context, ContextHandle, + GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, + PublisherOptions, RclrsError, Service, ServiceBase, ServiceOptions, Subscription, + SubscriptionBase, SubscriptionCallback, SubscriptionOptions, TimeSource, + ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -200,13 +201,45 @@ impl Node { /// Creates a [`Client`][1]. /// - /// [1]: crate::Client - // TODO: make client's lifetime depend on node's lifetime - pub fn create_client(&self, topic: &str) -> Result>, RclrsError> + /// Pass in only the service name for the `options` argument to use all default client options: + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let client = node.create_client::( + /// "my_service" + /// ) + /// .unwrap(); + /// ``` + /// + /// Take advantage of the [`IntoPrimitiveOptions`] API to easily build up the + /// client options: + /// + /// ``` + /// # use rclrs::*; + /// # let context = Context::new([]).unwrap(); + /// # let node = create_node(&context, "my_node").unwrap(); + /// let client = node.create_client::( + /// "my_service" + /// .keep_all() + /// .transient_local() + /// ) + /// .unwrap(); + /// ``` + /// + /// Any quality of service options that you explicitly specify will override + /// the default service options. Any that you do not explicitly specify will + /// remain the default service options. Note that clients are generally + /// expected to use [reliable][2], so it's best not to change the reliability + /// setting unless you know what you are doing. + pub fn create_client<'a, T>( + &self, + options: impl Into>, + ) -> Result>, RclrsError> where T: rosidl_runtime_rs::Service, { - let client = Arc::new(Client::::new(Arc::clone(&self.handle), topic)?); + let client = Arc::new(Client::::new(Arc::clone(&self.handle), options)?); { self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); Ok(client) } @@ -332,7 +365,7 @@ impl Node { /// Any quality of service options that you explicitly specify will override /// the default service options. Any that you do not explicitly specify will /// remain the default service options. Note that services are generally - /// expected to use [reliable][2], so is best not to change the reliability + /// expected to use [reliable][2], so it's best not to change the reliability /// setting unless you know what you are doing. /// /// [1]: crate::Service diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index b4f79f77f..dbba78502 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -9,8 +9,7 @@ use rosidl_runtime_rs::Sequence; use super::ParameterMap; use crate::{ parameter::{DeclaredValue, ParameterKind, ParameterStorage}, - rmw_request_id_t, Node, RclrsError, Service, IntoPrimitiveOptions, - QoSProfile, + rmw_request_id_t, IntoPrimitiveOptions, Node, QoSProfile, RclrsError, Service, }; // The variables only exist to keep a strong reference to the services and are technically unused. @@ -248,8 +247,7 @@ impl ParameterService { // destruction is made for the parameter map. let map = parameter_map.clone(); let describe_parameters_service = node.create_service( - (fqn.clone() + "/describe_parameters") - .qos(QoSProfile::parameter_services_default()), + (fqn.clone() + "/describe_parameters").qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: DescribeParameters_Request| { let map = map.lock().unwrap(); describe_parameters(req, &map) @@ -257,8 +255,7 @@ impl ParameterService { )?; let map = parameter_map.clone(); let get_parameter_types_service = node.create_service( - (fqn.clone() + "/get_parameter_types") - .qos(QoSProfile::parameter_services_default()), + (fqn.clone() + "/get_parameter_types").qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { let map = map.lock().unwrap(); get_parameter_types(req, &map) @@ -266,8 +263,7 @@ impl ParameterService { )?; let map = parameter_map.clone(); let get_parameters_service = node.create_service( - (fqn.clone() + "/get_parameters") - .qos(QoSProfile::parameter_services_default()), + (fqn.clone() + "/get_parameters").qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: GetParameters_Request| { let map = map.lock().unwrap(); get_parameters(req, &map) @@ -275,8 +271,7 @@ impl ParameterService { )?; let map = parameter_map.clone(); let list_parameters_service = node.create_service( - (fqn.clone() + "/list_parameters") - .qos(QoSProfile::parameter_services_default()), + (fqn.clone() + "/list_parameters").qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: ListParameters_Request| { let map = map.lock().unwrap(); list_parameters(req, &map) @@ -284,8 +279,7 @@ impl ParameterService { )?; let map = parameter_map.clone(); let set_parameters_service = node.create_service( - (fqn.clone() + "/set_parameters") - .qos(QoSProfile::parameter_services_default()), + (fqn.clone() + "/set_parameters").qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: SetParameters_Request| { let mut map = map.lock().unwrap(); set_parameters(req, &mut map) @@ -293,7 +287,7 @@ impl ParameterService { )?; let set_parameters_atomically_service = node.create_service( (fqn.clone() + "/set_parameters_atomically") - .qos(QoSProfile::parameter_services_default()), + .qos(QoSProfile::parameter_services_default()), move |_req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { let mut map = parameter_map.lock().unwrap(); set_parameters_atomically(req, &mut map) diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 7cff6df6c..c37c446e0 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -11,7 +11,7 @@ use crate::{ error::{RclrsError, ToResult}, qos::QoSProfile, rcl_bindings::*, - NodeHandle, ENTITY_LIFECYCLE_MUTEX, IntoPrimitiveOptions, + IntoPrimitiveOptions, NodeHandle, ENTITY_LIFECYCLE_MUTEX, }; mod loaned_message; @@ -249,7 +249,10 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From for PublisherOptions<'a> { let options = value.into_primitive_options(); let mut qos = QoSProfile::topics_default(); options.apply(&mut qos); - Self { topic: options.name, qos } + Self { + topic: options.name, + qos, + } } } @@ -294,9 +297,9 @@ mod tests { .node1 .create_publisher::("graph_test_topic_1")?; let topic1 = node_1_empty_publisher.topic_name(); - let node_1_basic_types_publisher = graph.node1.create_publisher::( - "graph_test_topic_2" - )?; + let node_1_basic_types_publisher = graph + .node1 + .create_publisher::("graph_test_topic_2")?; let topic2 = node_1_basic_types_publisher.topic_name(); let node_2_default_publisher = graph .node2 diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index f25727a29..eb810ad1a 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -9,7 +9,7 @@ use rosidl_runtime_rs::Message; use crate::{ error::{RclReturnCode, ToResult}, rcl_bindings::*, - IntoPrimitiveOptions, MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, QoSProfile, + IntoPrimitiveOptions, MessageCow, NodeHandle, QoSProfile, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -199,7 +199,10 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From for ServiceOptions<'a> { let options = value.into_primitive_options(); let mut qos = QoSProfile::services_default(); options.apply(&mut qos); - Self { name: options.name, qos } + Self { + name: options.name, + qos, + } } } From da0536152c26203d8cf5fd050a5680718a218e7b Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 20:05:01 +0800 Subject: [PATCH 05/14] Enable direct creation of the _Options for all primitive types Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 21 ++++++++++++++------- rclrs/src/publisher.rs | 21 ++++++++++++++------- rclrs/src/service.rs | 23 +++++++++++++++-------- rclrs/src/subscription.rs | 23 ++++++++++++++--------- 4 files changed, 57 insertions(+), 31 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index 428a0245f..05282c35c 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -294,18 +294,25 @@ pub struct ClientOptions<'a> { pub qos: QoSProfile, } -impl<'a, T: IntoPrimitiveOptions<'a>> From for ClientOptions<'a> { - fn from(value: T) -> Self { - let options = value.into_primitive_options(); - let mut qos = QoSProfile::services_default(); - options.apply(&mut qos); +impl<'a> ClientOptions<'a> { + /// Initialize a new [`ClientOptions`] with default settings. + pub fn new(service_name: &'a str) -> Self { Self { - service_name: options.name, - qos, + service_name, + qos: QoSProfile::services_default(), } } } +impl<'a, T: IntoPrimitiveOptions<'a>> From for ClientOptions<'a> { + fn from(value: T) -> Self { + let primitive = value.into_primitive_options(); + let mut options = Self::new(primitive.name); + primitive.apply(&mut options.qos); + options + } +} + impl ClientBase for Client where T: rosidl_runtime_rs::Service, diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index c37c446e0..bc299f14e 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -244,18 +244,25 @@ pub struct PublisherOptions<'a> { pub qos: QoSProfile, } -impl<'a, T: IntoPrimitiveOptions<'a>> From for PublisherOptions<'a> { - fn from(value: T) -> Self { - let options = value.into_primitive_options(); - let mut qos = QoSProfile::topics_default(); - options.apply(&mut qos); +impl<'a> PublisherOptions<'a> { + /// Initialize a new [`PublisherOptions`] with default settings. + pub fn new(topic: &'a str) -> Self { Self { - topic: options.name, - qos, + topic, + qos: QoSProfile::topics_default(), } } } +impl<'a, T: IntoPrimitiveOptions<'a>> From for PublisherOptions<'a> { + fn from(value: T) -> Self { + let primitive = value.into_primitive_options(); + let mut options = Self::new(primitive.name); + primitive.apply(&mut options.qos); + options + } +} + /// Convenience trait for [`Publisher::publish`]. pub trait MessageCow<'a, T: Message> { /// Wrap the owned or borrowed message in a `Cow`. diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index eb810ad1a..1ff18c5b3 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -190,22 +190,29 @@ where pub struct ServiceOptions<'a> { /// The name for the service pub name: &'a str, - /// The quality of service for the service. + /// The quality of service profile for the service. pub qos: QoSProfile, } -impl<'a, T: IntoPrimitiveOptions<'a>> From for ServiceOptions<'a> { - fn from(value: T) -> Self { - let options = value.into_primitive_options(); - let mut qos = QoSProfile::services_default(); - options.apply(&mut qos); +impl<'a> ServiceOptions<'a> { + /// Initialize a new [`ServiceOptions`] with default settings. + pub fn new(name: &'a str) -> Self { Self { - name: options.name, - qos, + name, + qos: QoSProfile::services_default(), } } } +impl<'a, T: IntoPrimitiveOptions<'a>> From for ServiceOptions<'a> { + fn from(value: T) -> Self { + let primitive = value.into_primitive_options(); + let mut options = Self::new(primitive.name); + primitive.apply(&mut options.qos); + options + } +} + impl ServiceBase for Service where T: rosidl_runtime_rs::Service, diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 0494f71c9..cfc77cfc5 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -274,20 +274,25 @@ pub struct SubscriptionOptions<'a> { pub qos: QoSProfile, } -impl<'a, T: IntoPrimitiveOptions<'a>> From for SubscriptionOptions<'a> { - fn from(value: T) -> Self { - let options = value.into_primitive_options(); - // Topics will use the QOS_PROFILE_DEFAULT by default, which is designed - // to roughly match the ROS 1 default topic behavior. - let mut qos = QoSProfile::topics_default(); - options.apply(&mut qos); +impl<'a> SubscriptionOptions<'a> { + /// Initialize a new [`SubscriptionOptions`] with default settings. + pub fn new(topic: &'a str) -> Self { Self { - topic: options.name, - qos, + topic, + qos: QoSProfile::topics_default(), } } } +impl<'a, T: IntoPrimitiveOptions<'a>> From for SubscriptionOptions<'a> { + fn from(value: T) -> Self { + let primitive = value.into_primitive_options(); + let mut options = Self::new(primitive.name); + primitive.apply(&mut options.qos); + options + } +} + impl SubscriptionBase for Subscription where T: Message, From daebaa888bbe615af7fa10d51a3787af4ecc3ac2 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 23:19:06 +0800 Subject: [PATCH 06/14] Fix example formatting Signed-off-by: Michael X. Grey --- examples/message_demo/src/message_demo.rs | 6 ++++-- examples/rust_pubsub/src/simple_subscriber.rs | 9 +++------ 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index cd42dfd94..bed233c9e 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -141,8 +141,10 @@ fn demonstrate_pubsub() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; let node = rclrs::create_node(&context, "message_demo")?; - let idiomatic_publisher = node.create_publisher::("topic")?; - let direct_publisher = node.create_publisher::("topic")?; + let idiomatic_publisher = + node.create_publisher::("topic")?; + let direct_publisher = + node.create_publisher::("topic")?; let _idiomatic_subscription = node .create_subscription::( diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index 1ba670f13..488f53c00 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -17,12 +17,9 @@ impl SimpleSubscriptionNode { let data: Arc>> = Arc::new(Mutex::new(None)); let data_mut: Arc>> = Arc::clone(&data); let _subscriber = node - .create_subscription::( - "publish_hello", - move |msg: StringMsg| { - *data_mut.lock().unwrap() = Some(msg); - }, - ) + .create_subscription::("publish_hello", move |msg: StringMsg| { + *data_mut.lock().unwrap() = Some(msg); + }) .unwrap(); Ok(Self { node, From 126aaca87f0447a03430b20988b1b2ebbfd447ae Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 21 Nov 2024 00:10:27 +0800 Subject: [PATCH 07/14] Fix docs Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 2 +- rclrs/src/node.rs | 8 +++++--- rclrs/src/publisher.rs | 2 +- rclrs/src/subscription.rs | 4 +++- 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index 05282c35c..b679402a0 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -284,7 +284,7 @@ where /// `ClientOptions` are used by [`Node::create_client`][1] to initialize a /// [`Client`] for a service. /// -/// [1]: crate::NodeState::create_client +/// [1]: crate::Node::create_client #[derive(Debug, Clone)] #[non_exhaustive] pub struct ClientOptions<'a> { diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index ad5dbda77..1e47a70f6 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -199,7 +199,7 @@ impl Node { unsafe { call_string_getter_with_rcl_node(&rcl_node, getter) } } - /// Creates a [`Client`][1]. + /// Creates a [`Client`]. /// /// Pass in only the service name for the `options` argument to use all default client options: /// ``` @@ -230,8 +230,10 @@ impl Node { /// Any quality of service options that you explicitly specify will override /// the default service options. Any that you do not explicitly specify will /// remain the default service options. Note that clients are generally - /// expected to use [reliable][2], so it's best not to change the reliability + /// expected to use [reliable][1], so it's best not to change the reliability /// setting unless you know what you are doing. + /// + /// [1]: crate::QoSReliabilityPolicy::Reliable pub fn create_client<'a, T>( &self, options: impl Into>, @@ -389,7 +391,7 @@ impl Node { Ok(service) } - /// Creates a [`Subscription`][1]. + /// Creates a [`Subscription`]. /// /// /// Pass in only the topic name for the `options` argument to use all default subscription options: diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index bc299f14e..cb43c1746 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -234,7 +234,7 @@ where /// `PublisherOptions` are used by [`Node::create_publisher`][1] to initialize /// a [`Publisher`]. /// -/// [1]: crate::NodeState::create_publisher +/// [1]: crate::Node::create_publisher #[derive(Debug, Clone)] #[non_exhaustive] pub struct PublisherOptions<'a> { diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index cfc77cfc5..f99d4924d 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -263,8 +263,10 @@ where } } -/// `SubscriptionOptions` are used by [`Node::create_subscription`] to initialize +/// `SubscriptionOptions` are used by [`Node::create_subscription`][1] to initialize /// a [`Subscription`]. +/// +/// [1]: crate::Node::create_subscription #[derive(Debug, Clone)] #[non_exhaustive] pub struct SubscriptionOptions<'a> { From 012ff2e245699666c94d55bfb5dac561487ccff4 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 21 Nov 2024 13:14:34 +0800 Subject: [PATCH 08/14] Make deadline, liveliness_lease, and lifespan all symmetric Signed-off-by: Michael X. Grey --- rclrs/src/node/primitive_options.rs | 2 +- rclrs/src/qos.rs | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rclrs/src/node/primitive_options.rs b/rclrs/src/node/primitive_options.rs index 7a8aa3eb8..5682873cf 100644 --- a/rclrs/src/node/primitive_options.rs +++ b/rclrs/src/node/primitive_options.rs @@ -37,7 +37,7 @@ pub struct PrimitiveOptions<'a> { pub lifespan: Option, /// Override the default [`QoSProfile::liveliness`] for the primitive. pub liveliness: Option, - /// Override the default [`QoSProfile::liveliness_lease_duration`] for the primitive. + /// Override the default [`QoSProfile::liveliness_lease`] for the primitive. pub liveliness_lease: Option, /// Override the default [`QoSProfile::avoid_ros_namespace_conventions`] for the primitive. pub avoid_ros_namespace_conventions: Option, diff --git a/rclrs/src/qos.rs b/rclrs/src/qos.rs index 83eb797d7..699576964 100644 --- a/rclrs/src/qos.rs +++ b/rclrs/src/qos.rs @@ -244,7 +244,7 @@ impl QoSProfile { } /// Sets the QoS profile deadline to the specified `Duration`. - pub fn deadline(mut self, deadline: Duration) -> Self { + pub fn deadline_duration(mut self, deadline: Duration) -> Self { self.deadline = QoSDuration::Custom(deadline); self } @@ -256,7 +256,7 @@ impl QoSProfile { } /// Sets the QoS profile lifespan to the specified `Duration`. - pub fn lifespan(mut self, lifespan: Duration) -> Self { + pub fn lifespan_duration(mut self, lifespan: Duration) -> Self { self.lifespan = QoSDuration::Custom(lifespan); self } From f33e8d5f24ef82a4b9f6bffaabd5e5466ff023b1 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 21 Nov 2024 13:17:33 +0800 Subject: [PATCH 09/14] Add an API to the primitive options builder for avoiding ROS namespace conventions Signed-off-by: Michael X. Grey --- rclrs/src/node/primitive_options.rs | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rclrs/src/node/primitive_options.rs b/rclrs/src/node/primitive_options.rs index 5682873cf..0299c70f0 100644 --- a/rclrs/src/node/primitive_options.rs +++ b/rclrs/src/node/primitive_options.rs @@ -171,6 +171,15 @@ pub trait IntoPrimitiveOptions<'a>: Sized { fn liveliness_lease_duration(self, duration: Duration) -> PrimitiveOptions<'a> { self.liveliness_lease(QoSDuration::Custom(duration)) } + + /// [Avoid the ROS namespace conventions][1] for the primitive. + /// + /// [1]: QoSProfile::avoid_ros_namespace_conventions + fn avoid_ros_namespace_conventions(self) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.avoid_ros_namespace_conventions = Some(true); + options + } } impl<'a> IntoPrimitiveOptions<'a> for PrimitiveOptions<'a> { From 98b8ea2461217116326e6a5db43aa4b6a08c9c01 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 21 Nov 2024 13:51:28 +0800 Subject: [PATCH 10/14] Retrigger CI Signed-off-by: Michael X. Grey From d67f04136da2d7682ff05fc465d6554dd889781d Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 16 Dec 2024 09:39:02 +0800 Subject: [PATCH 11/14] Fix: Override all profile options when using .qos Signed-off-by: Michael X. Grey --- rclrs/src/node/primitive_options.rs | 33 ++++++++++++++++++++++++++++- 1 file changed, 32 insertions(+), 1 deletion(-) diff --git a/rclrs/src/node/primitive_options.rs b/rclrs/src/node/primitive_options.rs index 0299c70f0..72c5d30ff 100644 --- a/rclrs/src/node/primitive_options.rs +++ b/rclrs/src/node/primitive_options.rs @@ -50,7 +50,21 @@ pub trait IntoPrimitiveOptions<'a>: Sized { /// Override all the quality of service settings for the primitive. fn qos(self, profile: QoSProfile) -> PrimitiveOptions<'a> { - self.into_primitive_options().history(profile.history) + let mut options = self + .into_primitive_options() + .history(profile.history) + .reliability(profile.reliability) + .durability(profile.durability) + .deadline(profile.deadline) + .lifespan(profile.lifespan) + .liveliness(profile.liveliness) + .liveliness_lease(profile.liveliness_lease); + + if profile.avoid_ros_namespace_conventions { + options.avoid_ros_namespace_conventions = Some(true); + } + + options } /// Use the default topics quality of service profile. @@ -160,6 +174,23 @@ pub trait IntoPrimitiveOptions<'a>: Sized { self.deadline(QoSDuration::Infinite) } + /// Override the default [`QoSProfile::liveliness`] for the primitive. + fn liveliness(self, liveliness: QoSLivelinessPolicy) -> PrimitiveOptions<'a> { + let mut options = self.into_primitive_options(); + options.liveliness = Some(liveliness); + options + } + + /// Set liveliness to [`QoSLivelinessPolicy::Automatic`]. + fn liveliness_automatic(self) -> PrimitiveOptions<'a> { + self.liveliness(QoSLivelinessPolicy::Automatic) + } + + /// Set liveliness to [`QoSLivelinessPolicy::ManualByTopic`] + fn liveliness_manual(self) -> PrimitiveOptions<'a> { + self.liveliness(QoSLivelinessPolicy::ManualByTopic) + } + /// Override the default [`QoSProfile::liveliness_lease`] for the primitive. fn liveliness_lease(self, lease: QoSDuration) -> PrimitiveOptions<'a> { let mut options = self.into_primitive_options(); From 5e656c3f8bd20d9065e59cee3c1f16a876074f5a Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 16 Dec 2024 17:13:19 +0800 Subject: [PATCH 12/14] Reword the warning for system_qos Signed-off-by: Michael X. Grey --- rclrs/src/node/primitive_options.rs | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/rclrs/src/node/primitive_options.rs b/rclrs/src/node/primitive_options.rs index 72c5d30ff..a17c2363f 100644 --- a/rclrs/src/node/primitive_options.rs +++ b/rclrs/src/node/primitive_options.rs @@ -82,9 +82,12 @@ pub trait IntoPrimitiveOptions<'a>: Sized { self.qos(QoSProfile::services_default()) } - /// Use the system-defined default quality of service profile. This profile - /// is determined by the underlying RMW implementation, so you cannot rely - /// on this profile being consistent or appropriate for your needs. + /// Use the system-defined default quality of service profile. Topics and + /// services created with this default do not use the recommended ROS + /// defaults; they will instead use the default as defined by the underlying + /// RMW implementation (rmw_fastrtps, rmw_connextdds, etc). These defaults + /// may not always be appropriate for every use-case, and may be different + /// depending on which RMW implementation you are using, so use caution! fn system_qos(self) -> PrimitiveOptions<'a> { self.qos(QoSProfile::system_default()) } From 4ff94e21372f59acb18aa26080b159081c4f0582 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 16 Dec 2024 17:16:38 +0800 Subject: [PATCH 13/14] Reword the warning for QoSProfile::system_default Signed-off-by: Michael X. Grey --- rclrs/src/qos.rs | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/rclrs/src/qos.rs b/rclrs/src/qos.rs index 699576964..dcde59023 100644 --- a/rclrs/src/qos.rs +++ b/rclrs/src/qos.rs @@ -286,9 +286,12 @@ impl QoSProfile { QOS_PROFILE_PARAMETER_EVENTS } - /// Get the system-defined default quality of service profile. This profile - /// is determined by the underlying RMW implementation, so you cannot rely - /// on this profile being consistent or appropriate for your needs. + /// Get the system-defined default quality of service profile. Topics and + /// services created with this default do not use the recommended ROS + /// defaults; they will instead use the default as defined by the underlying + /// RMW implementation (rmw_fastrtps, rmw_connextdds, etc). These defaults + /// may not always be appropriate for every use-case, and may be different + /// depending on which RMW implementation you are using, so use caution! pub fn system_default() -> Self { QOS_PROFILE_SYSTEM_DEFAULT } From a872cd6c8eee2919909cc2e7771cbe2bd97881cd Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 16 Dec 2024 23:40:31 +0800 Subject: [PATCH 14/14] Rename PrimitiveOptions::apply to apply_to Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 2 +- rclrs/src/node/primitive_options.rs | 2 +- rclrs/src/publisher.rs | 2 +- rclrs/src/service.rs | 2 +- rclrs/src/subscription.rs | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index b679402a0..5f79c0ea6 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -308,7 +308,7 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From for ClientOptions<'a> { fn from(value: T) -> Self { let primitive = value.into_primitive_options(); let mut options = Self::new(primitive.name); - primitive.apply(&mut options.qos); + primitive.apply_to(&mut options.qos); options } } diff --git a/rclrs/src/node/primitive_options.rs b/rclrs/src/node/primitive_options.rs index a17c2363f..609735703 100644 --- a/rclrs/src/node/primitive_options.rs +++ b/rclrs/src/node/primitive_options.rs @@ -251,7 +251,7 @@ impl<'a> PrimitiveOptions<'a> { } /// Apply the user-specified options to a pre-initialized [`QoSProfile`]. - pub fn apply(&self, qos: &mut QoSProfile) { + pub fn apply_to(&self, qos: &mut QoSProfile) { if let Some(history) = self.history { qos.history = history; } diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index cb43c1746..d12e2f73b 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -258,7 +258,7 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From for PublisherOptions<'a> { fn from(value: T) -> Self { let primitive = value.into_primitive_options(); let mut options = Self::new(primitive.name); - primitive.apply(&mut options.qos); + primitive.apply_to(&mut options.qos); options } } diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index 1ff18c5b3..b4ca2d1aa 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -208,7 +208,7 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From for ServiceOptions<'a> { fn from(value: T) -> Self { let primitive = value.into_primitive_options(); let mut options = Self::new(primitive.name); - primitive.apply(&mut options.qos); + primitive.apply_to(&mut options.qos); options } } diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index f99d4924d..1d0143e53 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -290,7 +290,7 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From for SubscriptionOptions<'a> { fn from(value: T) -> Self { let primitive = value.into_primitive_options(); let mut options = Self::new(primitive.name); - primitive.apply(&mut options.qos); + primitive.apply_to(&mut options.qos); options } }