feat: support new traffic signal interface

Signed-off-by: Tomohito Ando <[email protected]>

common/tier4_traffic_light_rviz_plugin/README.md

@@ -8,9 +8,9 @@ This plugin panel publishes dummy traffic light signals.
 
 ### Output
 
-| Name                                                    | Type                                                     | Description                   |
-| ------------------------------------------------------- | -------------------------------------------------------- | ----------------------------- |
-| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals |
+| Name                                                    | Type                                                | Description                   |
+| ------------------------------------------------------- | --------------------------------------------------- | ----------------------------- |
+| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals |
 
 ## HowToUse
 

common/tier4_traffic_light_rviz_plugin/package.xml

@@ -11,7 +11,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_auto_mapping_msgs</depend>
-  <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_perception_msgs</depend>
   <depend>lanelet2_extension</depend>
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>

common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp

@@ -34,6 +34,7 @@
 #include <utility>
 #include <vector>
 
+#undef signals
 namespace rviz_plugins
 {
 TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_common::Panel(parent)
@@ -138,55 +139,55 @@ void TrafficLightPublishPanel::onSetTrafficLightState()
   const auto shape = light_shape_combo_->currentText();
   const auto status = light_status_combo_->currentText();
 
-  TrafficLight traffic_light;
+  TrafficLightElement traffic_light;
   traffic_light.confidence = traffic_light_confidence_input_->value();
 
   if (color == "RED") {
-    traffic_light.color = TrafficLight::RED;
+    traffic_light.color = TrafficLightElement::RED;
   } else if (color == "AMBER") {
-    traffic_light.color = TrafficLight::AMBER;
+    traffic_light.color = TrafficLightElement::AMBER;
   } else if (color == "GREEN") {
-    traffic_light.color = TrafficLight::GREEN;
+    traffic_light.color = TrafficLightElement::GREEN;
   } else if (color == "WHITE") {
-    traffic_light.color = TrafficLight::WHITE;
+    traffic_light.color = TrafficLightElement::WHITE;
   } else if (color == "UNKNOWN") {
-    traffic_light.color = TrafficLight::UNKNOWN;
+    traffic_light.color = TrafficLightElement::UNKNOWN;
   }
 
   if (shape == "CIRCLE") {
-    traffic_light.shape = TrafficLight::CIRCLE;
+    traffic_light.shape = TrafficLightElement::CIRCLE;
   } else if (shape == "LEFT_ARROW") {
-    traffic_light.shape = TrafficLight::LEFT_ARROW;
+    traffic_light.shape = TrafficLightElement::LEFT_ARROW;
   } else if (shape == "RIGHT_ARROW") {
-    traffic_light.shape = TrafficLight::RIGHT_ARROW;
+    traffic_light.shape = TrafficLightElement::RIGHT_ARROW;
   } else if (shape == "UP_ARROW") {
-    traffic_light.shape = TrafficLight::UP_ARROW;
+    traffic_light.shape = TrafficLightElement::UP_ARROW;
   } else if (shape == "DOWN_ARROW") {
-    traffic_light.shape = TrafficLight::DOWN_ARROW;
+    traffic_light.shape = TrafficLightElement::DOWN_ARROW;
   } else if (shape == "DOWN_LEFT_ARROW") {
-    traffic_light.shape = TrafficLight::DOWN_LEFT_ARROW;
+    traffic_light.shape = TrafficLightElement::DOWN_LEFT_ARROW;
   } else if (shape == "DOWN_RIGHT_ARROW") {
-    traffic_light.shape = TrafficLight::DOWN_RIGHT_ARROW;
+    traffic_light.shape = TrafficLightElement::DOWN_RIGHT_ARROW;
   } else if (shape == "UNKNOWN") {
-    traffic_light.shape = TrafficLight::UNKNOWN;
+    traffic_light.shape = TrafficLightElement::UNKNOWN;
   }
 
   if (status == "SOLID_OFF") {
-    traffic_light.status = TrafficLight::SOLID_OFF;
+    traffic_light.status = TrafficLightElement::SOLID_OFF;
   } else if (status == "SOLID_ON") {
-    traffic_light.status = TrafficLight::SOLID_ON;
+    traffic_light.status = TrafficLightElement::SOLID_ON;
   } else if (status == "FLASHING") {
-    traffic_light.status = TrafficLight::FLASHING;
+    traffic_light.status = TrafficLightElement::FLASHING;
   } else if (status == "UNKNOWN") {
-    traffic_light.status = TrafficLight::UNKNOWN;
+    traffic_light.status = TrafficLightElement::UNKNOWN;
   }
 
   TrafficSignal traffic_signal;
-  traffic_signal.lights.push_back(traffic_light);
-  traffic_signal.map_primitive_id = traffic_light_id;
+  traffic_signal.elements.push_back(traffic_light);
+  traffic_signal.traffic_signal_id = traffic_light_id;
 
   for (auto & signal : extra_traffic_signals_.signals) {
-    if (signal.map_primitive_id == traffic_light_id) {
+    if (signal.traffic_signal_id == traffic_light_id) {
       signal = traffic_signal;
       return;
     }
@@ -247,7 +248,7 @@ void TrafficLightPublishPanel::createWallTimer()
 void TrafficLightPublishPanel::onTimer()
 {
   if (enable_publish_) {
-    extra_traffic_signals_.header.stamp = rclcpp::Clock().now();
+    extra_traffic_signals_.stamp = rclcpp::Clock().now();
     pub_traffic_signals_->publish(extra_traffic_signals_);
   }
 
@@ -260,35 +261,35 @@ void TrafficLightPublishPanel::onTimer()
   for (size_t i = 0; i < extra_traffic_signals_.signals.size(); ++i) {
     const auto & signal = extra_traffic_signals_.signals.at(i);
 
-    if (signal.lights.empty()) {
+    if (signal.elements.empty()) {
       continue;
     }
 
-    auto id_label = new QLabel(QString::number(signal.map_primitive_id));
+    auto id_label = new QLabel(QString::number(signal.traffic_signal_id));
     id_label->setAlignment(Qt::AlignCenter);
 
     auto color_label = new QLabel();
     color_label->setAlignment(Qt::AlignCenter);
 
-    const auto & light = signal.lights.front();
+    const auto & light = signal.elements.front();
     switch (light.color) {
-      case TrafficLight::RED:
+      case TrafficLightElement::RED:
         color_label->setText("RED");
         color_label->setStyleSheet("background-color: #FF0000;");
         break;
-      case TrafficLight::AMBER:
+      case TrafficLightElement::AMBER:
         color_label->setText("AMBER");
         color_label->setStyleSheet("background-color: #FFBF00;");
         break;
-      case TrafficLight::GREEN:
+      case TrafficLightElement::GREEN:
         color_label->setText("GREEN");
         color_label->setStyleSheet("background-color: #7CFC00;");
         break;
-      case TrafficLight::WHITE:
+      case TrafficLightElement::WHITE:
         color_label->setText("WHITE");
         color_label->setStyleSheet("background-color: #FFFFFF;");
         break;
-      case TrafficLight::UNKNOWN:
+      case TrafficLightElement::UNKNOWN:
         color_label->setText("UNKNOWN");
         color_label->setStyleSheet("background-color: #808080;");
         break;
@@ -300,31 +301,28 @@ void TrafficLightPublishPanel::onTimer()
     shape_label->setAlignment(Qt::AlignCenter);
 
     switch (light.shape) {
-      case TrafficLight::CIRCLE:
+      case TrafficLightElement::CIRCLE:
         shape_label->setText("CIRCLE");
         break;
-      case TrafficLight::LEFT_ARROW:
+      case TrafficLightElement::LEFT_ARROW:
         shape_label->setText("LEFT_ARROW");
         break;
-      case TrafficLight::RIGHT_ARROW:
+      case TrafficLightElement::RIGHT_ARROW:
         shape_label->setText("RIGHT_ARROW");
         break;
-      case TrafficLight::UP_ARROW:
+      case TrafficLightElement::UP_ARROW:
         shape_label->setText("UP_ARROW");
         break;
-      case TrafficLight::DOWN_ARROW:
+      case TrafficLightElement::DOWN_ARROW:
         shape_label->setText("DOWN_ARROW");
         break;
-      case TrafficLight::DOWN_LEFT_ARROW:
+      case TrafficLightElement::DOWN_LEFT_ARROW:
         shape_label->setText("DOWN_LEFT_ARROW");
         break;
-      case TrafficLight::DOWN_RIGHT_ARROW:
+      case TrafficLightElement::DOWN_RIGHT_ARROW:
         shape_label->setText("DOWN_RIGHT_ARROW");
         break;
-      case TrafficLight::FLASHING:
-        shape_label->setText("FLASHING");
-        break;
-      case TrafficLight::UNKNOWN:
+      case TrafficLightElement::UNKNOWN:
         shape_label->setText("UNKNOWN");
         break;
       default:
@@ -335,16 +333,16 @@ void TrafficLightPublishPanel::onTimer()
     status_label->setAlignment(Qt::AlignCenter);
 
     switch (light.status) {
-      case TrafficLight::SOLID_OFF:
+      case TrafficLightElement::SOLID_OFF:
         status_label->setText("SOLID_OFF");
         break;
-      case TrafficLight::SOLID_ON:
+      case TrafficLightElement::SOLID_ON:
         status_label->setText("SOLID_ON");
         break;
-      case TrafficLight::FLASHING:
+      case TrafficLightElement::FLASHING:
         status_label->setText("FLASHING");
         break;
-      case TrafficLight::UNKNOWN:
+      case TrafficLightElement::UNKNOWN:
         status_label->setText("UNKNOWN");
         break;
       default:

common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp

@@ -27,7 +27,7 @@
 #include <rviz_common/panel.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
-#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
+#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
 #endif
 
 #include <set>
@@ -36,10 +36,9 @@ namespace rviz_plugins
 {
 
 using autoware_auto_mapping_msgs::msg::HADMapBin;
-using autoware_auto_perception_msgs::msg::TrafficLight;
-using autoware_auto_perception_msgs::msg::TrafficSignal;
-using autoware_auto_perception_msgs::msg::TrafficSignalArray;
-
+using autoware_perception_msgs::msg::TrafficLightElement;
+using autoware_perception_msgs::msg::TrafficSignal;
+using autoware_perception_msgs::msg::TrafficSignalArray;
 class TrafficLightPublishPanel : public rviz_common::Panel
 {
   Q_OBJECT

perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp

@@ -22,9 +22,7 @@
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
-#include <autoware_auto_perception_msgs/msg/traffic_light.hpp>
-#include <autoware_auto_perception_msgs/msg/traffic_signal.hpp>
-#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
+#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <tier4_debug_msgs/msg/float64_stamped.hpp>
 
@@ -43,9 +41,9 @@ namespace traffic_light
 {
 
 using autoware_auto_mapping_msgs::msg::HADMapBin;
-using autoware_auto_perception_msgs::msg::TrafficLight;
-using autoware_auto_perception_msgs::msg::TrafficSignal;
-using autoware_auto_perception_msgs::msg::TrafficSignalArray;
+using autoware_perception_msgs::msg::TrafficLightElement;
+using autoware_perception_msgs::msg::TrafficSignal;
+using autoware_perception_msgs::msg::TrafficSignalArray;
 using autoware_planning_msgs::msg::LaneletRoute;
 using tier4_autoware_utils::DebugPublisher;
 using tier4_autoware_utils::StopWatch;

perception/crosswalk_traffic_light_estimator/package.xml

@@ -11,8 +11,8 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_auto_mapping_msgs</depend>
-  <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>lanelet2_extension</depend>
   <depend>rclcpp</depend>

perception/crosswalk_traffic_light_estimator/src/node.cpp

@@ -160,7 +160,7 @@ void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray(
 
   TrafficLightIdMap traffic_light_id_map;
   for (const auto & traffic_signal : msg->signals) {
-    traffic_light_id_map[traffic_signal.map_primitive_id] =
+    traffic_light_id_map[traffic_signal.traffic_signal_id] =
       std::pair<TrafficSignal, rclcpp::Time>(traffic_signal, get_clock()->now());
   }
 
@@ -185,17 +185,17 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal(
   const TrafficLightIdMap & traffic_light_id_map)
 {
   for (const auto & input_traffic_signal : traffic_light_id_map) {
-    const auto & lights = input_traffic_signal.second.first.lights;
+    const auto & lights = input_traffic_signal.second.first.elements;
 
     if (lights.empty()) {
       continue;
     }
 
-    if (lights.front().color == TrafficLight::UNKNOWN) {
+    if (lights.front().color == TrafficLightElement::UNKNOWN) {
       continue;
     }
 
-    const auto & id = input_traffic_signal.second.first.map_primitive_id;
+    const auto & id = input_traffic_signal.second.first.traffic_signal_id;
 
     if (last_detect_color_.count(id) == 0) {
       last_detect_color_.insert(std::make_pair(id, input_traffic_signal.second));
@@ -207,7 +207,7 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal(
 
   std::vector<int32_t> erase_id_list;
   for (auto & last_traffic_signal : last_detect_color_) {
-    const auto & id = last_traffic_signal.second.first.map_primitive_id;
+    const auto & id = last_traffic_signal.second.first.traffic_signal_id;
 
     if (traffic_light_id_map.count(id) == 0) {
       // hold signal recognition results for [last_detect_color_hold_time_] seconds.
@@ -233,11 +233,11 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal(
       const auto ll_traffic_light = static_cast<lanelet::ConstLineString3d>(traffic_light);
 
       TrafficSignal output_traffic_signal;
-      TrafficLight output_traffic_light;
+      TrafficLightElement output_traffic_light;
       output_traffic_light.color = color;
       output_traffic_light.confidence = 1.0;
-      output_traffic_signal.lights.push_back(output_traffic_light);
-      output_traffic_signal.map_primitive_id = ll_traffic_light.id();
+      output_traffic_signal.elements.push_back(output_traffic_light);
+      output_traffic_signal.traffic_signal_id = ll_traffic_light.id();
       msg.signals.push_back(output_traffic_signal);
     }
   }
@@ -265,13 +265,14 @@ lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getNonRedLanelets(
       continue;
     }
 
-    const auto current_is_not_red = current_detected_signal
-                                      ? current_detected_signal.get() == TrafficLight::GREEN ||
-                                          current_detected_signal.get() == TrafficLight::AMBER
-                                      : true;
+    const auto current_is_not_red =
+      current_detected_signal ? current_detected_signal.get() == TrafficLightElement::GREEN ||
+                                  current_detected_signal.get() == TrafficLightElement::AMBER
+                              : true;
 
     const auto current_is_unknown_or_none =
-      current_detected_signal ? current_detected_signal.get() == TrafficLight::UNKNOWN : true;
+      current_detected_signal ? current_detected_signal.get() == TrafficLightElement::UNKNOWN
+                              : true;
 
     const auto last_detected_signal =
       getHighestConfidenceTrafficSignal(traffic_lights_for_vehicle, last_detect_color_);
@@ -281,8 +282,8 @@ lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getNonRedLanelets(
     }
 
     const auto was_not_red = current_is_unknown_or_none &&
-                             (last_detected_signal.get() == TrafficLight::GREEN ||
-                              last_detected_signal.get() == TrafficLight::AMBER) &&
+                             (last_detected_signal.get() == TrafficLightElement::GREEN ||
+                              last_detected_signal.get() == TrafficLightElement::AMBER) &&
                              use_last_detect_color_;
 
     if (!current_is_not_red && !was_not_red) {
@@ -323,12 +324,13 @@ uint8_t CrosswalkTrafficLightEstimatorNode::estimateCrosswalkTrafficSignal(
   }
 
   if (has_straight_non_red_lane || has_related_non_red_tl) {
-    return TrafficLight::RED;
+    return TrafficLightElement::RED;
   }
 
   const auto has_merge_lane = hasMergeLane(non_red_lanelets, routing_graph_ptr_);
-  return !has_merge_lane && has_left_non_red_lane && has_right_non_red_lane ? TrafficLight::RED
-                                                                            : TrafficLight::UNKNOWN;
+  return !has_merge_lane && has_left_non_red_lane && has_right_non_red_lane
+           ? TrafficLightElement::RED
+           : TrafficLightElement::UNKNOWN;
 }
 
 boost::optional<uint8_t> CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal(
@@ -348,7 +350,7 @@ boost::optional<uint8_t> CrosswalkTrafficLightEstimatorNode::getHighestConfidenc
       continue;
     }
 
-    const auto & lights = traffic_light_id_map.at(id).first.lights;
+    const auto & lights = traffic_light_id_map.at(id).first.elements;
     if (lights.empty()) {
       continue;
     }

planning/behavior_velocity_crosswalk_module/package.xml

@@ -17,6 +17,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
+  <depend>autoware_perception_msgs</depend>
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_tf2</depend>