r2r/
parameters.rs

Help
1use crate::{Error, Result};
2use std::ffi::CStr;
3
4use crate::msg_types::generated_msgs::rcl_interfaces;
5use indexmap::IndexMap;
6use r2r_rcl::*;
7
8/// ROS parameter value.
9#[derive(Debug, PartialEq, Clone)]
10pub enum ParameterValue {
11    NotSet,
12    Bool(bool),
13    Integer(i64),
14    Double(f64),
15    String(String),
16    BoolArray(Vec<bool>),
17    ByteArray(Vec<u8>),
18    IntegerArray(Vec<i64>),
19    DoubleArray(Vec<f64>),
20    StringArray(Vec<String>),
21}
22
23#[derive(Debug)]
24pub struct WrongParameterType {
25    pub expected_type_name: &'static str,
26    pub actual_type_name: &'static str,
27}
28
29impl std::error::Error for WrongParameterType {}
30
31impl std::fmt::Display for WrongParameterType {
32    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
33        write!(f, "got `{}`, expected `{}`", self.actual_type_name, self.expected_type_name)
34    }
35}
36
37macro_rules! try_into_template {
38    ($ty:ty, $expected_type_name:literal, $variant:pat => $result:expr) => {
39        impl TryInto<$ty> for ParameterValue {
40            type Error = WrongParameterType;
41
42            fn try_into(self) -> std::prelude::v1::Result<$ty, Self::Error> {
43                match self {
44                    $variant => Ok($result),
45                    _ => Err(WrongParameterType {
46                        expected_type_name: $expected_type_name,
47                        actual_type_name: self.type_name(),
48                    }),
49                }
50            }
51        }
52    };
53}
54
55try_into_template!((), "not set", ParameterValue::NotSet => ());
56try_into_template!(bool, "boolean", ParameterValue::Bool(value) => value);
57try_into_template!(i64, "integer", ParameterValue::Integer(value) => value);
58try_into_template!(f64, "double", ParameterValue::Double(value) => value);
59try_into_template!(String, "string", ParameterValue::String(value) => value);
60try_into_template!(Vec<bool>, "boolean array", ParameterValue::BoolArray(value) => value);
61try_into_template!(Vec<u8>, "byte array", ParameterValue::ByteArray(value) => value);
62try_into_template!(Vec<i64>, "integer array", ParameterValue::IntegerArray(value) => value);
63try_into_template!(Vec<f64>, "double array", ParameterValue::DoubleArray(value) => value);
64try_into_template!(Vec<String>, "string array", ParameterValue::StringArray(value) => value);
65
66macro_rules! try_into_option_template {
67    ($ty:ty, $expected_type_name:literal, $variant:pat => $result:expr) => {
68        impl TryInto<Option<$ty>> for ParameterValue {
69            type Error = WrongParameterType;
70
71            fn try_into(self) -> std::prelude::v1::Result<Option<$ty>, Self::Error> {
72                match self {
73                    $variant => Ok(Some($result)),
74                    ParameterValue::NotSet => Ok(None),
75                    _ => Err(WrongParameterType {
76                        expected_type_name: $expected_type_name,
77                        actual_type_name: self.type_name(),
78                    }),
79                }
80            }
81        }
82    };
83}
84
85try_into_option_template!(bool, "boolean", ParameterValue::Bool(value) => value);
86try_into_option_template!(i64, "integer", ParameterValue::Integer(value) => value);
87try_into_option_template!(f64, "double", ParameterValue::Double(value) => value);
88try_into_option_template!(String, "string", ParameterValue::String(value) => value);
89try_into_option_template!(Vec<bool>, "boolean array", ParameterValue::BoolArray(value) => value);
90try_into_option_template!(Vec<u8>, "byte array", ParameterValue::ByteArray(value) => value);
91try_into_option_template!(Vec<i64>, "integer array", ParameterValue::IntegerArray(value) => value);
92try_into_option_template!(Vec<f64>, "double array", ParameterValue::DoubleArray(value) => value);
93try_into_option_template!(Vec<String>, "string array", ParameterValue::StringArray(value) => value);
94
95impl ParameterValue {
96    pub fn type_name(&self) -> &'static str {
97        match self {
98            ParameterValue::NotSet => "not set",
99            ParameterValue::Bool(_) => "boolean",
100            ParameterValue::Integer(_) => "integer",
101            ParameterValue::Double(_) => "double",
102            ParameterValue::String(_) => "string",
103            ParameterValue::BoolArray(_) => "boolean array",
104            ParameterValue::ByteArray(_) => "byte array",
105            ParameterValue::IntegerArray(_) => "integer array",
106            ParameterValue::DoubleArray(_) => "double array",
107            ParameterValue::StringArray(_) => "string array",
108        }
109    }
110
111    pub(crate) fn from_rcl(v: &rcl_variant_t) -> Self {
112        if !v.bool_value.is_null() {
113            ParameterValue::Bool(unsafe { *v.bool_value })
114        } else if !v.integer_value.is_null() {
115            ParameterValue::Integer(unsafe { *v.integer_value })
116        } else if !v.double_value.is_null() {
117            ParameterValue::Double(unsafe { *v.double_value })
118        } else if !v.string_value.is_null() {
119            let s = unsafe { CStr::from_ptr(v.string_value) };
120            let string = s.to_str().unwrap_or("").to_owned();
121            ParameterValue::String(string)
122        } else if !v.byte_array_value.is_null() {
123            let vals = unsafe {
124                if (*v.byte_array_value).values == std::ptr::null_mut() {
125                    &[]
126                } else {
127                    std::slice::from_raw_parts(
128                        (*v.byte_array_value).values,
129                        (*v.byte_array_value).size,
130                    )
131                }
132            };
133            ParameterValue::ByteArray(vals.to_vec())
134        } else if !v.bool_array_value.is_null() {
135            let vals = unsafe {
136                if (*v.bool_array_value).values == std::ptr::null_mut() {
137                    &[]
138                } else {
139                    std::slice::from_raw_parts(
140                        (*v.bool_array_value).values,
141                        (*v.bool_array_value).size,
142                    )
143                }
144            };
145            ParameterValue::BoolArray(vals.to_vec())
146        } else if !v.integer_array_value.is_null() {
147            let vals = unsafe {
148                if (*v.integer_array_value).values == std::ptr::null_mut() {
149                    &[]
150                } else {
151                    std::slice::from_raw_parts(
152                        (*v.integer_array_value).values,
153                        (*v.integer_array_value).size,
154                    )
155                }
156            };
157            ParameterValue::IntegerArray(vals.to_vec())
158        } else if !v.double_array_value.is_null() {
159            let vals = unsafe {
160                if (*v.double_array_value).values == std::ptr::null_mut() {
161                    &[]
162                } else {
163                    std::slice::from_raw_parts(
164                        (*v.double_array_value).values,
165                        (*v.double_array_value).size,
166                    )
167                }
168            };
169            ParameterValue::DoubleArray(vals.to_vec())
170        } else if !v.string_array_value.is_null() {
171            let vals = unsafe {
172                if (*v.string_array_value).data == std::ptr::null_mut() {
173                    &[]
174                } else {
175                    std::slice::from_raw_parts(
176                        (*v.string_array_value).data,
177                        (*v.string_array_value).size,
178                    )
179                }
180            };
181            let s = vals
182                .iter()
183                .map(|cs| {
184                    let s = unsafe { CStr::from_ptr(*cs) };
185                    s.to_str().unwrap_or("").to_owned()
186                })
187                .collect();
188            ParameterValue::StringArray(s)
189        } else {
190            ParameterValue::NotSet
191        }
192    }
193
194    pub(crate) fn from_parameter_value_msg(msg: rcl_interfaces::msg::ParameterValue) -> Self {
195        // todo: use constants from ParameterType message
196        match msg.type_ {
197            0 => ParameterValue::NotSet,
198            1 => ParameterValue::Bool(msg.bool_value),
199            2 => ParameterValue::Integer(msg.integer_value),
200            3 => ParameterValue::Double(msg.double_value),
201            4 => ParameterValue::String(msg.string_value),
202            5 => ParameterValue::ByteArray(msg.byte_array_value),
203            6 => ParameterValue::BoolArray(msg.bool_array_value),
204            7 => ParameterValue::IntegerArray(msg.integer_array_value),
205            8 => ParameterValue::DoubleArray(msg.double_array_value),
206            9 => ParameterValue::StringArray(msg.string_array_value),
207            _ => {
208                log::debug!("warning: malformed parametervalue message");
209                ParameterValue::NotSet
210            }
211        }
212    }
213
214    pub(crate) fn into_parameter_value_msg(self) -> rcl_interfaces::msg::ParameterValue {
215        let mut ret = rcl_interfaces::msg::ParameterValue::default();
216
217        match self {
218            ParameterValue::NotSet => {
219                ret.type_ = 0; // uint8 PARAMETER_NOT_SET=0
220            }
221            ParameterValue::Bool(b) => {
222                ret.type_ = 1; // uint8 PARAMETER_BOOL=1
223                ret.bool_value = b;
224            }
225            ParameterValue::Integer(i) => {
226                ret.type_ = 2; // uint8 PARAMETER_INTEGER=2
227                ret.integer_value = i;
228            }
229            ParameterValue::Double(d) => {
230                ret.type_ = 3; // uint8 PARAMETER_DOUBLE=3
231                ret.double_value = d;
232            }
233            ParameterValue::String(s) => {
234                ret.type_ = 4; // uint8 PARAMETER_STRING=4
235                ret.string_value = s;
236            }
237            ParameterValue::ByteArray(ba) => {
238                ret.type_ = 5; // uint8 PARAMETER_BYTE_ARRAY=5
239                ret.byte_array_value = ba;
240            }
241            ParameterValue::BoolArray(ba) => {
242                ret.type_ = 6; // uint8 PARAMETER_BOOL_ARRAY=6
243                ret.bool_array_value = ba;
244            }
245            ParameterValue::IntegerArray(ia) => {
246                ret.type_ = 7; // uint8 PARAMETER_INTEGER_ARRAY=7
247                ret.integer_array_value = ia;
248            }
249            ParameterValue::DoubleArray(da) => {
250                ret.type_ = 8; // uint8 PARAMETER_DOUBLE_ARRAY=8
251                ret.double_array_value = da;
252            }
253            ParameterValue::StringArray(sa) => {
254                ret.type_ = 9; // int PARAMETER_STRING_ARRAY=9
255                ret.string_array_value = sa;
256            }
257        }
258        ret
259    }
260
261    pub(crate) fn into_parameter_type(&self) -> u8 {
262        match self {
263            ParameterValue::NotSet => 0,          // uint8 PARAMETER_NOT_SET=0
264            ParameterValue::Bool(_) => 1,         // uint8 PARAMETER_BOOL=1
265            ParameterValue::Integer(_) => 2,      // uint8 PARAMETER_INTEGER=2
266            ParameterValue::Double(_) => 3,       // uint8 PARAMETER_DOUBLE=3
267            ParameterValue::String(_) => 4,       // uint8 PARAMETER_STRING=4
268            ParameterValue::ByteArray(_) => 5,    // uint8 PARAMETER_BYTE_ARRAY=5
269            ParameterValue::BoolArray(_) => 6,    // uint8 PARAMETER_BOOL_ARRAY=6
270            ParameterValue::IntegerArray(_) => 7, // uint8 PARAMETER_INTEGER_ARRAY=7
271            ParameterValue::DoubleArray(_) => 8,  // uint8 PARAMETER_DOUBLE_ARRAY=8
272            ParameterValue::StringArray(_) => 9,  // int PARAMETER_STRING_ARRAY=9
273        }
274    }
275}
276
277/// ROS parameter.
278pub struct Parameter {
279    pub value: ParameterValue,
280    pub description: &'static str,
281    // TODO: Add other fields like min, max, step. Use field
282    // attributes for defining them.
283}
284
285impl Parameter {
286    pub fn new(value: ParameterValue) -> Self {
287        Self {
288            value,
289            description: "",
290        }
291    }
292    pub fn empty() -> Self {
293        Self {
294            value: ParameterValue::NotSet,
295            description: "",
296        }
297    }
298}
299
300/// Trait for use it with
301/// [`Node::make_derived_parameter_handler()`](crate::Node::make_derived_parameter_handler()).
302///
303/// The trait is usually derived with `r2r_macros::RosParams`. See
304/// `parameters_derive.rs` example.
305pub trait RosParams {
306    fn register_parameters(
307        &mut self, prefix: &str, param: Option<Parameter>, params: &mut IndexMap<String, Parameter>,
308    ) -> Result<()>;
309    fn get_parameter(&mut self, param_name: &str) -> Result<ParameterValue>;
310    fn set_parameter(&mut self, param_name: &str, param_val: &ParameterValue) -> Result<()>;
311    fn check_parameter(&self, param_name: &str, param_val: &ParameterValue) -> Result<()>;
312}
313
314// Implementation of RosParams for primitive types, i.e. leaf parameters
315macro_rules! impl_ros_params {
316    ($type:path, $param_value_type:path, $to_param_conv:path, $from_param_conv:path) => {
317        impl RosParams for $type {
318            fn register_parameters(
319                &mut self, prefix: &str, param: Option<Parameter>,
320                params: &mut IndexMap<String, Parameter>,
321            ) -> Result<()> {
322                if let Some(cli_param) = params.get(prefix) {
323                    // Apply parameter value if set from command line or launch file
324                    self.set_parameter("", &cli_param.value)
325                        .map_err(|e| e.update_param_name(prefix))?;
326                    // Remove the parameter (will be re-inserted below with deterministic order)
327                    params.shift_remove(prefix);
328                }
329                // Insert the parameter with filled-in description etc.
330                let mut param = param.unwrap();
331                param.value = $param_value_type($to_param_conv(self)?);
332                params.insert(prefix.to_owned(), param);
333                Ok(())
334            }
335
336            fn get_parameter(&mut self, param_name: &str) -> Result<ParameterValue> {
337                match param_name {
338                    "" => Ok($param_value_type($to_param_conv(self)?)),
339                    _ => Err(Error::InvalidParameterName {
340                        name: param_name.to_owned(),
341                    }),
342                }
343            }
344
345            fn set_parameter(
346                &mut self, param_name: &str, param_val: &ParameterValue,
347            ) -> Result<()> {
348                if param_name != "" {
349                    return Err(Error::InvalidParameterName {
350                        name: param_name.to_owned(),
351                    });
352                }
353                match param_val {
354                    $param_value_type(val) => {
355                        *self = $from_param_conv(val)?;
356                        Ok(())
357                    }
358                    _ => Err(Error::InvalidParameterType {
359                        name: "".to_string(), // will be completed by callers who know the name
360                        ty: std::stringify!($param_value_type),
361                    }),
362                }
363            }
364
365            fn check_parameter(
366                &self, param_name: &str, param_val: &ParameterValue,
367            ) -> Result<()> {
368                if param_name != "" {
369                    return Err(Error::InvalidParameterName {
370                        name: param_name.to_owned(),
371                    });
372                }
373                match param_val {
374                    $param_value_type(val) => {
375                        let _: Self = $from_param_conv(val)?;
376                        Ok(())
377                    }
378                    _ => Err(Error::InvalidParameterType {
379                        name: "".to_string(), // will be completed by callers who know the name
380                        ty: std::stringify!($param_value_type),
381                    }),
382                }
383            }
384        }
385    };
386}
387
388impl_ros_params!(bool, ParameterValue::Bool, noop, noop);
389impl_ros_params!(i8, ParameterValue::Integer, try_conv, try_conv);
390impl_ros_params!(i16, ParameterValue::Integer, try_conv, try_conv);
391impl_ros_params!(i32, ParameterValue::Integer, try_conv, try_conv);
392impl_ros_params!(i64, ParameterValue::Integer, noop, noop);
393impl_ros_params!(u8, ParameterValue::Integer, try_conv, try_conv);
394impl_ros_params!(u16, ParameterValue::Integer, try_conv, try_conv);
395impl_ros_params!(u32, ParameterValue::Integer, try_conv, try_conv);
396impl_ros_params!(f64, ParameterValue::Double, noop, noop);
397impl_ros_params!(f32, ParameterValue::Double, to_f64, to_f32);
398impl_ros_params!(String, ParameterValue::String, to_string, to_string);
399// TODO: Implement array parameters
400
401// Helper conversion functions
402fn noop<T: Copy>(x: &T) -> Result<T> {
403    Ok(*x)
404}
405
406fn to_f32(x: &f64) -> Result<f32> {
407    Ok(*x as f32)
408}
409fn to_f64(x: &f32) -> Result<f64> {
410    Ok(*x as f64)
411}
412
413fn try_conv<T, U>(x: &T) -> Result<U>
414where
415    T: Copy,
416    U: TryFrom<T>,
417    <U as TryFrom<T>>::Error: std::error::Error,
418{
419    U::try_from(*x).map_err(|e| Error::ParameterValueConv {
420        name: "".into(),
421        msg: e.to_string(),
422    })
423}
424
425fn to_string(x: &str) -> Result<String> {
426    Ok(x.to_string())
427}
r2r/parameters.rs

r2r/
parameters.rs
