diff --git a/examples/parameters.rs b/examples/parameters.rs index 688892a..3d2b2ea 100644 --- a/examples/parameters.rs +++ b/examples/parameters.rs @@ -1,11 +1,13 @@ use r2r; // try to run like this -// cargo run --example parameters -- --ros-args -p param_key:=[hej,hopp] -p key2:=5.5 key2=true -r __ns:=/demo -r __node:=my_node +// 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 set /demo/my_node key2 false fn main() -> Result<(), Box> { let ctx = r2r::Context::create()?; - let node = r2r::Node::create(ctx, "testnode", "")?; + let mut node = r2r::Node::create(ctx, "to_be_replaced", "to_be_replaced")?; println!("node name: {}", node.name()?); println!( @@ -14,10 +16,20 @@ fn main() -> Result<(), Box> { ); println!("node namespace: {}", node.namespace()?); - println!("node parameters"); - node.params.iter().for_each(|(k, v)| { - println!("{} - {:?}", k, v); - }); + let mut i = 0; + loop { + node.spin_once(std::time::Duration::from_millis(100)); + if i % 20 == 0 { // every 2 seconds print all parameters + println!("node parameters"); + node.params.lock().unwrap().iter().for_each(|(k, v)| { + println!("{} - {:?}", k, v); + }); + } + i+=1; + if i > 1000 { + break; + } + } Ok(()) } diff --git a/src/lib.rs b/src/lib.rs index 1c07932..c3f3b95 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1608,7 +1608,7 @@ impl Drop for ContextHandle { } } -#[derive(Debug)] +#[derive(Debug, Clone, PartialEq)] pub enum ParameterValue { NotSet, Bool(bool), @@ -1679,11 +1679,31 @@ impl ParameterValue { ParameterValue::NotSet } } + + 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 struct Node { context: Context, - pub params: HashMap, + pub params: Arc>>, node_handle: Box, // the node owns the subscribers subs: Vec>, @@ -1785,11 +1805,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 = ParameterValue::from_rcl(&*v); - self.params.insert(key.to_owned(), val); + params.insert(key.to_owned(), val); } } @@ -1821,9 +1842,9 @@ 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: node_handle, + node_handle, subs: Vec::new(), services: Vec::new(), clients: Vec::new(), @@ -1833,6 +1854,9 @@ impl Node { pubs: Vec::new(), }; node.load_params()?; + + node.setup_parameter_services()?; + Ok(node) } else { eprintln!("could not create node{}", res); @@ -1840,6 +1864,31 @@ impl Node { } } + fn setup_parameter_services(&mut self) -> Result<()> { + let node_name = self.name()?; + let params_cb = self.params.clone(); + self.create_service::(&format!("{}/set_parameters", node_name), + Box::new(move |req: rcl_interfaces::srv::SetParameters::Request| { + let mut result = rcl_interfaces::srv::SetParameters::Response::default(); + for p in req.parameters { + let val = ParameterValue::from_parameter_value_msg(p.value); + params_cb.lock().unwrap().insert(p.name, val); + let r = rcl_interfaces::msg::SetParametersResult { + successful: true, + reason: "".into(), + }; + result.results.push(r); + } + result + }))?; + + Ok(()) + } + + pub fn get_param(&self, name: &str) -> Option { + self.params.lock().unwrap().get(name).cloned() + } + fn create_subscription_helper( &mut self, topic: &str,