weird test fix with consume lock

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

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;

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