Skip to content

Commit

Permalink
move parameter handling to separate class and library
Browse files Browse the repository at this point in the history
  • Loading branch information
@christianrauch
christianrauch committed Sep 13, 2024
1 parent 83df19c commit 36231da
Show file tree
Hide file tree
Showing 4 changed files with 286 additions and 199 deletions.
27 changes: 19 additions & 8 deletions CMakeLists.txt
View file
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
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

0 comments on commit 36231da

Please sign in to comment.