Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

dedicated class and library for parameter handling #57

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

dedicated class and library for parameter handling #57

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 19 additions & 8 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,24 +21,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")
Expand All @@ -56,7 +67,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)
Expand Down
221 changes: 30 additions & 191 deletions src/CameraNode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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 <algorithm>
#include <camera_info_manager/camera_info_manager.hpp>
#include <cassert>
Expand Down Expand Up @@ -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<std::string, const libcamera::ControlId *> parameter_ids;
// parameters that are to be set for every request
std::unordered_map<unsigned int, libcamera::ControlValue> 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);

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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<libcamera::ControlTypeInteger32>(info.min());
range_int.to_value = min<libcamera::ControlTypeInteger32>(info.max());
break;
case libcamera::ControlTypeInteger64:
range_int.from_value = max<libcamera::ControlTypeInteger64>(info.min());
range_int.to_value = min<libcamera::ControlTypeInteger64>(info.max());
break;
case libcamera::ControlTypeFloat:
range_float.from_value = max<libcamera::ControlTypeFloat>(info.min());
range_float.to_value = min<libcamera::ControlTypeFloat>(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<std::string> 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<rclcpp::Parameter> 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)
{
Expand Down Expand Up @@ -690,11 +571,24 @@ CameraNode::process(libcamera::Request *const request)
rcl_interfaces::msg::SetParametersResult
CameraNode::onParameterChange(const std::vector<rclcpp::Parameter> &parameters)
{
rcl_interfaces::msg::SetParametersResult result;
// check non-controls parameters
for (const rclcpp::Parameter &parameter : parameters) {
if (!parameter.get_name().compare("jpeg_quality")) {
jpeg_quality = parameter.get_parameter_value().get<uint8_t>();
}
}

ParameterHandler::ControlValueMap controls;
std::vector<std::string> 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<std::string> 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++) {
Expand All @@ -704,61 +598,6 @@ CameraNode::onParameterChange(const std::vector<rclcpp::Parameter> &parameters)
if (i < msgs.size() - 1)
result.reason += "; ";
}
return result;
}

result.successful = true;

for (const rclcpp::Parameter &parameter : 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<uint8_t>();
}
}

return result;
Expand Down
Loading