From e9c43b4a0470fb7d194f81a5f80f6fb84c97642c Mon Sep 17 00:00:00 2001 From: MasatoSaeki Date: Mon, 10 Feb 2025 17:47:16 +0900 Subject: [PATCH 1/3] add mutex Signed-off-by: MasatoSaeki --- .../src/traffic_light_selector_node.cpp | 8 ++++++-- .../src/traffic_light_selector_node.hpp | 9 ++++++--- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index 80e2450a287f7..6282e3ba03724 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -33,7 +33,8 @@ TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & n detected_rois_sub_(this, "input/detected_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), rough_rois_sub_(this, "input/rough_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), expected_rois_sub_(this, "input/expect_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), - sync_(SyncPolicy(10), detected_rois_sub_, rough_rois_sub_, expected_rois_sub_) + sync_(SyncPolicy(10), detected_rois_sub_, rough_rois_sub_, expected_rois_sub_), + camera_info_subscribed_(false) { { stop_watch_ptr_ = @@ -61,13 +62,14 @@ TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & n void TrafficLightSelectorNode::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_msg) { + std::lock_guard lock(mutex_); if (camera_info_subscribed_) { return; } - RCLCPP_INFO(get_logger(), "camera_info received"); image_width_ = input_msg->width; image_height_ = input_msg->height; camera_info_subscribed_ = true; + RCLCPP_INFO(get_logger(), "camera_info received"); } void TrafficLightSelectorNode::objectsCallback( @@ -76,9 +78,11 @@ void TrafficLightSelectorNode::objectsCallback( const TrafficLightRoiArray::ConstSharedPtr & expected_rois_msg) { stop_watch_ptr_->toc("processing_time", true); + mutex_.lock(); if (!camera_info_subscribed_) { return; } + mutex_.unlock(); std::map rough_rois_map; for (const auto & roi : rough_rois_msg->rois) { rough_rois_map[roi.traffic_light_id] = roi.roi; diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp index 9dab987b7523d..a4e3f2f32419a 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp @@ -35,6 +35,7 @@ #include #include +#include namespace autoware::traffic_light { @@ -73,9 +74,11 @@ class TrafficLightSelectorNode : public rclcpp::Node // Subscribe camera_info to get width and height of image rclcpp::Subscription::SharedPtr camera_info_sub_; bool camera_info_subscribed_; - uint32_t image_width_{1280}; - uint32_t image_height_{960}; - double max_iou_threshold_{0.0}; + uint32_t image_width_; + uint32_t image_height_; + double max_iou_threshold_; + + std::mutex mutex_; std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; From bff9d4029a26c709fee2dcd80f774223d35360c9 Mon Sep 17 00:00:00 2001 From: MasatoSaeki Date: Tue, 11 Feb 2025 13:12:07 +0900 Subject: [PATCH 2/3] change message filter Signed-off-by: MasatoSaeki --- .../src/traffic_light_selector_node.cpp | 41 ++++++------------- .../src/traffic_light_selector_node.hpp | 16 +++----- 2 files changed, 17 insertions(+), 40 deletions(-) diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index 6282e3ba03724..679f1757ce753 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -33,8 +33,8 @@ TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & n detected_rois_sub_(this, "input/detected_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), rough_rois_sub_(this, "input/rough_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), expected_rois_sub_(this, "input/expect_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), - sync_(SyncPolicy(10), detected_rois_sub_, rough_rois_sub_, expected_rois_sub_), - camera_info_subscribed_(false) + camera_info_sub_(this, "input/camera_info", rclcpp::SensorDataQoS().get_rmw_qos_profile()), + sync_(SyncPolicy(10), detected_rois_sub_, rough_rois_sub_, expected_rois_sub_, camera_info_sub_) { { stop_watch_ptr_ = @@ -49,40 +49,23 @@ TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & n using std::placeholders::_1; using std::placeholders::_2; using std::placeholders::_3; - sync_.registerCallback(std::bind(&TrafficLightSelectorNode::objectsCallback, this, _1, _2, _3)); + using std::placeholders::_4; + sync_.registerCallback(std::bind(&TrafficLightSelectorNode::objectsCallback, this, _1, _2, _3, _4)); - camera_info_sub_ = create_subscription( - "input/camera_info", rclcpp::SensorDataQoS(), - std::bind(&TrafficLightSelectorNode::cameraInfoCallback, this, _1)); // Publisher pub_traffic_light_rois_ = create_publisher("output/traffic_rois", rclcpp::QoS{1}); } -void TrafficLightSelectorNode::cameraInfoCallback( - const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_msg) -{ - std::lock_guard lock(mutex_); - if (camera_info_subscribed_) { - return; - } - image_width_ = input_msg->width; - image_height_ = input_msg->height; - camera_info_subscribed_ = true; - RCLCPP_INFO(get_logger(), "camera_info received"); -} - void TrafficLightSelectorNode::objectsCallback( const DetectedObjectsWithFeature::ConstSharedPtr & detected_traffic_light_msg, const TrafficLightRoiArray::ConstSharedPtr & rough_rois_msg, - const TrafficLightRoiArray::ConstSharedPtr & expected_rois_msg) + const TrafficLightRoiArray::ConstSharedPtr & expected_rois_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_msg) { stop_watch_ptr_->toc("processing_time", true); - mutex_.lock(); - if (!camera_info_subscribed_) { - return; - } - mutex_.unlock(); + uint32_t image_width = camera_info_msg->width; + uint32_t image_height = camera_info_msg->height; std::map rough_rois_map; for (const auto & roi : rough_rois_msg->rois) { rough_rois_map[roi.traffic_light_id] = roi.roi; @@ -103,9 +86,9 @@ void TrafficLightSelectorNode::objectsCallback( expect_rois.push_back(expected_roi.roi); } cv::Mat expect_roi_img = - utils::createBinaryImageFromRois(expect_rois, cv::Size(image_width_, image_height_)); + utils::createBinaryImageFromRois(expect_rois, cv::Size(image_width, image_height)); cv::Mat det_roi_img = - utils::createBinaryImageFromRois(det_rois, cv::Size(image_width_, image_height_)); + utils::createBinaryImageFromRois(det_rois, cv::Size(image_width, image_height)); // for (const auto expect_roi : expect_rois) { for (const auto & expect_traffic_roi : expected_rois_msg->rois) { const auto & expect_roi = expect_traffic_roi.roi; @@ -139,10 +122,10 @@ void TrafficLightSelectorNode::objectsCallback( // fit top lef corner of detected roi to inside of image detected_roi_shifted.x_offset = std::clamp( static_cast(detected_roi.feature.roi.x_offset) - det_roi_shift_x, 0, - static_cast(image_width_ - detected_roi.feature.roi.width)); + static_cast(image_width - detected_roi.feature.roi.width)); detected_roi_shifted.y_offset = std::clamp( static_cast(detected_roi.feature.roi.y_offset) - det_roi_shift_y, 0, - static_cast(image_height_ - detected_roi.feature.roi.height)); + static_cast(image_height - detected_roi.feature.roi.height)); double iou = utils::getGenIoU(expect_roi.roi, detected_roi_shifted); if (iou > max_iou) { diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp index a4e3f2f32419a..f33e585dfa3a3 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp @@ -35,7 +35,6 @@ #include #include -#include namespace autoware::traffic_light { @@ -58,27 +57,22 @@ class TrafficLightSelectorNode : public rclcpp::Node message_filters::Subscriber detected_rois_sub_; message_filters::Subscriber rough_rois_sub_; message_filters::Subscriber expected_rois_sub_; + message_filters::Subscriber camera_info_sub_; typedef message_filters::sync_policies::ApproximateTime< - DetectedObjectsWithFeature, TrafficLightRoiArray, TrafficLightRoiArray> + DetectedObjectsWithFeature, TrafficLightRoiArray, TrafficLightRoiArray, sensor_msgs::msg::CameraInfo> SyncPolicy; typedef message_filters::Synchronizer Sync; Sync sync_; void objectsCallback( const DetectedObjectsWithFeature::ConstSharedPtr & detected_rois_msg, const TrafficLightRoiArray::ConstSharedPtr & rough_rois_msg, - const TrafficLightRoiArray::ConstSharedPtr & expect_rois_msg); + const TrafficLightRoiArray::ConstSharedPtr & expect_rois_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_msg); - void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_msg); // Publisher rclcpp::Publisher::SharedPtr pub_traffic_light_rois_; - // Subscribe camera_info to get width and height of image - rclcpp::Subscription::SharedPtr camera_info_sub_; - bool camera_info_subscribed_; - uint32_t image_width_; - uint32_t image_height_; - double max_iou_threshold_; - std::mutex mutex_; + double max_iou_threshold_; std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; From 7a9596a72d1392390d1340db2327ec6548e408e8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 11 Feb 2025 04:15:04 +0000 Subject: [PATCH 3/3] style(pre-commit): autofix --- .../src/traffic_light_selector_node.cpp | 3 ++- .../src/traffic_light_selector_node.hpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index 679f1757ce753..240d2439cb29e 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -50,7 +50,8 @@ TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & n using std::placeholders::_2; using std::placeholders::_3; using std::placeholders::_4; - sync_.registerCallback(std::bind(&TrafficLightSelectorNode::objectsCallback, this, _1, _2, _3, _4)); + sync_.registerCallback( + std::bind(&TrafficLightSelectorNode::objectsCallback, this, _1, _2, _3, _4)); // Publisher pub_traffic_light_rois_ = diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp index f33e585dfa3a3..29357b32a0b45 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp @@ -59,7 +59,8 @@ class TrafficLightSelectorNode : public rclcpp::Node message_filters::Subscriber expected_rois_sub_; message_filters::Subscriber camera_info_sub_; typedef message_filters::sync_policies::ApproximateTime< - DetectedObjectsWithFeature, TrafficLightRoiArray, TrafficLightRoiArray, sensor_msgs::msg::CameraInfo> + DetectedObjectsWithFeature, TrafficLightRoiArray, TrafficLightRoiArray, + sensor_msgs::msg::CameraInfo> SyncPolicy; typedef message_filters::Synchronizer Sync; Sync sync_;