From 7bcd679ece8cbcc44fea33acb78bfc473b40ed4e Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Sat, 30 Nov 2024 16:23:25 +0100 Subject: [PATCH] callback --- src/CameraNode.cpp | 37 +++++++++++++++++++++++++++++-------- src/ParameterHandler.cpp | 33 +++++++++++++++++++++++++-------- src/ParameterHandler.hpp | 11 +++++++++-- 3 files changed, 63 insertions(+), 18 deletions(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 90d02af0..f1c4327d 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -128,6 +128,9 @@ class CameraNode : public rclcpp::Node rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector ¶meters); #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 ¶meters) } #endif +void +CameraNode::apply_parameters(const libcamera::ControlList &controls) +{ + for (const std::unique_ptr &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 diff --git a/src/ParameterHandler.cpp b/src/ParameterHandler.cpp index 0ed69300..1543c85f 100644 --- a/src/ParameterHandler.cpp +++ b/src/ParameterHandler.cpp @@ -414,16 +414,22 @@ ParameterHandler::get() const std::lock_guard 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 callback) { - parameters_lock.lock(); - control_values.clear(); - parameters_lock.unlock(); + on_apply_callback = callback; } void @@ -562,8 +568,8 @@ ParameterHandler::apply(const std::vector ¶meters) // 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 ¶meters) 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(); } diff --git a/src/ParameterHandler.hpp b/src/ParameterHandler.hpp index 23abefcc..22413ba7 100644 --- a/src/ParameterHandler.hpp +++ b/src/ParameterHandler.hpp @@ -1,5 +1,6 @@ #pragma once #include "parameter_conflict_check.hpp" +#include #include #include #include @@ -8,6 +9,7 @@ #include #include + namespace rclcpp { class Node; @@ -28,8 +30,11 @@ class ParameterHandler libcamera::ControlList & get(); + // void + // clear(); + void - clear(); + set_on_apply_callback(std::function callback); // std::tuple> // parameterCheckAndConvert(const std::vector ¶meters); @@ -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 on_apply_callback; void adjust(std::vector ¶meters);