|
12 | 12 | // See the License for the specific language governing permissions and
|
13 | 13 | // limitations under the License.
|
14 | 14 |
|
| 15 | +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" |
| 16 | + |
| 17 | +#include <lanelet2_extension/utility/utilities.hpp> |
15 | 18 | #include <scene_module/virtual_traffic_light/manager.hpp>
|
16 | 19 | #include <tier4_autoware_utils/math/unit_conversion.hpp>
|
17 | 20 |
|
@@ -81,11 +84,21 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
|
81 | 84 | void VirtualTrafficLightModuleManager::launchNewModules(
|
82 | 85 | const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
|
83 | 86 | {
|
| 87 | + tier4_autoware_utils::LineString2d ego_path_linestring; |
| 88 | + for (const auto & path_point : path.points) { |
| 89 | + ego_path_linestring.push_back( |
| 90 | + tier4_autoware_utils::fromMsg(path_point.point.pose.position).to_2d()); |
| 91 | + } |
| 92 | + |
84 | 93 | for (const auto & m : getRegElemMapOnPath<VirtualTrafficLight>(
|
85 | 94 | path, planner_data_->route_handler_->getLaneletMapPtr())) {
|
86 | 95 | // Use lanelet_id to unregister module when the route is changed
|
87 | 96 | const auto module_id = m.second.id();
|
88 |
| - if (!isModuleRegistered(module_id)) { |
| 97 | + const auto stop_line = |
| 98 | + lanelet::utils::to2D(m.first.get()->getStopLine().value()).basicLineString(); |
| 99 | + if ( |
| 100 | + !isModuleRegistered(module_id) && |
| 101 | + boost::geometry::intersects(ego_path_linestring, stop_line)) { |
89 | 102 | registerModule(std::make_shared<VirtualTrafficLightModule>(
|
90 | 103 | module_id, *m.first, m.second, planner_param_,
|
91 | 104 | logger_.get_child("virtual_traffic_light_module"), clock_));
|
|
0 commit comments