Skip to content

Commit

Permalink
switch cb confs
Browse files Browse the repository at this point in the history
  • Loading branch information
@christianrauch
christianrauch committed Nov 22, 2024
1 parent b780e24 commit 0077bbc
Show file tree
Hide file tree
Showing 4 changed files with 153 additions and 57 deletions.
16 changes: 15 additions & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,20 @@ find_package(camera_info_manager REQUIRED)
find_package(cv_bridge REQUIRED)
pkg_check_modules(libcamera REQUIRED libcamera>=0.1)

# TODO:
# b953bdddf8de213b4a051ecf2d668dad65ff9f89
# new param callbacks need at least 17.0.0
# message(FATAL_ERROR " rclcpp ${rclcpp_VERSION_MAJOR}")
# message(FATAL_ERROR " rclcpp ${rclcpp_VERSION}")

# add_compile_definitions(rclcpp_VERSION_MAJOR=${rclcpp_VERSION_MAJOR})

if (${rclcpp_VERSION} VERSION_GREATER_EQUAL "17")
# if (${rclcpp_VERSION} VERSION_GREATER_EQUAL "37")
add_compile_definitions(RCLCPP_HAS_PARAM_EXT_CB)
message(WARNING "bla")
endif()

# library with common utility functions for type conversions
add_library(utils OBJECT
src/format_mapping.cpp
Expand All @@ -37,7 +51,7 @@ set_property(TARGET utils PROPERTY POSITION_INDEPENDENT_CODE ON)
add_library(param OBJECT
src/clamp.cpp
src/cv_to_pv.cpp
src/parameter_conflict_check.cpp
# src/parameter_conflict_check.cpp
src/pv_to_cv.cpp
src/types.cpp
src/type_extent.cpp
Expand Down
37 changes: 32 additions & 5 deletions src/CameraNode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,12 @@ class CameraNode : public rclcpp::Node

ParameterHandler parameter_handler;

OnSetParametersCallbackHandle::SharedPtr callback_parameter_change;
#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<std::string, const libcamera::ControlId *> parameter_ids;
Expand All @@ -117,8 +122,13 @@ class CameraNode : public rclcpp::Node
void
process(libcamera::Request *const request);

void
postParameterChange(const std::vector<rclcpp::Parameter> &parameters);

#ifndef RCLCPP_HAS_PARAM_EXT_CB
rcl_interfaces::msg::SetParametersResult
onParameterChange(const std::vector<rclcpp::Parameter> &parameters);
#endif
};

RCLCPP_COMPONENTS_REGISTER_NODE(camera::CameraNode)
Expand Down Expand Up @@ -184,8 +194,13 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options)
: Node("camera", options),
cim(this),
parameter_handler(this),
callback_parameter_change(add_on_set_parameters_callback(
std::bind(&CameraNode::onParameterChange, this, std::placeholders::_1)))
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;
Expand Down Expand Up @@ -565,19 +580,31 @@ CameraNode::process(libcamera::Request *const request)
}
}

rcl_interfaces::msg::SetParametersResult
CameraNode::onParameterChange(const std::vector<rclcpp::Parameter> &parameters)
void
CameraNode::postParameterChange(const std::vector<rclcpp::Parameter> &parameters)
{
std::cout << "postParameterChange" << std::endl;

// check non-controls parameters
for (const rclcpp::Parameter &parameter : parameters) {
if (parameter.get_name() == "jpeg_quality") {
jpeg_quality = parameter.get_parameter_value().get<uint8_t>();
}
}
}

#ifndef RCLCPP_HAS_PARAM_EXT_CB
rcl_interfaces::msg::SetParametersResult
CameraNode::onParameterChange(const std::vector<rclcpp::Parameter> &parameters)
{
std::cout << "onParameterChange" << std::endl;

postParameterChange(parameters);

rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
return result;
}
#endif

} // namespace camera
132 changes: 87 additions & 45 deletions src/ParameterHandler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,23 @@ param_view(const std::vector<rclcpp::Parameter> &parameters)
return param_map;
}

rcl_interfaces::msg::SetParametersResult
format_result(const std::vector<std::string> &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;
}

// TODO: can be used as pre_set cb?
void
resolve(std::vector<rclcpp::Parameter> &parameters)
Expand Down Expand Up @@ -140,11 +157,15 @@ check(const std::vector<rclcpp::Parameter> &parameters_old,
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) && 1
// pre_set -> on_set -> post_set

// adjust parameters
// std::function<void (std::vector<rclcpp::Parameter> &)>
param_cb_adjust = node->add_pre_set_parameters_callback(
param_cb_pre = node->add_pre_set_parameters_callback(
[this](std::vector<rclcpp::Parameter> &parameters) -> void {
std::cout << "ParameterHandler pre >>" << std::endl;
for (const rclcpp::Parameter &param : parameters) {
Expand All @@ -153,12 +174,9 @@ ParameterHandler::ParameterHandler(rclcpp::Node *const node)
std::cout << "ParameterHandler pre ##" << std::endl;
});

// TODO: callback may take single param, need to gather old + new
// param_cb_adjust = node->add_pre_set_parameters_callback(resolve);

// validate parameters
// std::function<rcl_interfaces::msg::SetParametersResult(const std::vector<rclcpp::Parameter> &)>
param_cb_validate = node->add_on_set_parameters_callback(
param_cb_on = node->add_on_set_parameters_callback(
[this](const std::vector<rclcpp::Parameter> &parameters) -> rcl_interfaces::msg::SetParametersResult {
std::cout << "ParameterHandler set >>" << std::endl;
for (const rclcpp::Parameter &param : parameters) {
Expand All @@ -170,22 +188,24 @@ ParameterHandler::ParameterHandler(rclcpp::Node *const node)
return pr;
});

param_cb_validate = node->add_on_set_parameters_callback(
std::bind(&ParameterHandler::OnSetValidate, this, std::placeholders::_1));

// apply parameters
// std::function<void (const std::vector<rclcpp::Parameter> &)>
param_cb_apply = node->add_post_set_parameters_callback(
param_cb_post = node->add_post_set_parameters_callback(
[this](const std::vector<rclcpp::Parameter> &parameters) -> void {
std::cout << "ParameterHandler post >>" << std::endl;
for (const rclcpp::Parameter &param : parameters) {
std::cout << "ParameterHandler post: " << param.get_name() << " to " << param.value_to_string() << std::endl;
}
std::cout << "ParameterHandler post ##" << std::endl;
});
#endif

param_cb_apply = node->add_post_set_parameters_callback(
#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
Expand Down Expand Up @@ -249,7 +269,6 @@ ParameterHandler::declareFromControls(const libcamera::ControlInfoMap &controls)
// 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(),
Expand Down Expand Up @@ -389,17 +408,15 @@ ParameterHandler::clear()
}

void
ParameterHandler::PreSetResolve(std::vector<rclcpp::Parameter> &parameters)
ParameterHandler::resolve(std::vector<rclcpp::Parameter> &parameters)
{
//
(void)parameters;
// clamp?
}

rcl_interfaces::msg::SetParametersResult
ParameterHandler::OnSetValidate(const std::vector<rclcpp::Parameter> &parameters)
std::vector<std::string>
ParameterHandler::validate(const std::vector<rclcpp::Parameter> &parameters)
{
std::cout << "OnSetValidate..." << std::endl;

// TODO: should just go over "controls"
// const std::vector<std::string> parameter_names_old = node->list_parameters({}, {}).names;
std::vector<std::string> parameter_names_old;
Expand Down Expand Up @@ -431,22 +448,17 @@ ParameterHandler::OnSetValidate(const std::vector<rclcpp::Parameter> &parameters
// }
// const std::vector<rclcpp::Parameter> parameters_old = node->get_parameters(parameter_names_old);

{ // conflicts
const std::vector<std::string> msgs = check(parameters_old, parameters);
if (!msgs.empty()) {
rcl_interfaces::msg::SetParametersResult result;
result.successful = msgs.empty();
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 += "; ";
}
std::cout << "validate: " << result.reason << std::endl;
return result;
}
// conflicts
const std::vector<std::string> 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'

Expand Down Expand Up @@ -500,23 +512,15 @@ ParameterHandler::OnSetValidate(const std::vector<rclcpp::Parameter> &parameters
} // in parameter_ids
} // parameters

// TODO: move to function
rcl_interfaces::msg::SetParametersResult result;
result.successful = msgs_valid_check.empty();
for (size_t i = 0; i < msgs_valid_check.size(); i++) {
if (msgs_valid_check.size() > 1)
result.reason += "(" + std::to_string(i) + ") ";
result.reason += msgs_valid_check[i];
if (i < msgs_valid_check.size() - 1)
result.reason += "; ";
}
return result;
// return format_result(msgs_valid_check);

return msgs_valid_check;
}

void
ParameterHandler::PostSetApply(const std::vector<rclcpp::Parameter> &parameters)
ParameterHandler::apply(const std::vector<rclcpp::Parameter> &parameters)
{
std::cout << "PostSetApply..." << std::endl;
std::cout << "apply..." << std::endl;

parameters_lock.lock();
for (const rclcpp::Parameter &parameter : parameters) {
Expand All @@ -527,13 +531,51 @@ ParameterHandler::PostSetApply(const std::vector<rclcpp::Parameter> &parameters)
control_values[parameter_ids.at(parameter.get_name())->id()] = value;
}
parameters_lock.unlock();
}

rcl_interfaces::msg::SetParametersResult
ParameterHandler::OnSetValidate(const std::vector<rclcpp::Parameter> &parameters)
{
std::cout << "OnSetValidate..." << std::endl;

// If this is the only parameter callback available, call the pre-, on- and post-set callbacks manually.

#ifndef RCLCPP_HAS_PARAM_EXT_CB
resolve(parameters);
#endif

const rcl_interfaces::msg::SetParametersResult result = format_result(validate(parameters));

#ifndef RCLCPP_HAS_PARAM_EXT_CB
if (result.successful)
apply(parameters);
#endif

return result;
}

#ifdef RCLCPP_HAS_PARAM_EXT_CB
void
ParameterHandler::PreSetResolve(std::vector<rclcpp::Parameter> &parameters)
{
resolve(parameters);
// clamp?
}

void
ParameterHandler::PostSetApply(const std::vector<rclcpp::Parameter> &parameters)
{
std::cout << "PostSetApply..." << std::endl;

apply(parameters);

// std::tie(controls, msgs) = parameter_handler.parameterCheckAndConvert(parameters);

// parameters_lock.lock();
// this->parameters = controls;
// parameters_lock.unlock();
}
#endif

// std::tuple<ParameterHandler::ControlValueMap, std::vector<std::string>>
// ParameterHandler::parameterCheckAndConvert(const std::vector<rclcpp::Parameter> &parameters)
Expand Down
25 changes: 19 additions & 6 deletions src/ParameterHandler.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@ class Node;
class ParameterHandler
{
public:
// re-definition of the private ControlList::ControlListMap
typedef std::unordered_map<unsigned int, libcamera::ControlValue> ControlValueMap;

ParameterHandler(rclcpp::Node *const node);
Expand All @@ -36,11 +37,12 @@ class ParameterHandler
private:
rclcpp::Node *const node;

// NOTE: pre- and post-set handles not supported on humble
// PreSetParametersCallbackHandle and PostSetParametersCallbackHandle
rclcpp::node_interfaces::PreSetParametersCallbackHandle::SharedPtr param_cb_adjust;
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_validate;
rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr param_cb_apply;
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<string, {ControlId, ControlInfo}>
// std::vector<std::string> parameter_names;
Expand All @@ -53,11 +55,22 @@ class ParameterHandler
std::mutex parameters_lock;

void
PreSetResolve(std::vector<rclcpp::Parameter> &parameters);
resolve(std::vector<rclcpp::Parameter> &parameters);

std::vector<std::string>
validate(const std::vector<rclcpp::Parameter> &parameters);

void
apply(const std::vector<rclcpp::Parameter> &parameters);

rcl_interfaces::msg::SetParametersResult
OnSetValidate(const std::vector<rclcpp::Parameter> &parameters);

#ifdef RCLCPP_HAS_PARAM_EXT_CB
void
PreSetResolve(std::vector<rclcpp::Parameter> &parameters);

void
PostSetApply(const std::vector<rclcpp::Parameter> &parameters);
#endif
};

0 comments on commit 0077bbc

Please sign in to comment.