add mutex

Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp>

Author: MasatoSaeki

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<std::mutex> 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<int, sensor_msgs::msg::RegionOfInterest> rough_rois_map;
   for (const auto & roi : rough_rois_msg->rois) {
     rough_rois_map[roi.traffic_light_id] = roi.roi;

perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp

@@ -35,6 +35,7 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <memory>
+#include <mutex>
 
 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<sensor_msgs::msg::CameraInfo>::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<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
   std::unique_ptr<autoware::universe_utils::DebugPublisher> debug_publisher_;