From 0eca829961cfb0cb5d1568c776eca2aef818ecdf Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 14 May 2024 16:24:02 +0900 Subject: [PATCH 1/2] suppres launch Signed-off-by: Yuki Takagi --- .../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..47ad0a54528f4 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 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_)); From 671bab41f4ec43133d38453885c1bbc64752175d Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 14 May 2024 17:49:16 +0900 Subject: [PATCH 2/2] fix warn msg Signed-off-by: Yuki Takagi --- .../src/manager.cpp | 2 +- 1 file changed, 1 insertion(+), 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 47ad0a54528f4..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 @@ -71,7 +71,7 @@ void VirtualTrafficLightModuleManager::launchNewModules( const auto stop_line_opt = m.first->getStopLine(); if (!stop_line_opt) { RCLCPP_FATAL( - logger_, "No stop line at traffic_light_reg_elem_id = %ld, please fix the map!", + logger_, "No stop line at virtual_traffic_light_reg_elem_id = %ld, please fix the map!", m.first->id()); continue; }