From c1662fd6e2f0031d142239e3733ff9ad1f4348a1 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 3 Nov 2023 15:15:17 +0800 Subject: [PATCH 01/24] WIP Adding describe paramater service --- .../minimal_pub_sub/src/minimal_subscriber.rs | 4 + rclrs/src/node/builder.rs | 6 +- rclrs/src/parameter.rs | 18 +- rclrs/src/parameter/service.rs | 174 ++++++++++++++---- rclrs/src/parameter/value.rs | 15 ++ rosidl_runtime_rs/src/string.rs | 21 ++- 6 files changed, 194 insertions(+), 44 deletions(-) diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index ebc5fc19..39dc46c2 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -6,6 +6,10 @@ fn main() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; let node = rclrs::create_node(&context, "minimal_subscriber")?; + let param = node.declare_parameter("test").default(42).mandatory().unwrap(); + let p2 = node.declare_parameter("hello").default(1.23).read_only().unwrap(); + let p3 = node.declare_parameter::("bool").optional().unwrap(); + let p4 = node.declare_parameter::("dynamic").optional().unwrap(); let mut num_messages: usize = 0; diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index b6373b7d..2be87dc3 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -265,7 +265,7 @@ impl NodeBuilder { &rcl_node_options.arguments, &rcl_context.global_arguments, )?; - Ok(Node { + let node = Node { rcl_node_mtx, rcl_context_mtx: self.context.clone(), clients_mtx: Mutex::new(vec![]), @@ -273,7 +273,9 @@ impl NodeBuilder { services_mtx: Mutex::new(vec![]), subscriptions_mtx: Mutex::new(vec![]), _parameter, - }) + }; + node._parameter.create_services(&node)?; + Ok(node) } /// Creates a rcl_node_options_t struct from this builder. diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index d04e3662..e70a308e 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -1,11 +1,13 @@ mod override_map; +mod service; mod value; pub(crate) use override_map::*; pub use value::*; +use service::*; use crate::rcl_bindings::*; -use crate::{call_string_getter_with_handle, RclrsError}; +use crate::{call_string_getter_with_handle, Node, RclrsError}; use std::collections::{btree_map::Entry, BTreeMap}; use std::fmt::Debug; use std::marker::PhantomData; @@ -606,7 +608,7 @@ enum ParameterStorage { } #[derive(Debug, Default)] -struct ParameterMap { +pub(crate) struct ParameterMap { storage: BTreeMap, ParameterStorage>, } @@ -770,8 +772,7 @@ pub(crate) struct ParameterInterface { _parameter_map: Arc>, _override_map: ParameterOverrideMap, allow_undeclared: AtomicBool, - // NOTE(luca-della-vedova) add a ParameterService field to this struct to add support for - // services. + services: Mutex>, } impl ParameterInterface { @@ -780,8 +781,8 @@ impl ParameterInterface { node_arguments: &rcl_arguments_t, global_arguments: &rcl_arguments_t, ) -> Result { - let rcl_node = rcl_node_mtx.lock().unwrap(); let _override_map = unsafe { + let rcl_node = rcl_node_mtx.lock().unwrap(); let fqn = call_string_getter_with_handle(&rcl_node, rcl_node_get_fully_qualified_name); resolve_parameter_overrides(&fqn, node_arguments, global_arguments)? }; @@ -790,6 +791,8 @@ impl ParameterInterface { _parameter_map: Default::default(), _override_map, allow_undeclared: Default::default(), + services: Mutex::new(None), + //services: ParameterService::new(rcl_node_mtx, parameter_map)?, }) } @@ -808,6 +811,11 @@ impl ParameterInterface { } } + pub(crate) fn create_services(&self, node: &Node) -> Result<(), RclrsError> { + *self.services.lock().unwrap() = Some(ParameterService::new(node, self._parameter_map.clone())?); + Ok(()) + } + fn get_declaration_initial_value<'a, T: ParameterVariant + 'a>( &self, name: &str, diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index bcbe24be..0231b0c0 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -1,11 +1,12 @@ -use std::sync::{Arc, Weak, Mutex}; +use std::sync::{Arc, Mutex}; use crate::vendor::rcl_interfaces::srv::rmw::*; use crate::vendor::rcl_interfaces::msg::rmw::*; -use rosidl_runtime_rs::{Sequence, seq}; +use rosidl_runtime_rs::{BoundedSequence, Sequence, seq}; -use crate::{rmw_request_id_t, Node, RclrsError, Service, ServiceBase}; -use crate::rcl_bindings::rcl_node_t; +use crate::{rmw_request_id_t, Node, RclrsError, Service}; +use crate::parameter::{DeclaredValue, ParameterRanges, ParameterKind, ParameterStorage}; +use super::ParameterMap; pub struct ParameterService { describe_parameters_service: Arc>, @@ -16,74 +17,175 @@ pub struct ParameterService { set_parameters_atomically_service: Arc>, } +// TODO(luca) should this be a method in the DeclaredStorage impl? +fn storage_to_parameter_type(storage: &ParameterStorage) -> u8 { + match storage { + ParameterStorage::Declared(s) => { + match s.kind { + ParameterKind::Bool => ParameterType::PARAMETER_BOOL, + ParameterKind::Integer => ParameterType::PARAMETER_INTEGER, + ParameterKind::Double => ParameterType::PARAMETER_DOUBLE, + ParameterKind::String => ParameterType::PARAMETER_STRING, + ParameterKind::ByteArray => ParameterType::PARAMETER_BYTE_ARRAY, + ParameterKind::BoolArray => ParameterType::PARAMETER_BOOL_ARRAY, + ParameterKind::IntegerArray => ParameterType::PARAMETER_INTEGER_ARRAY, + ParameterKind::DoubleArray => ParameterType::PARAMETER_DOUBLE_ARRAY, + ParameterKind::StringArray => ParameterType::PARAMETER_STRING_ARRAY, + ParameterKind::Dynamic => { + match &s.value { + DeclaredValue::Mandatory(v) => v.read().unwrap().rcl_parameter_type(), + DeclaredValue::Optional(v) => v.read().unwrap().as_ref().map(|v| v.rcl_parameter_type()).unwrap_or(ParameterType::PARAMETER_NOT_SET), + DeclaredValue::ReadOnly(v) => v.rcl_parameter_type(), + } + } + + } + } + ParameterStorage::Undeclared(value) => { + value.rcl_parameter_type() + } + } +} + +// TODO(luca) should this be a methond in the ParameterRanges impl? +fn parameter_ranges_to_descriptor_ranges(ranges: &ParameterRanges) -> (BoundedSequence, BoundedSequence) { + let int_range = ranges.integer.as_ref().map(|range| { + // Converting step to a positive value is safe because declaring a parameter with a + // negative step is not allowed. + // TODO(luca) explore changing step into a positive value in the generic definition to + // make negative steps a compile error. + if range.lower.is_none() && range.upper.is_none() && range.step.is_none() { + // It's a default range, just return a default + Default::default() + } else { + seq![1 # IntegerRange { + from_value: range.lower.unwrap_or(i64::MIN), + to_value: range.upper.unwrap_or(i64::MAX), + step: range.step.unwrap_or(0).try_into().unwrap(), + }] + } + }) + .unwrap_or_default(); + let float_range = ranges.float.as_ref().map(|range| { + // TODO(luca) Double check whether we should use MIN/MAX or INFINITY/NEG_INFINITY + if range.lower.is_none() && range.upper.is_none() && range.step.is_none() { + Default::default() + } else { + seq![1 # FloatingPointRange { + from_value: range.lower.unwrap_or(f64::MIN), + to_value: range.upper.unwrap_or(f64::MAX), + step: range.step.unwrap_or(0.0), + }] + } + }) + .unwrap_or_default(); + (int_range, float_range) +} + impl ParameterService { - pub fn new(rcl_node_mtx: Arc>) -> Result { - let describe_parameters_service = Service::new(Arc::clone(&rcl_node_mtx), + pub(crate) fn new(node: &Node, parameter_map: Arc>) -> Result { + // TODO(luca) node fully qualified name for the service topic prefix + let fqn = node.fully_qualified_name(); + // TODO(luca) make sure it is OK to have an Arc instead of a Weak here and cleanup on + // destruction is made for the parameter map. + let map = parameter_map.clone(); + let describe_parameters_service = node.create_service( "describe_parameters", - |req_id: &rmw_request_id_t, req: DescribeParameters_Request| { + move |req_id: &rmw_request_id_t, req: DescribeParameters_Request| { + // TODO(luca) look at the request and filter names + let map = map.lock().unwrap(); + let descriptors = req.names.iter().map(|name| { + // TODO(luca) Remove conversion to string and implement Borrow + let storage = map.storage.get(name.to_cstr().to_str().unwrap())?; + let mut descriptor = match storage { + ParameterStorage::Declared(storage) => { + let (integer_range, floating_point_range) = parameter_ranges_to_descriptor_ranges(&storage.options.ranges); + ParameterDescriptor { + name: name.clone().into(), + type_: Default::default(), + description: storage.options._description.clone().into(), + additional_constraints: storage.options._constraints.clone().into(), + dynamic_typing: matches!(storage.kind, ParameterKind::Dynamic), + read_only: matches!(storage.value, DeclaredValue::ReadOnly(_)), + floating_point_range, + integer_range, + } + } + ParameterStorage::Undeclared(value) => { + ParameterDescriptor { + name: name.clone().into(), + dynamic_typing: true, + ..Default::default() + } + } + }; + descriptor.type_ = storage_to_parameter_type(storage); + Some(descriptor) + }) + .collect::>() + .unwrap_or_default(); + // TODO(luca) error logging if the service call failed DescribeParameters_Response { - descriptors: seq![] + descriptors, } }, )?; - let get_parameter_types_service = Service::new(Arc::clone(&rcl_node_mtx), + let map = parameter_map.clone(); + let get_parameter_types_service = node.create_service( "get_parameter_types", - |req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { + move |req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { + // TODO(luca) look at the request and filter names + let types = map.lock().unwrap().storage.iter().map(|(name, storage)| { + storage_to_parameter_type(storage) + }) + .collect(); GetParameterTypes_Response { - types: seq![] + types, } }, )?; - let get_parameters_service = Service::new(Arc::clone(&rcl_node_mtx), + let map = parameter_map.clone(); + let get_parameters_service = node.create_service( "get_parameters", - |req_id: &rmw_request_id_t, req: GetParameters_Request| { + move |req_id: &rmw_request_id_t, req: GetParameters_Request| { GetParameters_Response { values: seq![] } }, )?; - let list_parameters_service = Service::new(Arc::clone(&rcl_node_mtx), + let map = parameter_map.clone(); + let list_parameters_service = node.create_service( "list_parameters", - |req_id: &rmw_request_id_t, req: ListParameters_Request| { + move |req_id: &rmw_request_id_t, req: ListParameters_Request| { ListParameters_Response { result: ListParametersResult::default() } }, )?; - let set_parameters_service = Service::new(Arc::clone(&rcl_node_mtx), + let map = parameter_map.clone(); + let set_parameters_service = node.create_service( "set_parameters", - |req_id: &rmw_request_id_t, req: SetParameters_Request| { + move |req_id: &rmw_request_id_t, req: SetParameters_Request| { SetParameters_Response { results: seq![] } }, )?; - let set_parameters_atomically_service = Service::new(Arc::clone(&rcl_node_mtx), + let set_parameters_atomically_service = node.create_service( "set_parameters_atomically", - |req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { + move |req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { SetParametersAtomically_Response { result: SetParametersResult::default() } }, )?; Ok(Self { - describe_parameters_service: Arc::new(describe_parameters_service), - get_parameter_types_service: Arc::new(get_parameter_types_service), - get_parameters_service: Arc::new(get_parameters_service), - list_parameters_service: Arc::new(list_parameters_service), - set_parameters_service: Arc::new(set_parameters_service), - set_parameters_atomically_service: Arc::new(set_parameters_atomically_service), + describe_parameters_service, + get_parameter_types_service, + get_parameters_service, + list_parameters_service, + set_parameters_service, + set_parameters_atomically_service, }) } - - pub fn services(&self) -> Vec> { - vec![ - Arc::downgrade(&self.describe_parameters_service) as Weak, - Arc::downgrade(&self.get_parameter_types_service) as Weak, - Arc::downgrade(&self.get_parameters_service) as Weak, - Arc::downgrade(&self.list_parameters_service) as Weak, - Arc::downgrade(&self.set_parameters_service) as Weak, - Arc::downgrade(&self.set_parameters_atomically_service) as Weak, - ] - } } diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index 1802145f..f65e345c 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -3,6 +3,7 @@ use std::sync::Arc; use crate::rcl_bindings::*; use crate::{ParameterRange, ParameterRanges, ParameterValueError}; +use crate::vendor::rcl_interfaces::msg::rmw::ParameterType; /// A parameter value. /// @@ -392,6 +393,20 @@ impl ParameterValue { unreachable!() } } + + pub(crate) fn rcl_parameter_type(&self) -> u8 { + match self { + ParameterValue::Bool(_) => ParameterType::PARAMETER_BOOL, + ParameterValue::Integer(_) => ParameterType::PARAMETER_INTEGER, + ParameterValue::Double(_) => ParameterType::PARAMETER_DOUBLE, + ParameterValue::String(_) => ParameterType::PARAMETER_STRING, + ParameterValue::ByteArray(_) => ParameterType::PARAMETER_BYTE_ARRAY, + ParameterValue::BoolArray(_) => ParameterType::PARAMETER_BOOL_ARRAY, + ParameterValue::IntegerArray(_) => ParameterType::PARAMETER_INTEGER_ARRAY, + ParameterValue::DoubleArray(_) => ParameterType::PARAMETER_DOUBLE_ARRAY, + ParameterValue::StringArray(_) => ParameterType::PARAMETER_STRING_ARRAY, + } + } } #[cfg(test)] diff --git a/rosidl_runtime_rs/src/string.rs b/rosidl_runtime_rs/src/string.rs index 44043a60..22adad7c 100644 --- a/rosidl_runtime_rs/src/string.rs +++ b/rosidl_runtime_rs/src/string.rs @@ -3,6 +3,7 @@ use std::ffi::CStr; use std::fmt::{self, Debug, Display}; use std::hash::{Hash, Hasher}; use std::ops::{Deref, DerefMut}; +use std::sync::Arc; #[cfg(feature = "serde")] mod serde; @@ -308,7 +309,25 @@ impl From<&str> for String { size: 0, capacity: 0, }; - // SAFETY: It's okay to pass a non-zero-terminated string here since assignn uses the + // SAFETY: It's okay to pass a non-zero-terminated string here since assign uses the + // specified length and will append the 0 byte to the dest string itself. + if !unsafe { + rosidl_runtime_c__String__assignn(&mut msg as *mut _, s.as_ptr() as *const _, s.len()) + } { + panic!("rosidl_runtime_c__String__assignn failed"); + } + msg + } +} + +impl From> for String { + fn from(s: Arc) -> Self { + let mut msg = Self { + data: std::ptr::null_mut(), + size: 0, + capacity: 0, + }; + // SAFETY: It's okay to pass a non-zero-terminated string here since assign uses the // specified length and will append the 0 byte to the dest string itself. if !unsafe { rosidl_runtime_c__String__assignn(&mut msg as *mut _, s.as_ptr() as *const _, s.len()) From 2ff76384ee4dddee5b7b058a84a7b6e875799f9b Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 6 Nov 2023 15:19:39 +0800 Subject: [PATCH 02/24] Implement parameter setting services Signed-off-by: Luca Della Vedova --- rclrs/src/parameter.rs | 19 +- rclrs/src/parameter/service.rs | 382 +++++++++++++++++++++++--------- rclrs/src/parameter/value.rs | 117 +++++++++- rosidl_runtime_rs/src/string.rs | 6 + 4 files changed, 403 insertions(+), 121 deletions(-) diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index e70a308e..2d3579b0 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -3,18 +3,15 @@ mod service; mod value; pub(crate) use override_map::*; -pub use value::*; use service::*; +pub use value::*; use crate::rcl_bindings::*; use crate::{call_string_getter_with_handle, Node, RclrsError}; use std::collections::{btree_map::Entry, BTreeMap}; use std::fmt::Debug; use std::marker::PhantomData; -use std::sync::{ - atomic::{AtomicBool, Ordering}, - Arc, Mutex, RwLock, Weak, -}; +use std::sync::{Arc, Mutex, RwLock, Weak}; // This module implements the core logic of parameters in rclrs. // The implementation is fairly different from the existing ROS 2 client libraries. A detailed @@ -610,6 +607,7 @@ enum ParameterStorage { #[derive(Debug, Default)] pub(crate) struct ParameterMap { storage: BTreeMap, ParameterStorage>, + allow_undeclared: bool, } impl MandatoryParameter { @@ -725,6 +723,9 @@ impl<'a> Parameters<'a> { /// * `Ok(())` if setting was successful. /// * [`Err(DeclarationError::TypeMismatch)`] if the type of the requested value is different /// from the parameter's type. + /// * [`Err(DeclarationError::OutOfRange)`] if the requested value is out of the parameter's + /// range. + /// * [`Err(DeclarationError::ReadOnly)`] if the parameter is read only. pub fn set( &self, name: impl Into>, @@ -763,7 +764,6 @@ impl<'a> Parameters<'a> { entry.insert(ParameterStorage::Undeclared(value.into())); } } - Ok(()) } } @@ -771,7 +771,6 @@ impl<'a> Parameters<'a> { pub(crate) struct ParameterInterface { _parameter_map: Arc>, _override_map: ParameterOverrideMap, - allow_undeclared: AtomicBool, services: Mutex>, } @@ -790,7 +789,6 @@ impl ParameterInterface { Ok(ParameterInterface { _parameter_map: Default::default(), _override_map, - allow_undeclared: Default::default(), services: Mutex::new(None), //services: ParameterService::new(rcl_node_mtx, parameter_map)?, }) @@ -812,7 +810,8 @@ impl ParameterInterface { } pub(crate) fn create_services(&self, node: &Node) -> Result<(), RclrsError> { - *self.services.lock().unwrap() = Some(ParameterService::new(node, self._parameter_map.clone())?); + *self.services.lock().unwrap() = + Some(ParameterService::new(node, self._parameter_map.clone())?); Ok(()) } @@ -888,7 +887,7 @@ impl ParameterInterface { } pub(crate) fn allow_undeclared(&self) { - self.allow_undeclared.store(true, Ordering::Relaxed); + self._parameter_map.lock().unwrap().allow_undeclared = true; } } diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 0231b0c0..2396f0ec 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -1,12 +1,13 @@ +use std::collections::btree_map::Entry; use std::sync::{Arc, Mutex}; -use crate::vendor::rcl_interfaces::srv::rmw::*; use crate::vendor::rcl_interfaces::msg::rmw::*; -use rosidl_runtime_rs::{BoundedSequence, Sequence, seq}; +use crate::vendor::rcl_interfaces::srv::rmw::*; +use rosidl_runtime_rs::{seq, BoundedSequence, Sequence}; -use crate::{rmw_request_id_t, Node, RclrsError, Service}; -use crate::parameter::{DeclaredValue, ParameterRanges, ParameterKind, ParameterStorage}; use super::ParameterMap; +use crate::parameter::{DeclaredValue, ParameterKind, ParameterRanges, ParameterStorage}; +use crate::{rmw_request_id_t, Node, RclrsError, Service}; pub struct ParameterService { describe_parameters_service: Arc>, @@ -20,163 +21,324 @@ pub struct ParameterService { // TODO(luca) should this be a method in the DeclaredStorage impl? fn storage_to_parameter_type(storage: &ParameterStorage) -> u8 { match storage { - ParameterStorage::Declared(s) => { - match s.kind { - ParameterKind::Bool => ParameterType::PARAMETER_BOOL, - ParameterKind::Integer => ParameterType::PARAMETER_INTEGER, - ParameterKind::Double => ParameterType::PARAMETER_DOUBLE, - ParameterKind::String => ParameterType::PARAMETER_STRING, - ParameterKind::ByteArray => ParameterType::PARAMETER_BYTE_ARRAY, - ParameterKind::BoolArray => ParameterType::PARAMETER_BOOL_ARRAY, - ParameterKind::IntegerArray => ParameterType::PARAMETER_INTEGER_ARRAY, - ParameterKind::DoubleArray => ParameterType::PARAMETER_DOUBLE_ARRAY, - ParameterKind::StringArray => ParameterType::PARAMETER_STRING_ARRAY, - ParameterKind::Dynamic => { - match &s.value { - DeclaredValue::Mandatory(v) => v.read().unwrap().rcl_parameter_type(), - DeclaredValue::Optional(v) => v.read().unwrap().as_ref().map(|v| v.rcl_parameter_type()).unwrap_or(ParameterType::PARAMETER_NOT_SET), - DeclaredValue::ReadOnly(v) => v.rcl_parameter_type(), + ParameterStorage::Declared(s) => match s.kind { + ParameterKind::Bool => ParameterType::PARAMETER_BOOL, + ParameterKind::Integer => ParameterType::PARAMETER_INTEGER, + ParameterKind::Double => ParameterType::PARAMETER_DOUBLE, + ParameterKind::String => ParameterType::PARAMETER_STRING, + ParameterKind::ByteArray => ParameterType::PARAMETER_BYTE_ARRAY, + ParameterKind::BoolArray => ParameterType::PARAMETER_BOOL_ARRAY, + ParameterKind::IntegerArray => ParameterType::PARAMETER_INTEGER_ARRAY, + ParameterKind::DoubleArray => ParameterType::PARAMETER_DOUBLE_ARRAY, + ParameterKind::StringArray => ParameterType::PARAMETER_STRING_ARRAY, + ParameterKind::Dynamic => match &s.value { + DeclaredValue::Mandatory(v) => v.read().unwrap().rcl_parameter_type(), + DeclaredValue::Optional(v) => v + .read() + .unwrap() + .as_ref() + .map(|v| v.rcl_parameter_type()) + .unwrap_or(ParameterType::PARAMETER_NOT_SET), + DeclaredValue::ReadOnly(v) => v.rcl_parameter_type(), + }, + }, + ParameterStorage::Undeclared(value) => value.rcl_parameter_type(), + } +} + +// TODO(luca) should this be a methond in the ParameterRanges impl? +fn parameter_ranges_to_descriptor_ranges( + ranges: &ParameterRanges, +) -> ( + BoundedSequence, + BoundedSequence, +) { + let int_range = ranges + .integer + .as_ref() + .map(|range| { + // Converting step to a positive value is safe because declaring a parameter with a + // negative step is not allowed. + // TODO(luca) explore changing step into a positive value in the generic definition to + // make negative steps a compile error. + if range.lower.is_none() && range.upper.is_none() && range.step.is_none() { + // It's a default range, just return a default + Default::default() + } else { + seq![1 # IntegerRange { + from_value: range.lower.unwrap_or(i64::MIN), + to_value: range.upper.unwrap_or(i64::MAX), + step: range.step.unwrap_or(0).try_into().unwrap(), + }] + } + }) + .unwrap_or_default(); + let float_range = ranges + .float + .as_ref() + .map(|range| { + // TODO(luca) Double check whether we should use MIN/MAX or INFINITY/NEG_INFINITY + if range.lower.is_none() && range.upper.is_none() && range.step.is_none() { + Default::default() + } else { + seq![1 # FloatingPointRange { + from_value: range.lower.unwrap_or(f64::MIN), + to_value: range.upper.unwrap_or(f64::MAX), + step: range.step.unwrap_or(0.0), + }] + } + }) + .unwrap_or_default(); + (int_range, float_range) +} + +fn validate_parameter_setting( + map: &ParameterMap, + name: &str, + value: ParameterValue, +) -> Result { + let name = name.into(); + let Ok(value): Result = value.try_into() else { + return Err("Invalid parameter type".into()); + }; + match map.storage.get(name) { + Some(entry) => { + if let ParameterStorage::Declared(storage) = entry { + if std::mem::discriminant(&storage.kind) + == std::mem::discriminant(&value.static_kind()) + || matches!(storage.kind, ParameterKind::Dynamic) + { + if !storage.options.ranges.in_range(&value) { + return Err("Parameter value is out of range".into()); } + if matches!(&storage.value, DeclaredValue::ReadOnly(_)) { + return Err("Parameter is read only".into()); + } + } else { + return Err( + "Parameter set to different type and dynamic typing is disabled".into(), + ); } - } } - ParameterStorage::Undeclared(value) => { - value.rcl_parameter_type() + None => { + if !map.allow_undeclared { + return Err( + "Parameter was not declared and undeclared parameters are not allowed".into(), + ); + } } } + Ok(value) } -// TODO(luca) should this be a methond in the ParameterRanges impl? -fn parameter_ranges_to_descriptor_ranges(ranges: &ParameterRanges) -> (BoundedSequence, BoundedSequence) { - let int_range = ranges.integer.as_ref().map(|range| { - // Converting step to a positive value is safe because declaring a parameter with a - // negative step is not allowed. - // TODO(luca) explore changing step into a positive value in the generic definition to - // make negative steps a compile error. - if range.lower.is_none() && range.upper.is_none() && range.step.is_none() { - // It's a default range, just return a default - Default::default() - } else { - seq![1 # IntegerRange { - from_value: range.lower.unwrap_or(i64::MIN), - to_value: range.upper.unwrap_or(i64::MAX), - step: range.step.unwrap_or(0).try_into().unwrap(), - }] - } - }) - .unwrap_or_default(); - let float_range = ranges.float.as_ref().map(|range| { - // TODO(luca) Double check whether we should use MIN/MAX or INFINITY/NEG_INFINITY - if range.lower.is_none() && range.upper.is_none() && range.step.is_none() { - Default::default() - } else { - seq![1 # FloatingPointRange { - from_value: range.lower.unwrap_or(f64::MIN), - to_value: range.upper.unwrap_or(f64::MAX), - step: range.step.unwrap_or(0.0), - }] +fn store_parameter( + map: &mut ParameterMap, + name: Arc, + value: crate::parameter::ParameterValue, +) { + match map.storage.entry(name.into()) { + Entry::Occupied(mut entry) => match entry.get_mut() { + ParameterStorage::Declared(storage) => match &storage.value { + DeclaredValue::Mandatory(p) => *p.write().unwrap() = value, + DeclaredValue::Optional(p) => *p.write().unwrap() = Some(value), + DeclaredValue::ReadOnly(_) => {} + }, + ParameterStorage::Undeclared(param) => { + *param = value; + } + }, + Entry::Vacant(entry) => { + entry.insert(ParameterStorage::Undeclared(value)); } - }) - .unwrap_or_default(); - (int_range, float_range) + } } impl ParameterService { - pub(crate) fn new(node: &Node, parameter_map: Arc>) -> Result { - // TODO(luca) node fully qualified name for the service topic prefix + pub(crate) fn new( + node: &Node, + parameter_map: Arc>, + ) -> Result { let fqn = node.fully_qualified_name(); // TODO(luca) make sure it is OK to have an Arc instead of a Weak here and cleanup on // destruction is made for the parameter map. let map = parameter_map.clone(); let describe_parameters_service = node.create_service( - "describe_parameters", + &(fqn.clone() + "/describe_parameters"), move |req_id: &rmw_request_id_t, req: DescribeParameters_Request| { // TODO(luca) look at the request and filter names let map = map.lock().unwrap(); - let descriptors = req.names.iter().map(|name| { - // TODO(luca) Remove conversion to string and implement Borrow - let storage = map.storage.get(name.to_cstr().to_str().unwrap())?; - let mut descriptor = match storage { - ParameterStorage::Declared(storage) => { - let (integer_range, floating_point_range) = parameter_ranges_to_descriptor_ranges(&storage.options.ranges); - ParameterDescriptor { - name: name.clone().into(), - type_: Default::default(), - description: storage.options._description.clone().into(), - additional_constraints: storage.options._constraints.clone().into(), - dynamic_typing: matches!(storage.kind, ParameterKind::Dynamic), - read_only: matches!(storage.value, DeclaredValue::ReadOnly(_)), - floating_point_range, - integer_range, + let descriptors = req + .names + .into_iter() + .map(|name| { + // TODO(luca) Remove conversion to string and implement Borrow + let storage = map.storage.get(name.to_cstr().to_str().unwrap())?; + let mut descriptor = match storage { + ParameterStorage::Declared(storage) => { + let (integer_range, floating_point_range) = + parameter_ranges_to_descriptor_ranges(&storage.options.ranges); + ParameterDescriptor { + name: name.clone().into(), + type_: Default::default(), + description: storage.options._description.clone().into(), + additional_constraints: storage + .options + ._constraints + .clone() + .into(), + dynamic_typing: matches!(storage.kind, ParameterKind::Dynamic), + read_only: matches!(storage.value, DeclaredValue::ReadOnly(_)), + floating_point_range, + integer_range, + } } - } - ParameterStorage::Undeclared(value) => { - ParameterDescriptor { + ParameterStorage::Undeclared(value) => ParameterDescriptor { name: name.clone().into(), dynamic_typing: true, ..Default::default() - } - } - }; - descriptor.type_ = storage_to_parameter_type(storage); - Some(descriptor) - }) - .collect::>() - .unwrap_or_default(); + }, + }; + descriptor.type_ = storage_to_parameter_type(storage); + Some(descriptor) + }) + .collect::>() + .unwrap_or_default(); // TODO(luca) error logging if the service call failed - DescribeParameters_Response { - descriptors, - } + DescribeParameters_Response { descriptors } }, )?; let map = parameter_map.clone(); let get_parameter_types_service = node.create_service( - "get_parameter_types", + &(fqn.clone() + "/get_parameter_types"), move |req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { - // TODO(luca) look at the request and filter names - let types = map.lock().unwrap().storage.iter().map(|(name, storage)| { - storage_to_parameter_type(storage) - }) - .collect(); - GetParameterTypes_Response { - types, - } + let map = map.lock().unwrap(); + let types = req + .names + .into_iter() + .map(|name| { + // TODO(luca) Remove conversion to string and implement Borrow + let storage = map.storage.get(name.to_cstr().to_str().unwrap())?; + Some(storage_to_parameter_type(storage)) + }) + .collect::>() + .unwrap_or_default(); + GetParameterTypes_Response { types } }, )?; let map = parameter_map.clone(); let get_parameters_service = node.create_service( - "get_parameters", + &(fqn.clone() + "/get_parameters"), move |req_id: &rmw_request_id_t, req: GetParameters_Request| { - GetParameters_Response { - values: seq![] - } + let map = map.lock().unwrap(); + let values = req + .names + .into_iter() + .map(|name| { + // TODO(luca) Remove conversion to string and implement Borrow + let storage = map.storage.get(name.to_cstr().to_str().unwrap())?; + match storage { + ParameterStorage::Declared(storage) => match &storage.value { + DeclaredValue::Mandatory(v) => { + Some(v.read().unwrap().clone().into()) + } + DeclaredValue::Optional(v) => Some( + v.read() + .unwrap() + .clone() + .map(|v| v.into()) + .unwrap_or_default(), + ), + DeclaredValue::ReadOnly(v) => Some(v.clone().into()), + }, + ParameterStorage::Undeclared(value) => Some(value.clone().into()), + } + }) + .collect::>() + .unwrap_or_default(); + GetParameters_Response { values } }, )?; let map = parameter_map.clone(); let list_parameters_service = node.create_service( - "list_parameters", + &(fqn.clone() + "/list_parameters"), move |req_id: &rmw_request_id_t, req: ListParameters_Request| { + let map = map.lock().unwrap(); + let names = map + .storage + .keys() + .filter_map(|name| { + // TODO(luca) filter by prefix + Some(name.clone().into()) + }) + .collect(); + // TODO(luca) populate prefixes in result ListParameters_Response { - result: ListParametersResult::default() + result: ListParametersResult { + names, + prefixes: seq![], + }, } }, )?; let map = parameter_map.clone(); let set_parameters_service = node.create_service( - "set_parameters", + &(fqn.clone() + "/set_parameters"), move |req_id: &rmw_request_id_t, req: SetParameters_Request| { - SetParameters_Response { - results: seq![] - } + let mut map = map.lock().unwrap(); + let results = req + .parameters + .into_iter() + .map(|param| { + let name = param.name.to_cstr().to_str().unwrap(); + let value = validate_parameter_setting(&map, &name, param.value)?; + store_parameter(&mut map, name.into(), value); + Ok(()) + }) + .map(|res| match res { + Ok(()) => SetParametersResult { + successful: true, + reason: Default::default(), + }, + Err(e) => SetParametersResult { + successful: false, + reason: e, + }, + }) + .collect(); + SetParameters_Response { results } }, )?; let set_parameters_atomically_service = node.create_service( - "set_parameters_atomically", + &(fqn.clone() + "/set_parameters_atomically"), move |req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { - SetParametersAtomically_Response { - result: SetParametersResult::default() - } + let mut map = parameter_map.lock().unwrap(); + let results = req + .parameters + .into_iter() + .map(|param| { + let name = param.name.to_cstr().to_str().unwrap(); + let value = validate_parameter_setting(&map, &name, param.value)?; + Ok((name.into(), value)) + }) + .collect::, _>>(); + // Check if there was any error and update parameters accordingly + let result = match results { + Ok(results) => { + for (name, value) in results.into_iter() { + store_parameter(&mut map, name, value); + } + SetParametersResult { + successful: true, + reason: Default::default(), + } + } + Err(reason) => SetParametersResult { + successful: false, + reason, + }, + }; + SetParametersAtomically_Response { result } }, )?; Ok(Self { diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index f65e345c..21a42f27 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -2,8 +2,9 @@ use std::ffi::CStr; use std::sync::Arc; use crate::rcl_bindings::*; -use crate::{ParameterRange, ParameterRanges, ParameterValueError}; use crate::vendor::rcl_interfaces::msg::rmw::ParameterType; +use crate::vendor::rcl_interfaces::msg::rmw::ParameterValue as RmwParameterValue; +use crate::{ParameterRange, ParameterRanges, ParameterValueError}; /// A parameter value. /// @@ -320,6 +321,105 @@ impl ParameterVariant for ParameterValue { } } +impl From for RmwParameterValue { + fn from(value: ParameterValue) -> Self { + match value { + ParameterValue::Bool(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_BOOL, + bool_value: v, + ..Default::default() + }, + ParameterValue::Integer(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_INTEGER, + integer_value: v, + ..Default::default() + }, + ParameterValue::Double(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_DOUBLE, + double_value: v, + ..Default::default() + }, + ParameterValue::String(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_STRING, + string_value: v.into(), + ..Default::default() + }, + ParameterValue::ByteArray(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_BYTE_ARRAY, + byte_array_value: (*v).into(), + ..Default::default() + }, + ParameterValue::BoolArray(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_BOOL_ARRAY, + bool_array_value: (*v).into(), + ..Default::default() + }, + ParameterValue::IntegerArray(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_INTEGER_ARRAY, + integer_array_value: (*v).into(), + ..Default::default() + }, + ParameterValue::DoubleArray(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_DOUBLE_ARRAY, + double_array_value: (*v).into(), + ..Default::default() + }, + ParameterValue::StringArray(v) => RmwParameterValue { + type_: ParameterType::PARAMETER_STRING_ARRAY, + // TODO(luca) check if there is a more efficient way to perform this conversion + string_array_value: v + .iter() + .map(|v| v.clone().into()) + .collect::>() + .into(), + ..Default::default() + }, + } + } +} + +pub enum RmwParameterConversionError { + InvalidParameterType, +} + +impl TryFrom for ParameterValue { + type Error = RmwParameterConversionError; + + fn try_from(param: RmwParameterValue) -> Result { + // TODO(luca) how to deal with PARAMETER_NOT_SET? Should we allow service calls to unset + // parameters? + match param.type_ { + ParameterType::PARAMETER_BOOL => Ok(ParameterValue::Bool(param.bool_value)), + ParameterType::PARAMETER_INTEGER => Ok(ParameterValue::Integer(param.integer_value)), + ParameterType::PARAMETER_DOUBLE => Ok(ParameterValue::Double(param.double_value)), + ParameterType::PARAMETER_STRING => { + Ok(ParameterValue::String(param.string_value.into())) + } + ParameterType::PARAMETER_BYTE_ARRAY => { + Ok(ParameterValue::ByteArray((*param.byte_array_value).into())) + } + ParameterType::PARAMETER_BOOL_ARRAY => { + Ok(ParameterValue::BoolArray((*param.bool_array_value).into())) + } + ParameterType::PARAMETER_INTEGER_ARRAY => Ok(ParameterValue::IntegerArray( + (*param.integer_array_value).into(), + )), + ParameterType::PARAMETER_DOUBLE_ARRAY => Ok(ParameterValue::DoubleArray( + (*param.double_array_value).into(), + )), + ParameterType::PARAMETER_STRING_ARRAY => Ok(ParameterValue::StringArray( + param + .string_array_value + .iter() + .map(|s| s.clone().into()) + .collect::>() + .into(), + )), + _ => Err(RmwParameterConversionError::InvalidParameterType), + } + } +} + impl ParameterValue { // Panics if the rcl_variant_t does not have exactly one field set. // @@ -407,6 +507,21 @@ impl ParameterValue { ParameterValue::StringArray(_) => ParameterType::PARAMETER_STRING_ARRAY, } } + + /// Returns the `ParameterKind` for the parameter as if it was a static parameter. + pub(crate) fn static_kind(&self) -> ParameterKind { + match self { + ParameterValue::Bool(_) => ParameterKind::Bool, + ParameterValue::Integer(_) => ParameterKind::Integer, + ParameterValue::Double(_) => ParameterKind::Double, + ParameterValue::String(_) => ParameterKind::String, + ParameterValue::ByteArray(_) => ParameterKind::ByteArray, + ParameterValue::BoolArray(_) => ParameterKind::BoolArray, + ParameterValue::IntegerArray(_) => ParameterKind::IntegerArray, + ParameterValue::DoubleArray(_) => ParameterKind::DoubleArray, + ParameterValue::StringArray(_) => ParameterKind::StringArray, + } + } } #[cfg(test)] diff --git a/rosidl_runtime_rs/src/string.rs b/rosidl_runtime_rs/src/string.rs index 22adad7c..53294ef6 100644 --- a/rosidl_runtime_rs/src/string.rs +++ b/rosidl_runtime_rs/src/string.rs @@ -338,6 +338,12 @@ impl From> for String { } } +impl From for Arc { + fn from(s: String) -> Self { + s.to_cstr().to_str().unwrap().into() + } +} + impl String { /// Creates a CStr from this String. /// From 3b6e6e3abd93e7c49911e9688a5c3351ec1608e5 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 6 Nov 2023 16:00:31 +0800 Subject: [PATCH 03/24] Restructure and cleanup Signed-off-by: Luca Della Vedova --- rclrs/src/parameter.rs | 247 +++++++++++++-------------------- rclrs/src/parameter/range.rs | 201 +++++++++++++++++++++++++++ rclrs/src/parameter/service.rs | 161 ++------------------- rclrs/src/parameter/value.rs | 8 +- 4 files changed, 314 insertions(+), 303 deletions(-) create mode 100644 rclrs/src/parameter/range.rs diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index 2d3579b0..5276648e 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -1,12 +1,15 @@ mod override_map; +mod range; mod service; mod value; pub(crate) use override_map::*; +use range::*; use service::*; pub use value::*; use crate::rcl_bindings::*; +use crate::vendor::rcl_interfaces::msg::rmw::{ParameterType, ParameterValue as RmwParameterValue}; use crate::{call_string_getter_with_handle, Node, RclrsError}; use std::collections::{btree_map::Entry, BTreeMap}; use std::fmt::Debug; @@ -69,159 +72,6 @@ impl Default for ParameterOptions { } } -impl From> for ParameterRanges { - fn from(params: ParameterRange) -> Self { - Self { - float: Some(params), - ..Default::default() - } - } -} - -impl From> for ParameterRanges { - fn from(params: ParameterRange) -> Self { - Self { - integer: Some(params), - ..Default::default() - } - } -} - -impl From<()> for ParameterRanges { - fn from(_empty: ()) -> Self { - Self::default() - } -} - -/// Contains all the possible type of ranges that can be applied to a value. -/// Usually only one of these ranges will be applied, but all have to be stored since: -/// -/// * A dynamic parameter can change its type at runtime, in which case a different range could be -/// applied. -/// * Introspection through service calls requires all the ranges to be reported to the user. -#[derive(Clone, Debug, Default)] -pub struct ParameterRanges { - float: Option>, - integer: Option>, -} - -impl ParameterRanges { - fn validate(&self) -> Result<(), DeclarationError> { - if let Some(integer) = &self.integer { - integer.validate()?; - } - if let Some(float) = &self.float { - float.validate()?; - } - Ok(()) - } - - fn in_range(&self, value: &ParameterValue) -> bool { - match value { - ParameterValue::Integer(v) => { - if let Some(range) = &self.integer { - if !range.in_range(*v) { - return false; - } - } - } - ParameterValue::Double(v) => { - if let Some(range) = &self.float { - if !range.in_range(*v) { - return false; - } - } - } - _ => {} - } - true - } -} - -/// Describes the range for paramter type T. -#[derive(Clone, Debug, Default)] -pub struct ParameterRange { - /// Lower limit, if set the parameter must be >= l. - pub lower: Option, - /// Upper limit, if set the parameter must be <= u. - pub upper: Option, - /// Step size, if set and `lower` is set the parameter must be within an integer number of - /// steps of size `step` from `lower`, or equal to the upper limit if set. - /// Example: - /// If lower is `Some(0)`, upper is `Some(10)` and step is `Some(3)`, acceptable values are: - /// `[0, 3, 6, 9, 10]`. - pub step: Option, -} - -impl ParameterRange { - fn inside_boundary(&self, value: &T) -> bool { - if self.lower.as_ref().is_some_and(|l| value < l) { - return false; - } - if self.upper.as_ref().is_some_and(|u| value > u) { - return false; - } - true - } - - fn validate(&self) -> Result<(), DeclarationError> { - if self - .lower - .as_ref() - .zip(self.upper.as_ref()) - .is_some_and(|(l, u)| l > u) - { - return Err(DeclarationError::InvalidRange); - } - if self.step.as_ref().is_some_and(|s| s <= &T::default()) { - return Err(DeclarationError::InvalidRange); - } - Ok(()) - } -} - -impl ParameterRange { - fn in_range(&self, value: i64) -> bool { - if !self.inside_boundary(&value) { - return false; - } - if self.upper.is_some_and(|u| u == value) { - return true; - } - if let (Some(l), Some(s)) = (self.lower, self.step) { - if (value - l) % s != 0 { - return false; - } - } - true - } -} - -impl ParameterRange { - // Same comparison function as rclcpp. - fn are_close(v1: f64, v2: f64) -> bool { - const ULP_TOL: f64 = 100.0; - (v1 - v2).abs() <= (f64::EPSILON * (v1 + v2).abs() * ULP_TOL) - } - - fn in_range(&self, value: f64) -> bool { - if self.upper.is_some_and(|u| Self::are_close(u, value)) - || self.lower.is_some_and(|l| Self::are_close(l, value)) - { - return true; - } - if !self.inside_boundary(&value) { - return false; - } - if let (Some(l), Some(s)) = (self.lower, self.step) { - if !Self::are_close(((value - l) / s).round() * s + l, value) { - return false; - } - } - true - } -} - #[derive(Clone, Debug)] enum DeclaredValue { Mandatory(Arc>), @@ -604,12 +454,103 @@ enum ParameterStorage { Undeclared(ParameterValue), } +impl ParameterStorage { + pub(crate) fn to_parameter_type(&self) -> u8 { + match self { + ParameterStorage::Declared(s) => match s.kind { + ParameterKind::Bool => ParameterType::PARAMETER_BOOL, + ParameterKind::Integer => ParameterType::PARAMETER_INTEGER, + ParameterKind::Double => ParameterType::PARAMETER_DOUBLE, + ParameterKind::String => ParameterType::PARAMETER_STRING, + ParameterKind::ByteArray => ParameterType::PARAMETER_BYTE_ARRAY, + ParameterKind::BoolArray => ParameterType::PARAMETER_BOOL_ARRAY, + ParameterKind::IntegerArray => ParameterType::PARAMETER_INTEGER_ARRAY, + ParameterKind::DoubleArray => ParameterType::PARAMETER_DOUBLE_ARRAY, + ParameterKind::StringArray => ParameterType::PARAMETER_STRING_ARRAY, + ParameterKind::Dynamic => match &s.value { + DeclaredValue::Mandatory(v) => v.read().unwrap().rcl_parameter_type(), + DeclaredValue::Optional(v) => v + .read() + .unwrap() + .as_ref() + .map(|v| v.rcl_parameter_type()) + .unwrap_or(ParameterType::PARAMETER_NOT_SET), + DeclaredValue::ReadOnly(v) => v.rcl_parameter_type(), + }, + }, + ParameterStorage::Undeclared(value) => value.rcl_parameter_type(), + } + } +} + #[derive(Debug, Default)] pub(crate) struct ParameterMap { storage: BTreeMap, ParameterStorage>, allow_undeclared: bool, } +impl ParameterMap { + /// Validates the requested parameter setting and returns an error if the requested value is + /// not valid. + pub(crate) fn validate_parameter_setting( + &self, + name: &str, + value: RmwParameterValue, + ) -> Result { + let Ok(value): Result = value.try_into() else { + return Err("Invalid parameter type"); + }; + match self.storage.get(name) { + Some(entry) => { + if let ParameterStorage::Declared(storage) = entry { + if std::mem::discriminant(&storage.kind) + == std::mem::discriminant(&value.static_kind()) + || matches!(storage.kind, ParameterKind::Dynamic) + { + if !storage.options.ranges.in_range(&value) { + return Err("Parameter value is out of range"); + } + if matches!(&storage.value, DeclaredValue::ReadOnly(_)) { + return Err("Parameter is read only"); + } + } else { + return Err( + "Parameter set to different type and dynamic typing is disabled", + ); + } + } + } + None => { + if !self.allow_undeclared { + return Err( + "Parameter was not declared and undeclared parameters are not allowed", + ); + } + } + } + Ok(value) + } + + /// Stores the requested parameter in the map. + fn store_parameter(&mut self, name: Arc, value: ParameterValue) { + match self.storage.entry(name) { + Entry::Occupied(mut entry) => match entry.get_mut() { + ParameterStorage::Declared(storage) => match &storage.value { + DeclaredValue::Mandatory(p) => *p.write().unwrap() = value, + DeclaredValue::Optional(p) => *p.write().unwrap() = Some(value), + DeclaredValue::ReadOnly(_) => {} + }, + ParameterStorage::Undeclared(param) => { + *param = value; + } + }, + Entry::Vacant(entry) => { + entry.insert(ParameterStorage::Undeclared(value)); + } + } + } +} + impl MandatoryParameter { /// Returns a clone of the most recent value of the parameter. pub fn get(&self) -> T { diff --git a/rclrs/src/parameter/range.rs b/rclrs/src/parameter/range.rs new file mode 100644 index 00000000..b1ed0187 --- /dev/null +++ b/rclrs/src/parameter/range.rs @@ -0,0 +1,201 @@ +use crate::vendor::rcl_interfaces::msg::rmw::{FloatingPointRange, IntegerRange}; +use crate::{DeclarationError, ParameterValue, ParameterVariant}; +use rosidl_runtime_rs::{seq, BoundedSequence}; + +impl From> for ParameterRanges { + fn from(params: ParameterRange) -> Self { + Self { + float: Some(params), + ..Default::default() + } + } +} + +impl From> for ParameterRanges { + fn from(params: ParameterRange) -> Self { + Self { + integer: Some(params), + ..Default::default() + } + } +} + +impl From<()> for ParameterRanges { + fn from(_empty: ()) -> Self { + Self::default() + } +} + +/// Contains all the possible type of ranges that can be applied to a value. +/// Usually only one of these ranges will be applied, but all have to be stored since: +/// +/// * A dynamic parameter can change its type at runtime, in which case a different range could be +/// applied. +/// * Introspection through service calls requires all the ranges to be reported to the user. +#[derive(Clone, Debug, Default)] +pub struct ParameterRanges { + float: Option>, + integer: Option>, +} + +impl ParameterRanges { + pub(crate) fn to_descriptor_ranges( + &self, + ) -> ( + BoundedSequence, + BoundedSequence, + ) { + let int_range = self + .integer + .as_ref() + .map(|range| { + // Converting step to a positive value is safe because declaring a parameter with a + // negative step is not allowed. + // TODO(luca) explore changing step into a positive value in the generic definition to + // make negative steps a compile error. + if range.lower.is_none() && range.upper.is_none() && range.step.is_none() { + // It's a default range, just return a default + Default::default() + } else { + seq![1 # IntegerRange { + from_value: range.lower.unwrap_or(i64::MIN), + to_value: range.upper.unwrap_or(i64::MAX), + step: range.step.unwrap_or(0).try_into().unwrap(), + }] + } + }) + .unwrap_or_default(); + let float_range = self + .float + .as_ref() + .map(|range| { + // TODO(luca) Double check whether we should use MIN/MAX or INFINITY/NEG_INFINITY + if range.lower.is_none() && range.upper.is_none() && range.step.is_none() { + Default::default() + } else { + seq![1 # FloatingPointRange { + from_value: range.lower.unwrap_or(f64::MIN), + to_value: range.upper.unwrap_or(f64::MAX), + step: range.step.unwrap_or(0.0), + }] + } + }) + .unwrap_or_default(); + (int_range, float_range) + } + + pub(crate) fn validate(&self) -> Result<(), DeclarationError> { + if let Some(integer) = &self.integer { + integer.validate()?; + } + if let Some(float) = &self.float { + float.validate()?; + } + Ok(()) + } + + pub(crate) fn in_range(&self, value: &ParameterValue) -> bool { + match value { + ParameterValue::Integer(v) => { + if let Some(range) = &self.integer { + if !range.in_range(*v) { + return false; + } + } + } + ParameterValue::Double(v) => { + if let Some(range) = &self.float { + if !range.in_range(*v) { + return false; + } + } + } + _ => {} + } + true + } +} + +/// Describes the range for paramter type T. +#[derive(Clone, Debug, Default)] +pub struct ParameterRange { + /// Lower limit, if set the parameter must be >= l. + pub lower: Option, + /// Upper limit, if set the parameter must be <= u. + pub upper: Option, + /// Step size, if set and `lower` is set the parameter must be within an integer number of + /// steps of size `step` from `lower`, or equal to the upper limit if set. + /// Example: + /// If lower is `Some(0)`, upper is `Some(10)` and step is `Some(3)`, acceptable values are: + /// `[0, 3, 6, 9, 10]`. + pub step: Option, +} + +impl ParameterRange { + fn inside_boundary(&self, value: &T) -> bool { + if self.lower.as_ref().is_some_and(|l| value < l) { + return false; + } + if self.upper.as_ref().is_some_and(|u| value > u) { + return false; + } + true + } + + fn validate(&self) -> Result<(), DeclarationError> { + if self + .lower + .as_ref() + .zip(self.upper.as_ref()) + .is_some_and(|(l, u)| l > u) + { + return Err(DeclarationError::InvalidRange); + } + if self.step.as_ref().is_some_and(|s| s <= &T::default()) { + return Err(DeclarationError::InvalidRange); + } + Ok(()) + } +} + +impl ParameterRange { + fn in_range(&self, value: i64) -> bool { + if !self.inside_boundary(&value) { + return false; + } + if self.upper.is_some_and(|u| u == value) { + return true; + } + if let (Some(l), Some(s)) = (self.lower, self.step) { + if (value - l) % s != 0 { + return false; + } + } + true + } +} + +impl ParameterRange { + // Same comparison function as rclcpp. + fn are_close(v1: f64, v2: f64) -> bool { + const ULP_TOL: f64 = 100.0; + (v1 - v2).abs() <= (f64::EPSILON * (v1 + v2).abs() * ULP_TOL) + } + + fn in_range(&self, value: f64) -> bool { + if self.upper.is_some_and(|u| Self::are_close(u, value)) + || self.lower.is_some_and(|l| Self::are_close(l, value)) + { + return true; + } + if !self.inside_boundary(&value) { + return false; + } + if let (Some(l), Some(s)) = (self.lower, self.step) { + if !Self::are_close(((value - l) / s).round() * s + l, value) { + return false; + } + } + true + } +} diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 2396f0ec..a19b4246 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -1,12 +1,11 @@ -use std::collections::btree_map::Entry; use std::sync::{Arc, Mutex}; use crate::vendor::rcl_interfaces::msg::rmw::*; use crate::vendor::rcl_interfaces::srv::rmw::*; -use rosidl_runtime_rs::{seq, BoundedSequence, Sequence}; +use rosidl_runtime_rs::{seq, Sequence}; use super::ParameterMap; -use crate::parameter::{DeclaredValue, ParameterKind, ParameterRanges, ParameterStorage}; +use crate::parameter::{DeclaredValue, ParameterKind, ParameterStorage}; use crate::{rmw_request_id_t, Node, RclrsError, Service}; pub struct ParameterService { @@ -18,142 +17,6 @@ pub struct ParameterService { set_parameters_atomically_service: Arc>, } -// TODO(luca) should this be a method in the DeclaredStorage impl? -fn storage_to_parameter_type(storage: &ParameterStorage) -> u8 { - match storage { - ParameterStorage::Declared(s) => match s.kind { - ParameterKind::Bool => ParameterType::PARAMETER_BOOL, - ParameterKind::Integer => ParameterType::PARAMETER_INTEGER, - ParameterKind::Double => ParameterType::PARAMETER_DOUBLE, - ParameterKind::String => ParameterType::PARAMETER_STRING, - ParameterKind::ByteArray => ParameterType::PARAMETER_BYTE_ARRAY, - ParameterKind::BoolArray => ParameterType::PARAMETER_BOOL_ARRAY, - ParameterKind::IntegerArray => ParameterType::PARAMETER_INTEGER_ARRAY, - ParameterKind::DoubleArray => ParameterType::PARAMETER_DOUBLE_ARRAY, - ParameterKind::StringArray => ParameterType::PARAMETER_STRING_ARRAY, - ParameterKind::Dynamic => match &s.value { - DeclaredValue::Mandatory(v) => v.read().unwrap().rcl_parameter_type(), - DeclaredValue::Optional(v) => v - .read() - .unwrap() - .as_ref() - .map(|v| v.rcl_parameter_type()) - .unwrap_or(ParameterType::PARAMETER_NOT_SET), - DeclaredValue::ReadOnly(v) => v.rcl_parameter_type(), - }, - }, - ParameterStorage::Undeclared(value) => value.rcl_parameter_type(), - } -} - -// TODO(luca) should this be a methond in the ParameterRanges impl? -fn parameter_ranges_to_descriptor_ranges( - ranges: &ParameterRanges, -) -> ( - BoundedSequence, - BoundedSequence, -) { - let int_range = ranges - .integer - .as_ref() - .map(|range| { - // Converting step to a positive value is safe because declaring a parameter with a - // negative step is not allowed. - // TODO(luca) explore changing step into a positive value in the generic definition to - // make negative steps a compile error. - if range.lower.is_none() && range.upper.is_none() && range.step.is_none() { - // It's a default range, just return a default - Default::default() - } else { - seq![1 # IntegerRange { - from_value: range.lower.unwrap_or(i64::MIN), - to_value: range.upper.unwrap_or(i64::MAX), - step: range.step.unwrap_or(0).try_into().unwrap(), - }] - } - }) - .unwrap_or_default(); - let float_range = ranges - .float - .as_ref() - .map(|range| { - // TODO(luca) Double check whether we should use MIN/MAX or INFINITY/NEG_INFINITY - if range.lower.is_none() && range.upper.is_none() && range.step.is_none() { - Default::default() - } else { - seq![1 # FloatingPointRange { - from_value: range.lower.unwrap_or(f64::MIN), - to_value: range.upper.unwrap_or(f64::MAX), - step: range.step.unwrap_or(0.0), - }] - } - }) - .unwrap_or_default(); - (int_range, float_range) -} - -fn validate_parameter_setting( - map: &ParameterMap, - name: &str, - value: ParameterValue, -) -> Result { - let name = name.into(); - let Ok(value): Result = value.try_into() else { - return Err("Invalid parameter type".into()); - }; - match map.storage.get(name) { - Some(entry) => { - if let ParameterStorage::Declared(storage) = entry { - if std::mem::discriminant(&storage.kind) - == std::mem::discriminant(&value.static_kind()) - || matches!(storage.kind, ParameterKind::Dynamic) - { - if !storage.options.ranges.in_range(&value) { - return Err("Parameter value is out of range".into()); - } - if matches!(&storage.value, DeclaredValue::ReadOnly(_)) { - return Err("Parameter is read only".into()); - } - } else { - return Err( - "Parameter set to different type and dynamic typing is disabled".into(), - ); - } - } - } - None => { - if !map.allow_undeclared { - return Err( - "Parameter was not declared and undeclared parameters are not allowed".into(), - ); - } - } - } - Ok(value) -} - -fn store_parameter( - map: &mut ParameterMap, - name: Arc, - value: crate::parameter::ParameterValue, -) { - match map.storage.entry(name.into()) { - Entry::Occupied(mut entry) => match entry.get_mut() { - ParameterStorage::Declared(storage) => match &storage.value { - DeclaredValue::Mandatory(p) => *p.write().unwrap() = value, - DeclaredValue::Optional(p) => *p.write().unwrap() = Some(value), - DeclaredValue::ReadOnly(_) => {} - }, - ParameterStorage::Undeclared(param) => { - *param = value; - } - }, - Entry::Vacant(entry) => { - entry.insert(ParameterStorage::Undeclared(value)); - } - } -} - impl ParameterService { pub(crate) fn new( node: &Node, @@ -177,9 +40,9 @@ impl ParameterService { let mut descriptor = match storage { ParameterStorage::Declared(storage) => { let (integer_range, floating_point_range) = - parameter_ranges_to_descriptor_ranges(&storage.options.ranges); + storage.options.ranges.to_descriptor_ranges(); ParameterDescriptor { - name: name.clone().into(), + name, type_: Default::default(), description: storage.options._description.clone().into(), additional_constraints: storage @@ -193,13 +56,13 @@ impl ParameterService { integer_range, } } - ParameterStorage::Undeclared(value) => ParameterDescriptor { - name: name.clone().into(), + ParameterStorage::Undeclared(_) => ParameterDescriptor { + name, dynamic_typing: true, ..Default::default() }, }; - descriptor.type_ = storage_to_parameter_type(storage); + descriptor.type_ = storage.to_parameter_type(); Some(descriptor) }) .collect::>() @@ -219,7 +82,7 @@ impl ParameterService { .map(|name| { // TODO(luca) Remove conversion to string and implement Borrow let storage = map.storage.get(name.to_cstr().to_str().unwrap())?; - Some(storage_to_parameter_type(storage)) + Some(storage.to_parameter_type()) }) .collect::>() .unwrap_or_default(); @@ -291,8 +154,8 @@ impl ParameterService { .into_iter() .map(|param| { let name = param.name.to_cstr().to_str().unwrap(); - let value = validate_parameter_setting(&map, &name, param.value)?; - store_parameter(&mut map, name.into(), value); + let value = map.validate_parameter_setting(name, param.value)?; + map.store_parameter(name.into(), value); Ok(()) }) .map(|res| match res { @@ -318,7 +181,7 @@ impl ParameterService { .into_iter() .map(|param| { let name = param.name.to_cstr().to_str().unwrap(); - let value = validate_parameter_setting(&map, &name, param.value)?; + let value = map.validate_parameter_setting(name, param.value)?; Ok((name.into(), value)) }) .collect::, _>>(); @@ -326,7 +189,7 @@ impl ParameterService { let result = match results { Ok(results) => { for (name, value) in results.into_iter() { - store_parameter(&mut map, name, value); + map.store_parameter(name, value); } SetParametersResult { successful: true, diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index 21a42f27..be9d5e6f 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -4,7 +4,10 @@ use std::sync::Arc; use crate::rcl_bindings::*; use crate::vendor::rcl_interfaces::msg::rmw::ParameterType; use crate::vendor::rcl_interfaces::msg::rmw::ParameterValue as RmwParameterValue; -use crate::{ParameterRange, ParameterRanges, ParameterValueError}; +use crate::{ + parameter::{ParameterRange, ParameterRanges}, + ParameterValueError, +}; /// A parameter value. /// @@ -378,7 +381,10 @@ impl From for RmwParameterValue { } } +/// An error that occured when trying to convert a parameter from a +/// [crate::vendor::rcl_interfaces::msg::rmw::ParameterValue] pub enum RmwParameterConversionError { + /// The parameter type was not valid. InvalidParameterType, } From c14417b331f89eaa280a606825559fd9171ee484 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 7 Nov 2023 14:12:06 +0800 Subject: [PATCH 04/24] Implement list_parameters with prefixes Signed-off-by: Luca Della Vedova --- rclrs/src/parameter/service.rs | 54 +++++++++++++++++++++++++++++----- 1 file changed, 46 insertions(+), 8 deletions(-) diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index a19b4246..945bd55b 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -1,8 +1,9 @@ +use std::collections::BTreeSet; use std::sync::{Arc, Mutex}; use crate::vendor::rcl_interfaces::msg::rmw::*; use crate::vendor::rcl_interfaces::srv::rmw::*; -use rosidl_runtime_rs::{seq, Sequence}; +use rosidl_runtime_rs::Sequence; use super::ParameterMap; use crate::parameter::{DeclaredValue, ParameterKind, ParameterStorage}; @@ -29,7 +30,6 @@ impl ParameterService { let describe_parameters_service = node.create_service( &(fqn.clone() + "/describe_parameters"), move |req_id: &rmw_request_id_t, req: DescribeParameters_Request| { - // TODO(luca) look at the request and filter names let map = map.lock().unwrap(); let descriptors = req .names @@ -81,8 +81,9 @@ impl ParameterService { .into_iter() .map(|name| { // TODO(luca) Remove conversion to string and implement Borrow - let storage = map.storage.get(name.to_cstr().to_str().unwrap())?; - Some(storage.to_parameter_type()) + map.storage + .get(name.to_cstr().to_str().unwrap()) + .map(|s| s.to_parameter_type()) }) .collect::>() .unwrap_or_default(); @@ -126,20 +127,57 @@ impl ParameterService { let list_parameters_service = node.create_service( &(fqn.clone() + "/list_parameters"), move |req_id: &rmw_request_id_t, req: ListParameters_Request| { + let check_parameter_name_depth = |substring: &[i8]| { + if req.depth == ListParameters_Request::DEPTH_RECURSIVE { + return true; + } + u64::try_from(substring.iter().filter(|c| **c == ('.' as i8)).count()).unwrap() + < req.depth + }; let map = map.lock().unwrap(); - let names = map + let names: Sequence<_> = map .storage .keys() .filter_map(|name| { - // TODO(luca) filter by prefix - Some(name.clone().into()) + let name: rosidl_runtime_rs::String = name.clone().into(); + if req.prefixes.len() == 0 && check_parameter_name_depth(&name[..]) { + return Some(name); + } + req.prefixes + .iter() + .any(|prefix| { + if name == *prefix { + return true; + } + let mut prefix = prefix.clone(); + prefix.extend(".".chars()); + if name.len() > prefix.len() + && name[0..prefix.len()] == prefix[0..] + && check_parameter_name_depth(&name[prefix.len()..]) + { + return true; + } + false + }) + .then_some(name) + }) + .collect(); + let prefixes: BTreeSet = names + .iter() + .filter_map(|name| { + let name = name.to_string(); + if let Some(pos) = name.rfind('.') { + return Some(name[0..pos].into()); + //return Some(name[0..pos].iter().map(|c| *c as char).collect()); + } + None }) .collect(); // TODO(luca) populate prefixes in result ListParameters_Response { result: ListParametersResult { names, - prefixes: seq![], + prefixes: prefixes.into_iter().collect(), }, } }, From f9bc86ddcdbc134ccfd08d026111b2ec48fefae8 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 7 Nov 2023 14:28:31 +0800 Subject: [PATCH 05/24] Minor cleanups Signed-off-by: Luca Della Vedova --- rclrs/src/parameter.rs | 9 ++++----- rclrs/src/parameter/range.rs | 9 ++++++--- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index 5276648e..ac4d1478 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -712,7 +712,7 @@ impl<'a> Parameters<'a> { pub(crate) struct ParameterInterface { _parameter_map: Arc>, _override_map: ParameterOverrideMap, - services: Mutex>, + _services: Mutex>, } impl ParameterInterface { @@ -721,8 +721,8 @@ impl ParameterInterface { node_arguments: &rcl_arguments_t, global_arguments: &rcl_arguments_t, ) -> Result { + let rcl_node = rcl_node_mtx.lock().unwrap(); let _override_map = unsafe { - let rcl_node = rcl_node_mtx.lock().unwrap(); let fqn = call_string_getter_with_handle(&rcl_node, rcl_node_get_fully_qualified_name); resolve_parameter_overrides(&fqn, node_arguments, global_arguments)? }; @@ -730,8 +730,7 @@ impl ParameterInterface { Ok(ParameterInterface { _parameter_map: Default::default(), _override_map, - services: Mutex::new(None), - //services: ParameterService::new(rcl_node_mtx, parameter_map)?, + _services: Mutex::new(None), }) } @@ -751,7 +750,7 @@ impl ParameterInterface { } pub(crate) fn create_services(&self, node: &Node) -> Result<(), RclrsError> { - *self.services.lock().unwrap() = + *self._services.lock().unwrap() = Some(ParameterService::new(node, self._parameter_map.clone())?); Ok(()) } diff --git a/rclrs/src/parameter/range.rs b/rclrs/src/parameter/range.rs index b1ed0187..40c8ebb3 100644 --- a/rclrs/src/parameter/range.rs +++ b/rclrs/src/parameter/range.rs @@ -53,8 +53,7 @@ impl ParameterRanges { // negative step is not allowed. // TODO(luca) explore changing step into a positive value in the generic definition to // make negative steps a compile error. - if range.lower.is_none() && range.upper.is_none() && range.step.is_none() { - // It's a default range, just return a default + if range.is_default() { Default::default() } else { seq![1 # IntegerRange { @@ -70,7 +69,7 @@ impl ParameterRanges { .as_ref() .map(|range| { // TODO(luca) Double check whether we should use MIN/MAX or INFINITY/NEG_INFINITY - if range.lower.is_none() && range.upper.is_none() && range.step.is_none() { + if range.is_default() { Default::default() } else { seq![1 # FloatingPointRange { @@ -132,6 +131,10 @@ pub struct ParameterRange { } impl ParameterRange { + fn is_default(&self) -> bool { + self.lower.is_none() && self.upper.is_none() && self.step.is_none() + } + fn inside_boundary(&self, value: &T) -> bool { if self.lower.as_ref().is_some_and(|l| value < l) { return false; From 0605261c0a64cb22c3ac38269778dcd9eca3d9ae Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 7 Nov 2023 14:42:06 +0800 Subject: [PATCH 06/24] Fix tests, cleanups Signed-off-by: Luca Della Vedova --- examples/minimal_pub_sub/src/minimal_subscriber.rs | 4 ---- rclrs/src/node/graph.rs | 6 +++--- rclrs/src/parameter.rs | 2 +- rclrs/src/parameter/value.rs | 4 ++-- rosidl_runtime_rs/src/string.rs | 10 ++-------- 5 files changed, 8 insertions(+), 18 deletions(-) diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index 39dc46c2..ebc5fc19 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -6,10 +6,6 @@ fn main() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; let node = rclrs::create_node(&context, "minimal_subscriber")?; - let param = node.declare_parameter("test").default(42).mandatory().unwrap(); - let p2 = node.declare_parameter("hello").default(1.23).read_only().unwrap(); - let p3 = node.declare_parameter::("bool").optional().unwrap(); - let p4 = node.declare_parameter::("dynamic").optional().unwrap(); let mut num_messages: usize = 0; diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index cacf9683..8a8cfbbe 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -497,16 +497,16 @@ mod tests { assert!(subscription_infos.is_empty()); - // Test that the graph has no services + // Test that the graph only has 6 services (parameter services) let names_and_topics = node .get_service_names_and_types_by_node(node_name, "") .unwrap(); - assert_eq!(names_and_topics.len(), 0); + assert_eq!(names_and_topics.len(), 6); let names_and_topics = node.get_service_names_and_types().unwrap(); - assert_eq!(names_and_topics.len(), 0); + assert_eq!(names_and_topics.len(), 6); // Test that the graph has no clients let names_and_topics = node diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index ac4d1478..4fb28c2e 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -4,7 +4,7 @@ mod service; mod value; pub(crate) use override_map::*; -use range::*; +pub use range::*; use service::*; pub use value::*; diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index be9d5e6f..a063c430 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -399,7 +399,7 @@ impl TryFrom for ParameterValue { ParameterType::PARAMETER_INTEGER => Ok(ParameterValue::Integer(param.integer_value)), ParameterType::PARAMETER_DOUBLE => Ok(ParameterValue::Double(param.double_value)), ParameterType::PARAMETER_STRING => { - Ok(ParameterValue::String(param.string_value.into())) + Ok(ParameterValue::String(param.string_value.to_string().into())) } ParameterType::PARAMETER_BYTE_ARRAY => { Ok(ParameterValue::ByteArray((*param.byte_array_value).into())) @@ -417,7 +417,7 @@ impl TryFrom for ParameterValue { param .string_array_value .iter() - .map(|s| s.clone().into()) + .map(|s| s.to_string().into()) .collect::>() .into(), )), diff --git a/rosidl_runtime_rs/src/string.rs b/rosidl_runtime_rs/src/string.rs index 53294ef6..ecc7c49e 100644 --- a/rosidl_runtime_rs/src/string.rs +++ b/rosidl_runtime_rs/src/string.rs @@ -309,7 +309,7 @@ impl From<&str> for String { size: 0, capacity: 0, }; - // SAFETY: It's okay to pass a non-zero-terminated string here since assign uses the + // SAFETY: It's okay to pass a non-zero-terminated string here since assignn uses the // specified length and will append the 0 byte to the dest string itself. if !unsafe { rosidl_runtime_c__String__assignn(&mut msg as *mut _, s.as_ptr() as *const _, s.len()) @@ -327,7 +327,7 @@ impl From> for String { size: 0, capacity: 0, }; - // SAFETY: It's okay to pass a non-zero-terminated string here since assign uses the + // SAFETY: It's okay to pass a non-zero-terminated string here since assignn uses the // specified length and will append the 0 byte to the dest string itself. if !unsafe { rosidl_runtime_c__String__assignn(&mut msg as *mut _, s.as_ptr() as *const _, s.len()) @@ -338,12 +338,6 @@ impl From> for String { } } -impl From for Arc { - fn from(s: String) -> Self { - s.to_cstr().to_str().unwrap().into() - } -} - impl String { /// Creates a CStr from this String. /// From 6f8a6efa9a9b718d5c86bd7f08e91456772b4732 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 7 Nov 2023 16:04:54 +0800 Subject: [PATCH 07/24] Fix order of drop calls Signed-off-by: Luca Della Vedova --- rclrs/src/node.rs | 7 +++++-- rclrs/src/parameter/value.rs | 6 +++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index b91e1627..e5b21adf 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -65,14 +65,17 @@ unsafe impl Send for rcl_node_t {} /// [3]: crate::NodeBuilder::new /// [4]: crate::NodeBuilder::namespace pub struct Node { - pub(crate) rcl_node_mtx: Arc>, - pub(crate) rcl_context_mtx: Arc>, pub(crate) clients_mtx: Mutex>>, pub(crate) guard_conditions_mtx: Mutex>>, pub(crate) services_mtx: Mutex>>, pub(crate) subscriptions_mtx: Mutex>>, _time_source: TimeSource, _parameter: ParameterInterface, + // Note: it's important to have those last since `drop` will be called in order of declaration + // in the struct and both `TimeSource` and `ParameterInterface` contain subscriptions / + // services that will fail to be dropped if the context or node is destroyed first. + pub(crate) rcl_node_mtx: Arc>, + pub(crate) rcl_context_mtx: Arc>, } impl Eq for Node {} diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index a063c430..ed801a50 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -398,9 +398,9 @@ impl TryFrom for ParameterValue { ParameterType::PARAMETER_BOOL => Ok(ParameterValue::Bool(param.bool_value)), ParameterType::PARAMETER_INTEGER => Ok(ParameterValue::Integer(param.integer_value)), ParameterType::PARAMETER_DOUBLE => Ok(ParameterValue::Double(param.double_value)), - ParameterType::PARAMETER_STRING => { - Ok(ParameterValue::String(param.string_value.to_string().into())) - } + ParameterType::PARAMETER_STRING => Ok(ParameterValue::String( + param.string_value.to_string().into(), + )), ParameterType::PARAMETER_BYTE_ARRAY => { Ok(ParameterValue::ByteArray((*param.byte_array_value).into())) } From 45ccda9a6784be8a1ee71a8492b35bcada7ead9f Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 8 Nov 2023 16:22:58 +0800 Subject: [PATCH 08/24] Add first bunch of unit tests for list and get / set parameters Signed-off-by: Luca Della Vedova --- rclrs_tests/Cargo.toml | 2 + rclrs_tests/src/lib.rs | 1 + rclrs_tests/src/parameter_service_tests.rs | 418 +++++++++++++++++++++ 3 files changed, 421 insertions(+) create mode 100644 rclrs_tests/src/parameter_service_tests.rs diff --git a/rclrs_tests/Cargo.toml b/rclrs_tests/Cargo.toml index 51708f4e..d5485192 100644 --- a/rclrs_tests/Cargo.toml +++ b/rclrs_tests/Cargo.toml @@ -10,6 +10,8 @@ path = "src/lib.rs" [dependencies] anyhow = {version = "1", features = ["backtrace"]} test_msgs = {version = "*"} +rcl_interfaces = {version = "*"} +tokio = { version = "1", features = ["macros", "rt", "rt-multi-thread", "time"] } [dependencies.rclrs] version = "*" diff --git a/rclrs_tests/src/lib.rs b/rclrs_tests/src/lib.rs index 7cabab86..3a63dab1 100644 --- a/rclrs_tests/src/lib.rs +++ b/rclrs_tests/src/lib.rs @@ -2,4 +2,5 @@ mod client_service_tests; mod graph_tests; +mod parameter_service_tests; mod pub_sub_tests; diff --git a/rclrs_tests/src/parameter_service_tests.rs b/rclrs_tests/src/parameter_service_tests.rs new file mode 100644 index 00000000..74ebf91f --- /dev/null +++ b/rclrs_tests/src/parameter_service_tests.rs @@ -0,0 +1,418 @@ +//use rcl_interfaces::msg::rmw::*; +use rcl_interfaces::msg::rmw::{ + Parameter as RmwParameter, ParameterType, ParameterValue as RmwParameterValue, +}; +use rcl_interfaces::srv::rmw::*; +use rclrs::{ + Context, MandatoryParameter, Node, NodeBuilder, ParameterRange, ParameterValue, RclrsError, + ReadOnlyParameter, TopicEndpointInfo, TopicNamesAndTypes, QOS_PROFILE_SYSTEM_DEFAULT, +}; +use rosidl_runtime_rs::{seq, Sequence}; +use std::sync::{Arc, RwLock}; + +struct TestGraph { + node: Arc, + client: Arc, +} + +struct TestNode { + node: Arc, + bool_param: MandatoryParameter, + ns_param: MandatoryParameter, + read_only_param: ReadOnlyParameter, + dynamic_param: MandatoryParameter, +} + +async fn try_until_timeout(f: F) -> Result<(), ()> +where + F: FnOnce() -> bool + Copy, +{ + let mut retry_count = 0; + while !f() { + if retry_count > 50 { + return Err(()); + } + tokio::time::sleep(tokio::time::Duration::from_millis(10)).await; + retry_count += 1; + } + Ok(()) +} + +fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { + let node = NodeBuilder::new(&context, "node") + .namespace(ns) + .build() + .unwrap(); + let range = ParameterRange { + lower: Some(0), + upper: Some(100), + step: None, + }; + let bool_param = node + .declare_parameter("bool") + .default(true) + .mandatory() + .unwrap(); + let ns_param = node + .declare_parameter("ns1.ns2.ns3.int") + .default(42) + .range(range) + .mandatory() + .unwrap(); + let read_only_param = node + .declare_parameter("read_only") + .default(1.23) + .read_only() + .unwrap(); + let dynamic_param = node + .declare_parameter("dynamic") + .default(ParameterValue::String("hello".into())) + .mandatory() + .unwrap(); + + let client = NodeBuilder::new(&context, "client") + .namespace(ns) + .build() + .unwrap(); + + ( + TestNode { + node, + bool_param, + ns_param, + read_only_param, + dynamic_param, + }, + client, + ) +} + +#[test] +fn test_parameter_services_names_and_types() -> Result<(), RclrsError> { + let context = Context::new([]).unwrap(); + let (node, client) = construct_test_nodes(&context, "names_types"); + let check_names_and_types = |names_and_types: TopicNamesAndTypes| { + let types = names_and_types + .get("/names_types/node/describe_parameters") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/DescribeParameters".to_string())); + let types = names_and_types + .get("/names_types/node/get_parameters") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/GetParameters".to_string())); + let types = names_and_types + .get("/names_types/node/set_parameters") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/SetParameters".to_string())); + let types = names_and_types + .get("/names_types/node/set_parameters_atomically") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/SetParametersAtomically".to_string())); + let types = names_and_types + .get("/names_types/node/list_parameters") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/ListParameters".to_string())); + let types = names_and_types + .get("/names_types/node/get_parameter_types") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/GetParameterTypes".to_string())); + }; + + std::thread::sleep(std::time::Duration::from_millis(100)); + + let service_names_and_types = node.node.get_service_names_and_types()?; + check_names_and_types(service_names_and_types); + Ok(()) +} + +//#[tokio::test(flavor = "multi_thread", worker_threads = 1)] +#[tokio::test] +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")?; + + try_until_timeout(|| list_client.service_is_ready().unwrap()) + .await + .unwrap(); + + let done = Arc::new(RwLock::new(false)); + + let inner_done = done.clone(); + let rclrs_spin = tokio::task::spawn(async move { + try_until_timeout(|| { + rclrs::spin_once(node.node.clone(), Some(std::time::Duration::ZERO)).ok(); + rclrs::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); + *inner_done.read().unwrap() + }) + .await + .unwrap(); + }); + + let res = tokio::task::spawn(async move { + // List all parameters + let request = ListParameters_Request { + prefixes: seq![], + depth: 0, + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + list_client + .async_send_request_with_callback(&request, move |response: ListParameters_Response| { + // use_sim_time + all the manually defined ones + *call_done.write().unwrap() = true; + let names = response.result.names; + assert_eq!(names.len(), 5); + // Parameter names are returned in alphabetical order + assert_eq!(names[0].to_string(), "bool"); + assert_eq!(names[1].to_string(), "dynamic"); + assert_eq!(names[2].to_string(), "ns1.ns2.ns3.int"); + assert_eq!(names[3].to_string(), "read_only"); + assert_eq!(names[4].to_string(), "use_sim_time"); + // Only one prefix + assert_eq!(response.result.prefixes.len(), 1); + assert_eq!(response.result.prefixes[0].to_string(), "ns1.ns2.ns3"); + }) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // Limit depth, namespaced parameter is not returned + let request = ListParameters_Request { + prefixes: seq![], + depth: 1, + }; + let call_done = client_finished.clone(); + *call_done.write().unwrap() = false; + list_client + .async_send_request_with_callback(&request, move |response: ListParameters_Response| { + *call_done.write().unwrap() = true; + let names = response.result.names; + assert_eq!(names.len(), 4); + assert!(names.iter().all(|n| n.to_string() != "ns1.ns2.ns3.int")); + assert_eq!(response.result.prefixes.len(), 0); + }) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // Filter by prefix, just return the requested one with the right prefix + let request = ListParameters_Request { + prefixes: seq!["ns1.ns2".into()], + depth: 0, + }; + let call_done = client_finished.clone(); + *call_done.write().unwrap() = false; + list_client + .async_send_request_with_callback(&request, move |response: ListParameters_Response| { + *call_done.write().unwrap() = true; + let names = response.result.names; + assert_eq!(names.len(), 1); + assert_eq!(names[0].to_string(), "ns1.ns2.ns3.int"); + assert_eq!(response.result.prefixes.len(), 1); + assert_eq!(response.result.prefixes[0].to_string(), "ns1.ns2.ns3"); + }) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // If prefix is equal to names, parameters should be returned + let request = ListParameters_Request { + prefixes: seq!["use_sim_time".into(), "bool".into()], + depth: 0, + }; + let call_done = client_finished.clone(); + *call_done.write().unwrap() = false; + list_client + .async_send_request_with_callback(&request, move |response: ListParameters_Response| { + *call_done.write().unwrap() = true; + let names = response.result.names; + dbg!(&names); + assert_eq!(names.len(), 2); + assert_eq!(names[0].to_string(), "bool"); + assert_eq!(names[1].to_string(), "use_sim_time"); + assert_eq!(response.result.prefixes.len(), 0); + }) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + *done.write().unwrap() = true; + }); + + res.await.unwrap(); + rclrs_spin.await.unwrap(); + + Ok(()) +} + +#[tokio::test] +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")?; + + try_until_timeout(|| get_client.service_is_ready().unwrap()) + .await + .unwrap(); + + let done = Arc::new(RwLock::new(false)); + + let inner_done = done.clone(); + let rclrs_spin = tokio::task::spawn(async move { + try_until_timeout(|| { + rclrs::spin_once(node.node.clone(), Some(std::time::Duration::ZERO)).ok(); + rclrs::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); + *inner_done.read().unwrap() + }) + .await + .unwrap(); + }); + + let res = tokio::task::spawn(async move { + // Get an existing parameter + let request = GetParameters_Request { + names: seq!["bool".into()], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + get_client + .async_send_request_with_callback(&request, move |response: GetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.values.len(), 1); + let param = &response.values[0]; + assert_eq!(param.type_, ParameterType::PARAMETER_BOOL); + assert!(param.bool_value); + }) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // Getting both existing and non existing parameters should fail + let request = GetParameters_Request { + names: seq!["bool".into(), "non_existing".into()], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + get_client + .async_send_request_with_callback(&request, move |response: GetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.values.len(), 0); + }) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // Set a mix of existing, non existing, dynamic and out of range parameters + let bool_parameter = RmwParameter { + name: "bool".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_BOOL, + bool_value: false, + ..Default::default() + }, + }; + let bool_parameter_mismatched = RmwParameter { + name: "bool".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_INTEGER, + integer_value: 42, + ..Default::default() + }, + }; + let read_only_parameter = RmwParameter { + name: "read_only".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_DOUBLE, + double_value: 3.45, + ..Default::default() + }, + }; + let dynamic_parameter = RmwParameter { + name: "dynamic".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_BOOL, + bool_value: true, + ..Default::default() + }, + }; + let out_of_range_parameter = RmwParameter { + name: "ns1.ns2.ns3.int".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_INTEGER, + integer_value: 1000, + ..Default::default() + }, + }; + let invalid_parameter_type = RmwParameter { + name: "dynamic".into(), + value: RmwParameterValue { + type_: 200, + integer_value: 1000, + ..Default::default() + }, + }; + let undeclared_bool = RmwParameter { + name: "undeclared_bool".into(), + value: RmwParameterValue { + type_: ParameterType::PARAMETER_BOOL, + bool_value: true, + ..Default::default() + }, + }; + let request = SetParameters_Request { + parameters: seq![ + bool_parameter, + read_only_parameter, + bool_parameter_mismatched, + dynamic_parameter, + out_of_range_parameter, + invalid_parameter_type, + undeclared_bool + ], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + // Parameter is default assigned true + assert_eq!(node.bool_param.get(), true); + set_client + .async_send_request_with_callback(&request, move |response: SetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.results.len(), 7); + // Setting a bool value set for a bool parameter + assert!(response.results[0].successful); + // Value was set to false, node parameter get should reflect this + assert_eq!(node.bool_param.get(), false); + // Setting a parameter to the wrong type + assert_eq!(response.results[1].successful, false); + // Setting a read only parameter + assert_eq!(response.results[2].successful, false); + // Setting a dynamic parameter to a new type + assert!(response.results[3].successful); + // Setting a value out of range + assert_eq!(response.results[4].successful, false); + // Setting an invalid type + assert_eq!(response.results[5].successful, false); + // Setting an undeclared parameter, without allowing undeclared parameters + assert_eq!(response.results[6].successful, false); + }) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + *done.write().unwrap() = true; + }); + + res.await.unwrap(); + rclrs_spin.await.unwrap(); + + Ok(()) +} From 315e088c2440225b459a79cf460bbc49892cd26a Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 8 Nov 2023 16:40:45 +0800 Subject: [PATCH 09/24] Clear warnings in rclrs Signed-off-by: Luca Della Vedova --- rclrs/src/parameter/service.rs | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 945bd55b..9d9b353f 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -9,12 +9,20 @@ use super::ParameterMap; use crate::parameter::{DeclaredValue, ParameterKind, ParameterStorage}; use crate::{rmw_request_id_t, Node, RclrsError, Service}; +// The variables only exist to keep a strong reference to the services and are technically unused. +// What is used is the Weak that is stored in the node, and is upgraded when spinning. pub struct ParameterService { + #[allow(dead_code)] describe_parameters_service: Arc>, + #[allow(dead_code)] get_parameter_types_service: Arc>, + #[allow(dead_code)] get_parameters_service: Arc>, + #[allow(dead_code)] list_parameters_service: Arc>, + #[allow(dead_code)] set_parameters_service: Arc>, + #[allow(dead_code)] set_parameters_atomically_service: Arc>, } @@ -29,7 +37,7 @@ impl ParameterService { let map = parameter_map.clone(); let describe_parameters_service = node.create_service( &(fqn.clone() + "/describe_parameters"), - move |req_id: &rmw_request_id_t, req: DescribeParameters_Request| { + move |_req_id: &rmw_request_id_t, req: DescribeParameters_Request| { let map = map.lock().unwrap(); let descriptors = req .names @@ -74,7 +82,7 @@ impl ParameterService { let map = parameter_map.clone(); let get_parameter_types_service = node.create_service( &(fqn.clone() + "/get_parameter_types"), - move |req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { + move |_req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { let map = map.lock().unwrap(); let types = req .names @@ -93,7 +101,7 @@ impl ParameterService { let map = parameter_map.clone(); let get_parameters_service = node.create_service( &(fqn.clone() + "/get_parameters"), - move |req_id: &rmw_request_id_t, req: GetParameters_Request| { + move |_req_id: &rmw_request_id_t, req: GetParameters_Request| { let map = map.lock().unwrap(); let values = req .names @@ -126,7 +134,7 @@ impl ParameterService { let map = parameter_map.clone(); let list_parameters_service = node.create_service( &(fqn.clone() + "/list_parameters"), - move |req_id: &rmw_request_id_t, req: ListParameters_Request| { + move |_req_id: &rmw_request_id_t, req: ListParameters_Request| { let check_parameter_name_depth = |substring: &[i8]| { if req.depth == ListParameters_Request::DEPTH_RECURSIVE { return true; @@ -185,7 +193,7 @@ impl ParameterService { let map = parameter_map.clone(); let set_parameters_service = node.create_service( &(fqn.clone() + "/set_parameters"), - move |req_id: &rmw_request_id_t, req: SetParameters_Request| { + move |_req_id: &rmw_request_id_t, req: SetParameters_Request| { let mut map = map.lock().unwrap(); let results = req .parameters @@ -212,7 +220,7 @@ impl ParameterService { )?; let set_parameters_atomically_service = node.create_service( &(fqn.clone() + "/set_parameters_atomically"), - move |req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { + move |_req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { let mut map = parameter_map.lock().unwrap(); let results = req .parameters From 8d53f2a5c8201d97b0c5861f31ab9e6a5738c079 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 9 Nov 2023 11:19:27 +0800 Subject: [PATCH 10/24] Fix clippy, add set atomically tests Signed-off-by: Luca Della Vedova --- rclrs_tests/src/parameter_service_tests.rs | 93 +++++++++++++++------- 1 file changed, 66 insertions(+), 27 deletions(-) diff --git a/rclrs_tests/src/parameter_service_tests.rs b/rclrs_tests/src/parameter_service_tests.rs index 74ebf91f..e94c4e7e 100644 --- a/rclrs_tests/src/parameter_service_tests.rs +++ b/rclrs_tests/src/parameter_service_tests.rs @@ -5,21 +5,16 @@ use rcl_interfaces::msg::rmw::{ use rcl_interfaces::srv::rmw::*; use rclrs::{ Context, MandatoryParameter, Node, NodeBuilder, ParameterRange, ParameterValue, RclrsError, - ReadOnlyParameter, TopicEndpointInfo, TopicNamesAndTypes, QOS_PROFILE_SYSTEM_DEFAULT, + ReadOnlyParameter, TopicNamesAndTypes, }; use rosidl_runtime_rs::{seq, Sequence}; use std::sync::{Arc, RwLock}; -struct TestGraph { - node: Arc, - client: Arc, -} - struct TestNode { node: Arc, bool_param: MandatoryParameter, - ns_param: MandatoryParameter, - read_only_param: ReadOnlyParameter, + _ns_param: MandatoryParameter, + _read_only_param: ReadOnlyParameter, dynamic_param: MandatoryParameter, } @@ -39,7 +34,7 @@ where } fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { - let node = NodeBuilder::new(&context, "node") + let node = NodeBuilder::new(context, "node") .namespace(ns) .build() .unwrap(); @@ -53,13 +48,13 @@ fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { .default(true) .mandatory() .unwrap(); - let ns_param = node + let _ns_param = node .declare_parameter("ns1.ns2.ns3.int") .default(42) .range(range) .mandatory() .unwrap(); - let read_only_param = node + let _read_only_param = node .declare_parameter("read_only") .default(1.23) .read_only() @@ -70,7 +65,7 @@ fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { .mandatory() .unwrap(); - let client = NodeBuilder::new(&context, "client") + let client = NodeBuilder::new(context, "client") .namespace(ns) .build() .unwrap(); @@ -79,8 +74,8 @@ fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { TestNode { node, bool_param, - ns_param, - read_only_param, + _ns_param, + _read_only_param, dynamic_param, }, client, @@ -90,7 +85,7 @@ fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { #[test] fn test_parameter_services_names_and_types() -> Result<(), RclrsError> { let context = Context::new([]).unwrap(); - let (node, client) = construct_test_nodes(&context, "names_types"); + let (node, _client) = construct_test_nodes(&context, "names_types"); let check_names_and_types = |names_and_types: TopicNamesAndTypes| { let types = names_and_types .get("/names_types/node/describe_parameters") @@ -264,10 +259,11 @@ async fn test_get_set_parameters_service() -> Result<(), RclrsError> { let done = Arc::new(RwLock::new(false)); + let inner_node = node.node.clone(); let inner_done = done.clone(); let rclrs_spin = tokio::task::spawn(async move { try_until_timeout(|| { - rclrs::spin_once(node.node.clone(), Some(std::time::Duration::ZERO)).ok(); + rclrs::spin_once(inner_node.clone(), Some(std::time::Duration::ZERO)).ok(); rclrs::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); *inner_done.read().unwrap() }) @@ -370,19 +366,19 @@ async fn test_get_set_parameters_service() -> Result<(), RclrsError> { }; let request = SetParameters_Request { parameters: seq![ - bool_parameter, - read_only_parameter, + bool_parameter.clone(), + read_only_parameter.clone(), bool_parameter_mismatched, dynamic_parameter, out_of_range_parameter, invalid_parameter_type, - undeclared_bool + undeclared_bool.clone() ], }; let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); - // Parameter is default assigned true - assert_eq!(node.bool_param.get(), true); + // Parameter is assigned a default of true at declaration time + assert!(node.bool_param.get()); set_client .async_send_request_with_callback(&request, move |response: SetParameters_Response| { *call_done.write().unwrap() = true; @@ -390,24 +386,67 @@ async fn test_get_set_parameters_service() -> Result<(), RclrsError> { // Setting a bool value set for a bool parameter assert!(response.results[0].successful); // Value was set to false, node parameter get should reflect this - assert_eq!(node.bool_param.get(), false); + assert!(!node.bool_param.get()); // Setting a parameter to the wrong type - assert_eq!(response.results[1].successful, false); + assert!(!response.results[1].successful); // Setting a read only parameter - assert_eq!(response.results[2].successful, false); + assert!(!response.results[2].successful); // Setting a dynamic parameter to a new type assert!(response.results[3].successful); + assert_eq!(node.dynamic_param.get(), ParameterValue::Bool(true)); // Setting a value out of range - assert_eq!(response.results[4].successful, false); + assert!(!response.results[4].successful); // Setting an invalid type - assert_eq!(response.results[5].successful, false); + assert!(!response.results[5].successful); // Setting an undeclared parameter, without allowing undeclared parameters - assert_eq!(response.results[6].successful, false); + assert!(!response.results[6].successful); + }) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // Set the node to use undeclared parameters and try to set one + node.node.use_undeclared_parameters(); + let request = SetParameters_Request { + parameters: seq![undeclared_bool], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + set_client + .async_send_request_with_callback(&request, move |response: SetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.results.len(), 1); + // Setting the undeclared parameter is now allowed + assert!(response.results[0].successful); + assert_eq!( + node.node.use_undeclared_parameters().get("undeclared_bool"), + Some(ParameterValue::Bool(true)) + ); }) .unwrap(); try_until_timeout(|| *client_finished.read().unwrap()) .await .unwrap(); + + // With set_parameters_atomically, if one fails all should fail + let request = SetParametersAtomically_Request { + parameters: seq![bool_parameter, read_only_parameter], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + set_atomically_client + .async_send_request_with_callback( + &request, + move |response: SetParametersAtomically_Response| { + *call_done.write().unwrap() = true; + assert!(!response.result.successful); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); *done.write().unwrap() = true; }); From 095a00f66f86a60a42c5198ce80d32b2d3ddb680 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 9 Nov 2023 11:48:50 +0800 Subject: [PATCH 11/24] Add describe parameter and get parameter types tests Signed-off-by: Luca Della Vedova --- rclrs/src/parameter/value.rs | 4 +- rclrs_tests/src/parameter_service_tests.rs | 170 ++++++++++++++++++++- 2 files changed, 168 insertions(+), 6 deletions(-) diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index ed801a50..2b5716cb 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -381,8 +381,8 @@ impl From for RmwParameterValue { } } -/// An error that occured when trying to convert a parameter from a -/// [crate::vendor::rcl_interfaces::msg::rmw::ParameterValue] +/// An error that occured when trying to convert a parameter from an +/// `rcl_interfaces::msg::ParameterValue` pub enum RmwParameterConversionError { /// The parameter type was not valid. InvalidParameterType, diff --git a/rclrs_tests/src/parameter_service_tests.rs b/rclrs_tests/src/parameter_service_tests.rs index e94c4e7e..8f73cc86 100644 --- a/rclrs_tests/src/parameter_service_tests.rs +++ b/rclrs_tests/src/parameter_service_tests.rs @@ -46,12 +46,14 @@ fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { let bool_param = node .declare_parameter("bool") .default(true) + .description("A boolean value") .mandatory() .unwrap(); let _ns_param = node .declare_parameter("ns1.ns2.ns3.int") .default(42) .range(range) + .constraints("Only the answer") .mandatory() .unwrap(); let _read_only_param = node @@ -120,7 +122,6 @@ fn test_parameter_services_names_and_types() -> Result<(), RclrsError> { Ok(()) } -//#[tokio::test(flavor = "multi_thread", worker_threads = 1)] #[tokio::test] async fn test_list_parameters_service() -> Result<(), RclrsError> { let context = Context::new([]).unwrap(); @@ -253,9 +254,13 @@ async fn test_get_set_parameters_service() -> Result<(), RclrsError> { let set_atomically_client = client .create_client::("/get_set/node/set_parameters_atomically")?; - try_until_timeout(|| get_client.service_is_ready().unwrap()) - .await - .unwrap(); + try_until_timeout(|| { + get_client.service_is_ready().unwrap() + && set_client.service_is_ready().unwrap() + && set_atomically_client.service_is_ready().unwrap() + }) + .await + .unwrap(); let done = Arc::new(RwLock::new(false)); @@ -455,3 +460,160 @@ async fn test_get_set_parameters_service() -> Result<(), RclrsError> { Ok(()) } + +#[tokio::test] +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")?; + + try_until_timeout(|| { + describe_client.service_is_ready().unwrap() && get_types_client.service_is_ready().unwrap() + }) + .await + .unwrap(); + + let done = Arc::new(RwLock::new(false)); + + let inner_done = done.clone(); + let rclrs_spin = tokio::task::spawn(async move { + try_until_timeout(|| { + rclrs::spin_once(node.node.clone(), Some(std::time::Duration::ZERO)).ok(); + rclrs::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); + *inner_done.read().unwrap() + }) + .await + .unwrap(); + }); + + let res = tokio::task::spawn(async move { + // Desctibe all parameters + let request = DescribeParameters_Request { + names: seq![ + "bool".into(), + "ns1.ns2.ns3.int".into(), + "read_only".into(), + "dynamic".into() + ], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + describe_client + .async_send_request_with_callback( + &request, + move |response: DescribeParameters_Response| { + *call_done.write().unwrap() = true; + let desc = response.descriptors; + assert_eq!(desc.len(), 4); + // Descriptors are returned in the requested order + assert_eq!(desc[0].name.to_string(), "bool"); + assert_eq!(desc[0].type_, ParameterType::PARAMETER_BOOL); + assert_eq!(desc[0].description.to_string(), "A boolean value"); + assert!(!desc[0].read_only); + assert!(!desc[0].dynamic_typing); + assert_eq!(desc[1].name.to_string(), "ns1.ns2.ns3.int"); + assert_eq!(desc[1].type_, ParameterType::PARAMETER_INTEGER); + assert_eq!(desc[1].integer_range.len(), 1); + assert_eq!(desc[1].integer_range[0].from_value, 0); + assert_eq!(desc[1].integer_range[0].to_value, 100); + assert_eq!(desc[1].integer_range[0].step, 0); + assert!(!desc[1].read_only); + assert!(!desc[1].dynamic_typing); + assert_eq!( + desc[1].additional_constraints.to_string(), + "Only the answer" + ); + assert_eq!(desc[2].name.to_string(), "read_only"); + assert_eq!(desc[2].type_, ParameterType::PARAMETER_DOUBLE); + assert!(desc[2].read_only); + assert!(!desc[2].dynamic_typing); + assert_eq!(desc[3].name.to_string(), "dynamic"); + assert_eq!(desc[3].type_, ParameterType::PARAMETER_STRING); + assert!(desc[3].dynamic_typing); + assert!(!desc[3].read_only); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // If a describe parameters request is sent with a non existing parameter, an empty + // response should be returned + let request = DescribeParameters_Request { + names: seq!["bool".into(), "non_existing".into()], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + describe_client + .async_send_request_with_callback( + &request, + move |response: DescribeParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.descriptors.len(), 0); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // Get all parameter types + let request = GetParameterTypes_Request { + names: seq![ + "bool".into(), + "ns1.ns2.ns3.int".into(), + "read_only".into(), + "dynamic".into() + ], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + get_types_client + .async_send_request_with_callback( + &request, + move |response: GetParameterTypes_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.types.len(), 4); + // Types are returned in the requested order + assert_eq!(response.types[0], ParameterType::PARAMETER_BOOL); + assert_eq!(response.types[1], ParameterType::PARAMETER_INTEGER); + assert_eq!(response.types[2], ParameterType::PARAMETER_DOUBLE); + assert_eq!(response.types[3], ParameterType::PARAMETER_STRING); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // If a get parameter types request is sent with a non existing parameter, an empty + // response should be returned + let request = GetParameterTypes_Request { + names: seq!["bool".into(), "non_existing".into()], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + get_types_client + .async_send_request_with_callback( + &request, + move |response: GetParameterTypes_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.types.len(), 0); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + *done.write().unwrap() = true; + }); + + res.await.unwrap(); + rclrs_spin.await.unwrap(); + + Ok(()) +} From d3585caa7f4bdfd547ecf6784b50421fc0f5c3a2 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 18 Mar 2024 15:06:11 +0800 Subject: [PATCH 12/24] Minor cleanups, remove several unwraps Signed-off-by: Luca Della Vedova --- rclrs/src/parameter.rs | 2 +- rclrs/src/parameter/service.rs | 60 +++++++++++++++++---------------- rclrs/src/parameter/value.rs | 1 - rosidl_runtime_rs/src/string.rs | 28 ++++----------- 4 files changed, 39 insertions(+), 52 deletions(-) diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index b34450ef..889d2683 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -492,7 +492,7 @@ pub(crate) struct ParameterMap { impl ParameterMap { /// Validates the requested parameter setting and returns an error if the requested value is /// not valid. - pub(crate) fn validate_parameter_setting( + fn validate_parameter_setting( &self, name: &str, value: RmwParameterValue, diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index a8ac3b91..f470ad7b 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -43,14 +43,14 @@ impl ParameterService { .names .into_iter() .map(|name| { - // TODO(luca) Remove conversion to string and implement Borrow - let storage = map.storage.get(name.to_cstr().to_str().unwrap())?; + let name = name.to_cstr().to_str().ok()?; + let storage = map.storage.get(name)?; let mut descriptor = match storage { ParameterStorage::Declared(storage) => { let (integer_range, floating_point_range) = storage.options.ranges.to_descriptor_ranges(); ParameterDescriptor { - name, + name: name.into(), type_: Default::default(), description: storage.options.description.clone().into(), additional_constraints: storage @@ -65,7 +65,7 @@ impl ParameterService { } } ParameterStorage::Undeclared(_) => ParameterDescriptor { - name, + name: name.into(), dynamic_typing: true, ..Default::default() }, @@ -88,10 +88,8 @@ impl ParameterService { .names .into_iter() .map(|name| { - // TODO(luca) Remove conversion to string and implement Borrow - map.storage - .get(name.to_cstr().to_str().unwrap()) - .map(|s| s.to_parameter_type()) + let name = name.to_cstr().to_str().ok()?; + map.storage.get(name).map(|s| s.to_parameter_type()) }) .collect::>() .unwrap_or_default(); @@ -107,9 +105,8 @@ impl ParameterService { .names .into_iter() .map(|name| { - // TODO(luca) Remove conversion to string and implement Borrow - let storage = map.storage.get(name.to_cstr().to_str().unwrap())?; - match storage { + let name = name.to_cstr().to_str().ok()?; + match map.storage.get(name)? { ParameterStorage::Declared(storage) => match &storage.value { DeclaredValue::Mandatory(v) => { Some(v.read().unwrap().clone().into()) @@ -160,7 +157,7 @@ impl ParameterService { let mut prefix = prefix.clone(); prefix.extend(".".chars()); if name.len() > prefix.len() - && name[0..prefix.len()] == prefix[0..] + && name.starts_with(&prefix) && check_parameter_name_depth(&name[prefix.len()..]) { return true; @@ -176,12 +173,10 @@ impl ParameterService { let name = name.to_string(); if let Some(pos) = name.rfind('.') { return Some(name[0..pos].into()); - //return Some(name[0..pos].iter().map(|c| *c as char).collect()); } None }) .collect(); - // TODO(luca) populate prefixes in result ListParameters_Response { result: ListParametersResult { names, @@ -199,20 +194,25 @@ impl ParameterService { .parameters .into_iter() .map(|param| { - let name = param.name.to_cstr().to_str().unwrap(); - let value = map.validate_parameter_setting(name, param.value)?; - map.store_parameter(name.into(), value); - Ok(()) - }) - .map(|res| match res { - Ok(()) => SetParametersResult { - successful: true, - reason: Default::default(), - }, - Err(e) => SetParametersResult { - successful: false, - reason: e, - }, + let Ok(name) = param.name.to_cstr().to_str() else { + return SetParametersResult { + successful: false, + reason: "Failed parsing into UTF-8".into(), + }; + }; + match map.validate_parameter_setting(name, param.value) { + Ok(value) => { + map.store_parameter(name.into(), value); + SetParametersResult { + successful: true, + reason: Default::default(), + } + } + Err(e) => SetParametersResult { + successful: false, + reason: e.into(), + }, + } }) .collect(); SetParameters_Response { results } @@ -226,7 +226,9 @@ impl ParameterService { .parameters .into_iter() .map(|param| { - let name = param.name.to_cstr().to_str().unwrap(); + let Ok(name) = param.name.to_cstr().to_str() else { + return Err("Failed parsing into UTF-8".into()); + }; let value = map.validate_parameter_setting(name, param.value)?; Ok((name.into(), value)) }) diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index 2b5716cb..d7b88095 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -369,7 +369,6 @@ impl From for RmwParameterValue { }, ParameterValue::StringArray(v) => RmwParameterValue { type_: ParameterType::PARAMETER_STRING_ARRAY, - // TODO(luca) check if there is a more efficient way to perform this conversion string_array_value: v .iter() .map(|v| v.clone().into()) diff --git a/rosidl_runtime_rs/src/string.rs b/rosidl_runtime_rs/src/string.rs index f956b7d7..78608231 100644 --- a/rosidl_runtime_rs/src/string.rs +++ b/rosidl_runtime_rs/src/string.rs @@ -1,9 +1,9 @@ +use std::borrow::Borrow; use std::cmp::Ordering; use std::ffi::CStr; use std::fmt::{self, Debug, Display}; use std::hash::{Hash, Hasher}; use std::ops::{Deref, DerefMut}; -use std::sync::Arc; #[cfg(feature = "serde")] mod serde; @@ -302,31 +302,17 @@ string_impl!( rosidl_runtime_c__U16String__Sequence__copy ); -impl From<&str> for String { - fn from(s: &str) -> Self { - let mut msg = Self { - data: std::ptr::null_mut(), - size: 0, - capacity: 0, - }; - // SAFETY: It's okay to pass a non-zero-terminated string here since assignn uses the - // specified length and will append the 0 byte to the dest string itself. - if !unsafe { - rosidl_runtime_c__String__assignn(&mut msg as *mut _, s.as_ptr() as *const _, s.len()) - } { - panic!("rosidl_runtime_c__String__assignn failed"); - } - msg - } -} - -impl From> for String { - fn from(s: Arc) -> Self { +impl From for String +where + T: Borrow, +{ + fn from(s: T) -> Self { let mut msg = Self { data: std::ptr::null_mut(), size: 0, capacity: 0, }; + let s = s.borrow(); // SAFETY: It's okay to pass a non-zero-terminated string here since assignn uses the // specified length and will append the 0 byte to the dest string itself. if !unsafe { From 74f384d977afbf370562314b48fa805835981b1d Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 18 Mar 2024 15:39:15 +0800 Subject: [PATCH 13/24] Remove commented code Signed-off-by: Luca Della Vedova --- rclrs_tests/src/parameter_service_tests.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/rclrs_tests/src/parameter_service_tests.rs b/rclrs_tests/src/parameter_service_tests.rs index 8f73cc86..273f99d0 100644 --- a/rclrs_tests/src/parameter_service_tests.rs +++ b/rclrs_tests/src/parameter_service_tests.rs @@ -1,4 +1,3 @@ -//use rcl_interfaces::msg::rmw::*; use rcl_interfaces::msg::rmw::{ Parameter as RmwParameter, ParameterType, ParameterValue as RmwParameterValue, }; From 68ccd97eab3e3387908425d176b90ce7f8cfd45e Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 19 Mar 2024 13:53:14 +0800 Subject: [PATCH 14/24] Address first round of feedback Signed-off-by: Luca Della Vedova --- rclrs/src/parameter.rs | 4 +- rclrs/src/parameter/range.rs | 5 +- rclrs/src/parameter/service.rs | 380 +++++++++++---------- rclrs/src/parameter/value.rs | 10 +- rclrs_tests/src/parameter_service_tests.rs | 55 ++- 5 files changed, 236 insertions(+), 218 deletions(-) diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index 889d2683..b15a0494 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -504,7 +504,7 @@ impl ParameterMap { Some(entry) => { if let ParameterStorage::Declared(storage) = entry { if std::mem::discriminant(&storage.kind) - == std::mem::discriminant(&value.static_kind()) + == std::mem::discriminant(&value.kind()) || matches!(storage.kind, ParameterKind::Dynamic) { if !storage.options.ranges.in_range(&value) { @@ -538,7 +538,7 @@ impl ParameterMap { ParameterStorage::Declared(storage) => match &storage.value { DeclaredValue::Mandatory(p) => *p.write().unwrap() = value, DeclaredValue::Optional(p) => *p.write().unwrap() = Some(value), - DeclaredValue::ReadOnly(_) => {} + DeclaredValue::ReadOnly(_) => unreachable!(), }, ParameterStorage::Undeclared(param) => { *param = value; diff --git a/rclrs/src/parameter/range.rs b/rclrs/src/parameter/range.rs index 40c8ebb3..2ba2abd1 100644 --- a/rclrs/src/parameter/range.rs +++ b/rclrs/src/parameter/range.rs @@ -68,13 +68,12 @@ impl ParameterRanges { .float .as_ref() .map(|range| { - // TODO(luca) Double check whether we should use MIN/MAX or INFINITY/NEG_INFINITY if range.is_default() { Default::default() } else { seq![1 # FloatingPointRange { - from_value: range.lower.unwrap_or(f64::MIN), - to_value: range.upper.unwrap_or(f64::MAX), + from_value: range.lower.unwrap_or(f64::NEG_INFINITY), + to_value: range.upper.unwrap_or(f64::INFINITY), step: range.step.unwrap_or(0.0), }] } diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index f470ad7b..4c4d2da6 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -26,6 +26,203 @@ pub struct ParameterService { set_parameters_atomically_service: Arc>, } +fn describe_parameters( + req: DescribeParameters_Request, + map: &ParameterMap, +) -> DescribeParameters_Response { + let descriptors = req + .names + .into_iter() + .map(|name| { + let name = name.to_cstr().to_str().ok()?; + let storage = map.storage.get(name)?; + let mut descriptor = match storage { + ParameterStorage::Declared(storage) => { + let (integer_range, floating_point_range) = + storage.options.ranges.to_descriptor_ranges(); + ParameterDescriptor { + name: name.into(), + type_: Default::default(), + description: storage.options.description.clone().into(), + additional_constraints: storage.options.constraints.clone().into(), + dynamic_typing: matches!(storage.kind, ParameterKind::Dynamic), + read_only: matches!(storage.value, DeclaredValue::ReadOnly(_)), + floating_point_range, + integer_range, + } + } + ParameterStorage::Undeclared(_) => ParameterDescriptor { + name: name.into(), + dynamic_typing: true, + ..Default::default() + }, + }; + descriptor.type_ = storage.to_parameter_type(); + Some(descriptor) + }) + .collect::>() + .unwrap_or_default(); + // TODO(luca) error logging if the service call failed + DescribeParameters_Response { descriptors } +} + +fn get_parameter_types( + req: GetParameterTypes_Request, + map: &ParameterMap, +) -> GetParameterTypes_Response { + let types = req + .names + .into_iter() + .map(|name| { + let name = name.to_cstr().to_str().ok()?; + map.storage.get(name).map(|s| s.to_parameter_type()) + }) + .collect::>() + .unwrap_or_default(); + GetParameterTypes_Response { types } +} + +fn get_parameters(req: GetParameters_Request, map: &ParameterMap) -> GetParameters_Response { + let values = req + .names + .into_iter() + .map(|name| { + let name = name.to_cstr().to_str().ok()?; + match map.storage.get(name)? { + ParameterStorage::Declared(storage) => match &storage.value { + DeclaredValue::Mandatory(v) => Some(v.read().unwrap().clone().into()), + DeclaredValue::Optional(v) => Some( + v.read() + .unwrap() + .clone() + .map(|v| v.into()) + .unwrap_or_default(), + ), + DeclaredValue::ReadOnly(v) => Some(v.clone().into()), + }, + ParameterStorage::Undeclared(value) => Some(value.clone().into()), + } + }) + .collect::>() + .unwrap_or_default(); + GetParameters_Response { values } +} + +fn list_parameters(req: ListParameters_Request, map: &ParameterMap) -> ListParameters_Response { + let check_parameter_name_depth = |substring: &[i8]| { + if req.depth == ListParameters_Request::DEPTH_RECURSIVE { + return true; + } + u64::try_from(substring.iter().filter(|c| **c == ('.' as i8)).count()).unwrap() < req.depth + }; + let names: Sequence<_> = map + .storage + .keys() + .filter_map(|name| { + let name: rosidl_runtime_rs::String = name.clone().into(); + if req.prefixes.len() == 0 && check_parameter_name_depth(&name[..]) { + return Some(name); + } + req.prefixes + .iter() + .any(|prefix| { + if name == *prefix { + return true; + } + let mut prefix = prefix.clone(); + prefix.extend(".".chars()); + if name.len() > prefix.len() + && name.starts_with(&prefix) + && check_parameter_name_depth(&name[prefix.len()..]) + { + return true; + } + false + }) + .then_some(name) + }) + .collect(); + let prefixes: BTreeSet = names + .iter() + .filter_map(|name| { + let name = name.to_string(); + if let Some(pos) = name.rfind('.') { + return Some(name[0..pos].into()); + } + None + }) + .collect(); + ListParameters_Response { + result: ListParametersResult { + names, + prefixes: prefixes.into_iter().collect(), + }, + } +} + +fn set_parameters(req: SetParameters_Request, map: &mut ParameterMap) -> SetParameters_Response { + let results = req + .parameters + .into_iter() + .map(|param| { + let Ok(name) = param.name.to_cstr().to_str() else { + return SetParametersResult { + successful: false, + reason: "Failed parsing into UTF-8".into(), + }; + }; + match map.validate_parameter_setting(name, param.value) { + Ok(value) => { + map.store_parameter(name.into(), value); + SetParametersResult { + successful: true, + reason: Default::default(), + } + } + Err(e) => SetParametersResult { + successful: false, + reason: e.into(), + }, + } + }) + .collect(); + SetParameters_Response { results } +} + +fn set_parameters_atomically( + req: SetParametersAtomically_Request, + map: &mut ParameterMap, +) -> SetParametersAtomically_Response { + let results = req + .parameters + .into_iter() + .map(|param| { + let Ok(name) = param.name.to_cstr().to_str() else { + return Err("Failed parsing into UTF-8".into()); + }; + let value = map.validate_parameter_setting(name, param.value)?; + Ok((name.into(), value)) + }) + .collect::, _>>(); + // Check if there was any error and update parameters accordingly + let result = match results { + Ok(results) => { + for (name, value) in results.into_iter() { + map.store_parameter(name, value); + } + SetParametersResult { + successful: true, + reason: Default::default(), + } + } + Err(reason) => SetParametersResult { + successful: false, + reason, + }, + }; + SetParametersAtomically_Response { result } +} + impl ParameterService { pub(crate) fn new( node: &Node, @@ -39,44 +236,7 @@ impl ParameterService { &(fqn.clone() + "/describe_parameters"), move |_req_id: &rmw_request_id_t, req: DescribeParameters_Request| { let map = map.lock().unwrap(); - let descriptors = req - .names - .into_iter() - .map(|name| { - let name = name.to_cstr().to_str().ok()?; - let storage = map.storage.get(name)?; - let mut descriptor = match storage { - ParameterStorage::Declared(storage) => { - let (integer_range, floating_point_range) = - storage.options.ranges.to_descriptor_ranges(); - ParameterDescriptor { - name: name.into(), - type_: Default::default(), - description: storage.options.description.clone().into(), - additional_constraints: storage - .options - .constraints - .clone() - .into(), - dynamic_typing: matches!(storage.kind, ParameterKind::Dynamic), - read_only: matches!(storage.value, DeclaredValue::ReadOnly(_)), - floating_point_range, - integer_range, - } - } - ParameterStorage::Undeclared(_) => ParameterDescriptor { - name: name.into(), - dynamic_typing: true, - ..Default::default() - }, - }; - descriptor.type_ = storage.to_parameter_type(); - Some(descriptor) - }) - .collect::>() - .unwrap_or_default(); - // TODO(luca) error logging if the service call failed - DescribeParameters_Response { descriptors } + describe_parameters(req, &map) }, )?; let map = parameter_map.clone(); @@ -84,16 +244,7 @@ impl ParameterService { &(fqn.clone() + "/get_parameter_types"), move |_req_id: &rmw_request_id_t, req: GetParameterTypes_Request| { let map = map.lock().unwrap(); - let types = req - .names - .into_iter() - .map(|name| { - let name = name.to_cstr().to_str().ok()?; - map.storage.get(name).map(|s| s.to_parameter_type()) - }) - .collect::>() - .unwrap_or_default(); - GetParameterTypes_Response { types } + get_parameter_types(req, &map) }, )?; let map = parameter_map.clone(); @@ -101,88 +252,15 @@ impl ParameterService { &(fqn.clone() + "/get_parameters"), move |_req_id: &rmw_request_id_t, req: GetParameters_Request| { let map = map.lock().unwrap(); - let values = req - .names - .into_iter() - .map(|name| { - let name = name.to_cstr().to_str().ok()?; - match map.storage.get(name)? { - ParameterStorage::Declared(storage) => match &storage.value { - DeclaredValue::Mandatory(v) => { - Some(v.read().unwrap().clone().into()) - } - DeclaredValue::Optional(v) => Some( - v.read() - .unwrap() - .clone() - .map(|v| v.into()) - .unwrap_or_default(), - ), - DeclaredValue::ReadOnly(v) => Some(v.clone().into()), - }, - ParameterStorage::Undeclared(value) => Some(value.clone().into()), - } - }) - .collect::>() - .unwrap_or_default(); - GetParameters_Response { values } + get_parameters(req, &map) }, )?; let map = parameter_map.clone(); let list_parameters_service = node.create_service( &(fqn.clone() + "/list_parameters"), move |_req_id: &rmw_request_id_t, req: ListParameters_Request| { - let check_parameter_name_depth = |substring: &[i8]| { - if req.depth == ListParameters_Request::DEPTH_RECURSIVE { - return true; - } - u64::try_from(substring.iter().filter(|c| **c == ('.' as i8)).count()).unwrap() - < req.depth - }; let map = map.lock().unwrap(); - let names: Sequence<_> = map - .storage - .keys() - .filter_map(|name| { - let name: rosidl_runtime_rs::String = name.clone().into(); - if req.prefixes.len() == 0 && check_parameter_name_depth(&name[..]) { - return Some(name); - } - req.prefixes - .iter() - .any(|prefix| { - if name == *prefix { - return true; - } - let mut prefix = prefix.clone(); - prefix.extend(".".chars()); - if name.len() > prefix.len() - && name.starts_with(&prefix) - && check_parameter_name_depth(&name[prefix.len()..]) - { - return true; - } - false - }) - .then_some(name) - }) - .collect(); - let prefixes: BTreeSet = names - .iter() - .filter_map(|name| { - let name = name.to_string(); - if let Some(pos) = name.rfind('.') { - return Some(name[0..pos].into()); - } - None - }) - .collect(); - ListParameters_Response { - result: ListParametersResult { - names, - prefixes: prefixes.into_iter().collect(), - }, - } + list_parameters(req, &map) }, )?; let map = parameter_map.clone(); @@ -190,66 +268,14 @@ impl ParameterService { &(fqn.clone() + "/set_parameters"), move |_req_id: &rmw_request_id_t, req: SetParameters_Request| { let mut map = map.lock().unwrap(); - let results = req - .parameters - .into_iter() - .map(|param| { - let Ok(name) = param.name.to_cstr().to_str() else { - return SetParametersResult { - successful: false, - reason: "Failed parsing into UTF-8".into(), - }; - }; - match map.validate_parameter_setting(name, param.value) { - Ok(value) => { - map.store_parameter(name.into(), value); - SetParametersResult { - successful: true, - reason: Default::default(), - } - } - Err(e) => SetParametersResult { - successful: false, - reason: e.into(), - }, - } - }) - .collect(); - SetParameters_Response { results } + set_parameters(req, &mut map) }, )?; let set_parameters_atomically_service = node.create_service( &(fqn.clone() + "/set_parameters_atomically"), move |_req_id: &rmw_request_id_t, req: SetParametersAtomically_Request| { let mut map = parameter_map.lock().unwrap(); - let results = req - .parameters - .into_iter() - .map(|param| { - let Ok(name) = param.name.to_cstr().to_str() else { - return Err("Failed parsing into UTF-8".into()); - }; - let value = map.validate_parameter_setting(name, param.value)?; - Ok((name.into(), value)) - }) - .collect::, _>>(); - // Check if there was any error and update parameters accordingly - let result = match results { - Ok(results) => { - for (name, value) in results.into_iter() { - map.store_parameter(name, value); - } - SetParametersResult { - successful: true, - reason: Default::default(), - } - } - Err(reason) => SetParametersResult { - successful: false, - reason, - }, - }; - SetParametersAtomically_Response { result } + set_parameters_atomically(req, &mut map) }, )?; Ok(Self { diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index d7b88095..cd78f87e 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -369,11 +369,7 @@ impl From for RmwParameterValue { }, ParameterValue::StringArray(v) => RmwParameterValue { type_: ParameterType::PARAMETER_STRING_ARRAY, - string_array_value: v - .iter() - .map(|v| v.clone().into()) - .collect::>() - .into(), + string_array_value: v.iter().map(|v| v.clone().into()).collect(), ..Default::default() }, } @@ -513,8 +509,8 @@ impl ParameterValue { } } - /// Returns the `ParameterKind` for the parameter as if it was a static parameter. - pub(crate) fn static_kind(&self) -> ParameterKind { + /// Returns the `ParameterKind` for the parameter. + pub(crate) fn kind(&self) -> ParameterKind { match self { ParameterValue::Bool(_) => ParameterKind::Bool, ParameterValue::Integer(_) => ParameterKind::Integer, diff --git a/rclrs_tests/src/parameter_service_tests.rs b/rclrs_tests/src/parameter_service_tests.rs index 273f99d0..12390512 100644 --- a/rclrs_tests/src/parameter_service_tests.rs +++ b/rclrs_tests/src/parameter_service_tests.rs @@ -4,7 +4,7 @@ use rcl_interfaces::msg::rmw::{ use rcl_interfaces::srv::rmw::*; use rclrs::{ Context, MandatoryParameter, Node, NodeBuilder, ParameterRange, ParameterValue, RclrsError, - ReadOnlyParameter, TopicNamesAndTypes, + ReadOnlyParameter, }; use rosidl_runtime_rs::{seq, Sequence}; use std::sync::{Arc, RwLock}; @@ -87,37 +87,34 @@ fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { fn test_parameter_services_names_and_types() -> Result<(), RclrsError> { let context = Context::new([]).unwrap(); let (node, _client) = construct_test_nodes(&context, "names_types"); - let check_names_and_types = |names_and_types: TopicNamesAndTypes| { - let types = names_and_types - .get("/names_types/node/describe_parameters") - .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/DescribeParameters".to_string())); - let types = names_and_types - .get("/names_types/node/get_parameters") - .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/GetParameters".to_string())); - let types = names_and_types - .get("/names_types/node/set_parameters") - .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/SetParameters".to_string())); - let types = names_and_types - .get("/names_types/node/set_parameters_atomically") - .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/SetParametersAtomically".to_string())); - let types = names_and_types - .get("/names_types/node/list_parameters") - .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/ListParameters".to_string())); - let types = names_and_types - .get("/names_types/node/get_parameter_types") - .unwrap(); - assert!(types.contains(&"rcl_interfaces/srv/GetParameterTypes".to_string())); - }; std::thread::sleep(std::time::Duration::from_millis(100)); - let service_names_and_types = node.node.get_service_names_and_types()?; - check_names_and_types(service_names_and_types); + let names_and_types = node.node.get_service_names_and_types()?; + let types = names_and_types + .get("/names_types/node/describe_parameters") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/DescribeParameters".to_string())); + let types = names_and_types + .get("/names_types/node/get_parameters") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/GetParameters".to_string())); + let types = names_and_types + .get("/names_types/node/set_parameters") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/SetParameters".to_string())); + let types = names_and_types + .get("/names_types/node/set_parameters_atomically") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/SetParametersAtomically".to_string())); + let types = names_and_types + .get("/names_types/node/list_parameters") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/ListParameters".to_string())); + let types = names_and_types + .get("/names_types/node/get_parameter_types") + .unwrap(); + assert!(types.contains(&"rcl_interfaces/srv/GetParameterTypes".to_string())); Ok(()) } From 136047989bd259c46a5638ff41a91430b8f5835a Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 19 Mar 2024 15:04:47 +0800 Subject: [PATCH 15/24] Allow undeclared parameters in parameter getting services Signed-off-by: Luca Della Vedova --- rclrs/src/parameter/service.rs | 20 ++++++-- rclrs_tests/src/parameter_service_tests.rs | 53 ++++++++++++++++++++-- 2 files changed, 65 insertions(+), 8 deletions(-) diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 4c4d2da6..c96a3024 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -35,7 +35,12 @@ fn describe_parameters( .into_iter() .map(|name| { let name = name.to_cstr().to_str().ok()?; - let storage = map.storage.get(name)?; + let Some(storage) = map.storage.get(name) else { + return map.allow_undeclared.then(|| ParameterDescriptor { + name: name.into(), + ..Default::default() + }); + }; let mut descriptor = match storage { ParameterStorage::Declared(storage) => { let (integer_range, floating_point_range) = @@ -75,7 +80,13 @@ fn get_parameter_types( .into_iter() .map(|name| { let name = name.to_cstr().to_str().ok()?; - map.storage.get(name).map(|s| s.to_parameter_type()) + map.storage + .get(name) + .map(|s| s.to_parameter_type()) + .or_else(|| { + map.allow_undeclared + .then_some(ParameterType::PARAMETER_NOT_SET) + }) }) .collect::>() .unwrap_or_default(); @@ -88,7 +99,10 @@ fn get_parameters(req: GetParameters_Request, map: &ParameterMap) -> GetParamete .into_iter() .map(|name| { let name = name.to_cstr().to_str().ok()?; - match map.storage.get(name)? { + let Some(storage) = map.storage.get(name) else { + return map.allow_undeclared.then(|| ParameterValue::default()); + }; + match storage { ParameterStorage::Declared(storage) => match &storage.value { DeclaredValue::Mandatory(v) => Some(v.read().unwrap().clone().into()), DeclaredValue::Optional(v) => Some( diff --git a/rclrs_tests/src/parameter_service_tests.rs b/rclrs_tests/src/parameter_service_tests.rs index 12390512..0e7ae575 100644 --- a/rclrs_tests/src/parameter_service_tests.rs +++ b/rclrs_tests/src/parameter_service_tests.rs @@ -430,6 +430,23 @@ async fn test_get_set_parameters_service() -> Result<(), RclrsError> { .await .unwrap(); + // Now getting an undefined parameter should work + let request = GetParameters_Request { + names: seq!["non_existing".into()], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + get_client + .async_send_request_with_callback(&request, move |response: GetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.values.len(), 1); + assert_eq!(response.values[0].type_, ParameterType::PARAMETER_NOT_SET); + }) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + // With set_parameters_atomically, if one fails all should fail let request = SetParametersAtomically_Request { parameters: seq![bool_parameter, read_only_parameter], @@ -475,9 +492,10 @@ async fn test_describe_get_types_parameters_service() -> Result<(), RclrsError> let done = Arc::new(RwLock::new(false)); let inner_done = done.clone(); + let inner_node = node.node.clone(); let rclrs_spin = tokio::task::spawn(async move { try_until_timeout(|| { - rclrs::spin_once(node.node.clone(), Some(std::time::Duration::ZERO)).ok(); + rclrs::spin_once(inner_node.clone(), Some(std::time::Duration::ZERO)).ok(); rclrs::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); *inner_done.read().unwrap() }) @@ -586,10 +604,10 @@ async fn test_describe_get_types_parameters_service() -> Result<(), RclrsError> .await .unwrap(); - // If a get parameter types request is sent with a non existing parameter, an empty - // response should be returned + // Once undeclared parameters are allowed, non existing should return a not set type + node.node.use_undeclared_parameters(); let request = GetParameterTypes_Request { - names: seq!["bool".into(), "non_existing".into()], + names: seq!["non_existing".into()], }; let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); @@ -598,7 +616,32 @@ async fn test_describe_get_types_parameters_service() -> Result<(), RclrsError> &request, move |response: GetParameterTypes_Response| { *call_done.write().unwrap() = true; - assert_eq!(response.types.len(), 0); + assert_eq!(response.types.len(), 1); + assert_eq!(response.types[0], ParameterType::PARAMETER_NOT_SET); + }, + ) + .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); + + // Once undeclared parameters are allowed, non existing should return a not set descriptor + let request = DescribeParameters_Request { + names: seq!["non_existing".into()], + }; + let client_finished = Arc::new(RwLock::new(false)); + let call_done = client_finished.clone(); + describe_client + .async_send_request_with_callback( + &request, + move |response: DescribeParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.descriptors.len(), 1); + assert_eq!(response.descriptors[0].name.to_string(), "non_existing"); + assert_eq!( + response.descriptors[0].type_, + ParameterType::PARAMETER_NOT_SET + ); }, ) .unwrap(); From eb354a6e8f255e94cbcdb89289e3ace5d04c94d2 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 19 Mar 2024 15:18:19 +0800 Subject: [PATCH 16/24] Clippy Signed-off-by: Luca Della Vedova --- rclrs/src/parameter/service.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index c96a3024..af836a91 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -100,7 +100,7 @@ fn get_parameters(req: GetParameters_Request, map: &ParameterMap) -> GetParamete .map(|name| { let name = name.to_cstr().to_str().ok()?; let Some(storage) = map.storage.get(name) else { - return map.allow_undeclared.then(|| ParameterValue::default()); + return map.allow_undeclared.then(ParameterValue::default); }; match storage { ParameterStorage::Declared(storage) => match &storage.value { From fbfdf2e6c895ab236f5bb5ff649f89763aae3db3 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 8 Apr 2024 08:56:32 +0000 Subject: [PATCH 17/24] Run rustfmt Signed-off-by: Michael X. Grey --- rclrs/src/parameter.rs | 2 +- rclrs/src/parameter/service.rs | 202 +++++++++++++++++++-------------- 2 files changed, 116 insertions(+), 88 deletions(-) diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index 67ae88fc..d3918fab 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -11,7 +11,7 @@ pub use value::*; use crate::vendor::rcl_interfaces::msg::rmw::{ParameterType, ParameterValue as RmwParameterValue}; use crate::rcl_bindings::*; -use crate::{Node, call_string_getter_with_rcl_node, RclrsError}; +use crate::{call_string_getter_with_rcl_node, Node, RclrsError}; use std::collections::{btree_map::Entry, BTreeMap}; use std::fmt::Debug; use std::marker::PhantomData; diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 34d8ec0b..51666628 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -457,21 +457,24 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); list_client - .async_send_request_with_callback(&request, move |response: ListParameters_Response| { - // use_sim_time + all the manually defined ones - *call_done.write().unwrap() = true; - let names = response.result.names; - assert_eq!(names.len(), 5); - // Parameter names are returned in alphabetical order - assert_eq!(names[0].to_string(), "bool"); - assert_eq!(names[1].to_string(), "dynamic"); - assert_eq!(names[2].to_string(), "ns1.ns2.ns3.int"); - assert_eq!(names[3].to_string(), "read_only"); - assert_eq!(names[4].to_string(), "use_sim_time"); - // Only one prefix - assert_eq!(response.result.prefixes.len(), 1); - assert_eq!(response.result.prefixes[0].to_string(), "ns1.ns2.ns3"); - }) + .async_send_request_with_callback( + &request, + move |response: ListParameters_Response| { + // use_sim_time + all the manually defined ones + *call_done.write().unwrap() = true; + let names = response.result.names; + assert_eq!(names.len(), 5); + // Parameter names are returned in alphabetical order + assert_eq!(names[0].to_string(), "bool"); + assert_eq!(names[1].to_string(), "dynamic"); + assert_eq!(names[2].to_string(), "ns1.ns2.ns3.int"); + assert_eq!(names[3].to_string(), "read_only"); + assert_eq!(names[4].to_string(), "use_sim_time"); + // Only one prefix + assert_eq!(response.result.prefixes.len(), 1); + assert_eq!(response.result.prefixes[0].to_string(), "ns1.ns2.ns3"); + }, + ) .unwrap(); try_until_timeout(|| *client_finished.read().unwrap()) .await @@ -485,13 +488,16 @@ mod tests { let call_done = client_finished.clone(); *call_done.write().unwrap() = false; list_client - .async_send_request_with_callback(&request, move |response: ListParameters_Response| { - *call_done.write().unwrap() = true; - let names = response.result.names; - assert_eq!(names.len(), 4); - assert!(names.iter().all(|n| n.to_string() != "ns1.ns2.ns3.int")); - assert_eq!(response.result.prefixes.len(), 0); - }) + .async_send_request_with_callback( + &request, + move |response: ListParameters_Response| { + *call_done.write().unwrap() = true; + let names = response.result.names; + assert_eq!(names.len(), 4); + assert!(names.iter().all(|n| n.to_string() != "ns1.ns2.ns3.int")); + assert_eq!(response.result.prefixes.len(), 0); + }, + ) .unwrap(); try_until_timeout(|| *client_finished.read().unwrap()) .await @@ -505,14 +511,17 @@ mod tests { let call_done = client_finished.clone(); *call_done.write().unwrap() = false; list_client - .async_send_request_with_callback(&request, move |response: ListParameters_Response| { - *call_done.write().unwrap() = true; - let names = response.result.names; - assert_eq!(names.len(), 1); - assert_eq!(names[0].to_string(), "ns1.ns2.ns3.int"); - assert_eq!(response.result.prefixes.len(), 1); - assert_eq!(response.result.prefixes[0].to_string(), "ns1.ns2.ns3"); - }) + .async_send_request_with_callback( + &request, + move |response: ListParameters_Response| { + *call_done.write().unwrap() = true; + let names = response.result.names; + assert_eq!(names.len(), 1); + assert_eq!(names[0].to_string(), "ns1.ns2.ns3.int"); + assert_eq!(response.result.prefixes.len(), 1); + assert_eq!(response.result.prefixes[0].to_string(), "ns1.ns2.ns3"); + }, + ) .unwrap(); try_until_timeout(|| *client_finished.read().unwrap()) .await @@ -526,15 +535,18 @@ mod tests { let call_done = client_finished.clone(); *call_done.write().unwrap() = false; list_client - .async_send_request_with_callback(&request, move |response: ListParameters_Response| { - *call_done.write().unwrap() = true; - let names = response.result.names; - dbg!(&names); - assert_eq!(names.len(), 2); - assert_eq!(names[0].to_string(), "bool"); - assert_eq!(names[1].to_string(), "use_sim_time"); - assert_eq!(response.result.prefixes.len(), 0); - }) + .async_send_request_with_callback( + &request, + move |response: ListParameters_Response| { + *call_done.write().unwrap() = true; + let names = response.result.names; + dbg!(&names); + assert_eq!(names.len(), 2); + assert_eq!(names[0].to_string(), "bool"); + assert_eq!(names[1].to_string(), "use_sim_time"); + assert_eq!(response.result.prefixes.len(), 0); + }, + ) .unwrap(); try_until_timeout(|| *client_finished.read().unwrap()) .await @@ -587,13 +599,16 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); get_client - .async_send_request_with_callback(&request, move |response: GetParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.values.len(), 1); - let param = &response.values[0]; - assert_eq!(param.type_, ParameterType::PARAMETER_BOOL); - assert!(param.bool_value); - }) + .async_send_request_with_callback( + &request, + move |response: GetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.values.len(), 1); + let param = &response.values[0]; + assert_eq!(param.type_, ParameterType::PARAMETER_BOOL); + assert!(param.bool_value); + }, + ) .unwrap(); try_until_timeout(|| *client_finished.read().unwrap()) .await @@ -606,10 +621,13 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); get_client - .async_send_request_with_callback(&request, move |response: GetParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.values.len(), 0); - }) + .async_send_request_with_callback( + &request, + move |response: GetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.values.len(), 0); + }, + ) .unwrap(); try_until_timeout(|| *client_finished.read().unwrap()) .await @@ -688,27 +706,30 @@ mod tests { // Parameter is assigned a default of true at declaration time assert!(node.bool_param.get()); set_client - .async_send_request_with_callback(&request, move |response: SetParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.results.len(), 7); - // Setting a bool value set for a bool parameter - assert!(response.results[0].successful); - // Value was set to false, node parameter get should reflect this - assert!(!node.bool_param.get()); - // Setting a parameter to the wrong type - assert!(!response.results[1].successful); - // Setting a read only parameter - assert!(!response.results[2].successful); - // Setting a dynamic parameter to a new type - assert!(response.results[3].successful); - assert_eq!(node.dynamic_param.get(), ParameterValue::Bool(true)); - // Setting a value out of range - assert!(!response.results[4].successful); - // Setting an invalid type - assert!(!response.results[5].successful); - // Setting an undeclared parameter, without allowing undeclared parameters - assert!(!response.results[6].successful); - }) + .async_send_request_with_callback( + &request, + move |response: SetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.results.len(), 7); + // Setting a bool value set for a bool parameter + assert!(response.results[0].successful); + // Value was set to false, node parameter get should reflect this + assert!(!node.bool_param.get()); + // Setting a parameter to the wrong type + assert!(!response.results[1].successful); + // Setting a read only parameter + assert!(!response.results[2].successful); + // Setting a dynamic parameter to a new type + assert!(response.results[3].successful); + assert_eq!(node.dynamic_param.get(), ParameterValue::Bool(true)); + // Setting a value out of range + assert!(!response.results[4].successful); + // Setting an invalid type + assert!(!response.results[5].successful); + // Setting an undeclared parameter, without allowing undeclared parameters + assert!(!response.results[6].successful); + }, + ) .unwrap(); try_until_timeout(|| *client_finished.read().unwrap()) .await @@ -722,16 +743,19 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); set_client - .async_send_request_with_callback(&request, move |response: SetParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.results.len(), 1); - // Setting the undeclared parameter is now allowed - assert!(response.results[0].successful); - assert_eq!( - node.node.use_undeclared_parameters().get("undeclared_bool"), - Some(ParameterValue::Bool(true)) - ); - }) + .async_send_request_with_callback( + &request, + move |response: SetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.results.len(), 1); + // Setting the undeclared parameter is now allowed + assert!(response.results[0].successful); + assert_eq!( + node.node.use_undeclared_parameters().get("undeclared_bool"), + Some(ParameterValue::Bool(true)) + ); + }, + ) .unwrap(); try_until_timeout(|| *client_finished.read().unwrap()) .await @@ -744,11 +768,14 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); get_client - .async_send_request_with_callback(&request, move |response: GetParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.values.len(), 1); - assert_eq!(response.values[0].type_, ParameterType::PARAMETER_NOT_SET); - }) + .async_send_request_with_callback( + &request, + move |response: GetParameters_Response| { + *call_done.write().unwrap() = true; + assert_eq!(response.values.len(), 1); + assert_eq!(response.values[0].type_, ParameterType::PARAMETER_NOT_SET); + }, + ) .unwrap(); try_until_timeout(|| *client_finished.read().unwrap()) .await @@ -791,7 +818,8 @@ mod tests { client.create_client::("/describe/node/get_parameter_types")?; try_until_timeout(|| { - describe_client.service_is_ready().unwrap() && get_types_client.service_is_ready().unwrap() + describe_client.service_is_ready().unwrap() + && get_types_client.service_is_ready().unwrap() }) .await .unwrap(); From cdf3854e508656ba58e57fbf43d88225f3872fe4 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 12 Apr 2024 17:22:02 +0800 Subject: [PATCH 18/24] Update rclrs/src/parameter/service.rs Co-authored-by: jhdcs <48914066+jhdcs@users.noreply.github.com> --- rclrs/src/parameter/service.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 51666628..9715e1a2 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -839,7 +839,7 @@ mod tests { }); let res = tokio::task::spawn(async move { - // Desctibe all parameters + // Describe all parameters let request = DescribeParameters_Request { names: seq![ "bool".into(), From 3274f42ce33ae094bd4b2020a03f72dec75d716b Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 23 Apr 2024 11:43:37 +0800 Subject: [PATCH 19/24] Change behavior to return NOT_SET for non existing parameters Signed-off-by: Luca Della Vedova --- rclrs/src/parameter/service.rs | 99 ++++++++-------------------------- 1 file changed, 23 insertions(+), 76 deletions(-) diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 9715e1a2..5b278024 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -36,7 +36,7 @@ fn describe_parameters( .map(|name| { let name = name.to_cstr().to_str().ok()?; let Some(storage) = map.storage.get(name) else { - return map.allow_undeclared.then(|| ParameterDescriptor { + return Some(ParameterDescriptor { name: name.into(), ..Default::default() }); @@ -83,10 +83,7 @@ fn get_parameter_types( map.storage .get(name) .map(|s| s.to_parameter_type()) - .or_else(|| { - map.allow_undeclared - .then_some(ParameterType::PARAMETER_NOT_SET) - }) + .or(Some(ParameterType::PARAMETER_NOT_SET)) }) .collect::>() .unwrap_or_default(); @@ -100,7 +97,7 @@ fn get_parameters(req: GetParameters_Request, map: &ParameterMap) -> GetParamete .map(|name| { let name = name.to_cstr().to_str().ok()?; let Some(storage) = map.storage.get(name) else { - return map.allow_undeclared.then(ParameterValue::default); + return Some(ParameterValue::default()); }; match storage { ParameterStorage::Declared(storage) => match &storage.value { @@ -614,7 +611,8 @@ mod tests { .await .unwrap(); - // Getting both existing and non existing parameters should fail + // Getting both existing and non existing parameters, missing one should return + // PARAMETER_NOT_SET let request = GetParameters_Request { names: seq!["bool".into(), "non_existing".into()], }; @@ -625,7 +623,11 @@ mod tests { &request, move |response: GetParameters_Response| { *call_done.write().unwrap() = true; - assert_eq!(response.values.len(), 0); + assert_eq!(response.values.len(), 2); + let param = &response.values[0]; + assert_eq!(param.type_, ParameterType::PARAMETER_BOOL); + assert!(param.bool_value); + assert_eq!(response.values[1].type_, ParameterType::PARAMETER_NOT_SET); }, ) .unwrap(); @@ -761,26 +763,6 @@ mod tests { .await .unwrap(); - // Now getting an undefined parameter should work - let request = GetParameters_Request { - names: seq!["non_existing".into()], - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - get_client - .async_send_request_with_callback( - &request, - move |response: GetParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.values.len(), 1); - assert_eq!(response.values[0].type_, ParameterType::PARAMETER_NOT_SET); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - // With set_parameters_atomically, if one fails all should fail let request = SetParametersAtomically_Request { parameters: seq![bool_parameter, read_only_parameter], @@ -902,7 +884,14 @@ mod tests { &request, move |response: DescribeParameters_Response| { *call_done.write().unwrap() = true; - assert_eq!(response.descriptors.len(), 0); + assert_eq!(response.descriptors[0].name.to_string(), "bool"); + assert_eq!(response.descriptors[0].type_, ParameterType::PARAMETER_BOOL); + assert_eq!(response.descriptors.len(), 2); + assert_eq!(response.descriptors[1].name.to_string(), "non_existing"); + assert_eq!( + response.descriptors[1].type_, + ParameterType::PARAMETER_NOT_SET + ); }, ) .unwrap(); @@ -910,13 +899,14 @@ mod tests { .await .unwrap(); - // Get all parameter types + // Get all parameter types, including a non existing one that will be NOT_SET let request = GetParameterTypes_Request { names: seq![ "bool".into(), "ns1.ns2.ns3.int".into(), "read_only".into(), - "dynamic".into() + "dynamic".into(), + "non_existing".into() ], }; let client_finished = Arc::new(RwLock::new(false)); @@ -926,12 +916,13 @@ mod tests { &request, move |response: GetParameterTypes_Response| { *call_done.write().unwrap() = true; - assert_eq!(response.types.len(), 4); + assert_eq!(response.types.len(), 5); // Types are returned in the requested order assert_eq!(response.types[0], ParameterType::PARAMETER_BOOL); assert_eq!(response.types[1], ParameterType::PARAMETER_INTEGER); assert_eq!(response.types[2], ParameterType::PARAMETER_DOUBLE); assert_eq!(response.types[3], ParameterType::PARAMETER_STRING); + assert_eq!(response.types[4], ParameterType::PARAMETER_NOT_SET); }, ) .unwrap(); @@ -939,50 +930,6 @@ mod tests { .await .unwrap(); - // Once undeclared parameters are allowed, non existing should return a not set type - node.node.use_undeclared_parameters(); - let request = GetParameterTypes_Request { - names: seq!["non_existing".into()], - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - get_types_client - .async_send_request_with_callback( - &request, - move |response: GetParameterTypes_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.types.len(), 1); - assert_eq!(response.types[0], ParameterType::PARAMETER_NOT_SET); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); - - // Once undeclared parameters are allowed, non existing should return a not set descriptor - let request = DescribeParameters_Request { - names: seq!["non_existing".into()], - }; - let client_finished = Arc::new(RwLock::new(false)); - let call_done = client_finished.clone(); - describe_client - .async_send_request_with_callback( - &request, - move |response: DescribeParameters_Response| { - *call_done.write().unwrap() = true; - assert_eq!(response.descriptors.len(), 1); - assert_eq!(response.descriptors[0].name.to_string(), "non_existing"); - assert_eq!( - response.descriptors[0].type_, - ParameterType::PARAMETER_NOT_SET - ); - }, - ) - .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) - .await - .unwrap(); *done.write().unwrap() = true; }); From 00c7951778b4feddbe8329afd07dfbbaebcecbd8 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 23 Apr 2024 11:47:54 +0800 Subject: [PATCH 20/24] Make use_sim_time parameter read only Signed-off-by: Luca Della Vedova --- rclrs/src/time_source.rs | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index 143adf09..dcc7f3c2 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, - MandatoryParameter, Node, QoSProfile, Subscription, QOS_PROFILE_CLOCK, + ReadOnlyParameter, Node, QoSProfile, Subscription, QOS_PROFILE_CLOCK, }; use std::sync::{Arc, Mutex, RwLock, Weak}; @@ -16,7 +16,9 @@ pub(crate) struct TimeSource { clock_qos: QoSProfile, clock_subscription: Mutex>>>, last_received_time: Arc>>, - use_sim_time: Mutex>>, + // TODO(luca) Make this parameter editable when we have parameter callbacks implemented and can + // safely change clock type at runtime + use_sim_time: Mutex>>, } /// A builder for creating a [`TimeSource`][1]. @@ -84,12 +86,12 @@ impl TimeSource { /// Attaches the given node to to the `TimeSource`, using its interface to read the /// `use_sim_time` parameter and create the clock subscription. pub(crate) fn attach_node(&self, node: &Arc) { - // TODO(luca) register a parameter callback that calls set_ros_time(bool) once parameter - // callbacks are implemented. + // TODO(luca) Make this parameter editable and register a parameter callback + // that calls set_ros_time(bool) once parameter callbacks are implemented. let param = node .declare_parameter("use_sim_time") .default(false) - .mandatory() + .read_only() .unwrap(); *self.node.lock().unwrap() = Arc::downgrade(node); self.set_ros_time_enable(param.get()); From e4dd1760f75f05f56aa2f47a3610ae8c3caa141d Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 23 Apr 2024 11:54:54 +0800 Subject: [PATCH 21/24] Format Signed-off-by: Luca Della Vedova --- rclrs/src/time_source.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index dcc7f3c2..a6b60080 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, - ReadOnlyParameter, Node, QoSProfile, Subscription, QOS_PROFILE_CLOCK, + Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, }; use std::sync::{Arc, Mutex, RwLock, Weak}; From 5da50a2b4f533225ccb9c5273fcce28ff2c7c3c3 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 23 Apr 2024 12:01:08 +0800 Subject: [PATCH 22/24] Add a comment to denote why unwrap is safe Signed-off-by: Luca Della Vedova --- rclrs/src/parameter.rs | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index d3918fab..6921ed6f 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -469,6 +469,9 @@ impl ParameterStorage { ParameterKind::DoubleArray => ParameterType::PARAMETER_DOUBLE_ARRAY, ParameterKind::StringArray => ParameterType::PARAMETER_STRING_ARRAY, ParameterKind::Dynamic => match &s.value { + // Unwraps here are safe because None will only be returned if the RwLock is + // poisoned, but it is only written in internal set(value) calls that have no + // way to panic. DeclaredValue::Mandatory(v) => v.read().unwrap().rcl_parameter_type(), DeclaredValue::Optional(v) => v .read() From d73d07c597141f2ac7f4198e920e21b8a7fa1eaf Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 29 Apr 2024 16:32:00 +0800 Subject: [PATCH 23/24] Use main fmt Signed-off-by: Luca Della Vedova --- rclrs/src/parameter.rs | 13 +++++++------ rclrs/src/parameter/range.rs | 6 ++++-- rclrs/src/parameter/service.rs | 25 +++++++++++++++---------- rclrs/src/parameter/value.rs | 5 ++--- 4 files changed, 28 insertions(+), 21 deletions(-) diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index 6921ed6f..c8a710ee 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -10,12 +10,13 @@ pub use value::*; use crate::vendor::rcl_interfaces::msg::rmw::{ParameterType, ParameterValue as RmwParameterValue}; -use crate::rcl_bindings::*; -use crate::{call_string_getter_with_rcl_node, Node, RclrsError}; -use std::collections::{btree_map::Entry, BTreeMap}; -use std::fmt::Debug; -use std::marker::PhantomData; -use std::sync::{Arc, Mutex, RwLock, Weak}; +use crate::{call_string_getter_with_rcl_node, rcl_bindings::*, Node, RclrsError}; +use std::{ + collections::{btree_map::Entry, BTreeMap}, + fmt::Debug, + marker::PhantomData, + sync::{Arc, Mutex, RwLock, Weak}, +}; // This module implements the core logic of parameters in rclrs. // The implementation is fairly different from the existing ROS 2 client libraries. A detailed diff --git a/rclrs/src/parameter/range.rs b/rclrs/src/parameter/range.rs index 2ba2abd1..96f66d6e 100644 --- a/rclrs/src/parameter/range.rs +++ b/rclrs/src/parameter/range.rs @@ -1,5 +1,7 @@ -use crate::vendor::rcl_interfaces::msg::rmw::{FloatingPointRange, IntegerRange}; -use crate::{DeclarationError, ParameterValue, ParameterVariant}; +use crate::{ + vendor::rcl_interfaces::msg::rmw::{FloatingPointRange, IntegerRange}, + DeclarationError, ParameterValue, ParameterVariant, +}; use rosidl_runtime_rs::{seq, BoundedSequence}; impl From> for ParameterRanges { diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 5b278024..a52e3e55 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -1,13 +1,16 @@ -use std::collections::BTreeSet; -use std::sync::{Arc, Mutex}; +use std::{ + collections::BTreeSet, + sync::{Arc, Mutex}, +}; -use crate::vendor::rcl_interfaces::msg::rmw::*; -use crate::vendor::rcl_interfaces::srv::rmw::*; +use crate::vendor::rcl_interfaces::{msg::rmw::*, srv::rmw::*}; use rosidl_runtime_rs::Sequence; use super::ParameterMap; -use crate::parameter::{DeclaredValue, ParameterKind, ParameterStorage}; -use crate::{rmw_request_id_t, Node, RclrsError, Service}; +use crate::{ + parameter::{DeclaredValue, ParameterKind, ParameterStorage}, + rmw_request_id_t, Node, RclrsError, Service, +}; // The variables only exist to keep a strong reference to the services and are technically unused. // What is used is the Weak that is stored in the node, and is upgraded when spinning. @@ -302,11 +305,13 @@ impl ParameterService { #[cfg(test)] mod tests { - use crate::vendor::rcl_interfaces::msg::rmw::{ - Parameter as RmwParameter, ParameterType, ParameterValue as RmwParameterValue, - }; - use crate::vendor::rcl_interfaces::srv::rmw::*; use crate::{ + vendor::rcl_interfaces::{ + msg::rmw::{ + Parameter as RmwParameter, ParameterType, ParameterValue as RmwParameterValue, + }, + srv::rmw::*, + }, Context, MandatoryParameter, Node, NodeBuilder, ParameterRange, ParameterValue, RclrsError, ReadOnlyParameter, }; diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index 48000be4..9d431c03 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -1,10 +1,9 @@ use std::{ffi::CStr, sync::Arc}; -use crate::rcl_bindings::*; -use crate::vendor::rcl_interfaces::msg::rmw::ParameterType; -use crate::vendor::rcl_interfaces::msg::rmw::ParameterValue as RmwParameterValue; use crate::{ parameter::{ParameterRange, ParameterRanges}, + rcl_bindings::*, + vendor::rcl_interfaces::msg::rmw::{ParameterType, ParameterValue as RmwParameterValue}, ParameterValueError, }; From f2cc667b76c18bc0c4fbda28eba42ee079dcc543 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 29 Apr 2024 16:49:02 +0800 Subject: [PATCH 24/24] Add a builder parameter to start parameter services Signed-off-by: Luca Della Vedova --- rclrs/src/node/builder.rs | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 26e64a9d..011d5d2f 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -18,6 +18,7 @@ use crate::{ /// - `use_global_arguments: true` /// - `arguments: []` /// - `enable_rosout: true` +/// - `start_parameter_services: true` /// - `clock_type: ClockType::RosTime` /// - `clock_qos: QOS_PROFILE_CLOCK` /// @@ -49,6 +50,7 @@ pub struct NodeBuilder { use_global_arguments: bool, arguments: Vec, enable_rosout: bool, + start_parameter_services: bool, clock_type: ClockType, clock_qos: QoSProfile, } @@ -97,6 +99,7 @@ impl NodeBuilder { use_global_arguments: true, arguments: vec![], enable_rosout: true, + start_parameter_services: true, clock_type: ClockType::RosTime, clock_qos: QOS_PROFILE_CLOCK, } @@ -231,6 +234,15 @@ impl NodeBuilder { self } + /// Enables or disables parameter services. + /// + /// Parameter services can be used to allow external nodes to list, get and set + /// parameters for this node. + pub fn start_parameter_services(mut self, start: bool) -> Self { + self.start_parameter_services = start; + self + } + /// Sets the node's clock type. pub fn clock_type(mut self, clock_type: ClockType) -> Self { self.clock_type = clock_type; @@ -308,7 +320,9 @@ impl NodeBuilder { parameter, }); node.time_source.attach_node(&node); - node.parameter.create_services(&node)?; + if self.start_parameter_services { + node.parameter.create_services(&node)?; + } Ok(node) }