From 0a747285410c3d1ba4d6692155490c134e140be2 Mon Sep 17 00:00:00 2001
From: Christian Rauch <Rauch.Christian@gmx.de>
Date: Mon, 5 Aug 2024 22:19:54 +0200
Subject: [PATCH] check for supported parameter value type based on control id
 and not value

libcamera 'Control<T>' 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);