Merge pull request #75 from christianrauch/new_control_types

support new control types

src/CameraNode.cpp

@@ -70,6 +70,12 @@ namespace rclcpp
 class NodeOptions;
 }
 
+#define CASE_RANGE(T, R)                                       \
+  case libcamera::ControlType##T:                              \
+    R.from_value = max<libcamera::ControlType##T>(info.min()); \
+    R.to_value = min<libcamera::ControlType##T>(info.max());   \
+    break;
+
 
 namespace camera
 {
@@ -270,7 +276,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti
     RCLCPP_DEBUG_STREAM(get_logger(), "found camera by name: \"" << name << "\"");
   } break;
   default:
-    RCLCPP_ERROR_STREAM(get_logger(), "unuspported camera parameter type: "
+    RCLCPP_ERROR_STREAM(get_logger(), "unsupported camera parameter type: "
                                         << get_parameter("camera").get_type_name());
     break;
   }
@@ -331,7 +337,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti
 
   const libcamera::Size size(get_parameter("width").as_int(), get_parameter("height").as_int());
   if (size.isNull()) {
-    RCLCPP_INFO_STREAM(get_logger(), scfg);
+    RCLCPP_INFO_STREAM(get_logger(), list_format_sizes(scfg));
     RCLCPP_WARN_STREAM(get_logger(),
                        "no dimensions selected, auto-selecting: \"" << scfg.size << "\"");
     RCLCPP_WARN_STREAM(get_logger(), "set parameter 'width' or 'height' to silence this warning");
@@ -350,7 +356,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti
     if (selected_scfg.pixelFormat != scfg.pixelFormat)
       RCLCPP_INFO_STREAM(get_logger(), stream_formats);
     if (selected_scfg.size != scfg.size)
-      RCLCPP_INFO_STREAM(get_logger(), scfg);
+      RCLCPP_INFO_STREAM(get_logger(), list_format_sizes(scfg));
     RCLCPP_WARN_STREAM(get_logger(), "stream configuration adjusted from \""
                                        << selected_scfg.toString() << "\" to \"" << scfg.toString()
                                        << "\"");
@@ -528,18 +534,13 @@ CameraNode::declareParameters()
     rcl_interfaces::msg::FloatingPointRange range_float;
 
     switch (id->type()) {
-    case libcamera::ControlTypeInteger32:
-      range_int.from_value = max<libcamera::ControlTypeInteger32>(info.min());
-      range_int.to_value = min<libcamera::ControlTypeInteger32>(info.max());
-      break;
-    case libcamera::ControlTypeInteger64:
-      range_int.from_value = max<libcamera::ControlTypeInteger64>(info.min());
-      range_int.to_value = min<libcamera::ControlTypeInteger64>(info.max());
-      break;
-    case libcamera::ControlTypeFloat:
-      range_float.from_value = max<libcamera::ControlTypeFloat>(info.min());
-      range_float.to_value = min<libcamera::ControlTypeFloat>(info.max());
-      break;
+      CASE_RANGE(Integer32, range_int)
+      CASE_RANGE(Integer64, range_int)
+      CASE_RANGE(Float, range_float)
+#if LIBCAMERA_VER_GE(0, 4, 0)
+      CASE_RANGE(Unsigned16, range_int)
+      CASE_RANGE(Unsigned32, range_int)
+#endif
     default:
       break;
     }

src/clamp.cpp

@@ -1,3 +1,4 @@
+#include "libcamera_version_utils.hpp"
 #include "types.hpp"
 #include <algorithm>
 #include <cassert>
@@ -66,6 +67,12 @@ MIN(Float)
 MAX(Integer32)
 MAX(Integer64)
 MAX(Float)
+#if LIBCAMERA_VER_GE(0, 4, 0)
+MIN(Unsigned16)
+MIN(Unsigned32)
+MAX(Unsigned16)
+MAX(Unsigned32)
+#endif
 
 namespace std
 {
@@ -79,6 +86,17 @@ clamp(const CTRectangle &val, const CTRectangle &lo, const CTRectangle &hi)
 
   return CTRectangle {x, y, width, height};
 }
+
+#if LIBCAMERA_VER_GE(0, 4, 0)
+CTPoint
+clamp(const CTPoint &val, const CTPoint &lo, const CTPoint &hi)
+{
+  const int x = std::clamp(val.x, lo.x, hi.x);
+  const int y = std::clamp(val.y, lo.y, hi.y);
+
+  return CTPoint {x, y};
+}
+#endif
 } // namespace std
 
 
@@ -135,6 +153,11 @@ clamp(const libcamera::ControlValue &value, const libcamera::ControlValue &min,
     CASE_CLAMP(String)
     CASE_CLAMP(Rectangle)
     CASE_CLAMP(Size)
+#if LIBCAMERA_VER_GE(0, 4, 0)
+    CASE_CLAMP(Unsigned16)
+    CASE_CLAMP(Unsigned32)
+    CASE_CLAMP(Point)
+#endif
   }
 
   return {};
@@ -157,6 +180,28 @@ operator>(const libcamera::Rectangle &lhs, const libcamera::Rectangle &rhs)
          (lhs.y + lhs.height) > (rhs.y + rhs.height);
 }
 
+#if LIBCAMERA_VER_GE(0, 4, 0)
+int
+squared_sum(const libcamera::Point &p)
+{
+  return p.x * p.x + p.y * p.y;
+}
+
+bool
+operator<(const libcamera::Point &lhs, const libcamera::Point &rhs)
+{
+  // check if lhs point is closer to origin than rhs point
+  return squared_sum(lhs) < squared_sum(rhs);
+}
+
+bool
+operator>(const libcamera::Point &lhs, const libcamera::Point &rhs)
+{
+  // check if lhs point is further away from origin than rhs point
+  return squared_sum(lhs) > squared_sum(rhs);
+}
+#endif
+
 template<typename T>
 bool
 less(const libcamera::ControlValue &lhs, const libcamera::ControlValue &rhs)
@@ -261,6 +306,11 @@ operator<(const libcamera::ControlValue &lhs, const libcamera::ControlValue &rhs
     CASE_LESS(String)
     CASE_LESS(Rectangle)
     CASE_LESS(Size)
+#if LIBCAMERA_VER_GE(0, 4, 0)
+    CASE_LESS(Unsigned16)
+    CASE_LESS(Unsigned32)
+    CASE_LESS(Point)
+#endif
   }
 
   throw std::runtime_error("unhandled control type " + std::to_string(lhs.type()));
@@ -282,6 +332,11 @@ operator>(const libcamera::ControlValue &lhs, const libcamera::ControlValue &rhs
     CASE_GREATER(String)
     CASE_GREATER(Rectangle)
     CASE_GREATER(Size)
+#if LIBCAMERA_VER_GE(0, 4, 0)
+    CASE_GREATER(Unsigned16)
+    CASE_GREATER(Unsigned32)
+    CASE_GREATER(Point)
+#endif
   }
 
   throw std::runtime_error("unhandled control type " + std::to_string(lhs.type()));

src/cv_to_pv.cpp

@@ -1,4 +1,5 @@
 #include "cv_to_pv.hpp"
+#include "libcamera_version_utils.hpp"
 #include "type_extent.hpp"
 #include "types.hpp"
 #include <cstdint>
@@ -35,31 +36,43 @@ extract_value(const libcamera::ControlValue &value)
 
 template<
   typename T,
-  std::enable_if_t<std::is_arithmetic<T>::value || std::is_same<std::string, T>::value, bool> = true>
+  std::enable_if_t<std::is_constructible<rclcpp::ParameterValue, std::vector<T>>::value, bool> = true>
 rclcpp::ParameterValue
 cv_to_pv_array(const std::vector<T> &values)
 {
   return rclcpp::ParameterValue(values);
 }
 
-template<typename T,
-         std::enable_if_t<!std::is_arithmetic<T>::value && !std::is_same<std::string, T>::value,
-                          bool> = true>
+template<
+  typename T,
+  std::enable_if_t<!std::is_constructible<rclcpp::ParameterValue, std::vector<T>>::value, bool> = true>
 rclcpp::ParameterValue
 cv_to_pv_array(const std::vector<T> & /*values*/)
 {
-  throw invalid_conversion("ParameterValue only supported for arithmetic types");
+  throw invalid_conversion("ParameterValue not constructible from complex type.");
 }
 
 template<
   typename T,
-  std::enable_if_t<std::is_arithmetic<T>::value || std::is_same<std::string, T>::value, bool> = true>
+  std::enable_if_t<std::is_constructible<rclcpp::ParameterValue, T>::value, bool> = true>
 rclcpp::ParameterValue
 cv_to_pv_scalar(const T &value)
 {
   return rclcpp::ParameterValue(value);
 }
 
+rclcpp::ParameterValue
+cv_to_pv_scalar(const uint16_t &val)
+{
+  return rclcpp::ParameterValue(static_cast<int32_t>(val));
+}
+
+rclcpp::ParameterValue
+cv_to_pv_scalar(const uint32_t &val)
+{
+  return rclcpp::ParameterValue(static_cast<int64_t>(val));
+}
+
 rclcpp::ParameterValue
 cv_to_pv_scalar(const libcamera::Rectangle &rectangle)
 {
@@ -73,6 +86,14 @@ cv_to_pv_scalar(const libcamera::Size &size)
   return rclcpp::ParameterValue(std::vector<int64_t> {size.width, size.height});
 }
 
+#if LIBCAMERA_VER_GE(0, 4, 0)
+rclcpp::ParameterValue
+cv_to_pv_scalar(const libcamera::Point &point)
+{
+  return rclcpp::ParameterValue(std::vector<int64_t> {point.x, point.y});
+}
+#endif
+
 template<typename T>
 rclcpp::ParameterValue
 cv_to_pv(const std::vector<T> &values)
@@ -103,6 +124,11 @@ cv_to_pv(const libcamera::ControlValue &value)
     CASE_CONVERT(String)
     CASE_CONVERT(Rectangle)
     CASE_CONVERT(Size)
+#if LIBCAMERA_VER_GE(0, 4, 0)
+    CASE_CONVERT(Unsigned16)
+    CASE_CONVERT(Unsigned32)
+    CASE_CONVERT(Point)
+#endif
   }
 
   return {};
@@ -118,6 +144,10 @@ cv_to_pv_type(const libcamera::ControlId *const id)
     case libcamera::ControlType::ControlTypeBool:
       return rclcpp::ParameterType::PARAMETER_BOOL;
     case libcamera::ControlType::ControlTypeByte:
+#if LIBCAMERA_VER_GE(0, 4, 0)
+    case libcamera::ControlType::ControlTypeUnsigned16:
+    case libcamera::ControlType::ControlTypeUnsigned32:
+#endif
     case libcamera::ControlType::ControlTypeInteger32:
     case libcamera::ControlType::ControlTypeInteger64:
       return rclcpp::ParameterType::PARAMETER_INTEGER;
@@ -129,6 +159,10 @@ cv_to_pv_type(const libcamera::ControlId *const id)
       return rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY;
     case libcamera::ControlType::ControlTypeSize:
       return rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY;
+#if LIBCAMERA_VER_GE(0, 4, 0)
+    case libcamera::ControlType::ControlTypePoint:
+      return rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY;
+#endif
     }
   }
   else {
@@ -138,6 +172,10 @@ cv_to_pv_type(const libcamera::ControlId *const id)
     case libcamera::ControlType::ControlTypeBool:
       return rclcpp::ParameterType::PARAMETER_BOOL_ARRAY;
     case libcamera::ControlType::ControlTypeByte:
+#if LIBCAMERA_VER_GE(0, 4, 0)
+    case libcamera::ControlType::ControlTypeUnsigned16:
+    case libcamera::ControlType::ControlTypeUnsigned32:
+#endif
     case libcamera::ControlType::ControlTypeInteger32:
     case libcamera::ControlType::ControlTypeInteger64:
       return rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY;
@@ -149,6 +187,10 @@ cv_to_pv_type(const libcamera::ControlId *const id)
       return rclcpp::ParameterType::PARAMETER_NOT_SET;
     case libcamera::ControlType::ControlTypeSize:
       return rclcpp::ParameterType::PARAMETER_NOT_SET;
+#if LIBCAMERA_VER_GE(0, 4, 0)
+    case libcamera::ControlType::ControlTypePoint:
+      return rclcpp::ParameterType::PARAMETER_NOT_SET;
+#endif
     }
   }
 

src/cv_to_pv.hpp

@@ -1,6 +1,6 @@
 #pragma once
+#include "exceptions.hpp"
 #include <rclcpp/parameter_value.hpp>
-#include <stdexcept>
 #include <string>
 
 namespace libcamera
@@ -10,13 +10,6 @@ class ControlValue;
 } // namespace libcamera
 
 
-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);
 

src/exceptions.hpp

@@ -0,0 +1,15 @@
+#pragma once
+#include <stdexcept>
+
+
+class invalid_conversion : public std::runtime_error
+{
+public:
+  explicit invalid_conversion(const std::string &msg) : std::runtime_error(msg) {}
+};
+
+class should_not_reach : public std::runtime_error
+{
+public:
+  explicit should_not_reach() : std::runtime_error("should not reach here") {}
+};

src/libcamera_version_utils.hpp

@@ -0,0 +1,8 @@
+#pragma once
+#include <libcamera/version.h>
+
+#define LIBCAMERA_VER_GE(major, minor, patch)                               \
+  ((major < LIBCAMERA_VERSION_MAJOR) ||                                     \
+   (major == LIBCAMERA_VERSION_MAJOR && minor < LIBCAMERA_VERSION_MINOR) || \
+   (major == LIBCAMERA_VERSION_MAJOR && minor == LIBCAMERA_VERSION_MINOR && \
+    patch <= LIBCAMERA_VERSION_PATCH))

src/pretty_print.cpp

@@ -9,6 +9,7 @@
 #include <libcamera/stream.h>
 #include <memory>
 #include <optional>
+#include <sstream>
 #include <string>
 #include <vector>
 
@@ -43,13 +44,14 @@ operator<<(std::ostream &out, const libcamera::StreamFormats &formats)
   return out;
 }
 
-std::ostream &
-operator<<(std::ostream &out, const libcamera::StreamConfiguration &configuration)
+std::string
+list_format_sizes(const libcamera::StreamConfiguration &configuration)
 {
+  std::ostringstream out;
   out << std::endl
       << ">> " << configuration.pixelFormat << " format sizes:";
   for (const libcamera::Size &size : configuration.formats().sizes(configuration.pixelFormat))
     out << std::endl
         << "   - " << size.toString();
-  return out;
+  return out.str();
 }

src/pretty_print.hpp

@@ -15,5 +15,5 @@ operator<<(std::ostream &out, const libcamera::CameraManager &camera_manager);
 std::ostream &
 operator<<(std::ostream &out, const libcamera::StreamFormats &formats);
 
-std::ostream &
-operator<<(std::ostream &out, const libcamera::StreamConfiguration &configuration);
+std::string
+list_format_sizes(const libcamera::StreamConfiguration &configuration);