set controls as control list

Author: christianrauch

src/CameraNode.cpp

@@ -565,17 +565,24 @@ CameraNode::process(libcamera::Request *const request)
     request->reuse(libcamera::Request::ReuseBuffers);
 
     // update parameters
-    const ParameterHandler::ControlValueMap parameters = parameter_handler.get();
-    if (!parameters.empty()) {
-      RCLCPP_DEBUG_STREAM(get_logger(), request->toString() << " setting " << parameters.size() << " controls");
-      for (const auto &[id, value] : parameters) {
-        const std::string &name = libcamera::controls::controls.at(id)->name();
-        RCLCPP_DEBUG_STREAM(get_logger(), "apply " << name << ": " << value.toString());
-        request->controls().set(id, value);
-      }
-      parameter_handler.clear();
+    request->controls() = parameter_handler.get();
+    parameter_handler.clear();
+    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()) {
+    //   RCLCPP_DEBUG_STREAM(get_logger(), request->toString() << " setting " << parameters.size() << " controls");
+    //   for (const auto &[id, value] : parameters) {
+    //     const std::string &name = libcamera::controls::controls.at(id)->name();
+    //     RCLCPP_DEBUG_STREAM(get_logger(), "apply " << name << ": " << value.toString());
+    //     request->controls().set(id, value);
+    //   }
+    //   parameter_handler.clear();
+    // }
+
     camera->queueRequest(request);
   }
 }

src/ParameterHandler.cpp

@@ -408,7 +408,7 @@ ParameterHandler::declare(const libcamera::ControlInfoMap &controls)
   // node->set_parameters(parameters_init_list);
 }
 
-ParameterHandler::ControlValueMap
+libcamera::ControlList &
 ParameterHandler::get()
 {
   const std::lock_guard<std::mutex> lock(parameters_lock);
@@ -544,13 +544,22 @@ ParameterHandler::apply(const std::vector<rclcpp::Parameter> &parameters)
 {
   std::cout << "apply..." << std::endl;
 
+  // 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?
+
+  // TODO: use a callback to set controls immediately on request
+
   parameters_lock.lock();
+  // control_values.clear(); // need this??
   for (const rclcpp::Parameter &parameter : parameters) {
     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;
+    // 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());
+    control_values.set(parameter_ids[parameter.get_name()]->id(), value);
     // TODO: What if 'control_values' gets conflcit here? Should we gather this before?
   }
   parameters_lock.unlock();

src/ParameterHandler.hpp

@@ -18,14 +18,14 @@ class ParameterHandler
 {
 public:
   // re-definition of the private ControlList::ControlListMap
-  typedef std::unordered_map<unsigned int, libcamera::ControlValue> ControlValueMap;
+  // typedef std::unordered_map<unsigned int, libcamera::ControlValue> ControlValueMap;
 
   ParameterHandler(rclcpp::Node *const node);
 
   void
   declare(const libcamera::ControlInfoMap &controls);
 
-  ControlValueMap
+  libcamera::ControlList &
   get();
 
   void
@@ -51,7 +51,8 @@ class ParameterHandler
   // keep track of set parameters
   // ParameterMap parameters_full;
 
-  ControlValueMap control_values;
+  // ControlValueMap control_values;
+  libcamera::ControlList control_values;
   std::mutex parameters_lock;
 
   void