Merge pull request #15 from christianrauch/fix_dynamic_extent

handle dynamic extents

.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

src/CameraNode.cpp

@@ -440,29 +440,35 @@ 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
       RCLCPP_WARN_STREAM(get_logger(), e.what());
       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<rclcpp::Parameter> &parameters)
         }
 
         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;
         }

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<const T> vb = rhs.get<libcamera::Span<const T>>();
+    const T va = lhs.get<T>();
+    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<T>() < rhs.get<T>();
   }
@@ -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<const T> vb = rhs.get<libcamera::Span<const T>>();
+    const T va = lhs.get<T>();
+    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<T>() > rhs.get<T>();
   }

src/cv_to_pv.cpp

@@ -1,4 +1,5 @@
 #include "cv_to_pv.hpp"
+#include "type_extent.hpp"
 #include "types.hpp"
 #include <cstdint>
 #include <libcamera/base/span.h>
@@ -13,7 +14,7 @@
 
 #define CASE_CONVERT(T)           \
   case libcamera::ControlType##T: \
-    return cv_to_pv(extract_value<ControlTypeMap<libcamera::ControlType##T>::type>(value), extent);
+    return cv_to_pv(extract_value<ControlTypeMap<libcamera::ControlType##T>::type>(value));
 
 #define CASE_NONE(T)              \
   case libcamera::ControlType##T: \
@@ -48,7 +49,7 @@ template<typename T,
 rclcpp::ParameterValue
 cv_to_pv_array(const std::vector<T> & /*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<typename T>
 rclcpp::ParameterValue
-cv_to_pv(const std::vector<T> &values, const std::size_t &extent)
+cv_to_pv(const std::vector<T> &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<T>(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:

src/cv_to_pv.hpp

@@ -4,8 +4,15 @@
 #include <rclcpp/parameter_value.hpp>
 
 
+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);

src/type_extent.cpp

@@ -19,14 +19,20 @@ template<typename T, std::enable_if_t<!libcamera::details::is_span<T>::value, bo
 std::size_t
 get_extent(const libcamera::Control<T> &)
 {
+  // return an extent of 0 for non-span types
   return 0;
 }
 
 template<typename T, std::enable_if_t<libcamera::details::is_span<T>::value, bool> = true>
 std::size_t
 get_extent(const libcamera::Control<T> &)
 {
-  return libcamera::Control<T>::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<T>::type::extent;
+  static_assert(extent != 0);
+  return extent;
 }
 
 #define IF(T)                                  \
@@ -35,7 +41,7 @@ get_extent(const libcamera::Control<T> &)
 
 
 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)

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);