From a7d1befbe078418fca4852b200e3e1b6c4a62f9f Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 19 Mar 2024 13:53:14 +0800 Subject: [PATCH] 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 | 53 ++- 5 files changed, 235 insertions(+), 217 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..b6951e65 100644 --- a/rclrs_tests/src/parameter_service_tests.rs +++ b/rclrs_tests/src/parameter_service_tests.rs @@ -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(()) }