diff --git a/src/ParameterHandler.cpp b/src/ParameterHandler.cpp index 1543c85..8490ece 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 22413ba..656451c 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);