|
14 | 14 |
|
15 | 15 | #include "manager.hpp"
|
16 | 16 |
|
| 17 | +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" |
| 18 | + |
17 | 19 | #include <autoware_behavior_velocity_planner_common/utilization/util.hpp>
|
| 20 | +#include <lanelet2_extension/utility/utilities.hpp> |
18 | 21 | #include <tier4_autoware_utils/math/unit_conversion.hpp>
|
19 | 22 | #include <tier4_autoware_utils/ros/parameter.hpp>
|
20 | 23 |
|
| 24 | +#include <boost/geometry/algorithms/intersects.hpp> |
| 25 | + |
| 26 | +#include <lanelet2_core/geometry/LineString.h> |
| 27 | + |
21 | 28 | #include <memory>
|
22 | 29 | #include <set>
|
23 | 30 | #include <string>
|
@@ -52,13 +59,30 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
|
52 | 59 | void VirtualTrafficLightModuleManager::launchNewModules(
|
53 | 60 | const tier4_planning_msgs::msg::PathWithLaneId & path)
|
54 | 61 | {
|
| 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 | + |
55 | 68 | for (const auto & m : planning_utils::getRegElemMapOnPath<VirtualTrafficLight>(
|
56 | 69 | path, planner_data_->route_handler_->getLaneletMapPtr(),
|
57 | 70 | 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 | + |
58 | 79 | // Use lanelet_id to unregister module when the route is changed
|
59 | 80 | const auto lane_id = m.second.id();
|
60 | 81 | 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())) { |
62 | 86 | registerModule(std::make_shared<VirtualTrafficLightModule>(
|
63 | 87 | module_id, lane_id, *m.first, m.second, planner_param_,
|
64 | 88 | logger_.get_child("virtual_traffic_light_module"), clock_));
|
|
0 commit comments