feat(behavior_velocity): support new traffic signal interface (autowa…

…refoundation#4133)

* feat(behavior_velocity): support new traffic signal interface

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

* style(pre-commit): autofix

* add missing dependency

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

* style(pre-commit): autofix

* remove the external signal input source in behavior_planning_launch.py

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

* replace TrafficLightElement with TrafficSignalElement

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

* style(pre-commit): autofix

* use the regulatory element id instead of traffic light id

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

* change the input of traffic signal to traffic light arbiter

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

* style(pre-commit): autofix

* do not return until the all regulatory elements are checked

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

* change input topic of the traffic signals

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

* fix the traffic signal type in perception reproducer

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

* add debug log when the signal data is outdated

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

---------

Signed-off-by: Tomohito Ando <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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;
+  TrafficSignalElement traffic_light;
   traffic_light.confidence = traffic_light_confidence_input_->value();
 
   if (color == "RED") {
-    traffic_light.color = TrafficLight::RED;
+    traffic_light.color = TrafficSignalElement::RED;
   } else if (color == "AMBER") {
-    traffic_light.color = TrafficLight::AMBER;
+    traffic_light.color = TrafficSignalElement::AMBER;
   } else if (color == "GREEN") {
-    traffic_light.color = TrafficLight::GREEN;
+    traffic_light.color = TrafficSignalElement::GREEN;
   } else if (color == "WHITE") {
-    traffic_light.color = TrafficLight::WHITE;
+    traffic_light.color = TrafficSignalElement::WHITE;
   } else if (color == "UNKNOWN") {
-    traffic_light.color = TrafficLight::UNKNOWN;
+    traffic_light.color = TrafficSignalElement::UNKNOWN;
   }
 
   if (shape == "CIRCLE") {
-    traffic_light.shape = TrafficLight::CIRCLE;
+    traffic_light.shape = TrafficSignalElement::CIRCLE;
   } else if (shape == "LEFT_ARROW") {
-    traffic_light.shape = TrafficLight::LEFT_ARROW;
+    traffic_light.shape = TrafficSignalElement::LEFT_ARROW;
   } else if (shape == "RIGHT_ARROW") {
-    traffic_light.shape = TrafficLight::RIGHT_ARROW;
+    traffic_light.shape = TrafficSignalElement::RIGHT_ARROW;
   } else if (shape == "UP_ARROW") {
-    traffic_light.shape = TrafficLight::UP_ARROW;
+    traffic_light.shape = TrafficSignalElement::UP_ARROW;
   } else if (shape == "DOWN_ARROW") {
-    traffic_light.shape = TrafficLight::DOWN_ARROW;
+    traffic_light.shape = TrafficSignalElement::DOWN_ARROW;
   } else if (shape == "DOWN_LEFT_ARROW") {
-    traffic_light.shape = TrafficLight::DOWN_LEFT_ARROW;
+    traffic_light.shape = TrafficSignalElement::DOWN_LEFT_ARROW;
   } else if (shape == "DOWN_RIGHT_ARROW") {
-    traffic_light.shape = TrafficLight::DOWN_RIGHT_ARROW;
+    traffic_light.shape = TrafficSignalElement::DOWN_RIGHT_ARROW;
   } else if (shape == "UNKNOWN") {
-    traffic_light.shape = TrafficLight::UNKNOWN;
+    traffic_light.shape = TrafficSignalElement::UNKNOWN;
   }
 
   if (status == "SOLID_OFF") {
-    traffic_light.status = TrafficLight::SOLID_OFF;
+    traffic_light.status = TrafficSignalElement::SOLID_OFF;
   } else if (status == "SOLID_ON") {
-    traffic_light.status = TrafficLight::SOLID_ON;
+    traffic_light.status = TrafficSignalElement::SOLID_ON;
   } else if (status == "FLASHING") {
-    traffic_light.status = TrafficLight::FLASHING;
+    traffic_light.status = TrafficSignalElement::FLASHING;
   } else if (status == "UNKNOWN") {
-    traffic_light.status = TrafficLight::UNKNOWN;
+    traffic_light.status = TrafficSignalElement::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 TrafficSignalElement::RED:
         color_label->setText("RED");
         color_label->setStyleSheet("background-color: #FF0000;");
         break;
-      case TrafficLight::AMBER:
+      case TrafficSignalElement::AMBER:
         color_label->setText("AMBER");
         color_label->setStyleSheet("background-color: #FFBF00;");
         break;
-      case TrafficLight::GREEN:
+      case TrafficSignalElement::GREEN:
         color_label->setText("GREEN");
         color_label->setStyleSheet("background-color: #7CFC00;");
         break;
-      case TrafficLight::WHITE:
+      case TrafficSignalElement::WHITE:
         color_label->setText("WHITE");
         color_label->setStyleSheet("background-color: #FFFFFF;");
         break;
-      case TrafficLight::UNKNOWN:
+      case TrafficSignalElement::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 TrafficSignalElement::CIRCLE:
         shape_label->setText("CIRCLE");
         break;
-      case TrafficLight::LEFT_ARROW:
+      case TrafficSignalElement::LEFT_ARROW:
         shape_label->setText("LEFT_ARROW");
         break;
-      case TrafficLight::RIGHT_ARROW:
+      case TrafficSignalElement::RIGHT_ARROW:
         shape_label->setText("RIGHT_ARROW");
         break;
-      case TrafficLight::UP_ARROW:
+      case TrafficSignalElement::UP_ARROW:
         shape_label->setText("UP_ARROW");
         break;
-      case TrafficLight::DOWN_ARROW:
+      case TrafficSignalElement::DOWN_ARROW:
         shape_label->setText("DOWN_ARROW");
         break;
-      case TrafficLight::DOWN_LEFT_ARROW:
+      case TrafficSignalElement::DOWN_LEFT_ARROW:
         shape_label->setText("DOWN_LEFT_ARROW");
         break;
-      case TrafficLight::DOWN_RIGHT_ARROW:
+      case TrafficSignalElement::DOWN_RIGHT_ARROW:
         shape_label->setText("DOWN_RIGHT_ARROW");
         break;
-      case TrafficLight::FLASHING:
-        shape_label->setText("FLASHING");
-        break;
-      case TrafficLight::UNKNOWN:
+      case TrafficSignalElement::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 TrafficSignalElement::SOLID_OFF:
         status_label->setText("SOLID_OFF");
         break;
-      case TrafficLight::SOLID_ON:
+      case TrafficSignalElement::SOLID_ON:
         status_label->setText("SOLID_ON");
         break;
-      case TrafficLight::FLASHING:
+      case TrafficSignalElement::FLASHING:
         status_label->setText("FLASHING");
         break;
-      case TrafficLight::UNKNOWN:
+      case TrafficSignalElement::UNKNOWN:
         status_label->setText("UNKNOWN");
         break;
       default:
@@ -375,11 +373,9 @@ void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg)
   std::string info = "Fetching traffic lights :";
   std::string delim = " ";
   for (auto && tl_reg_elem_ptr : tl_reg_elems) {
-    for (auto && traffic_light : tl_reg_elem_ptr->trafficLights()) {
-      auto id = static_cast<int>(traffic_light.id());
-      info += (std::exchange(delim, ", ") + std::to_string(id));
-      traffic_light_ids_.insert(id);
-    }
+    auto id = static_cast<int>(tl_reg_elem_ptr->id());
+    info += (std::exchange(delim, ", ") + std::to_string(id));
+    traffic_light_ids_.insert(id);
   }
   RCLCPP_INFO_STREAM(raw_node_->get_logger(), info);
   received_vector_map_ = true;

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::TrafficSignal;
+using autoware_perception_msgs::msg::TrafficSignalArray;
+using autoware_perception_msgs::msg::TrafficSignalElement;
 class TrafficLightPublishPanel : public rviz_common::Panel
 {
   Q_OBJECT

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -177,10 +177,6 @@ def launch_setup(context, *args, **kwargs):
                 "~/input/traffic_signals",
                 "/perception/traffic_light_recognition/traffic_signals",
             ),
-            (
-                "~/input/external_traffic_signals",
-                "/external/traffic_light_recognition/traffic_signals",
-            ),
             (
                 "~/input/external_velocity_limit_mps",
                 "/planning/scenario_planning/max_velocity_default",

planning/behavior_velocity_crosswalk_module/package.xml

@@ -21,6 +21,7 @@
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_tf2</depend>
+  <depend>autoware_perception_msgs</depend>
   <depend>behavior_velocity_planner_common</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -938,28 +938,25 @@ bool CrosswalkModule::isRedSignalForPedestrians() const
     crosswalk_.regulatoryElementsAs<const lanelet::TrafficLight>();
 
   for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) {
-    lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_lights_reg_elem->trafficLights();
-    for (const auto & traffic_light : traffic_lights) {
-      const auto ll_traffic_light = static_cast<lanelet::ConstLineString3d>(traffic_light);
-      const auto traffic_signal_stamped = planner_data_->getTrafficSignal(ll_traffic_light.id());
-      if (!traffic_signal_stamped) {
-        continue;
-      }
+    const auto traffic_signal_stamped =
+      planner_data_->getTrafficSignal(traffic_lights_reg_elem->id());
+    if (!traffic_signal_stamped) {
+      continue;
+    }
 
-      if (
-        planner_param_.traffic_light_state_timeout <
-        (clock_->now() - traffic_signal_stamped->header.stamp).seconds()) {
-        continue;
-      }
+    if (
+      planner_param_.traffic_light_state_timeout <
+      (clock_->now() - traffic_signal_stamped->stamp).seconds()) {
+      continue;
+    }
 
-      const auto & lights = traffic_signal_stamped->signal.lights;
-      if (lights.empty()) {
-        continue;
-      }
+    const auto & lights = traffic_signal_stamped->signal.elements;
+    if (lights.empty()) {
+      continue;
+    }
 
-      if (lights.front().color == TrafficLight::RED) {
-        return true;
-      }
+    if (lights.front().color == TrafficSignalElement::RED) {
+      return true;
     }
   }
 

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -50,8 +50,8 @@ namespace behavior_velocity_planner
 using autoware_auto_perception_msgs::msg::ObjectClassification;
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
-using autoware_auto_perception_msgs::msg::TrafficLight;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using autoware_perception_msgs::msg::TrafficSignalElement;
 using lanelet::autoware::Crosswalk;
 using tier4_api_msgs::msg::CrosswalkStatus;
 using tier4_autoware_utils::Polygon2d;

planning/behavior_velocity_intersection_module/package.xml

@@ -19,6 +19,7 @@
 
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_perception_msgs</depend>
   <depend>behavior_velocity_planner_common</depend>
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -704,9 +704,10 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane)
 }
 
 bool isTrafficLightArrowActivated(
-  lanelet::ConstLanelet lane,
-  const std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped> & tl_infos)
+  lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos)
 {
+  using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
+
   const auto & turn_direction = lane.attributeOr("turn_direction", "else");
   std::optional<int> tl_id = std::nullopt;
   for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
@@ -723,16 +724,13 @@ bool isTrafficLightArrowActivated(
     return false;
   }
   const auto & tl_info = tl_info_it->second;
-  for (auto && tl_light : tl_info.signal.lights) {
-    if (tl_light.color != autoware_auto_perception_msgs::msg::TrafficLight::GREEN) continue;
-    if (tl_light.status != autoware_auto_perception_msgs::msg::TrafficLight::SOLID_ON) continue;
-    if (
-      turn_direction == std::string("left") &&
-      tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW)
+  for (auto && tl_light : tl_info.signal.elements) {
+    if (tl_light.color != TrafficSignalElement::GREEN) continue;
+    if (tl_light.status != TrafficSignalElement::SOLID_ON) continue;
+    if (turn_direction == std::string("left") && tl_light.shape == TrafficSignalElement::LEFT_ARROW)
       return true;
     if (
-      turn_direction == std::string("right") &&
-      tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW)
+      turn_direction == std::string("right") && tl_light.shape == TrafficSignalElement::RIGHT_ARROW)
       return true;
   }
   return false;

planning/behavior_velocity_intersection_module/src/util.hpp

@@ -112,8 +112,7 @@ std::optional<Polygon2d> getIntersectionArea(
 
 bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane);
 bool isTrafficLightArrowActivated(
-  lanelet::ConstLanelet lane,
-  const std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped> & tl_infos);
+  lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos);
 
 std::vector<DiscretizedLane> generateDetectionLaneDivisions(
   lanelet::ConstLanelets detection_lanelets,