Merge pull request #70 from christianrauch/thread_fixes

thread and control fixes

CMakeLists.txt

@@ -42,23 +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")
-
-target_include_directories(camera_component PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-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

src/CameraNode.cpp

@@ -19,7 +19,6 @@
 #include <cv_bridge/cv_bridge.h>
 #endif
 #include <atomic>
-#include <functional>
 #include <iostream>
 #include <libcamera/base/shared_fd.h>
 #include <libcamera/base/signal.h>
@@ -62,11 +61,10 @@
 #include <string>
 #include <sys/mman.h>
 #include <thread>
-#include <tuple>
 #include <unordered_map>
 #include <utility>
 #include <vector>
-namespace enc = sensor_msgs::image_encodings;
+
 namespace rclcpp
 {
 class NodeOptions;
@@ -89,7 +87,8 @@ class CameraNode : public rclcpp::Node
   std::shared_ptr<libcamera::FrameBufferAllocator> allocator;
   std::vector<std::unique_ptr<libcamera::Request>> requests;
   std::vector<std::thread> request_threads;
-  std::unordered_map<libcamera::Request *, std::unique_ptr<std::mutex>> request_locks;
+  std::unordered_map<const libcamera::Request *, std::mutex> request_mutexes;
+  std::unordered_map<const libcamera::Request *, std::condition_variable> request_condvars;
   std::atomic<bool> running;
 
   struct buffer_info_t
@@ -116,7 +115,8 @@ class CameraNode : public rclcpp::Node
   std::unordered_map<unsigned int, libcamera::ControlValue> parameters;
   // keep track of set parameters
   ParameterMap parameters_full;
-  std::mutex parameters_lock;
+  std::mutex parameters_mutex;
+  std::unique_lock<std::mutex> parameters_lock {parameters_mutex, std::defer_lock};
   // compression quality parameter
   std::atomic_uint8_t jpeg_quality;
 
@@ -163,6 +163,8 @@ compressImageMsg(const sensor_msgs::msg::Image &source,
                  sensor_msgs::msg::CompressedImage &destination,
                  const std::vector<int> &params = std::vector<int>())
 {
+  namespace enc = sensor_msgs::image_encodings;
+
   std::shared_ptr<CameraNode> tracked_object;
   cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(source, tracked_object);
 
@@ -424,8 +426,9 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti
   // create a processing thread per request
   running = true;
   for (const std::unique_ptr<libcamera::Request> &request : requests) {
-    request_locks[request.get()] = std::make_unique<std::mutex>();
-    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 +449,18 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti
 
 CameraNode::~CameraNode()
 {
+  // stop request callbacks
+  for (std::unique_ptr<libcamera::Request> &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();
 
@@ -558,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
@@ -577,8 +590,10 @@ CameraNode::declareParameters()
     RCLCPP_WARN_STREAM(get_logger(), s);
 
   std::vector<rclcpp::Parameter> 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)
@@ -588,15 +603,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);

src/parameter_conflict_check.cpp

@@ -13,7 +13,9 @@ resolve_conflicts(const ParameterMap &parameters_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<bool>() &&
+  if (parameters_init.count("AeEnable") &&
+      (parameters_init.at("AeEnable").get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) &&
+      parameters_init.at("AeEnable").get<bool>() &&
       parameters_init.count("ExposureTime"))
   {
     // disable exposure
@@ -28,7 +30,9 @@ resolve_conflicts(const ParameterMap &parameters_default, const ParameterMap &pa
   }
 
   // overrides: prefer provided exposure
-  if (parameters_init.count("AeEnable") && parameters_init.at("AeEnable").get<bool>() &&
+  if (parameters_init.count("AeEnable") &&
+      (parameters_init.at("AeEnable").get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) &&
+      parameters_init.at("AeEnable").get<bool>() &&
       parameters_init.count("ExposureTime"))
   {
     // disable auto exposure
@@ -56,7 +60,9 @@ check_conflicts(const std::vector<rclcpp::Parameter> &parameters_new,
 
   // is auto exposure going to be enabled?
   const bool ae_enabled =
-    parameter_map.count("AeEnable") && parameter_map.at("AeEnable").get<bool>();
+    parameter_map.count("AeEnable") &&
+    (parameter_map.at("AeEnable").get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) &&
+    parameter_map.at("AeEnable").get<bool>();
   // are new parameters setting the exposure manually?
   const bool exposure_updated =
     std::find_if(parameters_new.begin(), parameters_new.end(), [](const rclcpp::Parameter &param) {