Skip to content

Commit 94db029

Browse files
christianrauchchristianrauch
committed
switch cb confs
1 parent b780e24 commit 94db029

File tree

4 files changed

+150
-58
lines changed

4 files changed

+150
-58
lines changed

‎CMakeLists.txt

Lines changed: 16 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -13,13 +13,27 @@ find_package(PkgConfig REQUIRED)
1313

1414
# find dependencies
1515
find_package(ament_cmake REQUIRED)
16-
find_package(rclcpp REQUIRED)
16+
find_package(rclcpp 28.1.5 REQUIRED)
1717
find_package(rclcpp_components REQUIRED)
1818
find_package(sensor_msgs REQUIRED)
1919
find_package(camera_info_manager REQUIRED)
2020
find_package(cv_bridge REQUIRED)
2121
pkg_check_modules(libcamera REQUIRED libcamera>=0.1)
2222

23+
# TODO:
24+
# b953bdddf8de213b4a051ecf2d668dad65ff9f89
25+
# new param callbacks need at least 17.0.0
26+
# message(FATAL_ERROR " rclcpp ${rclcpp_VERSION_MAJOR}")
27+
# message(FATAL_ERROR " rclcpp ${rclcpp_VERSION}")
28+
29+
# add_compile_definitions(rclcpp_VERSION_MAJOR=${rclcpp_VERSION_MAJOR})
30+
31+
if (${rclcpp_VERSION} VERSION_GREATER_EQUAL "17")
32+
# if (${rclcpp_VERSION} VERSION_GREATER_EQUAL "37")
33+
add_compile_definitions(RCLCPP_HAS_PARAM_EXT_CB)
34+
message(WARNING "bla")
35+
endif()
36+
2337
# library with common utility functions for type conversions
2438
add_library(utils OBJECT
2539
src/format_mapping.cpp
@@ -37,7 +51,7 @@ set_property(TARGET utils PROPERTY POSITION_INDEPENDENT_CODE ON)
3751
add_library(param OBJECT
3852
src/clamp.cpp
3953
src/cv_to_pv.cpp
40-
src/parameter_conflict_check.cpp
54+
# src/parameter_conflict_check.cpp
4155
src/pv_to_cv.cpp
4256
src/types.cpp
4357
src/type_extent.cpp

‎src/CameraNode.cpp

Lines changed: 32 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -104,7 +104,12 @@ class CameraNode : public rclcpp::Node
104104

105105
ParameterHandler parameter_handler;
106106

107-
OnSetParametersCallbackHandle::SharedPtr callback_parameter_change;
107+
#ifdef RCLCPP_HAS_PARAM_EXT_CB
108+
// use new "post_set" callback to apply parameters
109+
PostSetParametersCallbackHandle::SharedPtr param_cb_change;
110+
#else
111+
OnSetParametersCallbackHandle::SharedPtr param_cb_change;
112+
#endif
108113

109114
// map parameter names to libcamera control id
110115
std::unordered_map<std::string, const libcamera::ControlId *> parameter_ids;
@@ -117,8 +122,13 @@ class CameraNode : public rclcpp::Node
117122
void
118123
process(libcamera::Request *const request);
119124

125+
void
126+
postParameterChange(const std::vector<rclcpp::Parameter> &parameters);
127+
128+
#ifndef RCLCPP_HAS_PARAM_EXT_CB
120129
rcl_interfaces::msg::SetParametersResult
121130
onParameterChange(const std::vector<rclcpp::Parameter> &parameters);
131+
#endif
122132
};
123133

124134
RCLCPP_COMPONENTS_REGISTER_NODE(camera::CameraNode)
@@ -184,8 +194,13 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options)
184194
: Node("camera", options),
185195
cim(this),
186196
parameter_handler(this),
187-
callback_parameter_change(add_on_set_parameters_callback(
188-
std::bind(&CameraNode::onParameterChange, this, std::placeholders::_1)))
197+
param_cb_change(
198+
#ifdef RCLCPP_HAS_PARAM_EXT_CB
199+
add_post_set_parameters_callback(std::bind(&CameraNode::postParameterChange, this, std::placeholders::_1))
200+
#else
201+
add_on_set_parameters_callback(std::bind(&CameraNode::onParameterChange, this, std::placeholders::_1))
202+
#endif
203+
)
189204
{
190205
// pixel format
191206
rcl_interfaces::msg::ParameterDescriptor param_descr_format;
@@ -565,19 +580,31 @@ CameraNode::process(libcamera::Request *const request)
565580
}
566581
}
567582

568-
rcl_interfaces::msg::SetParametersResult
569-
CameraNode::onParameterChange(const std::vector<rclcpp::Parameter> &parameters)
583+
void
584+
CameraNode::postParameterChange(const std::vector<rclcpp::Parameter> &parameters)
570585
{
586+
std::cout << "postParameterChange" << std::endl;
587+
571588
// check non-controls parameters
572589
for (const rclcpp::Parameter &parameter : parameters) {
573590
if (parameter.get_name() == "jpeg_quality") {
574591
jpeg_quality = parameter.get_parameter_value().get<uint8_t>();
575592
}
576593
}
594+
}
595+
596+
#ifndef RCLCPP_HAS_PARAM_EXT_CB
597+
rcl_interfaces::msg::SetParametersResult
598+
CameraNode::onParameterChange(const std::vector<rclcpp::Parameter> &parameters)
599+
{
600+
std::cout << "onParameterChange" << std::endl;
601+
602+
postParameterChange(parameters);
577603

578604
rcl_interfaces::msg::SetParametersResult result;
579605
result.successful = true;
580606
return result;
581607
}
608+
#endif
582609

583610
} // namespace camera

‎src/ParameterHandler.cpp

Lines changed: 83 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,23 @@ param_view(const std::vector<rclcpp::Parameter> &parameters)
5353
return param_map;
5454
}
5555

56+
rcl_interfaces::msg::SetParametersResult
57+
format_result(const std::vector<std::string> &msgs)
58+
{
59+
rcl_interfaces::msg::SetParametersResult result;
60+
result.successful = msgs.empty();
61+
if (!result.successful) {
62+
for (size_t i = 0; i < msgs.size(); i++) {
63+
if (msgs.size() > 1)
64+
result.reason += "(" + std::to_string(i) + ") ";
65+
result.reason += msgs[i];
66+
if (i < msgs.size() - 1)
67+
result.reason += "; ";
68+
}
69+
}
70+
return result;
71+
}
72+
5673
// TODO: can be used as pre_set cb?
5774
void
5875
resolve(std::vector<rclcpp::Parameter> &parameters)
@@ -140,11 +157,15 @@ check(const std::vector<rclcpp::Parameter> &parameters_old,
140157
ParameterHandler::ParameterHandler(rclcpp::Node *const node)
141158
: node(node)
142159
{
160+
param_cb_on = node->add_on_set_parameters_callback(
161+
std::bind(&ParameterHandler::OnSetValidate, this, std::placeholders::_1));
162+
163+
#if defined(RCLCPP_HAS_PARAM_EXT_CB) && 1
143164
// pre_set -> on_set -> post_set
144165

145166
// adjust parameters
146167
// std::function<void (std::vector<rclcpp::Parameter> &)>
147-
param_cb_adjust = node->add_pre_set_parameters_callback(
168+
param_cb_pre = node->add_pre_set_parameters_callback(
148169
[this](std::vector<rclcpp::Parameter> &parameters) -> void {
149170
std::cout << "ParameterHandler pre >>" << std::endl;
150171
for (const rclcpp::Parameter &param : parameters) {
@@ -153,12 +174,9 @@ ParameterHandler::ParameterHandler(rclcpp::Node *const node)
153174
std::cout << "ParameterHandler pre ##" << std::endl;
154175
});
155176

156-
// TODO: callback may take single param, need to gather old + new
157-
// param_cb_adjust = node->add_pre_set_parameters_callback(resolve);
158-
159177
// validate parameters
160178
// std::function<rcl_interfaces::msg::SetParametersResult(const std::vector<rclcpp::Parameter> &)>
161-
param_cb_validate = node->add_on_set_parameters_callback(
179+
param_cb_on = node->add_on_set_parameters_callback(
162180
[this](const std::vector<rclcpp::Parameter> &parameters) -> rcl_interfaces::msg::SetParametersResult {
163181
std::cout << "ParameterHandler set >>" << std::endl;
164182
for (const rclcpp::Parameter &param : parameters) {
@@ -170,22 +188,24 @@ ParameterHandler::ParameterHandler(rclcpp::Node *const node)
170188
return pr;
171189
});
172190

173-
param_cb_validate = node->add_on_set_parameters_callback(
174-
std::bind(&ParameterHandler::OnSetValidate, this, std::placeholders::_1));
175-
176191
// apply parameters
177192
// std::function<void (const std::vector<rclcpp::Parameter> &)>
178-
param_cb_apply = node->add_post_set_parameters_callback(
193+
param_cb_post = node->add_post_set_parameters_callback(
179194
[this](const std::vector<rclcpp::Parameter> &parameters) -> void {
180195
std::cout << "ParameterHandler post >>" << std::endl;
181196
for (const rclcpp::Parameter &param : parameters) {
182197
std::cout << "ParameterHandler post: " << param.get_name() << " to " << param.value_to_string() << std::endl;
183198
}
184199
std::cout << "ParameterHandler post ##" << std::endl;
185200
});
201+
#endif
186202

187-
param_cb_apply = node->add_post_set_parameters_callback(
203+
#ifdef RCLCPP_HAS_PARAM_EXT_CB
204+
param_cb_pre = node->add_pre_set_parameters_callback(
205+
std::bind(&ParameterHandler::PreSetResolve, this, std::placeholders::_1));
206+
param_cb_post = node->add_post_set_parameters_callback(
188207
std::bind(&ParameterHandler::PostSetApply, this, std::placeholders::_1));
208+
#endif
189209
}
190210

191211
void
@@ -249,7 +269,6 @@ ParameterHandler::declareFromControls(const libcamera::ControlInfoMap &controls)
249269
// Camera controls can have arrays for minimum and maximum values, but the parameter descriptor
250270
// only supports scalar bounds.
251271
// Get smallest bounds for minimum and maximum set but warn user.
252-
253272
if (info.min().isArray() || info.max().isArray()) {
254273
RCLCPP_WARN_STREAM(
255274
node->get_logger(),
@@ -389,17 +408,15 @@ ParameterHandler::clear()
389408
}
390409

391410
void
392-
ParameterHandler::PreSetResolve(std::vector<rclcpp::Parameter> &parameters)
411+
ParameterHandler::resolve(std::vector<rclcpp::Parameter> &parameters)
393412
{
413+
//
394414
(void)parameters;
395-
// clamp?
396415
}
397416

398-
rcl_interfaces::msg::SetParametersResult
399-
ParameterHandler::OnSetValidate(const std::vector<rclcpp::Parameter> &parameters)
417+
std::vector<std::string>
418+
ParameterHandler::validate(const std::vector<rclcpp::Parameter> &parameters)
400419
{
401-
std::cout << "OnSetValidate..." << std::endl;
402-
403420
// TODO: should just go over "controls"
404421
// const std::vector<std::string> parameter_names_old = node->list_parameters({}, {}).names;
405422
std::vector<std::string> parameter_names_old;
@@ -431,22 +448,17 @@ ParameterHandler::OnSetValidate(const std::vector<rclcpp::Parameter> &parameters
431448
// }
432449
// const std::vector<rclcpp::Parameter> parameters_old = node->get_parameters(parameter_names_old);
433450

434-
{ // conflicts
435-
const std::vector<std::string> msgs = check(parameters_old, parameters);
436-
if (!msgs.empty()) {
437-
rcl_interfaces::msg::SetParametersResult result;
438-
result.successful = msgs.empty();
439-
for (size_t i = 0; i < msgs.size(); i++) {
440-
if (msgs.size() > 1)
441-
result.reason += "(" + std::to_string(i) + ") ";
442-
result.reason += msgs[i];
443-
if (i < msgs.size() - 1)
444-
result.reason += "; ";
445-
}
446-
std::cout << "validate: " << result.reason << std::endl;
447-
return result;
448-
}
451+
// conflicts
452+
const std::vector<std::string> msgs = check(parameters_old, parameters);
453+
// check(parameters_old, parameters)
454+
// const rcl_interfaces::msg::SetParametersResult result = format_result();
455+
if (!msgs.empty()) {
456+
// std::cout << "validate: " << result.reason << std::endl;
457+
// return format_result(msgs);
458+
return msgs;
449459
}
460+
// if (!result.successful)
461+
// return result;
450462

451463
// TODO: check other mismatches from 'parameterCheckAndConvert'
452464

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

503-
// TODO: move to function
504-
rcl_interfaces::msg::SetParametersResult result;
505-
result.successful = msgs_valid_check.empty();
506-
for (size_t i = 0; i < msgs_valid_check.size(); i++) {
507-
if (msgs_valid_check.size() > 1)
508-
result.reason += "(" + std::to_string(i) + ") ";
509-
result.reason += msgs_valid_check[i];
510-
if (i < msgs_valid_check.size() - 1)
511-
result.reason += "; ";
512-
}
513-
return result;
515+
// return format_result(msgs_valid_check);
516+
517+
return msgs_valid_check;
514518
}
515519

516520
void
517-
ParameterHandler::PostSetApply(const std::vector<rclcpp::Parameter> &parameters)
521+
ParameterHandler::apply(const std::vector<rclcpp::Parameter> &parameters)
518522
{
519-
std::cout << "PostSetApply..." << std::endl;
523+
std::cout << "apply..." << std::endl;
520524

521525
parameters_lock.lock();
522526
for (const rclcpp::Parameter &parameter : parameters) {
@@ -527,13 +531,47 @@ ParameterHandler::PostSetApply(const std::vector<rclcpp::Parameter> &parameters)
527531
control_values[parameter_ids.at(parameter.get_name())->id()] = value;
528532
}
529533
parameters_lock.unlock();
534+
}
535+
536+
rcl_interfaces::msg::SetParametersResult
537+
ParameterHandler::OnSetValidate(const std::vector<rclcpp::Parameter> &parameters)
538+
{
539+
std::cout << "OnSetValidate..." << std::endl;
540+
541+
#ifdef RCLCPP_HAS_PARAM_EXT_CB
542+
return format_result(validate(parameters));
543+
#else
544+
// This is the only parameter callback available.
545+
// Call the pre-, on= and post-set callbacks manually.
546+
resolve(parameters);
547+
const std::vector<std::string> msgs = validate(parameters);
548+
apply(parameters);
549+
return format_result(msgs);
550+
#endif
551+
}
552+
553+
#ifdef RCLCPP_HAS_PARAM_EXT_CB
554+
void
555+
ParameterHandler::PreSetResolve(std::vector<rclcpp::Parameter> &parameters)
556+
{
557+
resolve(parameters);
558+
// clamp?
559+
}
560+
561+
void
562+
ParameterHandler::PostSetApply(const std::vector<rclcpp::Parameter> &parameters)
563+
{
564+
std::cout << "PostSetApply..." << std::endl;
565+
566+
apply(parameters);
530567

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

533570
// parameters_lock.lock();
534571
// this->parameters = controls;
535572
// parameters_lock.unlock();
536573
}
574+
#endif
537575

538576
// std::tuple<ParameterHandler::ControlValueMap, std::vector<std::string>>
539577
// ParameterHandler::parameterCheckAndConvert(const std::vector<rclcpp::Parameter> &parameters)

‎src/ParameterHandler.hpp

Lines changed: 19 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@ class Node;
1717
class ParameterHandler
1818
{
1919
public:
20+
// re-definition of the private ControlList::ControlListMap
2021
typedef std::unordered_map<unsigned int, libcamera::ControlValue> ControlValueMap;
2122

2223
ParameterHandler(rclcpp::Node *const node);
@@ -36,11 +37,12 @@ class ParameterHandler
3637
private:
3738
rclcpp::Node *const node;
3839

39-
// NOTE: pre- and post-set handles not supported on humble
40-
// PreSetParametersCallbackHandle and PostSetParametersCallbackHandle
41-
rclcpp::node_interfaces::PreSetParametersCallbackHandle::SharedPtr param_cb_adjust;
42-
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_validate;
43-
rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr param_cb_apply;
40+
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_on;
41+
42+
#ifdef RCLCPP_HAS_PARAM_EXT_CB
43+
rclcpp::node_interfaces::PreSetParametersCallbackHandle::SharedPtr param_cb_pre;
44+
rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr param_cb_post;
45+
#endif
4446

4547
// TODO: consider a map<string, {ControlId, ControlInfo}>
4648
// std::vector<std::string> parameter_names;
@@ -53,11 +55,22 @@ class ParameterHandler
5355
std::mutex parameters_lock;
5456

5557
void
56-
PreSetResolve(std::vector<rclcpp::Parameter> &parameters);
58+
resolve(std::vector<rclcpp::Parameter> &parameters);
59+
60+
std::vector<std::string>
61+
validate(const std::vector<rclcpp::Parameter> &parameters);
62+
63+
void
64+
apply(const std::vector<rclcpp::Parameter> &parameters);
5765

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

69+
#ifdef RCLCPP_HAS_PARAM_EXT_CB
70+
void
71+
PreSetResolve(std::vector<rclcpp::Parameter> &parameters);
72+
6173
void
6274
PostSetApply(const std::vector<rclcpp::Parameter> &parameters);
75+
#endif
6376
};

0 commit comments

Comments
 (0)