From aa180c5b1549352c9c4be6c905e3af3eff1bee4a Mon Sep 17 00:00:00 2001 From: Martin Dahl Date: Tue, 22 Apr 2025 10:20:32 +0200 Subject: [PATCH] Add `/set_parameters_atomically` (#120, #121) * Fixed failing `ros2 param ...` on r2r nodes for Jazzy (#120) Add `/set_parameters_atomically` service to `make_parameter_handler_internal` in `nodes.rs` to fix failing `ros2 param ...` on r2r nodes for Jazzy. * Atomic behavior for `set_parameters_atomically` (#121). --------- Co-authored-by: Desmond Germans --- r2r/src/nodes.rs | 84 +++++++++++++++++++++++++++++++++++++++++-- r2r/src/parameters.rs | 21 +++++++++++ r2r_macros/src/lib.rs | 16 +++++++++ 3 files changed, 119 insertions(+), 2 deletions(-) diff --git a/r2r/src/nodes.rs b/r2r/src/nodes.rs index c797f1d..435746a 100644 --- a/r2r/src/nodes.rs +++ b/r2r/src/nodes.rs @@ -283,9 +283,13 @@ impl Node { .register_parameters("", None, &mut self.params.lock().unwrap())?; } let mut handlers: Vec + Send>>> = Vec::new(); - let (mut event_tx, event_rx) = mpsc::channel::<(String, ParameterValue)>(10); + + let (mut set_event_tx, event_rx) = mpsc::channel::<(String, ParameterValue)>(10); + let mut set_atomically_event_tx = set_event_tx.clone(); let node_name = self.name()?; + + // rcl_interfaces/srv/SetParameters let set_params_request_stream = self .create_service::( &format!("{}/set_parameters", node_name), @@ -335,7 +339,7 @@ impl Node { }; // if the value changed, send out new value on parameter event stream if changed && r.successful { - if let Err(e) = event_tx.try_send((p.name.clone(), val)) { + if let Err(e) = set_event_tx.try_send((p.name.clone(), val)) { log::debug!("Warning: could not send parameter event ({}).", e); } } @@ -449,6 +453,82 @@ impl Node { handlers.push(Box::pin(get_param_types_future)); + // rcl_interfaces/srv/SetParametersAtomically + let set_params_atomically_request_stream = + self.create_service::( + &format!("{}/set_parameters_atomically", node_name), + QosProfile::default(), + )?; + + let params = self.params.clone(); + let params_struct_clone = params_struct.clone(); + let set_params_atomically_future = set_params_atomically_request_stream.for_each( + move |req: ServiceRequest| { + let mut result = rcl_interfaces::srv::SetParametersAtomically::Response::default(); + result.result.successful = true; + if let Some(ps) = ¶ms_struct_clone { + for p in &req.message.parameters { + let val = ParameterValue::from_parameter_value_msg(p.value.clone()); + if let Err(e) = ps.lock().unwrap().check_parameter(&p.name, &val) { + result.result.successful = false; + result.result.reason = format!("Can't set parameter {}: {}", p.name, e); + break; + } + } + } + if result.result.successful { + // Since we checked them above now we assume these will be set ok... + for p in &req.message.parameters { + let val = ParameterValue::from_parameter_value_msg(p.value.clone()); + let changed = params + .lock() + .unwrap() + .get(&p.name) + .map(|v| v.value != val) + .unwrap_or(true); // changed=true if new + let r = if let Some(ps) = ¶ms_struct_clone { + // Update parameter structure + let result = ps.lock().unwrap().set_parameter(&p.name, &val); + if result.is_ok() { + // Also update Node::params + params + .lock() + .unwrap() + .entry(p.name.clone()) + .and_modify(|p| p.value = val.clone()); + } + rcl_interfaces::msg::SetParametersResult { + successful: result.is_ok(), + reason: result.err().map_or("".into(), |e| e.to_string()), + } + } else { + // No parameter structure - update only Node::params + params + .lock() + .unwrap() + .entry(p.name.clone()) + .and_modify(|p| p.value = val.clone()) + .or_insert(Parameter::new(val.clone())); + rcl_interfaces::msg::SetParametersResult { + successful: true, + reason: "".into(), + } + }; + // if the value changed, send out new value on parameter event stream + if changed && r.successful { + if let Err(e) = set_atomically_event_tx.try_send((p.name.clone(), val)) { + log::debug!("Warning: could not send parameter event ({}).", e); + } + } + } + } + req.respond(result) + .expect("could not send reply to set parameter request"); + future::ready(()) + }, + ); + handlers.push(Box::pin(set_params_atomically_future)); + #[cfg(r2r__rosgraph_msgs__msg__Clock)] { // create TimeSource based on value of use_sim_time parameter diff --git a/r2r/src/parameters.rs b/r2r/src/parameters.rs index 1497af2..09049cb 100644 --- a/r2r/src/parameters.rs +++ b/r2r/src/parameters.rs @@ -308,6 +308,7 @@ pub trait RosParams { ) -> Result<()>; fn get_parameter(&mut self, param_name: &str) -> Result; fn set_parameter(&mut self, param_name: &str, param_val: &ParameterValue) -> Result<()>; + fn check_parameter(&self, param_name: &str, param_val: &ParameterValue) -> Result<()>; } // Implementation of RosParams for primitive types, i.e. leaf parameters @@ -360,6 +361,26 @@ macro_rules! impl_ros_params { }), } } + + fn check_parameter( + &self, param_name: &str, param_val: &ParameterValue, + ) -> Result<()> { + if param_name != "" { + return Err(Error::InvalidParameterName { + name: param_name.to_owned(), + }); + } + match param_val { + $param_value_type(val) => { + let _: Self = $from_param_conv(val)?; + Ok(()) + } + _ => Err(Error::InvalidParameterType { + name: "".to_string(), // will be completed by callers who know the name + ty: std::stringify!($param_value_type), + }), + } + } } }; } diff --git a/r2r_macros/src/lib.rs b/r2r_macros/src/lib.rs index 544a6d3..becb38b 100644 --- a/r2r_macros/src/lib.rs +++ b/r2r_macros/src/lib.rs @@ -18,6 +18,8 @@ pub fn derive_r2r_params(input: proc_macro::TokenStream) -> proc_macro::TokenStr let get_param_matches = param_matches_for(quote!(get_parameter(suffix)), &input.data); let set_param_matches = param_matches_for(quote!(set_parameter(suffix, param_val)), &input.data); + let check_param_matches = + param_matches_for(quote!(check_parameter(suffix, param_val)), &input.data); let expanded = quote! { // The generated impl. @@ -64,6 +66,20 @@ pub fn derive_r2r_params(input: proc_macro::TokenStream) -> proc_macro::TokenStr }; result.map_err(|e| e.update_param_name(¶m_name)) } + fn check_parameter(&self, param_name: &str, param_val: &::r2r::ParameterValue) -> ::r2r::Result<()> + { + let (prefix, suffix) = match param_name.split_once('.') { + None => (param_name, ""), + Some((prefix, suffix)) => (prefix, suffix) + }; + let result = match prefix { + #check_param_matches + _ => Err(::r2r::Error::InvalidParameterName { + name: "".into(), + }), + }; + result.map_err(|e| e.update_param_name(¶m_name)) + } } };