From d89f40ae054320cd4c1bc118958227f8a73224cd Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Fri, 15 Sep 2023 16:25:54 +0200 Subject: [PATCH] Handle type conversion errors + implement String parameter --- r2r/examples/parameters_derive.rs | 15 +++++--- r2r/src/error.rs | 3 ++ r2r/src/parameters.rs | 64 +++++++++++++++++++++++-------- 3 files changed, 61 insertions(+), 21 deletions(-) diff --git a/r2r/examples/parameters_derive.rs b/r2r/examples/parameters_derive.rs index 8e0a0f1a3..37e05f8ec 100644 --- a/r2r/examples/parameters_derive.rs +++ b/r2r/examples/parameters_derive.rs @@ -10,6 +10,7 @@ use std::sync::{Arc, Mutex}; // ros2 param get /demo/my_node nested.par4 # should return 42 // ros2 param set /demo/my_node nested.par4 43 // ros2 param set /demo/my_node nested.par4 xxx # fails due to invalid type +// ros2 param set /demo/my_node nested.nested2.par5 999 # fails with conversion error // ros2 param dump /demo/my_node // Prints: // /demo/my_node: @@ -17,8 +18,8 @@ use std::sync::{Arc, Mutex}; // nested: // nested2: // par5: 0 -// par3: 0.0 -// par4: 43 +// par3: initial value +// par4: 42 // par1: 5.1 // par2: 0 // @@ -37,7 +38,7 @@ struct Params { #[derive(RosParams, Default, Debug)] struct NestedParams { - par3: f32, + par3: String, par4: u16, nested2: NestedParams2, } @@ -58,8 +59,12 @@ fn main() -> Result<(), Box> { let ctx = r2r::Context::create()?; let mut node = r2r::Node::create(ctx, "to_be_replaced", "to_be_replaced")?; - // create our parameters - let params = Arc::new(Mutex::new(Params::default())); + // create our parameters and set default values + let params = Arc::new(Mutex::new({ + let mut p = Params::default(); + p.nested.par3 = "initial value".into(); + p + })); // make a parameter handler (once per node). // the parameter handler is optional, only spawn one if you need it. diff --git a/r2r/src/error.rs b/r2r/src/error.rs index ca096ff73..0d3d90039 100644 --- a/r2r/src/error.rs +++ b/r2r/src/error.rs @@ -122,6 +122,9 @@ pub enum Error { #[error("Invalid type for parameter {name} (should be {ty})")] InvalidParameterType { name: String, ty: &'static str }, + + #[error("Parameter {name} conversion failed: {msg}")] + ParameterValueConv { name: String, msg: String }, } impl Error { diff --git a/r2r/src/parameters.rs b/r2r/src/parameters.rs index 4c854513a..47efcf4c3 100644 --- a/r2r/src/parameters.rs +++ b/r2r/src/parameters.rs @@ -162,13 +162,17 @@ pub fn update_param_name_in_err(e: Error, param_name: &str) -> Error { name: param_name.to_string(), ty, }, + Error::ParameterValueConv { name: _, msg } => Error::ParameterValueConv { + name: param_name.to_string(), + msg, + }, _ => e, } } // Implementation of RosParams for primitive types, i.e. leaf parameters macro_rules! impl_ros_params { - ($type:path, $param_value_type:path) => { + ($type:path, $param_value_type:path, $to_param_conv:path, $from_param_conv:path) => { impl RosParams for $type { fn register_parameters( &mut self, prefix: &str, params: &mut HashMap, @@ -179,14 +183,14 @@ macro_rules! impl_ros_params { .map_err(|e| update_param_name_in_err(e, prefix))?; } else { // Insert missing parameter with its default value - params.insert(prefix.to_owned(), $param_value_type((*self).into())); + params.insert(prefix.to_owned(), $param_value_type($to_param_conv(self)?)); } Ok(()) } fn get_parameter(&mut self, param_name: &str) -> Result { match param_name { - "" => Ok($param_value_type((*self).into())), + "" => Ok($param_value_type($to_param_conv(self)?)), _ => Err(Error::InvalidParameterName { name: param_name.to_owned(), }), @@ -203,7 +207,7 @@ macro_rules! impl_ros_params { } match param_val { $param_value_type(val) => { - *self = *val as $type; + *self = $from_param_conv(val)?; Ok(()) } _ => Err(Error::InvalidParameterType { @@ -216,15 +220,43 @@ macro_rules! impl_ros_params { }; } -impl_ros_params!(bool, ParameterValue::Bool); -impl_ros_params!(i8, ParameterValue::Integer); -impl_ros_params!(i16, ParameterValue::Integer); -impl_ros_params!(i32, ParameterValue::Integer); -impl_ros_params!(i64, ParameterValue::Integer); -impl_ros_params!(u8, ParameterValue::Integer); -impl_ros_params!(u16, ParameterValue::Integer); -impl_ros_params!(u32, ParameterValue::Integer); -impl_ros_params!(f64, ParameterValue::Double); -impl_ros_params!(f32, ParameterValue::Double); -// TODO: String -// impl_ros_params!(String, ParameterValue::String); +impl_ros_params!(bool, ParameterValue::Bool, noop, noop); +impl_ros_params!(i8, ParameterValue::Integer, try_conv, try_conv); +impl_ros_params!(i16, ParameterValue::Integer, try_conv, try_conv); +impl_ros_params!(i32, ParameterValue::Integer, try_conv, try_conv); +impl_ros_params!(i64, ParameterValue::Integer, noop, noop); +impl_ros_params!(u8, ParameterValue::Integer, try_conv, try_conv); +impl_ros_params!(u16, ParameterValue::Integer, try_conv, try_conv); +impl_ros_params!(u32, ParameterValue::Integer, try_conv, try_conv); +impl_ros_params!(f64, ParameterValue::Double, noop, noop); +impl_ros_params!(f32, ParameterValue::Double, to_f64, to_f32); +impl_ros_params!(String, ParameterValue::String, to_string, to_string); +// TODO: Implement array parameters + +// Helper conversion functions +fn noop(x: &T) -> Result { + Ok(*x) +} + +fn to_f32(x: &f64) -> Result { + Ok(*x as f32) +} +fn to_f64(x: &f32) -> Result { + Ok(*x as f64) +} + +fn try_conv(x: &T) -> Result +where + T: Copy, + U: TryFrom, + >::Error: std::error::Error, +{ + U::try_from(*x).map_err(|e| Error::ParameterValueConv { + name: "".into(), + msg: e.to_string(), + }) +} + +fn to_string(x: &str) -> Result { + Ok(x.to_string()) +}