diff --git a/r2r/src/nodes.rs b/r2r/src/nodes.rs index 0f889ec..6fd7e12 100644 --- a/r2r/src/nodes.rs +++ b/r2r/src/nodes.rs @@ -354,6 +354,48 @@ impl Node { handlers.push(Box::pin(list_params_future)); + // rcl_interfaces/srv/DescribeParameters + use rcl_interfaces::srv::DescribeParameters; + let desc_params_request_stream = self.create_service::( + &format!("{node_name}/describe_parameters"), + )?; + + let params = self.params.clone(); + let desc_params_future = desc_params_request_stream.for_each( + move |req: ServiceRequest| { + Self::handle_desc_parameters(req, ¶ms) + }, + ); + + handlers.push(Box::pin(desc_params_future)); + + // rcl_interfaces/srv/GetParameterTypes + use rcl_interfaces::srv::GetParameterTypes; + let get_param_types_request_stream = self.create_service::( + &format!("{node_name}/get_parameter_types"), + )?; + + let params = self.params.clone(); + let get_param_types_future = get_param_types_request_stream.for_each( + move |req: ServiceRequest| { + let params = params.lock().unwrap(); + let types = req + .message + .names + .iter() + .map(|name| match params.get(name) { + Some(pv) => pv.into_parameter_type(), + None => rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET as u8, + }) + .collect(); + req.respond(GetParameterTypes::Response { types }) + .expect("could not send reply to get parameter types request"); + future::ready(()) + }, + ); + + handlers.push(Box::pin(get_param_types_future)); + // we don't care about the result, the futures will not complete anyway. Ok((join_all(handlers).map(|_| ()), event_rx)) } @@ -401,6 +443,34 @@ impl Node { future::ready(()) } + fn handle_desc_parameters( + req: ServiceRequest, + params: &Arc>>, + ) -> future::Ready<()> { + use rcl_interfaces::msg::ParameterDescriptor; + use rcl_interfaces::srv::DescribeParameters; + let mut descriptors = Vec::::new(); + let params = params.lock().unwrap(); + for name in &req.message.names { + if let Some(pv) = params.get(name) { + descriptors.push(ParameterDescriptor { + name: name.clone(), + type_: pv.into_parameter_type(), + ..Default::default() + }); + } else { + // parameter not found, but undeclared allowed, so return empty + descriptors.push(ParameterDescriptor { + name: name.clone(), + ..Default::default() + }); + } + } + req.respond(DescribeParameters::Response { descriptors }) + .expect("could not send reply to describe parameters request"); + future::ready(()) + } + /// Subscribe to a ROS topic. /// /// This function returns a `Stream` of ros messages. diff --git a/r2r/src/parameters.rs b/r2r/src/parameters.rs index f452c5e..465cf24 100644 --- a/r2r/src/parameters.rs +++ b/r2r/src/parameters.rs @@ -143,6 +143,21 @@ impl ParameterValue { } ret } + + pub(crate) fn into_parameter_type(&self) -> u8 { + match self { + ParameterValue::NotSet => 0, // uint8 PARAMETER_NOT_SET=0 + ParameterValue::Bool(_) => 1, // uint8 PARAMETER_BOOL=1 + ParameterValue::Integer(_) => 2, // uint8 PARAMETER_INTEGER=2 + ParameterValue::Double(_) => 3, // uint8 PARAMETER_DOUBLE=3 + ParameterValue::String(_) => 4, // uint8 PARAMETER_STRING=4 + ParameterValue::ByteArray(_) => 5, // uint8 PARAMETER_BYTE_ARRAY=5 + ParameterValue::BoolArray(_) => 6, // uint8 PARAMETER_BOOL_ARRAY=6 + ParameterValue::IntegerArray(_) => 7, // uint8 PARAMETER_INTEGER_ARRAY=7 + ParameterValue::DoubleArray(_) => 8, // uint8 PARAMETER_DOUBLE_ARRAY=8 + ParameterValue::StringArray(_) => 9, // int PARAMETER_STRING_ARRAY=9 + } + } } /// Trait for use it with