diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e3ca4a..d2ad0d1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.18) project(camera_ros) set(CMAKE_CXX_STANDARD 17) diff --git a/package.xml b/package.xml index 34a3fc8..84e5a25 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ camera_ros - 0.1.0 + 0.2.0 node for libcamera supported cameras (V4L2, Raspberry Pi Camera Modules) Christian Rauch MIT diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 1bfe1fb..c360d50 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -26,7 +26,7 @@ #include #include #include -#include +#include #include #include #include @@ -364,10 +364,6 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti RCLCPP_INFO_STREAM(get_logger(), "camera \"" << camera->id() << "\" configured with " << scfg.toString() << " stream"); - set_parameter(rclcpp::Parameter("width", int64_t(scfg.size.width))); - set_parameter(rclcpp::Parameter("height", int64_t(scfg.size.height))); - set_parameter(rclcpp::Parameter("format", scfg.pixelFormat.toString())); - // format camera name for calibration file const libcamera::ControlList &props = camera->properties(); std::string cname = camera->id() + '_' + scfg.size.toString(); @@ -426,10 +422,10 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti } // create a processing thread per request + running = true; for (const std::unique_ptr &request : requests) { request_locks[request.get()] = std::make_unique(); request_locks[request.get()]->lock(); - running = true; request_threads.emplace_back(&CameraNode::process, this, request.get()); } @@ -583,7 +579,10 @@ CameraNode::declareParameters() std::vector parameters_init_list; for (const auto &[name, value] : parameters_init) parameters_init_list.emplace_back(name, value); - set_parameters(parameters_init_list); + const rcl_interfaces::msg::SetParametersResult param_set_result = + set_parameters_atomically(parameters_init_list); + if (!param_set_result.successful) + RCLCPP_ERROR_STREAM(get_logger(), "Cannot declare parameters with default value: " << param_set_result.reason); } void @@ -678,9 +677,15 @@ CameraNode::process(libcamera::Request *const request) // update parameters parameters_lock.lock(); - for (const auto &[id, value] : parameters) - request->controls().set(id, value); - parameters.clear(); + if (!parameters.empty()) { + RCLCPP_DEBUG_STREAM(get_logger(), request->toString() << " setting " << parameters.size() << " controls"); + for (const auto &[id, value] : parameters) { + const std::string &name = libcamera::controls::controls.at(id)->name(); + RCLCPP_DEBUG_STREAM(get_logger(), "apply " << name << ": " << value.toString()); + request->controls().set(id, value); + } + parameters.clear(); + } parameters_lock.unlock(); camera->queueRequest(request); diff --git a/src/parameter_conflict_check.cpp b/src/parameter_conflict_check.cpp index 9f4f548..bab4e9e 100644 --- a/src/parameter_conflict_check.cpp +++ b/src/parameter_conflict_check.cpp @@ -21,8 +21,11 @@ resolve_conflicts(const ParameterMap ¶meters_default, const ParameterMap &pa } // apply parameter overrides - for (const auto &[name, value] : parameters_overrides) - parameters_init[name] = value; + for (const auto &[name, value] : parameters_overrides) { + // only override parameters that have matching controls + if (parameters_default.count(name)) + parameters_init[name] = value; + } // overrides: prefer provided exposure if (parameters_init.count("AeEnable") && parameters_init.at("AeEnable").get() &&