diff --git a/CMakeLists.txt b/CMakeLists.txt index d2ad0d10..aaebef0b 100644 --- a/CMakeLists.txt +++ b/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 - $ - $) -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 diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index c360d509..4a01e523 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -19,7 +19,6 @@ #include #endif #include -#include #include #include #include @@ -62,11 +61,10 @@ #include #include #include -#include #include #include #include -namespace enc = sensor_msgs::image_encodings; + namespace rclcpp { class NodeOptions; @@ -89,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 @@ -116,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; @@ -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); @@ -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 &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 +449,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(); @@ -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 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); 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) {