From 127ca599b0320a8a0ef4fe8aa2e9a8ba6175df46 Mon Sep 17 00:00:00 2001
From: Christian Rauch <Rauch.Christian@gmx.de>
Date: Sun, 24 Nov 2024 14:29:27 +0100
Subject: [PATCH] weird test fix with consume lock

---
 src/CameraNode.cpp       |  1 +
 src/ParameterHandler.cpp | 24 +++++++++++++++++++++---
 src/ParameterHandler.hpp |  1 +
 3 files changed, 23 insertions(+), 3 deletions(-)

diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp
index 5c1f47bb..90d02af0 100644
--- a/src/CameraNode.cpp
+++ b/src/CameraNode.cpp
@@ -582,6 +582,7 @@ CameraNode::process(libcamera::Request *const request)
     // update parameters
     request->controls() = parameter_handler.get();
     parameter_handler.clear();
+    RCLCPP_DEBUG_STREAM(get_logger(), "applied " << request->controls().size() << " controls");
     for (const auto &[id, value] : request->controls()) {
       const std::string &name = libcamera::controls::controls.at(id)->name();
       RCLCPP_DEBUG_STREAM(get_logger(), "applied " << name << ": " << value.toString());
diff --git a/src/ParameterHandler.cpp b/src/ParameterHandler.cpp
index d59b3623..0ed69300 100644
--- a/src/ParameterHandler.cpp
+++ b/src/ParameterHandler.cpp
@@ -413,6 +413,8 @@ ParameterHandler::get()
 {
   const std::lock_guard<std::mutex> lock(parameters_lock);
   // TODO: final check of conflicts for gathered controls?
+  // if (!control_values.empty())
+  parameters_consumed_lock.unlock();
   return control_values;
 }
 
@@ -547,13 +549,29 @@ ParameterHandler::apply(const std::vector<rclcpp::Parameter> &parameters)
   // NOTE: apply could be called multiple times before 'control_values' is read,
   // should we clear 'control_values' on every apply to keep previous checks valid?
 
+  // filter
+  std::vector<rclcpp::Parameter> controls;
+  for (const rclcpp::Parameter &parameter : parameters) {
+    if (parameter_ids.count(parameter.get_name()))
+      controls.push_back(parameter);
+  }
+
+  if (controls.empty())
+    return;
+
   // TODO: use a callback to set controls immediately on request
 
+  // wait for previous controls to be consumed
+  std::cout << "wait parameters_consumed_lock ..." << std::endl;
+  parameters_consumed_lock.lock();
+
   parameters_lock.lock();
   // control_values.clear(); // need this??
-  for (const rclcpp::Parameter &parameter : parameters) {
-    if (!parameter_ids.count(parameter.get_name()))
-      continue;
+  RCLCPP_DEBUG_STREAM(node->get_logger(), "apply " << controls.size() << " parameters");
+  for (const rclcpp::Parameter &parameter : controls) {
+    // RCLCPP_DEBUG_STREAM(node->get_logger(), "apply " << parameter.get_name());
+    // if (!parameter_ids.count(parameter.get_name()))
+    //   continue;
     const libcamera::ControlId *id = parameter_ids.at(parameter.get_name());
     const libcamera::ControlValue value = pv_to_cv(parameter, id->type());
     // control_values[parameter_ids.at(parameter.get_name())->id()] = value;
diff --git a/src/ParameterHandler.hpp b/src/ParameterHandler.hpp
index c3dfb018..23abefcc 100644
--- a/src/ParameterHandler.hpp
+++ b/src/ParameterHandler.hpp
@@ -54,6 +54,7 @@ class ParameterHandler
   // ControlValueMap control_values;
   libcamera::ControlList control_values;
   std::mutex parameters_lock;
+  std::mutex parameters_consumed_lock;
 
   void
   adjust(std::vector<rclcpp::Parameter> &parameters);