diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 474dbfe..61683a3 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -35,7 +35,7 @@ jobs: apt install -y apt-rdepends ros-${{ matrix.distribution }}-launch-ros python3 -m venv ~/.venvs/dev . ~/.venvs/dev/bin/activate - pip install colcon-ros lark + pip install lark pip install colcon-lint - name: build and test diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 4219c9e..8ead5a7 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -440,9 +440,28 @@ CameraNode::declareParameters() // store control id with name parameter_ids[id->name()] = id; - std::size_t extent; + 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 + rcl_interfaces::msg::ParameterDescriptor param_descr; try { - extent = get_extent(id); + 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 std::runtime_error &e) { // ignore @@ -450,19 +469,6 @@ CameraNode::declareParameters() continue; } - // format type description - const std::string cv_descr = - std::to_string(id->type()) + " " + - std::string(extent > 1 ? "array[" + std::to_string(extent) + "]" : "scalar") + " 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 - const rclcpp::ParameterValue value = cv_to_pv(clamp(info.def(), info.min(), info.max()), extent); - // get smallest bounds for minimum and maximum set rcl_interfaces::msg::IntegerRange range_int; rcl_interfaces::msg::FloatingPointRange range_float; @@ -484,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->type(), extent > 0), param_descr); + declare_parameter(id->name(), pv_type, param_descr); } else { declare_parameter(id->name(), value, param_descr); @@ -702,9 +720,12 @@ CameraNode::onParameterChange(const std::vector ¶meters) } const std::size_t extent = get_extent(id); - if ((value.isArray() && (extent > 0)) && value.numElements() != extent) { + if (value.isArray() && + (extent != libcamera::dynamic_extent) && + (value.numElements() != extent)) + { result.successful = false; - result.reason = parameter.get_name() + ": parameter dimensions mismatch, expected " + + result.reason = parameter.get_name() + ": array dimensions mismatch, expected " + std::to_string(extent) + ", got " + std::to_string(value.numElements()); return result; } diff --git a/src/clamp.cpp b/src/clamp.cpp index 88f0cf6..8a8b034 100644 --- a/src/clamp.cpp +++ b/src/clamp.cpp @@ -181,7 +181,17 @@ less(const libcamera::ControlValue &lhs, const libcamera::ControlValue &rhs) return false; } } + else if (rhs.isArray()) { + // scalar-array comparison + const libcamera::Span vb = rhs.get>(); + const T va = lhs.get(); + for (size_t i = 0; i < vb.size(); i++) + if (va < vb[i]) + return true; + return false; + } else { + assert(!lhs.isArray() && !rhs.isArray()); // scalar-scalar comparison return lhs.get() < rhs.get(); } @@ -211,7 +221,17 @@ greater(const libcamera::ControlValue &lhs, const libcamera::ControlValue &rhs) return false; } } + else if (rhs.isArray()) { + // scalar-array comparison + const libcamera::Span vb = rhs.get>(); + const T va = lhs.get(); + for (size_t i = 0; i < vb.size(); i++) + if (va > vb[i]) + return true; + return false; + } else { + assert(!lhs.isArray() && !rhs.isArray()); // scalar-scalar comparison return lhs.get() > rhs.get(); } diff --git a/src/cv_to_pv.cpp b/src/cv_to_pv.cpp index da77412..2a78dca 100644 --- a/src/cv_to_pv.cpp +++ b/src/cv_to_pv.cpp @@ -1,4 +1,5 @@ #include "cv_to_pv.hpp" +#include "type_extent.hpp" #include "types.hpp" #include #include @@ -13,7 +14,7 @@ #define CASE_CONVERT(T) \ case libcamera::ControlType##T: \ - return cv_to_pv(extract_value::type>(value), extent); + return cv_to_pv(extract_value::type>(value)); #define CASE_NONE(T) \ case libcamera::ControlType##T: \ @@ -48,7 +49,7 @@ template & /*values*/) { - throw std::runtime_error("ParameterValue only supported for arithmetic types"); + throw invalid_conversion("ParameterValue only supported for arithmetic types"); } template< @@ -75,25 +76,23 @@ cv_to_pv_scalar(const libcamera::Size &size) template rclcpp::ParameterValue -cv_to_pv(const std::vector &values, const std::size_t &extent) +cv_to_pv(const std::vector &values) { - if ((values.size() > 1 && extent > 1) && (values.size() != extent)) - throw std::runtime_error("type extent (" + std::to_string(extent) + ") and value size (" + - std::to_string(values.size()) + ") cannot be larger than 1 and differ"); - - if (values.size() > 1) - return cv_to_pv_array(values); - else if (values.size() == 1) - if (!extent) - return cv_to_pv_scalar(values[0]); - else - return cv_to_pv_array(std::vector(extent, values[0])); - else + switch (values.size()) { + case 0: + // empty array return rclcpp::ParameterValue(); + case 1: + // single element (scalar) + return cv_to_pv_scalar(values[0]); + default: + // dynamic array + return cv_to_pv_array(values); + } } rclcpp::ParameterValue -cv_to_pv(const libcamera::ControlValue &value, const std::size_t &extent) +cv_to_pv(const libcamera::ControlValue &value) { switch (value.type()) { CASE_NONE(None) @@ -111,10 +110,10 @@ cv_to_pv(const libcamera::ControlValue &value, const std::size_t &extent) } rclcpp::ParameterType -cv_to_pv_type(const libcamera::ControlType &type, const bool is_array) +cv_to_pv_type(const libcamera::ControlId *const id) { - if (!is_array) { - switch (type) { + if (get_extent(id) == 0) { + switch (id->type()) { case libcamera::ControlType::ControlTypeNone: return rclcpp::ParameterType::PARAMETER_NOT_SET; case libcamera::ControlType::ControlTypeBool: @@ -134,7 +133,7 @@ cv_to_pv_type(const libcamera::ControlType &type, const bool is_array) } } else { - switch (type) { + switch (id->type()) { case libcamera::ControlType::ControlTypeNone: return rclcpp::ParameterType::PARAMETER_NOT_SET; case libcamera::ControlType::ControlTypeBool: diff --git a/src/cv_to_pv.hpp b/src/cv_to_pv.hpp index 4f40e4a..b750c21 100644 --- a/src/cv_to_pv.hpp +++ b/src/cv_to_pv.hpp @@ -4,8 +4,15 @@ #include +class invalid_conversion : public std::runtime_error +{ +public: + explicit invalid_conversion(const std::string &msg) : std::runtime_error(msg) {} +}; + + rclcpp::ParameterValue -cv_to_pv(const libcamera::ControlValue &value, const std::size_t &extent); +cv_to_pv(const libcamera::ControlValue &value); rclcpp::ParameterType -cv_to_pv_type(const libcamera::ControlType &type, const bool is_array); +cv_to_pv_type(const libcamera::ControlId *const id); diff --git a/src/type_extent.cpp b/src/type_extent.cpp index d67f28b..b189cda 100644 --- a/src/type_extent.cpp +++ b/src/type_extent.cpp @@ -19,6 +19,7 @@ template::value, bo std::size_t get_extent(const libcamera::Control &) { + // return an extent of 0 for non-span types return 0; } @@ -26,7 +27,12 @@ template::value, boo std::size_t get_extent(const libcamera::Control &) { - return libcamera::Control::type::extent; + // return the span extent, excluding 0 + // This assumes that libcamera does not define control types + // with a fixed size span that does not hold any elements + constexpr std::size_t extent = libcamera::Control::type::extent; + static_assert(extent != 0); + return extent; } #define IF(T) \ @@ -35,7 +41,7 @@ get_extent(const libcamera::Control &) std::size_t -get_extent(const libcamera::ControlId *id) +get_extent(const libcamera::ControlId *const id) { #if LIBCAMERA_VER_GE(0, 1, 0) IF(AeEnable) diff --git a/src/type_extent.hpp b/src/type_extent.hpp index 40b1efd..ca9b9d1 100644 --- a/src/type_extent.hpp +++ b/src/type_extent.hpp @@ -6,5 +6,11 @@ namespace libcamera class ControlId; } +/** + * @brief get the extent of a libcamera::ControlId + * @param id + * @return the extent of the control: 0 if the control is not a span, + * otherwise [1 ... libcamera::dynamic_extent] + */ std::size_t -get_extent(const libcamera::ControlId *id); +get_extent(const libcamera::ControlId *const id);