Skip to content

Commit b55b63a

Browse files
christianrauchchristianrauch
committed
[WIP] get/set etc
1 parent 4b0fbe2 commit b55b63a

File tree

3 files changed

+164
-37
lines changed

3 files changed

+164
-37
lines changed

‎src/CameraNode.cpp

Lines changed: 9 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@
3030
#include <libcamera/request.h>
3131
#include <libcamera/stream.h>
3232
#include <memory>
33-
#include <mutex>
33+
// #include <mutex>
3434
#include <opencv2/core/mat.hpp>
3535
#include <opencv2/imgcodecs.hpp>
3636
#include <optional>
@@ -110,8 +110,8 @@ class CameraNode : public rclcpp::Node
110110
// map parameter names to libcamera control id
111111
std::unordered_map<std::string, const libcamera::ControlId *> parameter_ids;
112112
// parameters that are to be set for every request
113-
ParameterHandler::ControlValueMap parameters;
114-
std::mutex parameters_lock;
113+
// ParameterHandler::ControlValueMap parameters;
114+
// std::mutex parameters_lock;
115115
// compression quality parameter
116116
std::atomic_uint8_t jpeg_quality;
117117

@@ -434,6 +434,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options)
434434
camera->requestCompleted.connect(this, &CameraNode::requestComplete);
435435

436436
libcamera::ControlList controls_ = camera->controls();
437+
auto parameters = parameter_handler.get();
437438
for (const auto &[id, value] : parameters)
438439
controls_.set(id, value);
439440

@@ -556,17 +557,19 @@ CameraNode::process(libcamera::Request *const request)
556557
request->reuse(libcamera::Request::ReuseBuffers);
557558

558559
// update parameters
559-
parameters_lock.lock();
560+
// parameters_lock.lock();
561+
auto parameters = parameter_handler.get();
560562
if (!parameters.empty()) {
561563
RCLCPP_DEBUG_STREAM(get_logger(), request->toString() << " setting " << parameters.size() << " controls");
562564
for (const auto &[id, value] : parameters) {
563565
const std::string &name = libcamera::controls::controls.at(id)->name();
564566
RCLCPP_DEBUG_STREAM(get_logger(), "apply " << name << ": " << value.toString());
565567
request->controls().set(id, value);
566568
}
567-
parameters.clear();
569+
// parameters.clear();
570+
parameter_handler.clear();
568571
}
569-
parameters_lock.unlock();
572+
// parameters_lock.unlock();
570573

571574
camera->queueRequest(request);
572575
}

‎src/ParameterHandler.cpp

Lines changed: 133 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -168,7 +168,8 @@ ParameterHandler::ParameterHandler(rclcpp::Node *const node)
168168
return pr;
169169
});
170170

171-
param_cb_validate = node->add_on_set_parameters_callback(std::bind(&ParameterHandler::validate, this, std::placeholders::_1));
171+
param_cb_validate = node->add_on_set_parameters_callback(
172+
std::bind(&ParameterHandler::OnSetValidate, this, std::placeholders::_1));
172173

173174
// apply parameters
174175
// std::function<void (const std::vector<rclcpp::Parameter> &)>
@@ -180,6 +181,9 @@ ParameterHandler::ParameterHandler(rclcpp::Node *const node)
180181
}
181182
std::cout << "ParameterHandler post ##" << std::endl;
182183
});
184+
185+
param_cb_apply = node->add_post_set_parameters_callback(
186+
std::bind(&ParameterHandler::PostSetApply, this, std::placeholders::_1));
183187
}
184188

185189
void
@@ -346,7 +350,7 @@ ParameterHandler::declareFromControls(const libcamera::ControlInfoMap &controls)
346350

347351
node->set_parameters(parameters);
348352

349-
std::vector<rclcpp::Parameter> aa = node->get_parameters({"AeEnable", "ExposureTime"});
353+
// std::vector<rclcpp::Parameter> aa = node->get_parameters({"AeEnable", "ExposureTime"});
350354

351355
// resolve conflicts in default control values
352356
// TODO: let conflicts be resolved with callback?
@@ -366,10 +370,33 @@ ParameterHandler::declareFromControls(const libcamera::ControlInfoMap &controls)
366370
// node->set_parameters(parameters_init_list);
367371
}
368372

373+
ParameterHandler::ControlValueMap
374+
ParameterHandler::get()
375+
{
376+
// parameters_lock.lock();
377+
const std::lock_guard<std::mutex> lock(parameters_lock);
378+
return control_values;
379+
}
380+
381+
void
382+
ParameterHandler::clear()
383+
{
384+
parameters_lock.lock();
385+
control_values.clear();
386+
parameters_lock.unlock();
387+
}
388+
389+
void
390+
ParameterHandler::PreSetResolve(std::vector<rclcpp::Parameter> &parameters)
391+
{
392+
(void)parameters;
393+
// clamp?
394+
}
395+
369396
rcl_interfaces::msg::SetParametersResult
370-
ParameterHandler::validate(const std::vector<rclcpp::Parameter> &parameters)
397+
ParameterHandler::OnSetValidate(const std::vector<rclcpp::Parameter> &parameters)
371398
{
372-
std::cout << "validate..." << std::endl;
399+
std::cout << "OnSetValidate..." << std::endl;
373400

374401
// TODO: should just go over "controls"
375402
// const std::vector<std::string> parameter_names_old = node->list_parameters({}, {}).names;
@@ -403,7 +430,6 @@ ParameterHandler::validate(const std::vector<rclcpp::Parameter> &parameters)
403430
// const std::vector<rclcpp::Parameter> parameters_old = node->get_parameters(parameter_names_old);
404431

405432
{ // conflicts
406-
std::cout << "validate....." << std::endl;
407433
const std::vector<std::string> msgs = check(parameters_old, parameters);
408434
if (!msgs.empty()) {
409435
rcl_interfaces::msg::SetParametersResult result;
@@ -422,24 +448,7 @@ ParameterHandler::validate(const std::vector<rclcpp::Parameter> &parameters)
422448

423449
// TODO: check other mismatches from 'parameterCheckAndConvert'
424450

425-
rcl_interfaces::msg::SetParametersResult result;
426-
result.successful = true;
427-
return result;
428-
}
429-
430-
std::tuple<ParameterHandler::ControlValueMap, std::vector<std::string>>
431-
ParameterHandler::parameterCheckAndConvert(const std::vector<rclcpp::Parameter> &parameters)
432-
{
433-
// check target parameter state (current and new parameters)
434-
// for conflicting configuration
435-
const std::vector<std::string> msgs_conflicts = check_conflicts(parameters, parameters_full);
436-
if (!msgs_conflicts.empty()) {
437-
return {ControlValueMap {}, msgs_conflicts};
438-
}
439-
440-
ControlValueMap control_values;
441451
std::vector<std::string> msgs_valid_check;
442-
443452
for (const rclcpp::Parameter &parameter : parameters) {
444453
RCLCPP_DEBUG_STREAM(node->get_logger(),
445454
"setting " << parameter.get_type_name() << " parameter "
@@ -480,11 +489,108 @@ ParameterHandler::parameterCheckAndConvert(const std::vector<rclcpp::Parameter>
480489
continue;
481490
}
482491

483-
control_values[parameter_ids.at(parameter.get_name())->id()] = value;
484-
parameters_full[parameter.get_name()] = parameter.get_parameter_value();
485-
}
486-
}
492+
// TODO: value clamping into pre_set
493+
494+
// TODO: set control_values in post_set callback
495+
// control_values[parameter_ids.at(parameter.get_name())->id()] = value;
496+
// parameters_full[parameter.get_name()] = parameter.get_parameter_value();
497+
} // not None
498+
} // in parameter_ids
499+
} // parameters
500+
501+
// TODO: move to function
502+
rcl_interfaces::msg::SetParametersResult result;
503+
result.successful = msgs_valid_check.empty();
504+
for (size_t i = 0; i < msgs_valid_check.size(); i++) {
505+
if (msgs_valid_check.size() > 1)
506+
result.reason += "(" + std::to_string(i) + ") ";
507+
result.reason += msgs_valid_check[i];
508+
if (i < msgs_valid_check.size() - 1)
509+
result.reason += "; ";
487510
}
511+
return result;
512+
}
488513

489-
return {control_values, msgs_valid_check};
514+
void
515+
ParameterHandler::PostSetApply(const std::vector<rclcpp::Parameter> &parameters)
516+
{
517+
std::cout << "PostSetApply..." << std::endl;
518+
519+
parameters_lock.lock();
520+
for (const rclcpp::Parameter &parameter : parameters) {
521+
if (!parameter_ids.count(parameter.get_name()))
522+
continue;
523+
const libcamera::ControlId *id = parameter_ids.at(parameter.get_name());
524+
const libcamera::ControlValue value = pv_to_cv(parameter, id->type());
525+
control_values[parameter_ids.at(parameter.get_name())->id()] = value;
526+
}
527+
parameters_lock.unlock();
528+
529+
// std::tie(controls, msgs) = parameter_handler.parameterCheckAndConvert(parameters);
530+
531+
// parameters_lock.lock();
532+
// this->parameters = controls;
533+
// parameters_lock.unlock();
490534
}
535+
536+
// std::tuple<ParameterHandler::ControlValueMap, std::vector<std::string>>
537+
// ParameterHandler::parameterCheckAndConvert(const std::vector<rclcpp::Parameter> &parameters)
538+
// {
539+
// // check target parameter state (current and new parameters)
540+
// // for conflicting configuration
541+
// // const std::vector<std::string> msgs_conflicts = check_conflicts(parameters, parameters_full);
542+
// // if (!msgs_conflicts.empty()) {
543+
// // return {ControlValueMap {}, msgs_conflicts};
544+
// // }
545+
546+
// ControlValueMap control_values;
547+
// std::vector<std::string> msgs_valid_check;
548+
549+
// // for (const rclcpp::Parameter &parameter : parameters) {
550+
// // RCLCPP_DEBUG_STREAM(node->get_logger(),
551+
// // "setting " << parameter.get_type_name() << " parameter "
552+
// // << parameter.get_name() << " to "
553+
// // << parameter.value_to_string());
554+
555+
// // if (parameter_ids.count(parameter.get_name())) {
556+
// // const libcamera::ControlId *id = parameter_ids.at(parameter.get_name());
557+
// // const libcamera::ControlValue value = pv_to_cv(parameter, id->type());
558+
559+
// // if (!value.isNone()) {
560+
// // // verify parameter type and dimension against default
561+
// // const libcamera::ControlInfo &ci = parameter_info.at(parameter.get_name());
562+
563+
// // if (value.type() != id->type()) {
564+
// // msgs_valid_check.push_back(
565+
// // parameter.get_name() + ": parameter types mismatch, expected '" +
566+
// // std::to_string(id->type()) + "', got '" + std::to_string(value.type()) +
567+
// // "'");
568+
// // continue;
569+
// // }
570+
571+
// // const std::size_t extent = get_extent(id);
572+
// // if (value.isArray() &&
573+
// // (extent != libcamera::dynamic_extent) &&
574+
// // (value.numElements() != extent))
575+
// // {
576+
// // msgs_valid_check.push_back(
577+
// // parameter.get_name() + ": array dimensions mismatch, expected " +
578+
// // std::to_string(extent) + ", got " + std::to_string(value.numElements()));
579+
// // continue;
580+
// // }
581+
582+
// // // check bounds and return error
583+
// // if (value < ci.min() || value > ci.max()) {
584+
// // msgs_valid_check.push_back(
585+
// // "parameter value " + value.toString() + " outside of range: " + ci.toString());
586+
// // continue;
587+
// // }
588+
589+
// // control_values[parameter_ids.at(parameter.get_name())->id()] = value;
590+
// // parameters_full[parameter.get_name()] = parameter.get_parameter_value();
591+
// // } // not None
592+
// // } // in parameter_ids
593+
// // } // parameters
594+
595+
// return {control_values, msgs_valid_check};
596+
// }

‎src/ParameterHandler.hpp

Lines changed: 22 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#pragma once
22
#include "parameter_conflict_check.hpp"
33
#include <libcamera/controls.h>
4+
#include <mutex>
45
#include <rclcpp/node_interfaces/node_parameters_interface.hpp>
56
#include <string>
67
#include <tuple>
@@ -23,12 +24,20 @@ class ParameterHandler
2324
void
2425
declareFromControls(const libcamera::ControlInfoMap &controls);
2526

26-
std::tuple<ControlValueMap, std::vector<std::string>>
27-
parameterCheckAndConvert(const std::vector<rclcpp::Parameter> &parameters);
27+
ControlValueMap
28+
get();
29+
30+
void
31+
clear();
32+
33+
// std::tuple<ControlValueMap, std::vector<std::string>>
34+
// parameterCheckAndConvert(const std::vector<rclcpp::Parameter> &parameters);
2835

2936
private:
3037
rclcpp::Node *const node;
3138

39+
// NOTE: pre- and post-set handles not supported on humble
40+
// PreSetParametersCallbackHandle and PostSetParametersCallbackHandle
3241
rclcpp::node_interfaces::PreSetParametersCallbackHandle::SharedPtr param_cb_adjust;
3342
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_validate;
3443
rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr param_cb_apply;
@@ -38,8 +47,17 @@ class ParameterHandler
3847
std::unordered_map<std::string, const libcamera::ControlId *> parameter_ids;
3948
std::unordered_map<std::string, libcamera::ControlInfo> parameter_info;
4049
// keep track of set parameters
41-
ParameterMap parameters_full;
50+
// ParameterMap parameters_full;
51+
52+
ControlValueMap control_values;
53+
std::mutex parameters_lock;
54+
55+
void
56+
PreSetResolve(std::vector<rclcpp::Parameter> &parameters);
4257

4358
rcl_interfaces::msg::SetParametersResult
44-
validate(const std::vector<rclcpp::Parameter> &parameters);
59+
OnSetValidate(const std::vector<rclcpp::Parameter> &parameters);
60+
61+
void
62+
PostSetApply(const std::vector<rclcpp::Parameter> &parameters);
4563
};

0 commit comments

Comments
 (0)