From b24af7f03894f01029d40614a280899a1a97b632 Mon Sep 17 00:00:00 2001 From: Martin Dahl Date: Sun, 29 Aug 2021 14:39:41 +0200 Subject: [PATCH] Update parameter service handlers to be based on latest master. --- examples/parameters.rs | 39 +++++++--- src/lib.rs | 1 - src/nodes.rs | 67 +++++++++++++++- src/parameters.rs | 172 +++++++++++++++++++++++++++++------------ 4 files changed, 214 insertions(+), 65 deletions(-) diff --git a/examples/parameters.rs b/examples/parameters.rs index b7d5d32..1ce489e 100644 --- a/examples/parameters.rs +++ b/examples/parameters.rs @@ -1,13 +1,29 @@ +use futures::executor::LocalPool; +use futures::task::LocalSpawnExt; use r2r; // try to run like this // cargo run --example parameters -- --ros-args -p key1:=[hello,world] -p key2:=5.5 -r __ns:=/demo -r __node:=my_node // then run +// ros2 param get /demo/my_node key2 # (should return 5.5) // ros2 param set /demo/my_node key2 false +// ros2 param get /demo/my_node key2 # (should return false) fn main() -> Result<(), Box> { + // set up executor + let mut pool = LocalPool::new(); + let spawner = pool.spawner(); + + // set up ros node let ctx = r2r::Context::create()?; let mut node = r2r::Node::create(ctx, "to_be_replaced", "to_be_replaced")?; + let mut timer = node.create_wall_timer(std::time::Duration::from_millis(2000))?; + + // make a parameter handler (once per node). + // the parameter handler is optional, only spawn one if you need it. + let paramater_handler = node.make_parameter_handler()?; + // run parameter handler on your executor. + spawner.spawn_local(paramater_handler)?; println!("node name: {}", node.name()?); println!( @@ -16,21 +32,20 @@ fn main() -> Result<(), Box> { ); println!("node namespace: {}", node.namespace()?); - let mut i = 0; - loop { - node.spin_once(std::time::Duration::from_millis(100)); - if i % 20 == 0 { - // every 2 seconds print all parameters + // print all params every 2 seconds. + let params = node.params.clone(); + spawner.spawn_local(async move { + loop { println!("node parameters"); - node.params.lock().unwrap().iter().for_each(|(k, v)| { + params.lock().unwrap().iter().for_each(|(k, v)| { println!("{} - {:?}", k, v); }); + let _elapsed = timer.tick().await.expect("could not tick"); } - i += 1; - if i > 1000 { - break; - } - } + })?; - Ok(()) + loop { + node.spin_once(std::time::Duration::from_millis(100)); + pool.run_until_stalled(); + } } diff --git a/src/lib.rs b/src/lib.rs index 4905cd7..126471a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -66,7 +66,6 @@ pub use context::Context; mod parameters; pub use parameters::ParameterValue; -use parameters::*; mod clocks; pub use clocks::{Clock, ClockType}; diff --git a/src/nodes.rs b/src/nodes.rs index 06cc4a8..697b8b0 100644 --- a/src/nodes.rs +++ b/src/nodes.rs @@ -1,8 +1,9 @@ use super::*; +use futures::future::{self, join_all}; pub struct Node { context: Context, - pub params: HashMap, + pub params: Arc>>, node_handle: Box, // the node owns the subscribers subs: Vec>, @@ -104,11 +105,12 @@ impl Node { let param_values = unsafe { std::slice::from_raw_parts(np.parameter_values, np.num_params) }; + let mut params = self.params.lock().unwrap(); for (s, v) in param_names.iter().zip(param_values) { let s = unsafe { CStr::from_ptr(*s) }; let key = s.to_str().unwrap_or(""); - let val = parameter_value_from_rcl(&*v); - self.params.insert(key.to_owned(), val); + let val = ParameterValue::from_rcl(&*v); + params.insert(key.to_owned(), val); } } @@ -139,7 +141,7 @@ impl Node { if res == RCL_RET_OK as i32 { let mut node = Node { - params: HashMap::new(), + params: Arc::new(Mutex::new(HashMap::new())), context: ctx, node_handle, subs: Vec::new(), @@ -158,6 +160,63 @@ impl Node { } } + /// Returns a future which handles any parameter change requests. Spawn this onto the executor of choice. + pub fn make_parameter_handler(&mut self) -> Result> { + let mut handlers: Vec>>> = Vec::new(); + + let node_name = self.name()?; + let set_params_request_stream = self.create_service::( + &format!("{}/set_parameters", node_name))?; + + let params = self.params.clone(); + let set_params_future = set_params_request_stream + .for_each(move |req: ServiceRequest| { + let mut result = rcl_interfaces::srv::SetParameters::Response::default(); + for p in &req.message.parameters { + let val = ParameterValue::from_parameter_value_msg(p.value.clone()); + params.lock().unwrap().insert(p.name.clone(), val); + let r = rcl_interfaces::msg::SetParametersResult { + successful: true, + reason: "".into(), + }; + result.results.push(r); + } + req.respond(result).expect("could not send reply to set parameter request"); + future::ready(()) + }); + handlers.push(Box::pin(set_params_future)); + + // rcl_interfaces/srv/GetParameters + let get_params_request_stream = self.create_service::( + &format!("{}/get_parameters", node_name))?; + + let params = self.params.clone(); + let get_params_future = get_params_request_stream + .for_each(move |req: ServiceRequest| { + let params = params.lock().unwrap(); + let values = req.message.names.iter() + .map(|n| { + match params.get(n) { + Some(v) => v.clone(), + None => ParameterValue::NotSet + } + }) + .map(|v| v.clone().to_parameter_value_msg()) + .collect::>(); + + let result = rcl_interfaces::srv::GetParameters::Response { + values + }; + req.respond(result).expect("could not send reply to set parameter request"); + future::ready(()) + }); + + handlers.push(Box::pin(get_params_future)); + + // we don't care about the result, the futures will not complete anyway. + Ok(join_all(handlers).map(|_| ())) + } + pub fn subscribe(&mut self, topic: &str) -> Result + Unpin> where T: WrappedTypesupport, diff --git a/src/parameters.rs b/src/parameters.rs index 4d17b39..5c6e8dc 100644 --- a/src/parameters.rs +++ b/src/parameters.rs @@ -14,53 +14,129 @@ pub enum ParameterValue { StringArray(Vec), } -pub fn parameter_value_from_rcl(v: &rcl_variant_t) -> ParameterValue { - if v.bool_value != std::ptr::null_mut() { - ParameterValue::Bool(unsafe { *v.bool_value }) - } else if v.integer_value != std::ptr::null_mut() { - ParameterValue::Integer(unsafe { *v.integer_value }) - } else if v.double_value != std::ptr::null_mut() { - ParameterValue::Double(unsafe { *v.double_value }) - } else if v.string_value != std::ptr::null_mut() { - let s = unsafe { CStr::from_ptr(v.string_value) }; - let string = s.to_str().unwrap_or("").to_owned(); - ParameterValue::String(string) - } else if v.byte_array_value != std::ptr::null_mut() { - let vals = unsafe { - std::slice::from_raw_parts((*v.byte_array_value).values, (*v.byte_array_value).size) - }; - ParameterValue::ByteArray(vals.iter().cloned().collect()) - } else if v.bool_array_value != std::ptr::null_mut() { - let vals = unsafe { - std::slice::from_raw_parts((*v.bool_array_value).values, (*v.bool_array_value).size) - }; - ParameterValue::BoolArray(vals.iter().cloned().collect()) - } else if v.integer_array_value != std::ptr::null_mut() { - let vals = unsafe { - std::slice::from_raw_parts( - (*v.integer_array_value).values, - (*v.integer_array_value).size, - ) - }; - ParameterValue::IntegerArray(vals.iter().cloned().collect()) - } else if v.double_array_value != std::ptr::null_mut() { - let vals = unsafe { - std::slice::from_raw_parts((*v.double_array_value).values, (*v.double_array_value).size) - }; - ParameterValue::DoubleArray(vals.iter().cloned().collect()) - } else if v.string_array_value != std::ptr::null_mut() { - let vals = unsafe { - std::slice::from_raw_parts((*v.string_array_value).data, (*v.string_array_value).size) - }; - let s = vals - .iter() - .map(|cs| { - let s = unsafe { CStr::from_ptr(*cs) }; - s.to_str().unwrap_or("").to_owned() - }) - .collect(); - ParameterValue::StringArray(s) - } else { - ParameterValue::NotSet +impl ParameterValue { + pub(crate) fn from_rcl(v: &rcl_variant_t) -> Self { + if v.bool_value != std::ptr::null_mut() { + ParameterValue::Bool(unsafe { *v.bool_value }) + } else if v.integer_value != std::ptr::null_mut() { + ParameterValue::Integer(unsafe { *v.integer_value }) + } else if v.double_value != std::ptr::null_mut() { + ParameterValue::Double(unsafe { *v.double_value }) + } else if v.string_value != std::ptr::null_mut() { + let s = unsafe { CStr::from_ptr(v.string_value) }; + let string = s.to_str().unwrap_or("").to_owned(); + ParameterValue::String(string) + } else if v.byte_array_value != std::ptr::null_mut() { + let vals = unsafe { + std::slice::from_raw_parts((*v.byte_array_value).values, (*v.byte_array_value).size) + }; + ParameterValue::ByteArray(vals.iter().cloned().collect()) + } else if v.bool_array_value != std::ptr::null_mut() { + let vals = unsafe { + std::slice::from_raw_parts((*v.bool_array_value).values, (*v.bool_array_value).size) + }; + ParameterValue::BoolArray(vals.iter().cloned().collect()) + } else if v.integer_array_value != std::ptr::null_mut() { + let vals = unsafe { + std::slice::from_raw_parts( + (*v.integer_array_value).values, + (*v.integer_array_value).size, + ) + }; + ParameterValue::IntegerArray(vals.iter().cloned().collect()) + } else if v.double_array_value != std::ptr::null_mut() { + let vals = unsafe { + std::slice::from_raw_parts( + (*v.double_array_value).values, + (*v.double_array_value).size, + ) + }; + ParameterValue::DoubleArray(vals.iter().cloned().collect()) + } else if v.string_array_value != std::ptr::null_mut() { + let vals = unsafe { + std::slice::from_raw_parts( + (*v.string_array_value).data, + (*v.string_array_value).size, + ) + }; + let s = vals + .iter() + .map(|cs| { + let s = unsafe { CStr::from_ptr(*cs) }; + s.to_str().unwrap_or("").to_owned() + }) + .collect(); + ParameterValue::StringArray(s) + } else { + ParameterValue::NotSet + } } + + pub(crate) fn from_parameter_value_msg(msg: rcl_interfaces::msg::ParameterValue) -> Self { + // todo: use constants from ParameterType message + match msg.type_ { + 0 => ParameterValue::NotSet, + 1 => ParameterValue::Bool(msg.bool_value), + 2 => ParameterValue::Integer(msg.integer_value), + 3 => ParameterValue::Double(msg.double_value), + 4 => ParameterValue::String(msg.string_value), + 5 => ParameterValue::ByteArray(msg.byte_array_value), + 6 => ParameterValue::BoolArray(msg.bool_array_value), + 7 => ParameterValue::IntegerArray(msg.integer_array_value), + 8 => ParameterValue::DoubleArray(msg.double_array_value), + 9 => ParameterValue::StringArray(msg.string_array_value), + _ => { + println!("warning: malformed parametervalue message"); + ParameterValue::NotSet + } + } + } + + pub(crate) fn to_parameter_value_msg(self) -> rcl_interfaces::msg::ParameterValue { + let mut ret = rcl_interfaces::msg::ParameterValue::default(); + + match self { + ParameterValue::NotSet => { + ret.type_ = 0; // uint8 PARAMETER_NOT_SET=0 + }, + ParameterValue::Bool(b) => { + ret.type_ = 1; // uint8 PARAMETER_BOOL=1 + ret.bool_value = b; + }, + ParameterValue::Integer(i) => { + ret.type_ = 2; // uint8 PARAMETER_INTEGER=2 + ret.integer_value = i; + }, + ParameterValue::Double(d) => { + ret.type_ = 3; // uint8 PARAMETER_DOUBLE=3 + ret.double_value = d; + }, + ParameterValue::String(s) => { + ret.type_ = 4; // uint8 PARAMETER_STRING=4 + ret.string_value = s; + }, + ParameterValue::ByteArray(ba) => { + ret.type_ = 5; // uint8 PARAMETER_BYTE_ARRAY=5 + ret.byte_array_value = ba; + }, + ParameterValue::BoolArray(ba) => { + ret.type_ = 6; // uint8 PARAMETER_BOOL_ARRAY=6 + ret.bool_array_value = ba; + }, + ParameterValue::IntegerArray(ia) => { + ret.type_ = 7; // uint8 PARAMETER_INTEGER_ARRAY=7 + ret.integer_array_value = ia; + }, + ParameterValue::DoubleArray(da) => { + ret.type_ = 8; // uint8 PARAMETER_DOUBLE_ARRAY=8 + ret.double_array_value = da; + }, + ParameterValue::StringArray(sa) => { + ret.type_ = 9; // int PARAMETER_STRING_ARRAY=9 + ret.string_array_value = sa; + }, + } + ret + } + }