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 +#include #include #include +#include + +#include + #include #include #include @@ -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( 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( module_id, lane_id, *m.first, m.second, planner_param_, logger_.get_child("virtual_traffic_light_module"), clock_));