feat(autoware_overlay_rviz_plugin): get the current traffic light (#6899)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp

@@ -102,7 +102,7 @@ private Q_SLOTS:
     turn_signals_sub_;
   rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::HazardLightsReport>::SharedPtr
     hazard_lights_sub_;
-  rclcpp::Subscription<autoware_perception_msgs::msg::TrafficSignalArray>::SharedPtr traffic_sub_;
+  rclcpp::Subscription<autoware_perception_msgs::msg::TrafficSignal>::SharedPtr traffic_sub_;
   rclcpp::Subscription<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr speed_limit_sub_;
 
   std::mutex property_mutex_;
@@ -117,7 +117,7 @@ private Q_SLOTS:
     const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg);
   void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg);
   void updateTrafficLightData(
-    const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg);
+    const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr msg);
   void drawWidget(QImage & hud);
 };
 }  // namespace autoware_overlay_rviz_plugin

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp

@@ -24,7 +24,6 @@
 #include <rviz_common/ros_topic_display.hpp>
 
 #include <autoware_perception_msgs/msg/traffic_signal.hpp>
-#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
 #include <autoware_perception_msgs/msg/traffic_signal_element.hpp>
 
 #include <OgreColourValue.h>
@@ -40,8 +39,8 @@ class TrafficDisplay
   TrafficDisplay();
   void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect);
   void updateTrafficLightData(
-    const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & msg);
-  autoware_perception_msgs::msg::TrafficSignalArray current_traffic_;
+    const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr & msg);
+  autoware_perception_msgs::msg::TrafficSignal current_traffic_;
 
 private:
   QImage traffic_light_image_;

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp

@@ -111,8 +111,9 @@ void SignalDisplay::onInitialize()
   speed_limit_topic_property_->initialize(rviz_ros_node);
 
   traffic_topic_property_ = std::make_unique<rviz_common::properties::RosTopicProperty>(
-    "Traffic Topic", "/perception/traffic_light_recognition/traffic_signals",
-    "autoware_perception/msgs/msg/TrafficSignalArray", "Topic for Traffic Light Data", this,
+    "Traffic Topic",
+    "/planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal",
+    "autoware_perception/msgs/msg/TrafficSignal", "Topic for Traffic Light Data", this,
     SLOT(topic_updated_traffic()));
   traffic_topic_property_->initialize(rviz_ros_node);
 }
@@ -159,10 +160,10 @@ void SignalDisplay::setupRosSubscriptions()
         updateHazardLightsData(msg);
       });
 
-  traffic_sub_ = rviz_node_->create_subscription<autoware_perception_msgs::msg::TrafficSignalArray>(
+  traffic_sub_ = rviz_node_->create_subscription<autoware_perception_msgs::msg::TrafficSignal>(
     traffic_topic_property_->getTopicStd(),
     rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
-    [this](const autoware_perception_msgs::msg::TrafficSignalArray::SharedPtr msg) {
+    [this](const autoware_perception_msgs::msg::TrafficSignal::SharedPtr msg) {
       updateTrafficLightData(msg);
     });
 
@@ -237,7 +238,7 @@ void SignalDisplay::onDisable()
 }
 
 void SignalDisplay::updateTrafficLightData(
-  const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg)
+  const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr msg)
 {
   std::lock_guard<std::mutex> lock(property_mutex_);
 
@@ -506,14 +507,13 @@ void SignalDisplay::topic_updated_traffic()
   // resubscribe to the topic
   traffic_sub_.reset();
   auto rviz_ros_node = context_->getRosNodeAbstraction().lock();
-  traffic_sub_ =
-    rviz_ros_node->get_raw_node()
-      ->create_subscription<autoware_perception_msgs::msg::TrafficSignalArray>(
-        traffic_topic_property_->getTopicStd(),
-        rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
-        [this](const autoware_perception_msgs::msg::TrafficSignalArray::SharedPtr msg) {
-          updateTrafficLightData(msg);
-        });
+  traffic_sub_ = rviz_ros_node->get_raw_node()
+                   ->create_subscription<autoware_perception_msgs::msg::TrafficSignal>(
+                     traffic_topic_property_->getTopicStd(),
+                     rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
+                     [this](const autoware_perception_msgs::msg::TrafficSignal::SharedPtr msg) {
+                       updateTrafficLightData(msg);
+                     });
 }
 
 }  // namespace autoware_overlay_rviz_plugin

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp

@@ -48,7 +48,7 @@ TrafficDisplay::TrafficDisplay()
 }
 
 void TrafficDisplay::updateTrafficLightData(
-  const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & msg)
+  const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr & msg)
 {
   current_traffic_ = *msg;
 }
@@ -70,8 +70,8 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF
     backgroundRect.top() + circleRect.height() + 30));
   painter.drawEllipse(circleRect);
 
-  if (!current_traffic_.signals.empty()) {
-    switch (current_traffic_.signals[0].elements[0].color) {
+  if (!current_traffic_.elements.empty()) {
+    switch (current_traffic_.elements[0].color) {
       case 1:
         painter.setBrush(QBrush(tl_red_));
         painter.drawEllipse(circleRect);