Skip to content

Commit

Permalink
Migrate to ClientOptions
Browse files Browse the repository at this point in the history
Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
  • Loading branch information
mxgrey committed Nov 20, 2024
1 parent 2c32e20 commit 6c61c9c
Show file tree
Hide file tree
Showing 5 changed files with 100 additions and 36 deletions.
45 changes: 38 additions & 7 deletions rclrs/src/client.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -83,23 +83,29 @@ where
T: rosidl_runtime_rs::Service,
{
/// Creates a new client.
pub(crate) fn new(node_handle: Arc<NodeHandle>, topic: &str) -> Result<Self, RclrsError>
pub(crate) fn new<'a>(
node_handle: Arc<NodeHandle>,
options: impl Into<ClientOptions<'a>>,
) -> Result<Self, RclrsError>
// 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 = <T as rosidl_runtime_rs::Service>::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();
Expand Down Expand Up @@ -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<T> 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<T> ClientBase for Client<T>
where
T: rosidl_runtime_rs::Service,
Expand Down
51 changes: 42 additions & 9 deletions rclrs/src/node.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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<T>(&self, topic: &str) -> Result<Arc<Client<T>>, 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::<test_msgs::srv::Empty>(
/// "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::<test_msgs::srv::Empty>(
/// "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<ClientOptions<'a>>,
) -> Result<Arc<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(Client::<T>::new(Arc::clone(&self.handle), options)?);
{ self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak<dyn ClientBase>);
Ok(client)
}
Expand Down Expand Up @@ -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
Expand Down
20 changes: 7 additions & 13 deletions rclrs/src/parameter/service.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -248,52 +247,47 @@ 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)
},
)?;
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)
},
)?;
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)
},
)?;
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)
},
)?;
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)
},
)?;
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)
Expand Down
13 changes: 8 additions & 5 deletions rclrs/src/publisher.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -249,7 +249,10 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From<T> 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,
}
}
}

Expand Down Expand Up @@ -294,9 +297,9 @@ mod tests {
.node1
.create_publisher::<msg::Empty>("graph_test_topic_1")?;
let topic1 = node_1_empty_publisher.topic_name();
let node_1_basic_types_publisher = graph.node1.create_publisher::<msg::BasicTypes>(
"graph_test_topic_2"
)?;
let node_1_basic_types_publisher = graph
.node1
.create_publisher::<msg::BasicTypes>("graph_test_topic_2")?;
let topic2 = node_1_basic_types_publisher.topic_name();
let node_2_default_publisher = graph
.node2
Expand Down
7 changes: 5 additions & 2 deletions rclrs/src/service.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -199,7 +199,10 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From<T> 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,
}
}
}

Expand Down

0 comments on commit 6c61c9c

Please sign in to comment.