Skip to content

Commit

Permalink
[WIP] get/set etc
Browse files Browse the repository at this point in the history
  • Loading branch information
@christianrauch
christianrauch committed Oct 29, 2024
1 parent 5c99e93 commit bd65b3c
Show file tree
Hide file tree
Showing 3 changed files with 167 additions and 38 deletions.
19 changes: 12 additions & 7 deletions src/CameraNode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
#include <libcamera/request.h>
#include <libcamera/stream.h>
#include <memory>
#include <mutex>
// #include <mutex>
#include <opencv2/core/mat.hpp>
#include <opencv2/imgcodecs.hpp>
#include <optional>
Expand Down Expand Up @@ -110,8 +110,8 @@ class CameraNode : public rclcpp::Node
// 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
ParameterHandler::ControlValueMap parameters;
std::mutex parameters_lock;
// ParameterHandler::ControlValueMap parameters;
// std::mutex parameters_lock;
// compression quality parameter
std::atomic_uint8_t jpeg_quality;

Expand Down Expand Up @@ -438,6 +438,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options)
camera->requestCompleted.connect(this, &CameraNode::requestComplete);

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

Expand Down Expand Up @@ -560,11 +561,15 @@ CameraNode::process(libcamera::Request *const request)
request->reuse(libcamera::Request::ReuseBuffers);

// update parameters
parameters_lock.lock();
for (const auto &[id, value] : parameters)
// parameters_lock.lock();
auto parameters = parameter_handler.get();
for (const auto &[id, value] : parameters) {
std::cout << "set " << id << " to " << value.toString() << std::endl;
request->controls().set(id, value);
parameters.clear();
parameters_lock.unlock();
}
// parameters.clear();
parameter_handler.clear();
// parameters_lock.unlock();

camera->queueRequest(request);
}
Expand Down
160 changes: 133 additions & 27 deletions src/ParameterHandler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -168,7 +168,8 @@ ParameterHandler::ParameterHandler(rclcpp::Node *const node)
return pr;
});

param_cb_validate = node->add_on_set_parameters_callback(std::bind(&ParameterHandler::validate, this, std::placeholders::_1));
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> &)>
Expand All @@ -180,6 +181,9 @@ ParameterHandler::ParameterHandler(rclcpp::Node *const node)
}
std::cout << "ParameterHandler post ##" << std::endl;
});

param_cb_apply = node->add_post_set_parameters_callback(
std::bind(&ParameterHandler::PostSetApply, this, std::placeholders::_1));
}

void
Expand Down Expand Up @@ -346,7 +350,7 @@ ParameterHandler::declareFromControls(const libcamera::ControlInfoMap &controls)

node->set_parameters(parameters);

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

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

ParameterHandler::ControlValueMap
ParameterHandler::get()
{
// parameters_lock.lock();
const std::lock_guard<std::mutex> lock(parameters_lock);
return control_values;
}

void
ParameterHandler::clear()
{
parameters_lock.lock();
control_values.clear();
parameters_lock.unlock();
}

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

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

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

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

// TODO: check other mismatches from 'parameterCheckAndConvert'

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

std::tuple<ParameterHandler::ControlValueMap, std::vector<std::string>>
ParameterHandler::parameterCheckAndConvert(const std::vector<rclcpp::Parameter> &parameters)
{
// check target parameter state (current and new parameters)
// for conflicting configuration
const std::vector<std::string> msgs_conflicts = check_conflicts(parameters, parameters_full);
if (!msgs_conflicts.empty()) {
return {ControlValueMap {}, msgs_conflicts};
}

ControlValueMap control_values;
std::vector<std::string> msgs_valid_check;

for (const rclcpp::Parameter &parameter : parameters) {
RCLCPP_DEBUG_STREAM(node->get_logger(),
"setting " << parameter.get_type_name() << " parameter "
Expand Down Expand Up @@ -480,11 +489,108 @@ ParameterHandler::parameterCheckAndConvert(const std::vector<rclcpp::Parameter>
continue;
}

control_values[parameter_ids.at(parameter.get_name())->id()] = value;
parameters_full[parameter.get_name()] = parameter.get_parameter_value();
}
}
// TODO: value clamping into pre_set

// TODO: set control_values in post_set callback
// control_values[parameter_ids.at(parameter.get_name())->id()] = value;
// parameters_full[parameter.get_name()] = parameter.get_parameter_value();
} // not None
} // 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 {control_values, msgs_valid_check};
void
ParameterHandler::PostSetApply(const std::vector<rclcpp::Parameter> &parameters)
{
std::cout << "PostSetApply..." << std::endl;

parameters_lock.lock();
for (const rclcpp::Parameter &parameter : parameters) {
if (!parameter_ids.count(parameter.get_name()))
continue;
const libcamera::ControlId *id = parameter_ids.at(parameter.get_name());
const libcamera::ControlValue value = pv_to_cv(parameter, id->type());
control_values[parameter_ids.at(parameter.get_name())->id()] = value;
}
parameters_lock.unlock();

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

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

// std::tuple<ParameterHandler::ControlValueMap, std::vector<std::string>>
// ParameterHandler::parameterCheckAndConvert(const std::vector<rclcpp::Parameter> &parameters)
// {
// // check target parameter state (current and new parameters)
// // for conflicting configuration
// // const std::vector<std::string> msgs_conflicts = check_conflicts(parameters, parameters_full);
// // if (!msgs_conflicts.empty()) {
// // return {ControlValueMap {}, msgs_conflicts};
// // }

// ControlValueMap control_values;
// std::vector<std::string> msgs_valid_check;

// // for (const rclcpp::Parameter &parameter : parameters) {
// // RCLCPP_DEBUG_STREAM(node->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());
// // const libcamera::ControlValue value = pv_to_cv(parameter, id->type());

// // if (!value.isNone()) {
// // // verify parameter type and dimension against default
// // const libcamera::ControlInfo &ci = parameter_info.at(parameter.get_name());

// // if (value.type() != id->type()) {
// // msgs_valid_check.push_back(
// // parameter.get_name() + ": parameter types mismatch, expected '" +
// // std::to_string(id->type()) + "', got '" + std::to_string(value.type()) +
// // "'");
// // continue;
// // }

// // const std::size_t extent = get_extent(id);
// // if (value.isArray() &&
// // (extent != libcamera::dynamic_extent) &&
// // (value.numElements() != extent))
// // {
// // msgs_valid_check.push_back(
// // parameter.get_name() + ": array dimensions mismatch, expected " +
// // std::to_string(extent) + ", got " + std::to_string(value.numElements()));
// // continue;
// // }

// // // check bounds and return error
// // if (value < ci.min() || value > ci.max()) {
// // msgs_valid_check.push_back(
// // "parameter value " + value.toString() + " outside of range: " + ci.toString());
// // continue;
// // }

// // control_values[parameter_ids.at(parameter.get_name())->id()] = value;
// // parameters_full[parameter.get_name()] = parameter.get_parameter_value();
// // } // not None
// // } // in parameter_ids
// // } // parameters

// return {control_values, msgs_valid_check};
// }
26 changes: 22 additions & 4 deletions src/ParameterHandler.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#pragma once
#include "parameter_conflict_check.hpp"
#include <libcamera/controls.h>
#include <mutex>
#include <rclcpp/node_interfaces/node_parameters_interface.hpp>
#include <string>
#include <tuple>
Expand All @@ -23,12 +24,20 @@ class ParameterHandler
void
declareFromControls(const libcamera::ControlInfoMap &controls);

std::tuple<ControlValueMap, std::vector<std::string>>
parameterCheckAndConvert(const std::vector<rclcpp::Parameter> &parameters);
ControlValueMap
get();

void
clear();

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

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;
Expand All @@ -38,8 +47,17 @@ class ParameterHandler
std::unordered_map<std::string, const libcamera::ControlId *> parameter_ids;
std::unordered_map<std::string, libcamera::ControlInfo> parameter_info;
// keep track of set parameters
ParameterMap parameters_full;
// ParameterMap parameters_full;

ControlValueMap control_values;
std::mutex parameters_lock;

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

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

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

0 comments on commit bd65b3c

Please sign in to comment.