Skip to content

Commit 140cef9

Browse files
fix(virtual traffic light): suppress launch (#7010)
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
1 parent 213c7c3 commit 140cef9

File tree

1 file changed

+25
-1
lines changed
  • planning/autoware_behavior_velocity_virtual_traffic_light_module/src

1 file changed

+25
-1
lines changed

planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp

+25-1
Original file line numberDiff line numberDiff line change
@@ -14,10 +14,17 @@
1414

1515
#include "manager.hpp"
1616

17+
#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
18+
1719
#include <autoware_behavior_velocity_planner_common/utilization/util.hpp>
20+
#include <lanelet2_extension/utility/utilities.hpp>
1821
#include <tier4_autoware_utils/math/unit_conversion.hpp>
1922
#include <tier4_autoware_utils/ros/parameter.hpp>
2023

24+
#include <boost/geometry/algorithms/intersects.hpp>
25+
26+
#include <lanelet2_core/geometry/LineString.h>
27+
2128
#include <memory>
2229
#include <set>
2330
#include <string>
@@ -52,13 +59,30 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
5259
void VirtualTrafficLightModuleManager::launchNewModules(
5360
const tier4_planning_msgs::msg::PathWithLaneId & path)
5461
{
62+
tier4_autoware_utils::LineString2d ego_path_linestring;
63+
for (const auto & path_point : path.points) {
64+
ego_path_linestring.push_back(
65+
tier4_autoware_utils::fromMsg(path_point.point.pose.position).to_2d());
66+
}
67+
5568
for (const auto & m : planning_utils::getRegElemMapOnPath<VirtualTrafficLight>(
5669
path, planner_data_->route_handler_->getLaneletMapPtr(),
5770
planner_data_->current_odometry->pose)) {
71+
const auto stop_line_opt = m.first->getStopLine();
72+
if (!stop_line_opt) {
73+
RCLCPP_FATAL(
74+
logger_, "No stop line at virtual_traffic_light_reg_elem_id = %ld, please fix the map!",
75+
m.first->id());
76+
continue;
77+
}
78+
5879
// Use lanelet_id to unregister module when the route is changed
5980
const auto lane_id = m.second.id();
6081
const auto module_id = lane_id;
61-
if (!isModuleRegistered(module_id)) {
82+
if (
83+
!isModuleRegistered(module_id) &&
84+
boost::geometry::intersects(
85+
ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) {
6286
registerModule(std::make_shared<VirtualTrafficLightModule>(
6387
module_id, lane_id, *m.first, m.second, planner_param_,
6488
logger_.get_child("virtual_traffic_light_module"), clock_));

0 commit comments

Comments
 (0)