|
| 1 | +use crate::rcl_bindings::*; |
| 2 | +use crate::{ParameterValue, RclrsError, ToResult}; |
| 3 | +use libc::c_char; |
| 4 | +use std::collections::BTreeMap; |
| 5 | +use std::ffi::CStr; |
| 6 | + |
| 7 | +// Internal helper struct, iterator for rcl_params_t |
| 8 | +struct RclParamsIter<'a> { |
| 9 | + node_name_ptrs: &'a [*mut c_char], |
| 10 | + rcl_node_params: &'a [rcl_node_params_t], |
| 11 | +} |
| 12 | + |
| 13 | +// Internal helper struct, iterator for rcl_node_params_t |
| 14 | +struct RclNodeParamsIter<'a> { |
| 15 | + param_name_ptrs: &'a [*mut c_char], |
| 16 | + rcl_variants: &'a [rcl_variant_t], |
| 17 | +} |
| 18 | + |
| 19 | +impl<'a> Iterator for RclParamsIter<'a> { |
| 20 | + type Item = (String, &'a rcl_node_params_t); |
| 21 | + fn next(&mut self) -> Option<Self::Item> { |
| 22 | + let (next_node_param, rest) = self.rcl_node_params.split_first()?; |
| 23 | + self.rcl_node_params = rest; |
| 24 | + let (next_node_name_ptr, rest) = self.node_name_ptrs.split_first()?; |
| 25 | + self.node_name_ptrs = rest; |
| 26 | + debug_assert!(!next_node_name_ptr.is_null()); |
| 27 | + // SAFETY: Pointer can be assumed to be valid. No lifetime issues due to immediate |
| 28 | + // conversion to owned string. |
| 29 | + let mut next_node_name = unsafe { |
| 30 | + CStr::from_ptr(*next_node_name_ptr) |
| 31 | + .to_string_lossy() |
| 32 | + .into_owned() |
| 33 | + }; |
| 34 | + if !next_node_name.starts_with('/') { |
| 35 | + next_node_name.insert(0, '/'); |
| 36 | + } |
| 37 | + Some((next_node_name, next_node_param)) |
| 38 | + } |
| 39 | +} |
| 40 | + |
| 41 | +impl RclParamsIter<'_> { |
| 42 | + // This function is unsafe since the rcl_params argument might contain incorrect array sizes or |
| 43 | + // dangling pointers. |
| 44 | + pub unsafe fn new(rcl_params: *const rcl_params_t) -> Self { |
| 45 | + // The params will be null if none were given on the command line. |
| 46 | + // So, semantically, this can be treated like a struct with no parameters. |
| 47 | + if rcl_params.is_null() { |
| 48 | + Self { |
| 49 | + node_name_ptrs: &[], |
| 50 | + rcl_node_params: &[], |
| 51 | + } |
| 52 | + } else { |
| 53 | + let node_name_ptrs = |
| 54 | + std::slice::from_raw_parts((*rcl_params).node_names, (*rcl_params).num_nodes); |
| 55 | + let rcl_node_params = |
| 56 | + std::slice::from_raw_parts((*rcl_params).params, (*rcl_params).num_nodes); |
| 57 | + Self { |
| 58 | + node_name_ptrs, |
| 59 | + rcl_node_params, |
| 60 | + } |
| 61 | + } |
| 62 | + } |
| 63 | +} |
| 64 | + |
| 65 | +impl<'a> Iterator for RclNodeParamsIter<'a> { |
| 66 | + type Item = (String, &'a rcl_variant_t); |
| 67 | + fn next(&mut self) -> Option<Self::Item> { |
| 68 | + let (next_param_name_ptr, rest) = self.param_name_ptrs.split_first()?; |
| 69 | + self.param_name_ptrs = rest; |
| 70 | + let (next_rcl_variant, rest) = self.rcl_variants.split_first()?; |
| 71 | + self.rcl_variants = rest; |
| 72 | + // SAFETY: Pointer can be assumed to be valid. No lifetime issues due to immediate |
| 73 | + // conversion to owned string. |
| 74 | + let next_param_name = unsafe { |
| 75 | + CStr::from_ptr(*next_param_name_ptr) |
| 76 | + .to_string_lossy() |
| 77 | + .into_owned() |
| 78 | + }; |
| 79 | + Some((next_param_name, next_rcl_variant)) |
| 80 | + } |
| 81 | +} |
| 82 | + |
| 83 | +impl<'a> RclNodeParamsIter<'a> { |
| 84 | + // This function is unsafe since the rcl_node_params argument might contain incorrect array |
| 85 | + // sizes or dangling pointers. |
| 86 | + pub unsafe fn new(rcl_node_params: &'a rcl_node_params_t) -> Self { |
| 87 | + let param_name_ptrs = |
| 88 | + std::slice::from_raw_parts(rcl_node_params.parameter_names, rcl_node_params.num_params); |
| 89 | + let rcl_variants = std::slice::from_raw_parts( |
| 90 | + rcl_node_params.parameter_values, |
| 91 | + rcl_node_params.num_params, |
| 92 | + ); |
| 93 | + Self { |
| 94 | + param_name_ptrs, |
| 95 | + rcl_variants, |
| 96 | + } |
| 97 | + } |
| 98 | +} |
| 99 | + |
| 100 | +/// A map of parameters for this node. |
| 101 | +/// The key for this map is the parameter name. |
| 102 | +/// A parameter override "overrides" the default value set from within the node, hence the name. |
| 103 | +pub(crate) type ParameterOverrideMap = BTreeMap<String, ParameterValue>; |
| 104 | + |
| 105 | +/// Similar to `rclcpp::detail::resolve_parameter_overrides`, but returns a map instead of |
| 106 | +/// a vector. |
| 107 | +/// |
| 108 | +/// This function is unsafe since the rcl_global_arguments argument might contain incorrect array |
| 109 | +/// sizes or dangling pointers. |
| 110 | +pub(crate) unsafe fn resolve_parameter_overrides( |
| 111 | + node_fqn: String, |
| 112 | + rcl_global_arguments: &rcl_arguments_t, |
| 113 | +) -> Result<ParameterOverrideMap, RclrsError> { |
| 114 | + let mut map = BTreeMap::new(); |
| 115 | + let mut rcl_params = std::ptr::null_mut(); |
| 116 | + rcl_arguments_get_param_overrides(rcl_global_arguments, &mut rcl_params).ok()?; |
| 117 | + // Check for the /** node first, and later overwrite with the more specific node |
| 118 | + // parameters, if they exist |
| 119 | + for name_to_match in ["/**", node_fqn.as_str()] { |
| 120 | + for (node_name, node_params) in RclParamsIter::new(rcl_params) { |
| 121 | + if node_name == name_to_match { |
| 122 | + for (param_name, variant) in RclNodeParamsIter::new(node_params) { |
| 123 | + let value = ParameterValue::from_rcl_variant(variant); |
| 124 | + map.insert(param_name, value); |
| 125 | + } |
| 126 | + } |
| 127 | + } |
| 128 | + } |
| 129 | + rcl_yaml_node_struct_fini(rcl_params); |
| 130 | + Ok(map) |
| 131 | +} |
0 commit comments