Skip to content

Commit 627f64a

Browse files
suppress launch
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
1 parent 65e903c commit 627f64a

File tree

1 file changed

+14
-1
lines changed
  • planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light

1 file changed

+14
-1
lines changed

planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp

+14-1
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,9 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
16+
17+
#include <lanelet2_extension/utility/utilities.hpp>
1518
#include <scene_module/virtual_traffic_light/manager.hpp>
1619
#include <tier4_autoware_utils/math/unit_conversion.hpp>
1720

@@ -81,11 +84,21 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
8184
void VirtualTrafficLightModuleManager::launchNewModules(
8285
const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
8386
{
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+
8493
for (const auto & m : getRegElemMapOnPath<VirtualTrafficLight>(
8594
path, planner_data_->route_handler_->getLaneletMapPtr())) {
8695
// Use lanelet_id to unregister module when the route is changed
8796
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)) {
89102
registerModule(std::make_shared<VirtualTrafficLightModule>(
90103
module_id, *m.first, m.second, planner_param_,
91104
logger_.get_child("virtual_traffic_light_module"), clock_));

0 commit comments

Comments
 (0)