From 8146bae25d9e51ecf98001c6d43f3a4ac671ce30 Mon Sep 17 00:00:00 2001 From: Samouwow Date: Mon, 20 May 2024 03:44:58 +0000 Subject: [PATCH 1/3] * added qos parameters to create_client and create_service. Updated all test references --- rclrs/src/client.rs | 13 +++++++--- rclrs/src/node.rs | 10 ++++++-- rclrs/src/parameter/service.rs | 43 +++++++++++++++++++++++++--------- rclrs/src/service.rs | 23 ++++++++++-------- 4 files changed, 63 insertions(+), 26 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index b308f1de..1424a424 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -10,6 +10,7 @@ use rosidl_runtime_rs::Message; use crate::{ error::{RclReturnCode, ToResult}, + qos::QoSProfile, rcl_bindings::*, MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; @@ -83,7 +84,11 @@ where T: rosidl_runtime_rs::Service, { /// Creates a new client. - pub(crate) fn new(node_handle: Arc, topic: &str) -> Result + pub(crate) fn new( + node_handle: Arc, + topic: &str, + qos: QoSProfile, + ) -> Result // This uses pub(crate) visibility to avoid instantiating this struct outside // [`Node::create_client`], see the struct's documentation for the rationale where @@ -99,7 +104,8 @@ where })?; // 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(); @@ -311,6 +317,7 @@ where mod tests { use super::*; use crate::test_helpers::*; + use crate::QOS_PROFILE_SERVICES_DEFAULT; use test_msgs::srv; #[test] @@ -325,7 +332,7 @@ mod tests { let graph = construct_test_graph(namespace)?; let _node_2_empty_client = graph .node2 - .create_client::("graph_test_topic_4")?; + .create_client::("graph_test_topic_4", QOS_PROFILE_SERVICES_DEFAULT)?; std::thread::sleep(std::time::Duration::from_millis(200)); diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 97684d6b..17486c5d 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -201,11 +201,15 @@ impl Node { /// /// [1]: crate::Client // TODO: make client's lifetime depend on node's lifetime - pub fn create_client(&self, topic: &str) -> Result>, RclrsError> + pub fn create_client( + &self, + topic: &str, + qos: QoSProfile, + ) -> 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), topic, qos)?); { self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); Ok(client) } @@ -274,6 +278,7 @@ impl Node { pub fn create_service( &self, topic: &str, + qos: QoSProfile, callback: F, ) -> Result>, RclrsError> where @@ -283,6 +288,7 @@ impl Node { let service = Arc::new(Service::::new( Arc::clone(&self.handle), topic, + qos, callback, )?); { self.services_mtx.lock().unwrap() } diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 7c8ffe62..e617c3fa 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -9,7 +9,7 @@ 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, QOS_PROFILE_SERVICES_DEFAULT, }; // The variables only exist to keep a strong reference to the services and are technically unused. @@ -248,6 +248,7 @@ impl ParameterService { let map = parameter_map.clone(); let describe_parameters_service = node.create_service( &(fqn.clone() + "/describe_parameters"), + QOS_PROFILE_SERVICES_DEFAULT, move |_req_id: &rmw_request_id_t, req: DescribeParameters_Request| { let map = map.lock().unwrap(); describe_parameters(req, &map) @@ -256,6 +257,7 @@ impl ParameterService { let map = parameter_map.clone(); let get_parameter_types_service = node.create_service( &(fqn.clone() + "/get_parameter_types"), + QOS_PROFILE_SERVICES_DEFAULT, move |_req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { let map = map.lock().unwrap(); get_parameter_types(req, &map) @@ -264,6 +266,7 @@ impl ParameterService { let map = parameter_map.clone(); let get_parameters_service = node.create_service( &(fqn.clone() + "/get_parameters"), + QOS_PROFILE_SERVICES_DEFAULT, move |_req_id: &rmw_request_id_t, req: GetParameters_Request| { let map = map.lock().unwrap(); get_parameters(req, &map) @@ -272,6 +275,7 @@ impl ParameterService { let map = parameter_map.clone(); let list_parameters_service = node.create_service( &(fqn.clone() + "/list_parameters"), + QOS_PROFILE_SERVICES_DEFAULT, move |_req_id: &rmw_request_id_t, req: ListParameters_Request| { let map = map.lock().unwrap(); list_parameters(req, &map) @@ -280,6 +284,7 @@ impl ParameterService { let map = parameter_map.clone(); let set_parameters_service = node.create_service( &(fqn.clone() + "/set_parameters"), + QOS_PROFILE_SERVICES_DEFAULT, move |_req_id: &rmw_request_id_t, req: SetParameters_Request| { let mut map = map.lock().unwrap(); set_parameters(req, &mut map) @@ -287,6 +292,7 @@ impl ParameterService { )?; let set_parameters_atomically_service = node.create_service( &(fqn.clone() + "/set_parameters_atomically"), + QOS_PROFILE_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) @@ -313,7 +319,7 @@ mod tests { srv::rmw::*, }, Context, MandatoryParameter, Node, NodeBuilder, ParameterRange, ParameterValue, RclrsError, - ReadOnlyParameter, + ReadOnlyParameter, QOS_PROFILE_SERVICES_DEFAULT, }; use rosidl_runtime_rs::{seq, Sequence}; use std::sync::{Arc, RwLock}; @@ -431,7 +437,10 @@ mod tests { async fn test_list_parameters_service() -> Result<(), RclrsError> { let context = Context::new([]).unwrap(); let (node, client) = construct_test_nodes(&context, "list"); - let list_client = client.create_client::("/list/node/list_parameters")?; + let list_client = client.create_client::( + "/list/node/list_parameters", + QOS_PROFILE_SERVICES_DEFAULT, + )?; try_until_timeout(|| list_client.service_is_ready().unwrap()) .await @@ -566,10 +575,18 @@ mod tests { async fn test_get_set_parameters_service() -> Result<(), RclrsError> { let context = Context::new([]).unwrap(); let (node, client) = construct_test_nodes(&context, "get_set"); - let get_client = client.create_client::("/get_set/node/get_parameters")?; - let set_client = client.create_client::("/get_set/node/set_parameters")?; - let set_atomically_client = client - .create_client::("/get_set/node/set_parameters_atomically")?; + let get_client = client.create_client::( + "/get_set/node/get_parameters", + QOS_PROFILE_SERVICES_DEFAULT, + )?; + let set_client = client.create_client::( + "/get_set/node/set_parameters", + QOS_PROFILE_SERVICES_DEFAULT, + )?; + let set_atomically_client = client.create_client::( + "/get_set/node/set_parameters_atomically", + QOS_PROFILE_SERVICES_DEFAULT, + )?; try_until_timeout(|| { get_client.service_is_ready().unwrap() @@ -799,10 +816,14 @@ mod tests { async fn test_describe_get_types_parameters_service() -> Result<(), RclrsError> { let context = Context::new([]).unwrap(); let (node, client) = construct_test_nodes(&context, "describe"); - let describe_client = - client.create_client::("/describe/node/describe_parameters")?; - let get_types_client = - client.create_client::("/describe/node/get_parameter_types")?; + let describe_client = client.create_client::( + "/describe/node/describe_parameters", + QOS_PROFILE_SERVICES_DEFAULT, + )?; + let get_types_client = client.create_client::( + "/describe/node/get_parameter_types", + QOS_PROFILE_SERVICES_DEFAULT, + )?; try_until_timeout(|| { describe_client.service_is_ready().unwrap() diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index ac43e51a..f8a28560 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -8,6 +8,7 @@ use rosidl_runtime_rs::Message; use crate::{ error::{RclReturnCode, ToResult}, + qos::QoSProfile, rcl_bindings::*, MessageCow, NodeHandle, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; @@ -83,6 +84,7 @@ where pub(crate) fn new( node_handle: Arc, topic: &str, + qos: QoSProfile, callback: F, ) -> Result // This uses pub(crate) visibility to avoid instantiating this struct outside @@ -101,7 +103,8 @@ where })?; // 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(); @@ -231,6 +234,7 @@ mod tests { #[test] fn test_services() -> Result<(), RclrsError> { use crate::TopicNamesAndTypes; + use crate::QOS_PROFILE_SERVICES_DEFAULT; use test_msgs::srv; let namespace = "/test_services_graph"; @@ -242,17 +246,16 @@ mod tests { assert!(types.contains(&"test_msgs/srv/Empty".to_string())); }; - let _node_1_empty_service = - graph - .node1 - .create_service::("graph_test_topic_4", |_, _| { - srv::Empty_Response { - structure_needs_at_least_one_member: 0, - } - })?; + let _node_1_empty_service = graph.node1.create_service::( + "graph_test_topic_4", + QOS_PROFILE_SERVICES_DEFAULT, + |_, _| srv::Empty_Response { + structure_needs_at_least_one_member: 0, + }, + )?; let _node_2_empty_client = graph .node2 - .create_client::("graph_test_topic_4")?; + .create_client::("graph_test_topic_4", QOS_PROFILE_SERVICES_DEFAULT)?; std::thread::sleep(std::time::Duration::from_millis(100)); From 964bf104a8ddff5c35670890fd02fcd6725e6212 Mon Sep 17 00:00:00 2001 From: Samouwow Date: Mon, 20 May 2024 06:49:56 +0000 Subject: [PATCH 2/3] * Added linting fixes --- rclrs/src/client.rs | 3 +-- rclrs/src/service.rs | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index 1424a424..284fac35 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -316,8 +316,7 @@ where #[cfg(test)] mod tests { use super::*; - use crate::test_helpers::*; - use crate::QOS_PROFILE_SERVICES_DEFAULT; + use crate::{test_helpers::*, QOS_PROFILE_SERVICES_DEFAULT}; use test_msgs::srv; #[test] diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index f8a28560..b082f61c 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -233,8 +233,7 @@ mod tests { #[test] fn test_services() -> Result<(), RclrsError> { - use crate::TopicNamesAndTypes; - use crate::QOS_PROFILE_SERVICES_DEFAULT; + use crate::{TopicNamesAndTypes, QOS_PROFILE_SERVICES_DEFAULT}; use test_msgs::srv; let namespace = "/test_services_graph"; From 0a9bcb4aac190a101aa0957e85876a370bdab75d Mon Sep 17 00:00:00 2001 From: Samouwow Date: Mon, 20 May 2024 07:08:44 +0000 Subject: [PATCH 3/3] * Updated example minimal_client_server --- examples/minimal_client_service/src/minimal_client.rs | 5 ++++- .../minimal_client_service/src/minimal_client_async.rs | 5 ++++- examples/minimal_client_service/src/minimal_service.rs | 7 +++++-- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/examples/minimal_client_service/src/minimal_client.rs b/examples/minimal_client_service/src/minimal_client.rs index 915541d5..5eb9dbc0 100644 --- a/examples/minimal_client_service/src/minimal_client.rs +++ b/examples/minimal_client_service/src/minimal_client.rs @@ -7,7 +7,10 @@ fn main() -> Result<(), Error> { let node = rclrs::create_node(&context, "minimal_client")?; - let client = node.create_client::("add_two_ints")?; + let client = node.create_client::( + "add_two_ints", + rclrs::QOS_PROFILE_SERVICES_DEFAULT, + )?; let request = example_interfaces::srv::AddTwoInts_Request { a: 41, b: 1 }; diff --git a/examples/minimal_client_service/src/minimal_client_async.rs b/examples/minimal_client_service/src/minimal_client_async.rs index 0eeb87f4..882c9d62 100644 --- a/examples/minimal_client_service/src/minimal_client_async.rs +++ b/examples/minimal_client_service/src/minimal_client_async.rs @@ -8,7 +8,10 @@ async fn main() -> Result<(), Error> { let node = rclrs::create_node(&context, "minimal_client")?; - let client = node.create_client::("add_two_ints")?; + let client = node.create_client::( + "add_two_ints", + rclrs::QOS_PROFILE_SERVICES_DEFAULT, + )?; println!("Starting client"); diff --git a/examples/minimal_client_service/src/minimal_service.rs b/examples/minimal_client_service/src/minimal_service.rs index b4149c81..27700fe9 100644 --- a/examples/minimal_client_service/src/minimal_service.rs +++ b/examples/minimal_client_service/src/minimal_service.rs @@ -17,8 +17,11 @@ fn main() -> Result<(), Error> { let node = rclrs::create_node(&context, "minimal_service")?; - let _server = node - .create_service::("add_two_ints", handle_service)?; + let _server = node.create_service::( + "add_two_ints", + rclrs::QOS_PROFILE_SERVICES_DEFAULT, + handle_service, + )?; println!("Starting server"); rclrs::spin(node).map_err(|err| err.into())