Skip to content

Commit

Permalink
suppres launch
Browse files Browse the repository at this point in the history
Signed-off-by: Yuki Takagi <[email protected]>
  • Loading branch information
yuki-takagi-66 committed May 14, 2024
1 parent fd89ca2 commit 08ea2de
Showing 1 changed file with 25 additions and 1 deletion.
Original file line number Diff line number Diff line change
Expand Up @@ -14,10 +14,17 @@

#include "manager.hpp"

#include "tier4_autoware_utils/geometry/boost_geometry.hpp"

#include <behavior_velocity_planner_common/utilization/util.hpp>
#include <lanelet2_extension/utility/utilities.hpp>
#include <tier4_autoware_utils/math/unit_conversion.hpp>
#include <tier4_autoware_utils/ros/parameter.hpp>

#include <boost/geometry/algorithms/intersects.hpp>

#include <lanelet2_core/geometry/LineString.h>

#include <memory>
#include <set>
#include <string>
Expand Down Expand Up @@ -51,13 +58,30 @@ 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 : planning_utils::getRegElemMapOnPath<VirtualTrafficLight>(
path, planner_data_->route_handler_->getLaneletMapPtr(),
planner_data_->current_odometry->pose)) {
const auto stop_line_opt = m.first->getStopLine();
if (!stop_line_opt) {
RCLCPP_FATAL(
logger_, "No stop line at 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 lane_id = m.second.id();
const auto module_id = lane_id;
if (!isModuleRegistered(module_id)) {
if (
!isModuleRegistered(module_id) &&
boost::geometry::intersects(
ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) {
registerModule(std::make_shared<VirtualTrafficLightModule>(
module_id, lane_id, *m.first, m.second, planner_param_,
logger_.get_child("virtual_traffic_light_module"), clock_));
Expand Down

0 comments on commit 08ea2de

Please sign in to comment.