diff --git a/CMakeLists.txt b/CMakeLists.txt index f09a00b..d09cdc3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,24 +22,35 @@ pkg_check_modules(libcamera REQUIRED libcamera>=0.1) # 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") @@ -57,7 +68,7 @@ ament_target_dependencies( ) target_include_directories(camera_component PUBLIC ${libcamera_INCLUDE_DIRS}) -target_link_libraries(camera_component ${libcamera_LINK_LIBRARIES} utils) +target_link_libraries(camera_component ${libcamera_LINK_LIBRARIES} utils param) install(TARGETS camera_component DESTINATION lib) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 1bfe1fb..8529487 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 @@ -110,19 +105,16 @@ class CameraNode : public rclcpp::Node OnSetParametersCallbackHandle::SharedPtr callback_parameter_change; + ParameterHandler parameter_handler; + // 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; + ParameterHandler::ControlValueMap parameters; std::mutex parameters_lock; // compression quality parameter std::atomic_uint8_t jpeg_quality; - void - declareParameters(); - void requestComplete(libcamera::Request *const request); @@ -190,7 +182,8 @@ 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) { // pixel format rcl_interfaces::msg::ParameterDescriptor param_descr_format; @@ -385,7 +378,13 @@ 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.declareFromControls(camera->controls()); + + // 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)); // allocate stream buffers and create one request per buffer stream = scfg.stream(); @@ -468,124 +467,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) - parameters_init_list.emplace_back(name, value); - set_parameters(parameters_init_list); -} - void CameraNode::requestComplete(libcamera::Request *const request) { @@ -690,11 +571,24 @@ CameraNode::process(libcamera::Request *const request) rcl_interfaces::msg::SetParametersResult CameraNode::onParameterChange(const std::vector ¶meters) { - rcl_interfaces::msg::SetParametersResult result; + // check non-controls parameters + for (const rclcpp::Parameter ¶meter : parameters) { + if (!parameter.get_name().compare("jpeg_quality")) { + jpeg_quality = parameter.get_parameter_value().get(); + } + } + + ParameterHandler::ControlValueMap controls; + std::vector msgs; + + std::tie(controls, msgs) = parameter_handler.parameterCheckAndConvert(parameters); + + parameters_lock.lock(); + this->parameters = controls; + parameters_lock.unlock(); - // check target parameter state (current and new parameters) - // for conflicting configuration - const std::vector msgs = check_conflicts(parameters, parameters_full); + rcl_interfaces::msg::SetParametersResult result; + result.successful = msgs.empty(); if (!msgs.empty()) { result.successful = false; for (size_t i = 0; i < msgs.size(); i++) { @@ -704,61 +598,6 @@ CameraNode::onParameterChange(const std::vector ¶meters) if (i < msgs.size() - 1) result.reason += "; "; } - 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; - } - - // 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")) { - jpeg_quality = parameter.get_parameter_value().get(); - } } return result; diff --git a/src/ParameterHandler.cpp b/src/ParameterHandler.cpp new file mode 100644 index 0000000..8729f6d --- /dev/null +++ b/src/ParameterHandler.cpp @@ -0,0 +1,202 @@ +#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 + + +ParameterHandler::ParameterHandler(rclcpp::Node *const node) + : node(node) +{ + // +} + +void +ParameterHandler::declareFromControls(const libcamera::ControlInfoMap &controls) +{ + // dynamic camera configuration + ParameterMap parameters_init; + for (const auto &[id, info] : controls) { + // store control id with name + parameter_ids[id->name()] = id; + parameter_info[id->name()] = info; + + 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 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(node->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_default; + try { + value_default = cv_to_pv(clamp(info.def(), info.min(), info.max())); + } + 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 and set default or initial value + RCLCPP_DEBUG_STREAM(node->get_logger(), + "declare " << id->name() << " with default " << rclcpp::to_string(value_default)); + + if (value_default.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { + node->declare_parameter(id->name(), pv_type, param_descr); + } + else { + node->declare_parameter(id->name(), value_default, param_descr); + parameters_init[id->name()] = value_default; + } + } + + // resolve conflicts of default libcamera configuration and user provided overrides + std::vector status; + std::tie(parameters_init, status) = + resolve_conflicts(parameters_init, node->get_node_parameters_interface()->get_parameter_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); +} + +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(); + } + } + } + + return {control_values, msgs_valid_check}; +} diff --git a/src/ParameterHandler.hpp b/src/ParameterHandler.hpp new file mode 100644 index 0000000..a3e88df --- /dev/null +++ b/src/ParameterHandler.hpp @@ -0,0 +1,35 @@ +#pragma once +#include "parameter_conflict_check.hpp" +#include +#include +#include +#include +#include + +namespace rclcpp +{ +class Node; +class Parameter; +} // namespace rclcpp + + +class ParameterHandler +{ +public: + typedef std::unordered_map ControlValueMap; + + ParameterHandler(rclcpp::Node *const node); + + void + declareFromControls(const libcamera::ControlInfoMap &controls); + + std::tuple> + parameterCheckAndConvert(const std::vector ¶meters); + +private: + rclcpp::Node *const node; + std::unordered_map parameter_ids; + std::unordered_map parameter_info; + // keep track of set parameters + ParameterMap parameters_full; +};