From 627f64a97f6110ac29eefd7e12d89beafa556d4e Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Fri, 10 May 2024 19:46:49 +0900 Subject: [PATCH 1/2] suppress launch Signed-off-by: Yuki Takagi --- .../virtual_traffic_light/manager.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp index 7ac6918f13ed2..2ed64c1a5db19 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" + +#include #include #include @@ -81,11 +84,21 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node void VirtualTrafficLightModuleManager::launchNewModules( const autoware_auto_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 : getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr())) { // Use lanelet_id to unregister module when the route is changed const auto module_id = m.second.id(); - if (!isModuleRegistered(module_id)) { + const auto stop_line = + lanelet::utils::to2D(m.first.get()->getStopLine().value()).basicLineString(); + if ( + !isModuleRegistered(module_id) && + boost::geometry::intersects(ego_path_linestring, stop_line)) { registerModule(std::make_shared( module_id, *m.first, m.second, planner_param_, logger_.get_child("virtual_traffic_light_module"), clock_)); From 750180c31e8552778221f757c29bcd70cec3e80f Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 14 May 2024 18:04:45 +0900 Subject: [PATCH 2/2] add existence check Signed-off-by: Yuki Takagi --- .../scene_module/virtual_traffic_light/manager.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp index 2ed64c1a5db19..d5a302c3275c6 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp @@ -92,13 +92,20 @@ void VirtualTrafficLightModuleManager::launchNewModules( for (const auto & m : getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr())) { + 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 module_id = m.second.id(); - const auto stop_line = - lanelet::utils::to2D(m.first.get()->getStopLine().value()).basicLineString(); if ( !isModuleRegistered(module_id) && - boost::geometry::intersects(ego_path_linestring, stop_line)) { + boost::geometry::intersects( + ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) { registerModule(std::make_shared( module_id, *m.first, m.second, planner_param_, logger_.get_child("virtual_traffic_light_module"), clock_));