diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 93434743..b491ff70 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -30,7 +30,6 @@ #include #include #include -// #include #include #include #include @@ -109,9 +108,6 @@ class CameraNode : public rclcpp::Node // map parameter names to libcamera control id std::unordered_map parameter_ids; - // parameters that are to be set for every request - // ParameterHandler::ControlValueMap parameters; - // std::mutex parameters_lock; // compression quality parameter std::atomic_uint8_t jpeg_quality; @@ -191,10 +187,6 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) param_cb_change(add_post_set_parameters_callback( std::bind(&CameraNode::onParameterChange, this, std::placeholders::_1))) { - // register callback to handle parameter updates - // callback_parameter_change = add_post_set_parameters_callback( - // std::bind(&CameraNode::onParameterChange, this, std::placeholders::_1)); - // pixel format rcl_interfaces::msg::ParameterDescriptor param_descr_format; param_descr_format.description = "pixel format of streaming buffer"; @@ -436,8 +428,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) + for (const auto &[id, value] : parameter_handler.get()) controls_.set(id, value); // start camera and queue all requests @@ -559,8 +550,7 @@ CameraNode::process(libcamera::Request *const request) request->reuse(libcamera::Request::ReuseBuffers); // update parameters - // parameters_lock.lock(); - auto parameters = parameter_handler.get(); + const ParameterHandler::ControlValueMap parameters = parameter_handler.get(); if (!parameters.empty()) { RCLCPP_DEBUG_STREAM(get_logger(), request->toString() << " setting " << parameters.size() << " controls"); for (const auto &[id, value] : parameters) { @@ -568,10 +558,8 @@ CameraNode::process(libcamera::Request *const request) RCLCPP_DEBUG_STREAM(get_logger(), "apply " << name << ": " << value.toString()); request->controls().set(id, value); } - // parameters.clear(); parameter_handler.clear(); } - // parameters_lock.unlock(); camera->queueRequest(request); } @@ -580,46 +568,16 @@ CameraNode::process(libcamera::Request *const request) rcl_interfaces::msg::SetParametersResult CameraNode::onParameterChange(const std::vector ¶meters) { - for (const rclcpp::Parameter ¶m : parameters) { - std::cout << "CameraNode post: " << param.get_name() << std::endl; - } - // check non-controls parameters for (const rclcpp::Parameter ¶meter : parameters) { - if (!parameter.get_name().compare("jpeg_quality")) { + if (parameter.get_name() == "jpeg_quality") { jpeg_quality = parameter.get_parameter_value().get(); } } - // DBG: - // TODO: this should go into the 'add_post_set_parameters_callback' rcl_interfaces::msg::SetParametersResult result; result.successful = true; return result; - - // ParameterHandler::ControlValueMap controls; - // std::vector msgs; - - // std::tie(controls, msgs) = parameter_handler.parameterCheckAndConvert(parameters); - - // parameters_lock.lock(); - // this->parameters = controls; - // parameters_lock.unlock(); - - // rcl_interfaces::msg::SetParametersResult result; - // result.successful = msgs.empty(); - // if (!msgs.empty()) { - // result.successful = false; - // 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; } } // namespace camera