diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 01729796..8ead5a79 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -440,39 +440,32 @@ CameraNode::declareParameters() // store control id with name parameter_ids[id->name()] = id; - std::size_t extent; - try { - extent = get_extent(id); - } - catch (const std::runtime_error &e) { - // ignore - RCLCPP_WARN_STREAM(get_logger(), e.what()); + if (info.min().numElements() != info.max().numElements()) + throw std::runtime_error("minimum and maximum parameter array sizes do not match"); + + // check if the control can be mapped to a parameter + const rclcpp::ParameterType pv_type = cv_to_pv_type(id); + if (pv_type == rclcpp::ParameterType::PARAMETER_NOT_SET) { + RCLCPP_WARN_STREAM(get_logger(), "unsupported control '" << id->name() << "'"); continue; } // format type description - const bool scalar = (extent == 0); - const bool dynamic = (extent == libcamera::dynamic_extent); - const std::string cv_type_descr = - scalar ? "scalar" : "array[" + (dynamic ? std::string() : std::to_string(extent)) + "]"; - const std::string cv_descr = - std::to_string(id->type()) + " " + cv_type_descr + " range {" + info.min().toString() + - "}..{" + info.max().toString() + "}" + - (info.def().isNone() ? std::string {} : " (default: {" + info.def().toString() + "})"); - - if (info.min().numElements() != info.max().numElements()) - throw std::runtime_error("minimum and maximum parameter array sizes do not match"); - - // clamp default ControlValue to min/max range and cast ParameterValue - rclcpp::ParameterValue value; + rcl_interfaces::msg::ParameterDescriptor param_descr; try { - value = cv_to_pv(clamp(info.def(), info.min(), info.max())); + const std::size_t extent = get_extent(id); + const bool scalar = (extent == 0); + const bool dynamic = (extent == libcamera::dynamic_extent); + const std::string cv_type_descr = + scalar ? "scalar" : "array[" + (dynamic ? std::string() : std::to_string(extent)) + "]"; + param_descr.description = + std::to_string(id->type()) + " " + cv_type_descr + " range {" + info.min().toString() + + "}..{" + info.max().toString() + "}" + + (info.def().isNone() ? std::string {} : " (default: {" + info.def().toString() + "})"); } - catch (const invalid_conversion &e) { - RCLCPP_ERROR_STREAM(get_logger(), "unsupported control '" - << id->name() - << "' (type: " << std::to_string(info.def().type()) << "): " - << e.what()); + catch (const std::runtime_error &e) { + // ignore + RCLCPP_WARN_STREAM(get_logger(), e.what()); continue; } @@ -497,18 +490,30 @@ CameraNode::declareParameters() break; } - rcl_interfaces::msg::ParameterDescriptor param_descr; - param_descr.description = cv_descr; if (range_int.from_value != range_int.to_value) param_descr.integer_range = {range_int}; if (range_float.from_value != range_float.to_value) param_descr.floating_point_range = {range_float}; + // clamp default ControlValue to min/max range and cast ParameterValue + rclcpp::ParameterValue value; + try { + value = cv_to_pv(clamp(info.def(), info.min(), info.max())); + } + catch (const invalid_conversion &e) { + RCLCPP_ERROR_STREAM(get_logger(), "unsupported control '" + << id->name() + << "' (type: " << std::to_string(info.def().type()) << "): " + << e.what()); + continue; + } + // declare parameters and set default or initial value RCLCPP_DEBUG_STREAM(get_logger(), "declare " << id->name() << " with default " << rclcpp::to_string(value)); + if (value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { - declare_parameter(id->name(), cv_to_pv_type(id), param_descr); + declare_parameter(id->name(), pv_type, param_descr); } else { declare_parameter(id->name(), value, param_descr);