From 140cef943f5b2ad053b779271189876a2ee2cb1a Mon Sep 17 00:00:00 2001
From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com>
Date: Tue, 11 Jun 2024 00:26:47 +0900
Subject: [PATCH] fix(virtual traffic light): suppress launch (#7010)

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../src/manager.cpp                           | 26 ++++++++++++++++++-
 1 file changed, 25 insertions(+), 1 deletion(-)

diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp
index 39dca1f8f6303..69b91d11937a0 100644
--- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp
+++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp
@@ -14,10 +14,17 @@
 
 #include "manager.hpp"
 
+#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
+
 #include <autoware_behavior_velocity_planner_common/utilization/util.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <tier4_autoware_utils/math/unit_conversion.hpp>
 #include <tier4_autoware_utils/ros/parameter.hpp>
 
+#include <boost/geometry/algorithms/intersects.hpp>
+
+#include <lanelet2_core/geometry/LineString.h>
+
 #include <memory>
 #include <set>
 #include <string>
@@ -52,13 +59,30 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
 void VirtualTrafficLightModuleManager::launchNewModules(
   const tier4_planning_msgs::msg::PathWithLaneId & path)
 {
+  tier4_autoware_utils::LineString2d ego_path_linestring;
+  for (const auto & path_point : path.points) {
+    ego_path_linestring.push_back(
+      tier4_autoware_utils::fromMsg(path_point.point.pose.position).to_2d());
+  }
+
   for (const auto & m : planning_utils::getRegElemMapOnPath<VirtualTrafficLight>(
          path, planner_data_->route_handler_->getLaneletMapPtr(),
          planner_data_->current_odometry->pose)) {
+    const auto stop_line_opt = m.first->getStopLine();
+    if (!stop_line_opt) {
+      RCLCPP_FATAL(
+        logger_, "No stop line at virtual_traffic_light_reg_elem_id = %ld, please fix the map!",
+        m.first->id());
+      continue;
+    }
+
     // Use lanelet_id to unregister module when the route is changed
     const auto lane_id = m.second.id();
     const auto module_id = lane_id;
-    if (!isModuleRegistered(module_id)) {
+    if (
+      !isModuleRegistered(module_id) &&
+      boost::geometry::intersects(
+        ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) {
       registerModule(std::make_shared<VirtualTrafficLightModule>(
         module_id, lane_id, *m.first, m.second, planner_param_,
         logger_.get_child("virtual_traffic_light_module"), clock_));