From a2eb975e8fdedc53d13b5a630d231cfef9ad6123 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Sun, 15 Dec 2024 23:18:19 +0100 Subject: [PATCH 1/8] remove extra ; --- src/types.hpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/types.hpp b/src/types.hpp index 77738d75..38e6d09f 100644 --- a/src/types.hpp +++ b/src/types.hpp @@ -25,12 +25,12 @@ to_string(const libcamera::ControlType id); template struct ControlTypeMap; -MAP(void, None); -MAP(bool, Bool); -MAP(uint8_t, Byte); -MAP(int32_t, Integer32); -MAP(int64_t, Integer64); -MAP(float, Float); -MAP(std::string, String); -MAP(libcamera::Rectangle, Rectangle); -MAP(libcamera::Size, Size); +MAP(void, None) +MAP(bool, Bool) +MAP(uint8_t, Byte) +MAP(int32_t, Integer32) +MAP(int64_t, Integer64) +MAP(float, Float) +MAP(std::string, String) +MAP(libcamera::Rectangle, Rectangle) +MAP(libcamera::Size, Size) From ff3dcd3add8660938ef77109aa5d9548e324120c Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Sun, 20 Oct 2024 00:58:15 +0200 Subject: [PATCH 2/8] [DBG] turn 'switch' errors into warnings to notify about unhandled values Many sections of the code rely on switches over the ControlType enum to handle the different libcamera control types. When newer enum values are added with newer versions of libcamera, the 'switch' errors (-Wall) will prevent compilation even though the node will continue to work without supporting the new types. Relax this by turning all 'switch' errors into warnings again to notify the developer and implement support for these new type enum values later. --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index aaebef0b..7408e190 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,7 @@ set(CMAKE_CXX_STANDARD 17) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Werror) + add_compile_options(-Wno-error=switch) add_link_options("-Wl,-z,relro,-z,now,-z,defs") endif() From 8783204f048ccd3342b0e3548c26c26ef3fa3691 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 29 Oct 2024 22:22:51 +0100 Subject: [PATCH 3/8] [DBG] fail-fast: false --- .github/workflows/main.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 61683a3a..b048aac5 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -11,6 +11,7 @@ jobs: runs-on: ubuntu-latest strategy: + fail-fast: false matrix: include: - version: 22.04 From eb51642c39cc9dfb3ab391aa86faa337cb471995 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Thu, 8 Aug 2024 23:11:55 +0200 Subject: [PATCH 4/8] move parameter handling to dedicated class --- CMakeLists.txt | 32 +- src/CameraNode.cpp | 280 ++++------------- src/ParameterHandler.cpp | 659 +++++++++++++++++++++++++++++++++++++++ src/ParameterHandler.hpp | 77 +++++ 4 files changed, 820 insertions(+), 228 deletions(-) create mode 100644 src/ParameterHandler.cpp create mode 100644 src/ParameterHandler.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 7408e190..89f99d5a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,26 +20,42 @@ find_package(camera_info_manager REQUIRED) find_package(cv_bridge REQUIRED) pkg_check_modules(libcamera REQUIRED libcamera>=0.1) +# new param callbacks need at least 17.0.0 +if(${rclcpp_VERSION} VERSION_GREATER_EQUAL "17") + add_compile_definitions(RCLCPP_HAS_PARAM_EXT_CB) +endif() + # library with common utility functions for type conversions add_library(utils OBJECT - src/clamp.cpp - src/cv_to_pv.cpp src/format_mapping.cpp - src/parameter_conflict_check.cpp src/pretty_print.cpp - src/pv_to_cv.cpp - src/types.cpp - src/type_extent.cpp ) target_include_directories(utils PUBLIC ${libcamera_INCLUDE_DIRS}) target_link_libraries(utils ${libcamera_LINK_LIBRARIES}) ament_target_dependencies( utils - "rclcpp" "sensor_msgs" ) set_property(TARGET utils PROPERTY POSITION_INDEPENDENT_CODE ON) +# library for parameter/controls handling and conversion +add_library(param OBJECT + src/clamp.cpp + src/cv_to_pv.cpp + # src/parameter_conflict_check.cpp + src/pv_to_cv.cpp + src/types.cpp + src/type_extent.cpp + src/ParameterHandler.cpp +) +target_include_directories(param PUBLIC ${libcamera_INCLUDE_DIRS}) +target_link_libraries(param ${libcamera_LINK_LIBRARIES}) +ament_target_dependencies( + param + "rclcpp" +) +set_property(TARGET param PROPERTY POSITION_INDEPENDENT_CODE ON) + # composable ROS2 node add_library(camera_component SHARED src/CameraNode.cpp) rclcpp_components_register_node(camera_component PLUGIN "camera::CameraNode" EXECUTABLE "camera_node") @@ -53,7 +69,7 @@ set(PACKAGE_DEPENDENCIES ament_target_dependencies(camera_component PUBLIC ${PACKAGE_DEPENDENCIES}) target_include_directories(camera_component PUBLIC ${libcamera_INCLUDE_DIRS}) target_link_libraries(camera_component PUBLIC ${libcamera_LINK_LIBRARIES}) -target_link_libraries(camera_component PRIVATE utils) +target_link_libraries(camera_component PRIVATE utils param) ament_export_targets(camera_componentTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${PACKAGE_DEPENDENCIES}) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 4a01e523..5c1f47bb 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -1,11 +1,6 @@ -#include "clamp.hpp" -#include "cv_to_pv.hpp" +#include "ParameterHandler.hpp" #include "format_mapping.hpp" -#include "parameter_conflict_check.hpp" #include "pretty_print.hpp" -#include "pv_to_cv.hpp" -#include "type_extent.hpp" -#include "types.hpp" #include #include #include @@ -34,7 +29,6 @@ #include #include #include -#include #include #include #include @@ -107,30 +101,33 @@ class CameraNode : public rclcpp::Node camera_info_manager::CameraInfoManager cim; - OnSetParametersCallbackHandle::SharedPtr callback_parameter_change; + ParameterHandler parameter_handler; + +#ifdef RCLCPP_HAS_PARAM_EXT_CB + // use new "post_set" callback to apply parameters + PostSetParametersCallbackHandle::SharedPtr param_cb_change; +#else + OnSetParametersCallbackHandle::SharedPtr param_cb_change; +#endif // map parameter names to libcamera control id std::unordered_map parameter_ids; - // parameters that are to be set for every request - std::unordered_map parameters; - // keep track of set parameters - ParameterMap parameters_full; - std::mutex parameters_mutex; - std::unique_lock parameters_lock {parameters_mutex, std::defer_lock}; // compression quality parameter std::atomic_uint8_t jpeg_quality; - void - declareParameters(); - void requestComplete(libcamera::Request *const request); void process(libcamera::Request *const request); + void + postParameterChange(const std::vector ¶meters); + +#ifndef RCLCPP_HAS_PARAM_EXT_CB rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector ¶meters); +#endif }; RCLCPP_COMPONENTS_REGISTER_NODE(camera::CameraNode) @@ -192,7 +189,17 @@ compressImageMsg(const sensor_msgs::msg::Image &source, } -CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", options), cim(this) +CameraNode::CameraNode(const rclcpp::NodeOptions &options) + : Node("camera", options), + cim(this), + parameter_handler(this), + param_cb_change( +#ifdef RCLCPP_HAS_PARAM_EXT_CB + add_post_set_parameters_callback(std::bind(&CameraNode::postParameterChange, this, std::placeholders::_1)) +#else + add_on_set_parameters_callback(std::bind(&CameraNode::onParameterChange, this, std::placeholders::_1)) +#endif + ) { // pixel format rcl_interfaces::msg::ParameterDescriptor param_descr_format; @@ -383,7 +390,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti if (!cim.setCameraName(cname)) throw std::runtime_error("camera name must only contain alphanumeric characters"); - declareParameters(); + parameter_handler.declare(camera->controls()); // allocate stream buffers and create one request per buffer stream = scfg.stream(); @@ -436,7 +443,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti camera->requestCompleted.connect(this, &CameraNode::requestComplete); libcamera::ControlList controls_ = camera->controls(); - for (const auto &[id, value] : parameters) + for (const auto &[id, value] : parameter_handler.get()) controls_.set(id, value); // start camera and queue all requests @@ -477,129 +484,6 @@ CameraNode::~CameraNode() std::cerr << "munmap failed: " << std::strerror(errno) << std::endl; } -void -CameraNode::declareParameters() -{ - // dynamic camera configuration - ParameterMap parameters_init; - for (const auto &[id, info] : camera->controls()) { - // store control id with name - parameter_ids[id->name()] = id; - - if (info.min().numElements() != info.max().numElements()) - throw std::runtime_error("minimum and maximum parameter array sizes do not match"); - - // check if the control can be mapped to a parameter - rclcpp::ParameterType pv_type; - try { - pv_type = cv_to_pv_type(id); - if (pv_type == rclcpp::ParameterType::PARAMETER_NOT_SET) { - RCLCPP_WARN_STREAM(get_logger(), "unsupported control '" << id->name() << "'"); - continue; - } - } - catch (const std::runtime_error &e) { - // ignore - RCLCPP_WARN_STREAM(get_logger(), e.what()); - continue; - } - - // format type description - rcl_interfaces::msg::ParameterDescriptor param_descr; - try { - const std::size_t extent = get_extent(id); - const bool scalar = (extent == 0); - const bool dynamic = (extent == libcamera::dynamic_extent); - const std::string cv_type_descr = - scalar ? "scalar" : "array[" + (dynamic ? std::string() : std::to_string(extent)) + "]"; - param_descr.description = - std::to_string(id->type()) + " " + cv_type_descr + " range {" + info.min().toString() + - "}..{" + info.max().toString() + "}" + - (info.def().isNone() ? std::string {} : " (default: {" + info.def().toString() + "})"); - } - catch (const std::runtime_error &e) { - // ignore - RCLCPP_WARN_STREAM(get_logger(), e.what()); - continue; - } - - // get smallest bounds for minimum and maximum set - rcl_interfaces::msg::IntegerRange range_int; - rcl_interfaces::msg::FloatingPointRange range_float; - - switch (id->type()) { - case libcamera::ControlTypeInteger32: - range_int.from_value = max(info.min()); - range_int.to_value = min(info.max()); - break; - case libcamera::ControlTypeInteger64: - range_int.from_value = max(info.min()); - range_int.to_value = min(info.max()); - break; - case libcamera::ControlTypeFloat: - range_float.from_value = max(info.min()); - range_float.to_value = min(info.max()); - break; - default: - break; - } - - if (range_int.from_value != range_int.to_value) - param_descr.integer_range = {range_int}; - if (range_float.from_value != range_float.to_value) - param_descr.floating_point_range = {range_float}; - - // clamp default ControlValue to min/max range and cast ParameterValue - rclcpp::ParameterValue value; - try { - value = cv_to_pv(clamp(info.def(), info.min(), info.max())); - } - catch (const invalid_conversion &e) { - RCLCPP_ERROR_STREAM(get_logger(), "unsupported control '" - << id->name() - << "' (type: " << std::to_string(info.def().type()) << "): " - << e.what()); - continue; - } - - // declare parameters and set default or initial value - RCLCPP_DEBUG_STREAM(get_logger(), - "declare " << id->name() << " with default " << rclcpp::to_string(value)); - - if (value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { - declare_parameter(id->name(), pv_type, param_descr); - } - else { - declare_parameter(id->name(), value, param_descr); - } - parameters_init[id->name()] = value; - } - - // register callback to handle parameter changes - // We have to register the callback after parameter declaration - // to avoid callbacks interfering with the default parameter check. - callback_parameter_change = add_on_set_parameters_callback( - std::bind(&CameraNode::onParameterChange, this, std::placeholders::_1)); - - // resolve conflicts of default libcamera configuration and user provided overrides - std::vector status; - std::tie(parameters_init, status) = - resolve_conflicts(parameters_init, get_node_parameters_interface()->get_parameter_overrides()); - - for (const std::string &s : status) - RCLCPP_WARN_STREAM(get_logger(), s); - - std::vector parameters_init_list; - for (const auto &[name, value] : parameters_init) { - if (value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) - parameters_init_list.emplace_back(name, value); - } - const rcl_interfaces::msg::SetParametersResult param_set_result = - set_parameters_atomically(parameters_init_list); - if (!param_set_result.successful) - RCLCPP_ERROR_STREAM(get_logger(), "Cannot declare parameters with default value: " << param_set_result.reason); -} - void CameraNode::requestComplete(libcamera::Request *const request) { @@ -696,97 +580,53 @@ CameraNode::process(libcamera::Request *const request) request->reuse(libcamera::Request::ReuseBuffers); // update parameters - parameters_lock.lock(); - 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); - } - parameters.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()); } - parameters_lock.unlock(); + + // 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); } } -rcl_interfaces::msg::SetParametersResult -CameraNode::onParameterChange(const std::vector ¶meters) +void +CameraNode::postParameterChange(const std::vector ¶meters) { - rcl_interfaces::msg::SetParametersResult result; - - // check target parameter state (current and new parameters) - // for conflicting configuration - const std::vector msgs = check_conflicts(parameters, parameters_full); - if (!msgs.empty()) { - result.successful = false; - for (size_t i = 0; i < msgs.size(); i++) { - if (msgs.size() > 1) - result.reason += "(" + std::to_string(i) + ") "; - result.reason += msgs[i]; - if (i < msgs.size() - 1) - result.reason += "; "; - } - return result; - } - - result.successful = true; + std::cout << "postParameterChange" << std::endl; + // check non-controls parameters for (const rclcpp::Parameter ¶meter : parameters) { - RCLCPP_DEBUG_STREAM(get_logger(), "setting " << parameter.get_type_name() << " parameter " - << parameter.get_name() << " to " - << parameter.value_to_string()); - - if (parameter_ids.count(parameter.get_name())) { - const libcamera::ControlId *id = parameter_ids.at(parameter.get_name()); - libcamera::ControlValue value = pv_to_cv(parameter, id->type()); - - if (!value.isNone()) { - // verify parameter type and dimension against default - const libcamera::ControlInfo &ci = camera->controls().at(id); - - if (value.type() != id->type()) { - result.successful = false; - result.reason = parameter.get_name() + ": parameter types mismatch, expected '" + - std::to_string(id->type()) + "', got '" + std::to_string(value.type()) + - "'"; - return result; - } - - const std::size_t extent = get_extent(id); - if (value.isArray() && - (extent != libcamera::dynamic_extent) && - (value.numElements() != extent)) - { - result.successful = false; - result.reason = parameter.get_name() + ": array dimensions mismatch, expected " + - std::to_string(extent) + ", got " + std::to_string(value.numElements()); - return result; - } - - // check bounds and return error - if (value < ci.min() || value > ci.max()) { - result.successful = false; - result.reason = - "parameter value " + value.toString() + " outside of range: " + ci.toString(); - return result; - } - - parameters_lock.lock(); - this->parameters[parameter_ids.at(parameter.get_name())->id()] = value; - parameters_lock.unlock(); - - parameters_full[parameter.get_name()] = parameter.get_parameter_value(); - } - } - else if (!parameter.get_name().compare("jpeg_quality")) { + if (parameter.get_name() == "jpeg_quality") { jpeg_quality = parameter.get_parameter_value().get(); } } +} +#ifndef RCLCPP_HAS_PARAM_EXT_CB +rcl_interfaces::msg::SetParametersResult +CameraNode::onParameterChange(const std::vector ¶meters) +{ + std::cout << "onParameterChange" << std::endl; + + postParameterChange(parameters); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; return result; } +#endif } // namespace camera diff --git a/src/ParameterHandler.cpp b/src/ParameterHandler.cpp new file mode 100644 index 00000000..d59b3623 --- /dev/null +++ b/src/ParameterHandler.cpp @@ -0,0 +1,659 @@ +#include "ParameterHandler.hpp" +#include "clamp.hpp" +#include "cv_to_pv.hpp" +// #include "parameter_conflict_check.hpp" +#include "pv_to_cv.hpp" +#include "type_extent.hpp" +#include "types.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +// init: +// 1. declare parameters without overrides and without default values +// 2. resolve conflicts between default values and overrides +// 3. apply + +// runtime: +// 1. resolve conflicts, warn user +// 2. apply + +typedef std::unordered_map ParamValueMap; + +// typedef std::unordered_map ParameterView; +typedef std::unordered_map ParameterViewConst; + +// ParameterView +// param_view(std::vector ¶meters) +// { +// // create a mapping of parameter names to references of that parameter +// ParameterView param_map; +// for (rclcpp::Parameter ¶meter : parameters) { +// param_map.insert({parameter.get_name(), parameter}); +// } +// return param_map; +// } + +// std::vector +// param_view(std::unordered_map ¶meters) +// { +// // create a mapping of parameter names to references of that parameter +// // ParameterView param_map; +// // for (rclcpp::Parameter ¶meter : parameters) { +// // param_map.insert({parameter.get_name(), parameter}); +// // } +// // return param_map; + +// for (auto &[name, value] : parameters) { +// if (overrides.count(name)) +// value = overrides.at(name); +// } +// } + +ParameterViewConst +param_view(const std::vector ¶meters) +{ + // create a mapping of parameter names to references of that parameter + ParameterViewConst param_map; + for (const rclcpp::Parameter ¶meter : parameters) { + param_map.insert({parameter.get_name(), parameter}); + } + return param_map; +} + +rcl_interfaces::msg::SetParametersResult +format_result(const std::vector &msgs) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = msgs.empty(); + if (!result.successful) { + for (size_t i = 0; i < msgs.size(); i++) { + if (msgs.size() > 1) + result.reason += "(" + std::to_string(i) + ") "; + result.reason += msgs[i]; + if (i < msgs.size() - 1) + result.reason += "; "; + } + } + return result; +} + +bool +conflict_exposure(const ParamValueMap &p) +{ + // auto exposure must not be enabled while fixed exposure time is set + return p.count("AeEnable") && p.at("AeEnable").get() && + p.count("ExposureTime") && p.at("ExposureTime").get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET; +} + +std::vector +resolve_defaults(ParamValueMap &p) +{ + std::vector msgs; + + // default: prefer auto exposure + if (conflict_exposure(p)) { + // disable exposure + p.at("ExposureTime") = {}; + msgs.emplace_back("AeEnable and ExposureTime must not be enabled at the same time. 'ExposureTime' will be disabled."); + } + return msgs; +} + +std::vector +resolve_overrides(ParamValueMap &p) +{ + std::vector msgs; + + // overrides: prefer provided exposure + if (conflict_exposure(p)) { + // disable auto exposure + p.at("AeEnable") = rclcpp::ParameterValue {false}; + msgs.emplace_back("AeEnable and ExposureTime must not be enabled at the same time. 'AeEnable' will be set to off."); + } + return msgs; +} + +std::vector +check(const std::vector ¶meters_old, + const std::vector ¶meters_new) +{ + std::vector msgs; + + // initialise full parameter list with "new" parameters + ParameterViewConst parameter_map = param_view(parameters_new); + // move "old" parameters, which are not replaced by "new" parameters, to full parameter list + parameter_map.merge(param_view(parameters_old)); + + // ParameterMap parameter_map; + + // old configuration state + // for (const auto &[name, value] : parameters_old) + // parameter_map[name] = value; + + // apply new configuration update + // for (const auto &p : parameters_new) + // parameter_map[p.get_name()] = p.get_parameter_value(); + + // is auto exposure going to be enabled? + const bool ae_enabled = + parameter_map.count("AeEnable") && parameter_map.at("AeEnable").as_bool(); + // are new parameters setting the exposure manually? + const bool exposure_updated = + std::find_if(parameters_new.begin(), parameters_new.end(), [](const rclcpp::Parameter ¶m) { + return param.get_name() == "ExposureTime"; + }) != parameters_new.end(); + + // ExposureTime must not be set while AeEnable is true + if (ae_enabled && exposure_updated) + msgs.emplace_back("AeEnable and ExposureTime must not be set simultaneously"); + + return msgs; +} + +ParameterHandler::ParameterHandler(rclcpp::Node *const node) + : node(node) +{ + param_cb_on = node->add_on_set_parameters_callback( + std::bind(&ParameterHandler::OnSetValidate, this, std::placeholders::_1)); + +#if defined(RCLCPP_HAS_PARAM_EXT_CB) && 0 + // pre_set -> on_set -> post_set + + // adjust parameters + // std::function &)> + param_cb_pre = node->add_pre_set_parameters_callback( + [this](std::vector ¶meters) -> void { + std::cout << "ParameterHandler pre >>" << std::endl; + for (const rclcpp::Parameter ¶m : parameters) { + std::cout << "ParameterHandler pre: " << param.get_name() << " to " << param.value_to_string() << std::endl; + } + std::cout << "ParameterHandler pre ##" << std::endl; + }); + + // validate parameters + // std::function &)> + param_cb_on = node->add_on_set_parameters_callback( + [this](const std::vector ¶meters) -> rcl_interfaces::msg::SetParametersResult { + std::cout << "ParameterHandler set >>" << std::endl; + for (const rclcpp::Parameter ¶m : parameters) { + std::cout << "ParameterHandler set: " << param.get_name() << " to " << param.value_to_string() << std::endl; + } + std::cout << "ParameterHandler set ##" << std::endl; + rcl_interfaces::msg::SetParametersResult pr; + pr.successful = true; + return pr; + }); + + // apply parameters + // std::function &)> + param_cb_post = node->add_post_set_parameters_callback( + [this](const std::vector ¶meters) -> void { + std::cout << "ParameterHandler post >>" << std::endl; + for (const rclcpp::Parameter ¶m : parameters) { + std::cout << "ParameterHandler post: " << param.get_name() << " to " << param.value_to_string() << std::endl; + } + std::cout << "ParameterHandler post ##" << std::endl; + }); +#endif + +#ifdef RCLCPP_HAS_PARAM_EXT_CB + param_cb_pre = node->add_pre_set_parameters_callback( + std::bind(&ParameterHandler::PreSetResolve, this, std::placeholders::_1)); + param_cb_post = node->add_post_set_parameters_callback( + std::bind(&ParameterHandler::PostSetApply, this, std::placeholders::_1)); +#endif +} + +void +ParameterHandler::declare(const libcamera::ControlInfoMap &controls) +{ + // std::vector parameters; + std::unordered_map parameters; + // std::vector descriptors; + + // convert camera controls to parameters + for (const auto &[id, info] : controls) { + // store control id with name + parameter_ids[id->name()] = id; + parameter_info[id->name()] = info; + + std::cout << "# " << id->name() << ": " << info.toString() << ", " << info.def().toString() << std::endl; + + if (info.min().numElements() != info.max().numElements()) + throw std::runtime_error("minimum and maximum parameter array sizes do not match"); + + // check if the control can be mapped to a parameter + rclcpp::ParameterType pv_type; + try { + pv_type = cv_to_pv_type(id); + if (pv_type == rclcpp::ParameterType::PARAMETER_NOT_SET) { + RCLCPP_WARN_STREAM(node->get_logger(), "unsupported control '" << id->name() << "'"); + continue; + } + } + catch (const std::runtime_error &e) { + // ignore + RCLCPP_WARN_STREAM(node->get_logger(), e.what()); + continue; + } + + // format type description + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.name = id->name(); + descriptor.type = pv_type; + try { + const std::size_t extent = get_extent(id); + const bool scalar = (extent == 0); + const bool dynamic = (extent == libcamera::dynamic_extent); + const std::string cv_type_descr = + scalar ? "scalar" : "array[" + (dynamic ? std::string() : std::to_string(extent)) + "]"; + descriptor.description = + std::to_string(id->type()) + " " + cv_type_descr + " range {" + info.min().toString() + + "}..{" + info.max().toString() + "}" + + (info.def().isNone() ? std::string {} : " (default: {" + info.def().toString() + "})"); + } + catch (const std::runtime_error &e) { + // ignore + RCLCPP_WARN_STREAM(node->get_logger(), e.what()); + continue; + } + + // Camera controls can have arrays for minimum and maximum values, but the parameter descriptor + // only supports scalar bounds. + // Get smallest bounds for minimum and maximum set but warn user. + if (info.min().isArray() || info.max().isArray()) { + RCLCPP_WARN_STREAM( + node->get_logger(), + "unsupported control array bounds: {" << info.min().toString() << "} ... {" << info.max().toString() << "}"); + } + + switch (id->type()) { + case libcamera::ControlTypeInteger32: + { + rcl_interfaces::msg::IntegerRange range; + range.from_value = max(info.min()); + range.to_value = min(info.max()); + descriptor.integer_range = {range}; + } break; + case libcamera::ControlTypeInteger64: + { + rcl_interfaces::msg::IntegerRange range; + range.from_value = max(info.min()); + range.to_value = min(info.max()); + descriptor.integer_range = {range}; + } break; + case libcamera::ControlTypeFloat: + { + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = max(info.min()); + range.to_value = min(info.max()); + descriptor.floating_point_range = {range}; + } break; + default: + break; + } + + // if (range_int.from_value != range_int.to_value) + // descriptor.integer_range = {range_int}; + // if (range_float.from_value != range_float.to_value) + // descriptor.floating_point_range = {range_float}; + + // clamp default ControlValue to min/max range and cast ParameterValue + try { + // const rclcpp::ParameterValue value_default = cv_to_pv(clamp(info.def(), info.min(), info.max())); + // parameters.emplace_back(id->name(), value_default); + parameters[id->name()] = cv_to_pv(clamp(info.def(), info.min(), info.max())); + // descriptors.push_back(descriptor); + } + catch (const invalid_conversion &e) { + RCLCPP_ERROR_STREAM( + node->get_logger(), + "unsupported control '" << id->name() << "' (type: " << std::to_string(info.def().type()) << "): " << e.what()); + continue; + } + + // declare parameters without default values and overrides to avoid triggering the callbacks already + RCLCPP_DEBUG_STREAM(node->get_logger(), "declare '" << id->name() << "' (type: " << pv_type << ")"); + node->declare_parameter(id->name(), pv_type, descriptor, true); + } + + // TODO: merge default parameters and overrides and resolve conflicts + + // std::vector parameters; + std::cout << "defaults:" << std::endl; + for (const auto &[name, value] : parameters) { + std::cout << " " << name << ": " << rclcpp::to_string(value) << std::endl; + } + + // resolve conflicts in default configuration + const std::vector msgs_defaults = resolve_defaults(parameters); + for (const std::string &msg : msgs_defaults) { + RCLCPP_DEBUG_STREAM(node->get_logger(), "resolve defaults: " << msg); + } + + std::cout << "defaults (resolved):" << std::endl; + for (const auto &[name, value] : parameters) { + std::cout << " " << name << ": " << rclcpp::to_string(value) << std::endl; + } + + const std::map &overrides = + node->get_node_parameters_interface()->get_parameter_overrides(); + + std::cout << "overrides:" << std::endl; + for (const auto &[name, value] : overrides) { + std::cout << " " << name << ": " << rclcpp::to_string(value) << std::endl; + } + + // apply parameter overrides and ignore those, + // which are not supported by the camera controls + for (auto &[name, value] : parameters) { + if (overrides.count(name)) + value = overrides.at(name); + } + + // resolve conflicts in overridden configuration + const std::vector msgs_overrides = resolve_overrides(parameters); + for (const std::string &msg : msgs_overrides) { + RCLCPP_WARN_STREAM(node->get_logger(), "resolve overrides: " << msg); + } + + // TODO: setting auto-exposure to off here, should restore the default exposure time + + std::cout << "defaults (override):" << std::endl; + for (const auto &[name, value] : parameters) { + std::cout << " " << name << ": " << rclcpp::to_string(value) << std::endl; + } + + // convert parameters to list and set them atomically + std::vector parameters_init_list; + for (const auto &[name, value] : parameters) { + if (value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) + parameters_init_list.emplace_back(name, value); + } + + const rcl_interfaces::msg::SetParametersResult param_set_result = + node->set_parameters_atomically(parameters_init_list); + if (!param_set_result.successful) + RCLCPP_ERROR_STREAM(node->get_logger(), "Cannot set parameters: " << param_set_result.reason); + + + // std::vector aa = node->get_parameters({"AeEnable", "ExposureTime"}); + + // resolve conflicts in default control values + // TODO: let conflicts be resolved with callback? + // NO? override ExposureTime must set (override) AeEnable false + // ??? can we replace 'parameters_full' by runtime check of current param values? + + // resolve conflicts of default libcamera configuration and user provided overrides + // std::vector status; + // std::tie(parameters_init, status) = resolve_conflicts(parameters, overrides); + + // for (const std::string &s : status) + // RCLCPP_WARN_STREAM(node->get_logger(), s); + + // std::vector parameters_init_list; + // for (const auto &[name, value] : parameters_init) + // parameters_init_list.emplace_back(name, value); + // node->set_parameters(parameters_init_list); +} + +libcamera::ControlList & +ParameterHandler::get() +{ + const std::lock_guard lock(parameters_lock); + // TODO: final check of conflicts for gathered controls? + return control_values; +} + +void +ParameterHandler::clear() +{ + parameters_lock.lock(); + control_values.clear(); + parameters_lock.unlock(); +} + +void +ParameterHandler::adjust(std::vector ¶meters) +{ + // + (void)parameters; + // for (const rclcpp::Parameter ¶meter : parameters) { + // // + // } +} + +std::vector +ParameterHandler::validate(const std::vector ¶meters) +{ + // TODO: should just go over "controls" + // const std::vector parameter_names_old = node->list_parameters({}, {}).names; + // std::vector parameter_names_old; + // for (const auto &[name, id] : parameter_ids) { + // parameter_names_old.push_back(name); + // } + + // const std::vector parameter_types_old = node->get_parameter_types(parameter_names_old); + // rclcpp::ParameterType::PARAMETER_NOT_SET; + // terminate called after throwing an instance of 'rclcpp::exceptions::ParameterUninitializedException' + // what(): parameter 'AeEnable' is not initialized + std::vector parameters_old; + // for (const auto &[name, id] : parameter_ids) { + // parameters_old.push_back(node->get_parameter(name)); + // } + for (const auto &[name, id] : parameter_ids) { + // auto desr = node->describe_parameter(name); + // if (node->describe_parameter(name).type == rclcpp::ParameterType::PARAMETER_NOT_SET) + // continue; + // parameters_old.push_back(node->get_parameter(name)); + rclcpp::Parameter p; + node->get_parameter(name, p); + if (p.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) + continue; + parameters_old.push_back(p); + } + // for (const auto &[name, id] : parameter_ids) { + // node->get_parameter(name, ); + // } + // const std::vector parameters_old = node->get_parameters(parameter_names_old); + + // conflicts + const std::vector msgs = check(parameters_old, parameters); + // check(parameters_old, parameters) + // const rcl_interfaces::msg::SetParametersResult result = format_result(); + if (!msgs.empty()) { + // std::cout << "validate: " << result.reason << std::endl; + // return format_result(msgs); + return msgs; + } + // if (!result.successful) + // return result; + + // TODO: check other mismatches from 'parameterCheckAndConvert' + + std::vector msgs_valid_check; + for (const rclcpp::Parameter ¶meter : parameters) { + // ignore non-controls + if (!parameter_ids.count(parameter.get_name())) + continue; + + RCLCPP_DEBUG_STREAM(node->get_logger(), + "setting " << parameter.get_type_name() << " parameter " + << parameter.get_name() << " to " + << parameter.value_to_string()); + + const libcamera::ControlId *id = parameter_ids.at(parameter.get_name()); + const libcamera::ControlValue value = pv_to_cv(parameter, id->type()); + + if (!value.isNone()) { + // verify parameter type and dimension against default + const libcamera::ControlInfo &ci = parameter_info.at(parameter.get_name()); + + if (value.type() != id->type()) { + msgs_valid_check.push_back( + parameter.get_name() + ": parameter types mismatch, expected '" + + std::to_string(id->type()) + "', got '" + std::to_string(value.type()) + + "'"); + continue; + } + + const std::size_t extent = get_extent(id); + if (value.isArray() && + (extent != libcamera::dynamic_extent) && + (value.numElements() != extent)) + { + msgs_valid_check.push_back( + parameter.get_name() + ": array dimensions mismatch, expected " + + std::to_string(extent) + ", got " + std::to_string(value.numElements())); + continue; + } + + // check bounds and return error + if (value < ci.min() || value > ci.max()) { + msgs_valid_check.push_back( + "parameter value " + value.toString() + " outside of range: " + ci.toString()); + continue; + } + + // TODO: value clamping into pre_set + + // TODO: set control_values in post_set callback + // control_values[parameter_ids.at(parameter.get_name())->id()] = value; + // parameters_full[parameter.get_name()] = parameter.get_parameter_value(); + } // not None + } + + // return format_result(msgs_valid_check); + + return msgs_valid_check; +} + +void +ParameterHandler::apply(const std::vector ¶meters) +{ + 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 ¶meter : 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; + // 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(); +} + +rcl_interfaces::msg::SetParametersResult +ParameterHandler::OnSetValidate(const std::vector ¶meters) +{ +#ifdef RCLCPP_HAS_PARAM_EXT_CB + const rcl_interfaces::msg::SetParametersResult result = format_result(validate(parameters)); +#else + // If this is the only parameter callback available, call the pre-, on- and post-set callbacks + // manually on a writable copy of the paramters. + std::vector parameters2 = parameters; + adjust(parameters2); + const rcl_interfaces::msg::SetParametersResult result = format_result(validate(parameters2)); + if (result.successful) + apply(parameters2); +#endif + return result; +} + +#ifdef RCLCPP_HAS_PARAM_EXT_CB +void +ParameterHandler::PreSetResolve(std::vector ¶meters) +{ + adjust(parameters); +} + +void +ParameterHandler::PostSetApply(const std::vector ¶meters) +{ + apply(parameters); +} +#endif + +// std::tuple> +// ParameterHandler::parameterCheckAndConvert(const std::vector ¶meters) +// { +// // check target parameter state (current and new parameters) +// // for conflicting configuration +// // const std::vector msgs_conflicts = check_conflicts(parameters, parameters_full); +// // if (!msgs_conflicts.empty()) { +// // return {ControlValueMap {}, msgs_conflicts}; +// // } + +// ControlValueMap control_values; +// std::vector msgs_valid_check; + +// // for (const rclcpp::Parameter ¶meter : parameters) { +// // RCLCPP_DEBUG_STREAM(node->get_logger(), +// // "setting " << parameter.get_type_name() << " parameter " +// // << parameter.get_name() << " to " +// // << parameter.value_to_string()); + +// // if (parameter_ids.count(parameter.get_name())) { +// // const libcamera::ControlId *id = parameter_ids.at(parameter.get_name()); +// // const libcamera::ControlValue value = pv_to_cv(parameter, id->type()); + +// // if (!value.isNone()) { +// // // verify parameter type and dimension against default +// // const libcamera::ControlInfo &ci = parameter_info.at(parameter.get_name()); + +// // if (value.type() != id->type()) { +// // msgs_valid_check.push_back( +// // parameter.get_name() + ": parameter types mismatch, expected '" + +// // std::to_string(id->type()) + "', got '" + std::to_string(value.type()) + +// // "'"); +// // continue; +// // } + +// // const std::size_t extent = get_extent(id); +// // if (value.isArray() && +// // (extent != libcamera::dynamic_extent) && +// // (value.numElements() != extent)) +// // { +// // msgs_valid_check.push_back( +// // parameter.get_name() + ": array dimensions mismatch, expected " + +// // std::to_string(extent) + ", got " + std::to_string(value.numElements())); +// // continue; +// // } + +// // // check bounds and return error +// // if (value < ci.min() || value > ci.max()) { +// // msgs_valid_check.push_back( +// // "parameter value " + value.toString() + " outside of range: " + ci.toString()); +// // continue; +// // } + +// // control_values[parameter_ids.at(parameter.get_name())->id()] = value; +// // parameters_full[parameter.get_name()] = parameter.get_parameter_value(); +// // } // not None +// // } // in parameter_ids +// // } // parameters + +// return {control_values, msgs_valid_check}; +// } diff --git a/src/ParameterHandler.hpp b/src/ParameterHandler.hpp new file mode 100644 index 00000000..c3dfb018 --- /dev/null +++ b/src/ParameterHandler.hpp @@ -0,0 +1,77 @@ +#pragma once +#include "parameter_conflict_check.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace rclcpp +{ +class Node; +} // namespace rclcpp + + +class ParameterHandler +{ +public: + // re-definition of the private ControlList::ControlListMap + // typedef std::unordered_map ControlValueMap; + + ParameterHandler(rclcpp::Node *const node); + + void + declare(const libcamera::ControlInfoMap &controls); + + libcamera::ControlList & + get(); + + void + clear(); + + // std::tuple> + // parameterCheckAndConvert(const std::vector ¶meters); + +private: + rclcpp::Node *const node; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_on; + +#ifdef RCLCPP_HAS_PARAM_EXT_CB + rclcpp::node_interfaces::PreSetParametersCallbackHandle::SharedPtr param_cb_pre; + rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr param_cb_post; +#endif + + // TODO: consider a map + // std::vector parameter_names; + std::unordered_map parameter_ids; + std::unordered_map parameter_info; + // keep track of set parameters + // ParameterMap parameters_full; + + // ControlValueMap control_values; + libcamera::ControlList control_values; + std::mutex parameters_lock; + + void + adjust(std::vector ¶meters); + + std::vector + validate(const std::vector ¶meters); + + void + apply(const std::vector ¶meters); + + rcl_interfaces::msg::SetParametersResult + OnSetValidate(const std::vector ¶meters); + +#ifdef RCLCPP_HAS_PARAM_EXT_CB + void + PreSetResolve(std::vector ¶meters); + + void + PostSetApply(const std::vector ¶meters); +#endif +}; From b3fd89674ec6db2709989bf4063abf02670179ca Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Sun, 24 Nov 2024 14:29:27 +0100 Subject: [PATCH 5/8] 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 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 ¶meters) // 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 controls; + for (const rclcpp::Parameter ¶meter : 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 ¶meter : parameters) { - if (!parameter_ids.count(parameter.get_name())) - continue; + RCLCPP_DEBUG_STREAM(node->get_logger(), "apply " << controls.size() << " parameters"); + for (const rclcpp::Parameter ¶meter : 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 ¶meters); From 7bcd679ece8cbcc44fea33acb78bfc473b40ed4e Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Sat, 30 Nov 2024 16:23:25 +0100 Subject: [PATCH 6/8] 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); From b8c953aaaf170a89d474e768313aef9b1269a3da Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Mon, 2 Dec 2024 14:56:50 +0100 Subject: [PATCH 7/8] restore --- src/ParameterHandler.cpp | 63 ++++++++++++++++++++++++++++------------ src/ParameterHandler.hpp | 12 ++++++++ 2 files changed, 57 insertions(+), 18 deletions(-) diff --git a/src/ParameterHandler.cpp b/src/ParameterHandler.cpp index 1543c85f..8490ece7 100644 --- a/src/ParameterHandler.cpp +++ b/src/ParameterHandler.cpp @@ -7,6 +7,7 @@ #include "types.hpp" #include #include +#include #include #include #include @@ -28,9 +29,9 @@ // 1. resolve conflicts, warn user // 2. apply -typedef std::unordered_map ParamValueMap; +// typedef std::unordered_map ParamValueMap; -// typedef std::unordered_map ParameterView; +typedef std::unordered_map ParameterView; typedef std::unordered_map ParameterViewConst; // ParameterView @@ -46,19 +47,21 @@ typedef std::unordered_map ParameterView // std::vector // param_view(std::unordered_map ¶meters) -// { -// // create a mapping of parameter names to references of that parameter -// // ParameterView param_map; -// // for (rclcpp::Parameter ¶meter : parameters) { -// // param_map.insert({parameter.get_name(), parameter}); -// // } -// // return param_map; +ParameterView +param_view(std::vector ¶meters) +{ + // create a mapping of parameter names to references of that parameter + ParameterView param_map; + for (rclcpp::Parameter ¶meter : parameters) { + param_map.insert({parameter.get_name(), parameter}); + } + return param_map; -// for (auto &[name, value] : parameters) { -// if (overrides.count(name)) -// value = overrides.at(name); -// } -// } + // for (auto &[name, value] : parameters) { + // if (overrides.count(name)) + // value = overrides.at(name); + // } +} ParameterViewConst param_view(const std::vector ¶meters) @@ -89,7 +92,7 @@ format_result(const std::vector &msgs) } bool -conflict_exposure(const ParamValueMap &p) +ParameterHandler::conflict_exposure(const ParamValueMap &p) { // auto exposure must not be enabled while fixed exposure time is set return p.count("AeEnable") && p.at("AeEnable").get() && @@ -97,13 +100,14 @@ conflict_exposure(const ParamValueMap &p) } std::vector -resolve_defaults(ParamValueMap &p) +ParameterHandler::resolve_defaults(ParameterHandler::ParamValueMap &p) { std::vector msgs; // default: prefer auto exposure if (conflict_exposure(p)) { // disable exposure + disabled_restore["ExposureTime"] = p.at("ExposureTime"); p.at("ExposureTime") = {}; msgs.emplace_back("AeEnable and ExposureTime must not be enabled at the same time. 'ExposureTime' will be disabled."); } @@ -111,7 +115,7 @@ resolve_defaults(ParamValueMap &p) } std::vector -resolve_overrides(ParamValueMap &p) +ParameterHandler::resolve_overrides(ParamValueMap &p) { std::vector msgs; @@ -121,6 +125,11 @@ resolve_overrides(ParamValueMap &p) p.at("AeEnable") = rclcpp::ParameterValue {false}; msgs.emplace_back("AeEnable and ExposureTime must not be enabled at the same time. 'AeEnable' will be set to off."); } + // restore 'ExposureTime' + if (p.count("AeEnable") && !p.at("AeEnable").get()) { + p.at("ExposureTime") = disabled_restore.at("ExposureTime"); + disabled_restore.erase("ExposureTime"); + } return msgs; } @@ -161,6 +170,23 @@ check(const std::vector ¶meters_old, return msgs; } +void +restore(std::vector ¶meters, + std::unordered_map &disabled_restore) +{ + ParameterView p = param_view(parameters); + + // restore 'ExposureTime' when 'AeEnable' is off + if (p.count("AeEnable") && !p.at("AeEnable").as_bool()) { + // p.at("ExposureTime") = {"ExposureTime", disabled_restore.at("ExposureTime")}; + // NOTE: adding to ParameterView does not add to vector + if (disabled_restore.count("ExposureTime")) { + parameters.push_back({"ExposureTime", disabled_restore.at("ExposureTime")}); + disabled_restore.erase("ExposureTime"); + } + } +} + ParameterHandler::ParameterHandler(rclcpp::Node *const node) : node(node) { @@ -436,10 +462,11 @@ void ParameterHandler::adjust(std::vector ¶meters) { // - (void)parameters; + // (void)parameters; // for (const rclcpp::Parameter ¶meter : parameters) { // // // } + restore(parameters, disabled_restore); } std::vector diff --git a/src/ParameterHandler.hpp b/src/ParameterHandler.hpp index 22413ba7..656451cc 100644 --- a/src/ParameterHandler.hpp +++ b/src/ParameterHandler.hpp @@ -40,6 +40,8 @@ class ParameterHandler // parameterCheckAndConvert(const std::vector ¶meters); private: + typedef std::unordered_map ParamValueMap; + rclcpp::Node *const node; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_on; @@ -55,6 +57,7 @@ class ParameterHandler std::unordered_map parameter_info; // keep track of set parameters // ParameterMap parameters_full; + ParamValueMap disabled_restore; // ControlValueMap control_values; libcamera::ControlList control_values; @@ -63,6 +66,15 @@ class ParameterHandler // std::condition_variable cv; std::function on_apply_callback; + bool + conflict_exposure(const ParamValueMap &p); + + std::vector + resolve_defaults(ParamValueMap &p); + + std::vector + resolve_overrides(ParamValueMap &p); + void adjust(std::vector ¶meters); From 83e47a87e5a2a526fc837391ab224f542fbeff50 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Fri, 13 Dec 2024 22:21:20 +0100 Subject: [PATCH 8/8] unset ExposureTime when AeEnable on --- src/ParameterHandler.cpp | 26 +++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/src/ParameterHandler.cpp b/src/ParameterHandler.cpp index 8490ece7..60b79217 100644 --- a/src/ParameterHandler.cpp +++ b/src/ParameterHandler.cpp @@ -160,7 +160,7 @@ check(const std::vector ¶meters_old, // are new parameters setting the exposure manually? const bool exposure_updated = std::find_if(parameters_new.begin(), parameters_new.end(), [](const rclcpp::Parameter ¶m) { - return param.get_name() == "ExposureTime"; + return (param.get_name() == "ExposureTime") && (param.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET); }) != parameters_new.end(); // ExposureTime must not be set while AeEnable is true @@ -185,6 +185,14 @@ restore(std::vector ¶meters, disabled_restore.erase("ExposureTime"); } } + // NOTE: if validation fails, we cannot revert this + // TODO: this should go somewhere else + if (p.count("AeEnable") && p.at("AeEnable").as_bool()) { + // NOTE: We cannot "unset" the parameter as this will cause + // "cannot undeclare a statically typed parameter" + parameters.push_back({"ExposureTime", {}}); + // parameters.push_back({"ExposureTime", rclcpp::ParameterValue {0}}); + } } ParameterHandler::ParameterHandler(rclcpp::Node *const node) @@ -248,6 +256,12 @@ ParameterHandler::declare(const libcamera::ControlInfoMap &controls) std::unordered_map parameters; // std::vector descriptors; + // All "control" parameters are declared as dynamically typed in order to be able + // to unset them (set their type to 'rclcpp::ParameterType::PARAMETER_NOT_SET'). + // Unsetting a statically typed parameter causes "cannot undeclare a statically typed parameter". + + // TODO: store and check types manually + // convert camera controls to parameters for (const auto &[id, info] : controls) { // store control id with name @@ -278,6 +292,7 @@ ParameterHandler::declare(const libcamera::ControlInfoMap &controls) rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.name = id->name(); descriptor.type = pv_type; + descriptor.dynamic_typing = true; try { const std::size_t extent = get_extent(id); const bool scalar = (extent == 0); @@ -349,9 +364,12 @@ ParameterHandler::declare(const libcamera::ControlInfoMap &controls) continue; } - // declare parameters without default values and overrides to avoid triggering the callbacks already + // declare parameters without default values, types and overrides to avoid triggering the callbacks already + // parameters have to be dynamically typed in order to unset them at runtime RCLCPP_DEBUG_STREAM(node->get_logger(), "declare '" << id->name() << "' (type: " << pv_type << ")"); - node->declare_parameter(id->name(), pv_type, descriptor, true); + // node->declare_parameter(id->name(), pv_type, descriptor, true); + // node->declare_parameter(id->name(), rclcpp::ParameterType::PARAMETER_NOT_SET, descriptor, true); + node->declare_parameter(id->name(), rclcpp::ParameterValue {}, descriptor, true); } // TODO: merge default parameters and overrides and resolve conflicts @@ -503,6 +521,8 @@ ParameterHandler::validate(const std::vector ¶meters) // } // const std::vector parameters_old = node->get_parameters(parameter_names_old); + // TODO: store ExposureTime here if AeEnable, then disable? + // conflicts const std::vector msgs = check(parameters_old, parameters); // check(parameters_old, parameters)