diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 99b5e7f..852f107 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: - {distribution: ubuntu, version: 22.04, ros: humble} diff --git a/CMakeLists.txt b/CMakeLists.txt index aaebef0..89f99d5 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() @@ -19,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") @@ -52,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 6e987ba..536cc7d 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 @@ -70,12 +64,6 @@ namespace rclcpp class NodeOptions; } -#define CASE_RANGE(T, R) \ - case libcamera::ControlType##T: \ - R.from_value = max(info.min()); \ - R.to_value = min(info.max()); \ - break; - namespace camera { @@ -113,30 +101,36 @@ 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 + + void + apply_parameters(const libcamera::ControlList &controls); }; RCLCPP_COMPONENTS_REGISTER_NODE(camera::CameraNode) @@ -198,7 +192,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; @@ -394,7 +398,9 @@ 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()); + 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(); @@ -447,7 +453,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 @@ -488,124 +494,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_RANGE(Integer32, range_int) - CASE_RANGE(Integer64, range_int) - CASE_RANGE(Float, range_float) -#if LIBCAMERA_VER_GE(0, 4, 0) - CASE_RANGE(Unsigned16, range_int) - CASE_RANGE(Unsigned32, range_int) -#endif - 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) { @@ -700,99 +588,50 @@ CameraNode::process(libcamera::Request *const request) // queue the request again for the next frame 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(); - } - parameters_lock.unlock(); - 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; + std::cout << "postParameterChange" << std::endl; - // 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 += "; "; + // check non-controls parameters + for (const rclcpp::Parameter ¶meter : parameters) { + if (parameter.get_name() == "jpeg_quality") { + jpeg_quality = parameter.get_parameter_value().get(); } - return result; } +} - result.successful = true; - - 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; - } +#ifndef RCLCPP_HAS_PARAM_EXT_CB +rcl_interfaces::msg::SetParametersResult +CameraNode::onParameterChange(const std::vector ¶meters) +{ + std::cout << "onParameterChange" << std::endl; - // 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; - } + postParameterChange(parameters); - parameters_lock.lock(); - this->parameters[parameter_ids.at(parameter.get_name())->id()] = value; - parameters_lock.unlock(); + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; +} +#endif - parameters_full[parameter.get_name()] = parameter.get_parameter_value(); - } - } - else if (!parameter.get_name().compare("jpeg_quality")) { - jpeg_quality = parameter.get_parameter_value().get(); +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; + 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()); } } - - return result; } } // namespace camera diff --git a/src/ParameterHandler.cpp b/src/ParameterHandler.cpp new file mode 100644 index 0000000..db88105 --- /dev/null +++ b/src/ParameterHandler.cpp @@ -0,0 +1,788 @@ +#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 +#include + + +#define CASE_RANGE(T, RU, RL) \ + case libcamera::ControlType##T: \ + { \ + rcl_interfaces::msg::RU range; \ + range.from_value = max(info.min()); \ + range.to_value = min(info.max()); \ + descriptor.RL = {range}; \ + } break; + +// 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) +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); + // } +} + +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 +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_type() != rclcpp::ParameterType::PARAMETER_NOT_SET && p.at("AeEnable").get() && + p.count("ExposureTime") && p.at("ExposureTime").get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET; +} + +std::vector +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."); + } + return msgs; +} + +std::vector +ParameterHandler::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."); + } + // restore 'ExposureTime' + if (p.count("AeEnable") && p.at("AeEnable").get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET && !p.at("AeEnable").get()) { + p.at("ExposureTime") = disabled_restore.at("ExposureTime"); + disabled_restore.erase("ExposureTime"); + } + 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(); + std::cout << "ae_enabled: " << ae_enabled << std::endl; + // 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") && (param.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET); + }) != parameters_new.end(); + std::cout << "exposure_updated: " << exposure_updated << std::endl; + + // 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; +} + +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")) { + std::cout << "trying to restore ExposureTime ..." << std::endl; + // disabled_restore should have stored something, but it hasn't + parameters.push_back({"ExposureTime", disabled_restore.at("ExposureTime")}); + disabled_restore.erase("ExposureTime"); + std::cout << "... OK" << std::endl; + } + } + // NOTE: if validation fails, we cannot revert this + + // TODO: this should go somewhere else + // TODO: for "set_parameters({{"AeEnable", true}});", we only see a single parameter (AeEnable) here + // if (p.count("AeEnable") && p.at("AeEnable").as_bool() && p.count("ExposureTime")) { + // p.at("ExposureTime") = {"ExposureTime", {}}; + // } + 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) + : 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; + + // 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 + 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; + descriptor.dynamic_typing = true; + 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; + // #if LIBCAMERA_VER_GE(0, 4, 0) + // case libcamera::ControlTypeUnsigned16: + // { + // rcl_interfaces::msg::IntegerRange range; + // range.from_value = max(info.min()); + // range.to_value = min(info.max()); + // descriptor.integer_range = {range}; + // } break; + // case libcamera::ControlTypeUnsigned32: + // { + // rcl_interfaces::msg::IntegerRange range; + // range.from_value = max(info.min()); + // range.to_value = min(info.max()); + // descriptor.integer_range = {range}; + // } break; + // #endif + // default: + // break; + // } + + switch (id->type()) { + CASE_RANGE(Integer32, IntegerRange, integer_range) + CASE_RANGE(Integer64, IntegerRange, integer_range) + CASE_RANGE(Float, FloatingPointRange, floating_point_range) +#if LIBCAMERA_VER_GE(0, 4, 0) + CASE_RANGE(Unsigned16, IntegerRange, integer_range) + CASE_RANGE(Unsigned32, IntegerRange, integer_range) +#endif + 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, 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(), 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 + + // 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? + // if (!control_values.empty()) + // parameters_consumed_lock.unlock(); + return control_values; +} + +// void +// ParameterHandler::clear() +// { +// parameters_lock.lock(); +// control_values.clear(); +// parameters_lock.unlock(); +// } + +void +ParameterHandler::set_on_apply_callback(std::function callback) +{ + on_apply_callback = callback; +} + +void +ParameterHandler::adjust(std::vector ¶meters) +{ + // + // (void)parameters; + // for (const rclcpp::Parameter ¶meter : parameters) { + // // + // } + restore(parameters, disabled_restore); +} + +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); + + // TODO: store ExposureTime here if AeEnable, then disable? + + // 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? + + // 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?? + 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; + // const std::string &name = libcamera::controls::controls.at(id)->name(); + 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(); +} + +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 0000000..eecbce0 --- /dev/null +++ b/src/ParameterHandler.hpp @@ -0,0 +1,87 @@ +#pragma once +#include "parameter_conflict_check.hpp" +#include +#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 + set_on_apply_callback(std::function callback); + +private: + typedef std::unordered_map ParamValueMap; + + 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; + ParamValueMap disabled_restore; + + libcamera::ControlList control_values; + std::mutex parameters_lock; + + 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); + + 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 +};