From db8d20958c99be039e75bf1f5ca40405d64749a8 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 19 Nov 2024 22:11:38 +0100 Subject: [PATCH 1/9] move namespace shortcut --- src/CameraNode.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index c360d509..0c5771b6 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -66,7 +66,7 @@ #include #include #include -namespace enc = sensor_msgs::image_encodings; + namespace rclcpp { class NodeOptions; @@ -163,6 +163,8 @@ compressImageMsg(const sensor_msgs::msg::Image &source, sensor_msgs::msg::CompressedImage &destination, const std::vector ¶ms = std::vector()) { + namespace enc = sensor_msgs::image_encodings; + std::shared_ptr tracked_object; cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(source, tracked_object); From 8463ee303ec4c5b43bf0b957e6a73fa420870d73 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Sun, 24 Nov 2024 19:39:18 +0100 Subject: [PATCH 2/9] remove unused header --- src/CameraNode.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 0c5771b6..870d3be5 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -19,7 +19,6 @@ #include #endif #include -#include #include #include #include @@ -62,7 +61,6 @@ #include #include #include -#include #include #include #include From ad96edc08245721571b82cefb37ba387303598cc Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 26 Nov 2024 06:42:17 +0100 Subject: [PATCH 3/9] add condition_variable to synchronise producer and consumer thread --- src/CameraNode.cpp | 29 +++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 870d3be5..2a9fc6e3 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -87,7 +87,8 @@ class CameraNode : public rclcpp::Node std::shared_ptr allocator; std::vector> requests; std::vector request_threads; - std::unordered_map> request_locks; + std::unordered_map request_mutexes; + std::unordered_map request_condvars; std::atomic running; struct buffer_info_t @@ -424,8 +425,9 @@ 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(); + // create mutexes in-place + request_mutexes[request.get()]; + request_condvars[request.get()]; request_threads.emplace_back(&CameraNode::process, this, request.get()); } @@ -446,8 +448,18 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti CameraNode::~CameraNode() { + // stop request callbacks + for (std::unique_ptr &request : requests) + camera->requestCompleted.disconnect(request.get()); + // stop request processing threads running = false; + + // unlock all threads + for (auto &[req, condvar] : request_condvars) + condvar.notify_all(); + + // wait for all currently running threads to finish for (std::thread &thread : request_threads) thread.join(); @@ -588,15 +600,20 @@ CameraNode::declareParameters() void CameraNode::requestComplete(libcamera::Request *const request) { - request_locks[request]->unlock(); + std::unique_lock lk(request_mutexes.at(request)); + request_condvars.at(request).notify_one(); } void CameraNode::process(libcamera::Request *const request) { - while (running) { + while (true) { // block until request is available - request_locks[request]->lock(); + std::unique_lock lk(request_mutexes.at(request)); + request_condvars.at(request).wait(lk); + + if (!running) + return; if (request->status() == libcamera::Request::RequestComplete) { assert(request->buffers().size() == 1); From 31068d0bbb219ba359c82d36246ce8b3a3ea8300 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Thu, 28 Nov 2024 00:25:14 +0100 Subject: [PATCH 4/9] use unique_lock to detect potential deadlock --- src/CameraNode.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 2a9fc6e3..57396a20 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -115,7 +115,8 @@ class CameraNode : public rclcpp::Node std::unordered_map parameters; // keep track of set parameters ParameterMap parameters_full; - std::mutex parameters_lock; + std::mutex parameters_mutex; + std::unique_lock parameters_lock {parameters_mutex, std::defer_lock}; // compression quality parameter std::atomic_uint8_t jpeg_quality; From 6b020fc574e64a94a8ba143cc728795f238fe444 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Fri, 29 Nov 2024 21:03:58 +0100 Subject: [PATCH 5/9] remove non-existing include paths --- CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d2ad0d10..6e44670e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,9 +43,6 @@ set_property(TARGET utils PROPERTY POSITION_INDEPENDENT_CODE ON) add_library(camera_component SHARED src/CameraNode.cpp) rclcpp_components_register_node(camera_component PLUGIN "camera::CameraNode" EXECUTABLE "camera_node") -target_include_directories(camera_component PUBLIC - $ - $) ament_target_dependencies( camera_component "rclcpp" From e2c7a63993a0514bbb386e0eee332b9073ce223e Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Fri, 29 Nov 2024 21:16:38 +0100 Subject: [PATCH 6/9] export 'camera_component' --- CMakeLists.txt | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6e44670e..aaebef0b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,20 +42,23 @@ set_property(TARGET utils PROPERTY POSITION_INDEPENDENT_CODE ON) # composable ROS2 node add_library(camera_component SHARED src/CameraNode.cpp) rclcpp_components_register_node(camera_component PLUGIN "camera::CameraNode" EXECUTABLE "camera_node") - -ament_target_dependencies( - camera_component +set(PACKAGE_DEPENDENCIES "rclcpp" "rclcpp_components" "sensor_msgs" "camera_info_manager" "cv_bridge" ) - +ament_target_dependencies(camera_component PUBLIC ${PACKAGE_DEPENDENCIES}) target_include_directories(camera_component PUBLIC ${libcamera_INCLUDE_DIRS}) -target_link_libraries(camera_component ${libcamera_LINK_LIBRARIES} utils) +target_link_libraries(camera_component PUBLIC ${libcamera_LINK_LIBRARIES}) +target_link_libraries(camera_component PRIVATE utils) + +ament_export_targets(camera_componentTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${PACKAGE_DEPENDENCIES}) install(TARGETS camera_component + EXPORT camera_componentTargets DESTINATION lib) install(DIRECTORY From d5dcdcbd4051cfaeabebd9360af48e2ca2d2f79e Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Thu, 5 Dec 2024 22:32:37 +0100 Subject: [PATCH 7/9] always add unset parameters to initial list to know supported controls --- src/CameraNode.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 57396a20..b80b2ed1 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -571,8 +571,8 @@ CameraNode::declareParameters() } else { declare_parameter(id->name(), value, param_descr); - parameters_init[id->name()] = value; } + parameters_init[id->name()] = value; } // register callback to handle parameter changes From d15ba5591dd0bcf0859b3b521b3662b8c61dfbaf Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Sat, 7 Dec 2024 13:27:47 +0100 Subject: [PATCH 8/9] check that 'AeEnable' is set --- src/parameter_conflict_check.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/parameter_conflict_check.cpp b/src/parameter_conflict_check.cpp index bab4e9ec..55114fe8 100644 --- a/src/parameter_conflict_check.cpp +++ b/src/parameter_conflict_check.cpp @@ -13,7 +13,9 @@ resolve_conflicts(const ParameterMap ¶meters_default, const ParameterMap &pa // must not be enabled at the same time // default: prefer auto exposure - if (parameters_init.count("AeEnable") && parameters_init.at("AeEnable").get() && + if (parameters_init.count("AeEnable") && + (parameters_init.at("AeEnable").get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) && + parameters_init.at("AeEnable").get() && parameters_init.count("ExposureTime")) { // disable exposure @@ -28,7 +30,9 @@ resolve_conflicts(const ParameterMap ¶meters_default, const ParameterMap &pa } // overrides: prefer provided exposure - if (parameters_init.count("AeEnable") && parameters_init.at("AeEnable").get() && + if (parameters_init.count("AeEnable") && + (parameters_init.at("AeEnable").get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) && + parameters_init.at("AeEnable").get() && parameters_init.count("ExposureTime")) { // disable auto exposure @@ -56,7 +60,9 @@ check_conflicts(const std::vector ¶meters_new, // is auto exposure going to be enabled? const bool ae_enabled = - parameter_map.count("AeEnable") && parameter_map.at("AeEnable").get(); + parameter_map.count("AeEnable") && + (parameter_map.at("AeEnable").get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) && + parameter_map.at("AeEnable").get(); // are new parameters setting the exposure manually? const bool exposure_updated = std::find_if(parameters_new.begin(), parameters_new.end(), [](const rclcpp::Parameter ¶m) { From e43581a77f081ef0322d77eae15f0953861daead Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Sat, 7 Dec 2024 23:23:15 +0100 Subject: [PATCH 9/9] do not set parameters without value --- src/CameraNode.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index b80b2ed1..4a01e523 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -590,8 +590,10 @@ CameraNode::declareParameters() RCLCPP_WARN_STREAM(get_logger(), s); std::vector parameters_init_list; - for (const auto &[name, value] : parameters_init) - parameters_init_list.emplace_back(name, value); + for (const auto &[name, value] : parameters_init) { + if (value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) + parameters_init_list.emplace_back(name, value); + } const rcl_interfaces::msg::SetParametersResult param_set_result = set_parameters_atomically(parameters_init_list); if (!param_set_result.successful)