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 <desmond@germansmedia.nl>

r2r/src/nodes.rs

@@ -283,9 +283,13 @@ impl Node {
                 .register_parameters("", None, &mut self.params.lock().unwrap())?;
         }
         let mut handlers: Vec<std::pin::Pin<Box<dyn Future<Output = ()> + 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::<rcl_interfaces::srv::SetParameters::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::<rcl_interfaces::srv::SetParametersAtomically::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<rcl_interfaces::srv::SetParametersAtomically::Service>| {
+                let mut result = rcl_interfaces::srv::SetParametersAtomically::Response::default();
+                result.result.successful = true;
+                if let Some(ps) = &params_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) = &params_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

r2r/src/parameters.rs

@@ -308,6 +308,7 @@ pub trait RosParams {
     ) -> Result<()>;
     fn get_parameter(&mut self, param_name: &str) -> Result<ParameterValue>;
     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),
+                    }),
+                }
+            }
         }
     };
 }

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(&param_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(&param_name))
+            }
         }
     };