From 40af65036c2bf120b4bd553722814eb61c533999 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Sun, 29 Oct 2023 23:56:58 +0100 Subject: [PATCH 01/10] use the actual vector size in control value conversion Dynamic Spans have a maximum extent ('dynamic_extent') but their associated 'ControlInfo' can contain a variable number of elements. For dynamic Spans with a single element in the default 'ControlValue', this previously caused the exception 'std::length_error' since a vector with "maximum extent" elements was constructed. Fix this by ignoring the extent and using the actual number of elements. --- src/CameraNode.cpp | 2 +- src/cv_to_pv.cpp | 28 +++++++++++++--------------- src/cv_to_pv.hpp | 2 +- 3 files changed, 15 insertions(+), 17 deletions(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 4219c9e9..e6b235be 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -461,7 +461,7 @@ CameraNode::declareParameters() 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); + const rclcpp::ParameterValue value = cv_to_pv(clamp(info.def(), info.min(), info.max())); // get smallest bounds for minimum and maximum set rcl_interfaces::msg::IntegerRange range_int; diff --git a/src/cv_to_pv.cpp b/src/cv_to_pv.cpp index da77412a..687323bb 100644 --- a/src/cv_to_pv.cpp +++ b/src/cv_to_pv.cpp @@ -13,7 +13,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: \ @@ -75,25 +75,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) diff --git a/src/cv_to_pv.hpp b/src/cv_to_pv.hpp index 4f40e4af..0e565eb4 100644 --- a/src/cv_to_pv.hpp +++ b/src/cv_to_pv.hpp @@ -5,7 +5,7 @@ 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); From e7c29d22c564b391b32d5ee2e8900270b3ce4109 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Mon, 30 Oct 2023 20:59:15 +0100 Subject: [PATCH 02/10] handle dynamic extents in parameter description --- src/CameraNode.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index e6b235be..70b093db 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -451,10 +451,13 @@ CameraNode::declareParameters() } // format type description + const bool scalar = (extent == 0) || (extent == 1); + 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()) + " " + - std::string(extent > 1 ? "array[" + std::to_string(extent) + "]" : "scalar") + " range {" + - info.min().toString() + "}..{" + info.max().toString() + "}" + + 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()) From c4ce11c152b17718f59d945e87886ba8b214e714 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Mon, 30 Oct 2023 22:02:58 +0100 Subject: [PATCH 03/10] handle invalid conversion of non-arithmetic types via custom exception --- src/CameraNode.cpp | 12 +++++++++++- src/cv_to_pv.cpp | 2 +- src/cv_to_pv.hpp | 7 +++++++ 3 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 70b093db..bf946612 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -464,7 +464,17 @@ CameraNode::declareParameters() 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())); + 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; + } // get smallest bounds for minimum and maximum set rcl_interfaces::msg::IntegerRange range_int; diff --git a/src/cv_to_pv.cpp b/src/cv_to_pv.cpp index 687323bb..195dac42 100644 --- a/src/cv_to_pv.cpp +++ b/src/cv_to_pv.cpp @@ -48,7 +48,7 @@ template & /*values*/) { - throw std::runtime_error("ParameterValue only supported for arithmetic types"); + throw invalid_conversion("ParameterValue only supported for arithmetic types"); } template< diff --git a/src/cv_to_pv.hpp b/src/cv_to_pv.hpp index 0e565eb4..fc5d910f 100644 --- a/src/cv_to_pv.hpp +++ b/src/cv_to_pv.hpp @@ -4,6 +4,13 @@ #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); From 5232b4b6f07543274fe689fed70451a49d326726 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Mon, 17 Jun 2024 21:49:12 +0200 Subject: [PATCH 04/10] implement scalar-array less and greater comparison --- src/clamp.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/clamp.cpp b/src/clamp.cpp index 88f0cf6c..8a8b034e 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(); } From 197bb40c423b7158ed3504d7fd6bb2b7bde65ab0 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Fri, 21 Jun 2024 20:48:31 +0200 Subject: [PATCH 05/10] handle dynamic extents in array size check --- src/CameraNode.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index bf946612..0ce9f287 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -715,9 +715,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; } From 4f957a782a028b6ce9e426727ea320c8ded8cac8 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Sat, 22 Jun 2024 13:24:20 +0200 Subject: [PATCH 06/10] remove 'colcon-ros' as manual dependency --- .github/workflows/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 474dbfe3..61683a3a 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 From aa16bae567b798debaf3d53cb31d95fcb4802880 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Sun, 23 Jun 2024 20:43:12 +0200 Subject: [PATCH 07/10] remove redundant ';' --- src/cv_to_pv.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/cv_to_pv.hpp b/src/cv_to_pv.hpp index fc5d910f..7a3892af 100644 --- a/src/cv_to_pv.hpp +++ b/src/cv_to_pv.hpp @@ -7,7 +7,7 @@ class invalid_conversion : public std::runtime_error { public: - explicit invalid_conversion(const std::string &msg) : std::runtime_error(msg) {}; + explicit invalid_conversion(const std::string &msg) : std::runtime_error(msg) {} }; From 2c7789787b44b4d55f2eaa956108fd18f40e793c Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Mon, 5 Aug 2024 19:10:35 +0200 Subject: [PATCH 08/10] clarify the value range and meaning of the extracted ControlId extent Previously 'get_extent' would return the original 'extent' of a Span and 0 for non-span controls. This is ambiguous as an extent of 0 means that the original control type is either not a span or an empty span that can store no elements. Resolve this ambiguity by enforcing that libcamera controls cannot contain empty spans via a compile time assertion and clarify that an extent of 0 is only returned for non-span types. --- src/CameraNode.cpp | 2 +- src/type_extent.cpp | 10 ++++++++-- src/type_extent.hpp | 8 +++++++- 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 0ce9f287..8f83933a 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -451,7 +451,7 @@ CameraNode::declareParameters() } // format type description - const bool scalar = (extent == 0) || (extent == 1); + 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)) + "]"; diff --git a/src/type_extent.cpp b/src/type_extent.cpp index d67f28b0..b189cda4 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 40b1efda..ca9b9d19 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); From c76b8ddac7209de72866280f8fff4e4747e1d84e Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Mon, 5 Aug 2024 21:24:41 +0200 Subject: [PATCH 09/10] remove array flag from 'cv_to_pv_type' and determine this via the extent --- src/CameraNode.cpp | 2 +- src/cv_to_pv.cpp | 9 +++++---- src/cv_to_pv.hpp | 2 +- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 8f83933a..01729796 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -508,7 +508,7 @@ CameraNode::declareParameters() 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(), cv_to_pv_type(id), param_descr); } else { declare_parameter(id->name(), value, param_descr); diff --git a/src/cv_to_pv.cpp b/src/cv_to_pv.cpp index 195dac42..2a78dca3 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 @@ -109,10 +110,10 @@ 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) { - 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: @@ -132,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 7a3892af..b750c214 100644 --- a/src/cv_to_pv.hpp +++ b/src/cv_to_pv.hpp @@ -15,4 +15,4 @@ rclcpp::ParameterValue 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); From 0a747285410c3d1ba4d6692155490c134e140be2 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Mon, 5 Aug 2024 22:19:54 +0200 Subject: [PATCH 10/10] check for supported parameter value type based on control id and not value libcamera 'Control' and their related 'ControlInfo' do not necessarily have the same control and value types. This previously caused issues when an unsupported control type, such as a span of a complex type, is mapped to a ROS parameter via the type of the default 'ControlValue'. Fix this by using the the actual control type, regardless of the types of the values in the 'ControlInfo'. --- src/CameraNode.cpp | 65 +++++++++++++++++++++++++--------------------- 1 file changed, 35 insertions(+), 30 deletions(-) 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);