mirror
/
r2r
mirror of https://github.com/sequenceplanner/r2r.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Atomic behavior for `set_parameters_atomically`.

This commit is contained in:
Martin Dahl 2025-04-22 10:05:32 +02:00
parent 916631030d
commit 8cbd60bf9b
3 changed files with 88 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

101
r2r/src/nodes.rs
View File

@ -454,9 +454,6 @@ impl Node {
handlers.push(Box::pin(get_param_types_future));
// rcl_interfaces/srv/SetParametersAtomically
// NOTE: This is not a proper implementation of the specs, but rather a copy of set_parameters.
// On error, some of the parameters might already be set.
let set_params_atomically_request_stream =
self.create_service::<rcl_interfaces::srv::SetParametersAtomically::Service>(
&format!("{}/set_parameters_atomically", node_name),
@ -469,52 +466,60 @@ impl Node {
move |req: ServiceRequest<rcl_interfaces::srv::SetParametersAtomically::Service>| {
let mut result = rcl_interfaces::srv::SetParametersAtomically::Response::default();
result.result.successful = true;
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
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());
.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);
}
}
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);
}
}
// if this parameter failed, set the result and break
if !r.successful {
result.result = r;
break;
}
}
req.respond(result)
@ -984,9 +989,7 @@ impl Node {
pub fn destroy_publisher<T: WrappedTypesupport>(&mut self, p: Publisher<T>) {
if let Some(handle) = p.handle.upgrade() {
// Remove handle from list of publishers.
self.pubs
.iter()
.position(|p| Arc::ptr_eq(p, &handle))
self.pubs.iter().position(|p| Arc::ptr_eq(p, &handle))
.map(|i| self.pubs.swap_remove(i));
let handle = wait_until_unwrapped(handle);
@ -998,9 +1001,7 @@ impl Node {
pub fn destroy_publisher_untyped(&mut self, p: PublisherUntyped) {
if let Some(handle) = p.handle.upgrade() {
// Remove handle from list of publishers.
self.pubs
.iter()
.position(|p| Arc::ptr_eq(p, &handle))
self.pubs.iter().position(|p| Arc::ptr_eq(p, &handle))
.map(|i| self.pubs.swap_remove(i));
let handle = wait_until_unwrapped(handle);

21
r2r/src/parameters.rs
View File

@ -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),
}),
}
}
}
};
}

16
r2r_macros/src/lib.rs
View File

@ -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))
}
}
};