callback

src/CameraNode.cpp

@@ -128,6 +128,9 @@ class CameraNode : public rclcpp::Node
   rcl_interfaces::msg::SetParametersResult
   onParameterChange(const std::vector<rclcpp::Parameter> &parameters);
 #endif
+
+  void
+  apply_parameters(const libcamera::ControlList &controls);
 };
 
 RCLCPP_COMPONENTS_REGISTER_NODE(camera::CameraNode)
@@ -391,6 +394,8 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options)
     throw std::runtime_error("camera name must only contain alphanumeric characters");
 
   parameter_handler.declare(camera->controls());
+  parameter_handler.set_on_apply_callback(
+    std::bind(&CameraNode::apply_parameters, this, std::placeholders::_1));
 
   // allocate stream buffers and create one request per buffer
   stream = scfg.stream();
@@ -579,14 +584,14 @@ CameraNode::process(libcamera::Request *const request)
     // queue the request again for the next frame
     request->reuse(libcamera::Request::ReuseBuffers);
 
-    // 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());
-    }
+    // // 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());
+    // }
 
     // const auto parameters = parameter_handler.get();
     // if (!parameters.empty()) {
@@ -630,4 +635,20 @@ CameraNode::onParameterChange(const std::vector<rclcpp::Parameter> &parameters)
 }
 #endif
 
+void
+CameraNode::apply_parameters(const libcamera::ControlList &controls)
+{
+  for (const std::unique_ptr<libcamera::Request> &request : requests) {
+    std::unique_lock lk(request_mutexes.at(request.get()));
+    // update parameters
+    request->controls() = controls;
+    // 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(), "control applied " << name << ": " << value.toString());
+    }
+  }
+}
+
 } // namespace camera

src/ParameterHandler.cpp

@@ -414,16 +414,22 @@ 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();
+  // parameters_consumed_lock.unlock();
   return control_values;
 }
 
+// void
+// ParameterHandler::clear()
+// {
+//   parameters_lock.lock();
+//   control_values.clear();
+//   parameters_lock.unlock();
+// }
+
 void
-ParameterHandler::clear()
+ParameterHandler::set_on_apply_callback(std::function<void(const libcamera::ControlList &)> callback)
 {
-  parameters_lock.lock();
-  control_values.clear();
-  parameters_lock.unlock();
+  on_apply_callback = callback;
 }
 
 void
@@ -562,8 +568,8 @@ ParameterHandler::apply(const std::vector<rclcpp::Parameter> &parameters)
   // 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();
+  // std::cout << "wait parameters_consumed_lock ..." << std::endl;
+  // parameters_consumed_lock.lock();
 
   parameters_lock.lock();
   // control_values.clear(); // need this??
@@ -576,10 +582,21 @@ ParameterHandler::apply(const std::vector<rclcpp::Parameter> &parameters)
     const libcamera::ControlValue value = pv_to_cv(parameter, id->type());
     // control_values[parameter_ids.at(parameter.get_name())->id()] = value;
     // const std::string &name = libcamera::controls::controls.at(id)->name();
-    RCLCPP_DEBUG_STREAM(node->get_logger(), "apply " << id->name() << ": " << value.toString());
+    RCLCPP_DEBUG_STREAM(node->get_logger(), "param apply " << id->name() << ": " << value.toString());
     control_values.set(parameter_ids[parameter.get_name()]->id(), value);
     // TODO: What if 'control_values' gets conflcit here? Should we gather this before?
   }
+
+  if (on_apply_callback)
+    on_apply_callback(control_values);
+
+  control_values.clear();
+
+  // if (on_apply_callback)
+  //   on_apply_callback(control_values);
+  // else
+  //   RCLCPP_ERROR(node->get_logger(), "parameter apply callback not set");
+
   parameters_lock.unlock();
 }
 

src/ParameterHandler.hpp

@@ -1,5 +1,6 @@
 #pragma once
 #include "parameter_conflict_check.hpp"
+#include <functional>
 #include <libcamera/controls.h>
 #include <mutex>
 #include <rclcpp/node_interfaces/node_parameters_interface.hpp>
@@ -8,6 +9,7 @@
 #include <unordered_map>
 #include <vector>
 
+
 namespace rclcpp
 {
 class Node;
@@ -28,8 +30,11 @@ class ParameterHandler
   libcamera::ControlList &
   get();
 
+  // void
+  // clear();
+
   void
-  clear();
+  set_on_apply_callback(std::function<void(const libcamera::ControlList &)> callback);
 
   // std::tuple<ControlValueMap, std::vector<std::string>>
   // parameterCheckAndConvert(const std::vector<rclcpp::Parameter> &parameters);
@@ -54,7 +59,9 @@ class ParameterHandler
   // ControlValueMap control_values;
   libcamera::ControlList control_values;
   std::mutex parameters_lock;
-  std::mutex parameters_consumed_lock;
+  // std::mutex parameters_consumed_lock;
+  // std::condition_variable cv;
+  std::function<void(const libcamera::ControlList &)> on_apply_callback;
 
   void
   adjust(std::vector<rclcpp::Parameter> &parameters);