diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 1992adbf34c80..f5e263f7dc0c8 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -26,6 +26,7 @@ #include #endif +#include #include #include #include @@ -47,11 +48,10 @@ bool getDuplicatedPointIdx( /** * @brief get objective polygons for detection area */ -bool getObjectiveLanelets( +bool getDetectionLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id, const double detection_area_length, - std::vector * conflicting_lanelets_result, - lanelet::ConstLanelets * objective_lanelets_result, const rclcpp::Logger logger); + lanelet::ConstLanelets * detection_lanelets_result, const bool tl_arrow_solid_on = false); struct StopLineIdx { @@ -149,6 +149,10 @@ std::vector extendedAdjacentDirectionLanes( std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); +bool isTrafficLightArrowActivated( + lanelet::ConstLanelet lane, + const std::map & tl_infos); + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index d3bd968842adb..47361e6f5d664 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -91,19 +91,18 @@ bool IntersectionModule::modifyPathVelocity( const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); - /* get detection area and conflicting area */ - lanelet::ConstLanelets detection_area_lanelets; - std::vector conflicting_area_lanelets; + const auto & assigned_lanelet = + planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); - util::getObjectiveLanelets( + /* get detection area*/ + lanelet::ConstLanelets detection_area_lanelets; + const bool tl_arrow_solid_on = + util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map); + util::getDetectionLanelets( lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - &conflicting_area_lanelets, &detection_area_lanelets, logger_); - std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( - conflicting_area_lanelets, planner_param_.detection_area_length); + &detection_area_lanelets, tl_arrow_solid_on); std::vector detection_areas = util::getPolygon3dFromLanelets(detection_area_lanelets, planner_param_.detection_area_length); - std::vector conflicting_area_lanelet_ids = - util::getLaneletIdsFromLaneletsVec(conflicting_area_lanelets); std::vector detection_area_lanelet_ids = util::getLaneletIdsFromLaneletsVec({detection_area_lanelets}); @@ -116,8 +115,6 @@ bool IntersectionModule::modifyPathVelocity( debug_data_.detection_area = detection_areas; /* get intersection area */ - const auto & assigned_lanelet = - planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); if (intersection_area) { const auto intersection_area_2d = intersection_area.value(); @@ -136,7 +133,7 @@ bool IntersectionModule::modifyPathVelocity( /* set stop-line and stop-judgement-line for base_link */ util::StopLineIdx stop_line_idxs; if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, + lane_id_, detection_areas, planner_data_, planner_param_.stop_line_margin, planner_param_.keep_detection_line_margin, path, *path, &stop_line_idxs, logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index 61f587396d444..62a7ef9531a38 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -67,25 +67,23 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( /* get detection area */ lanelet::ConstLanelets detection_area_lanelets; - std::vector conflicting_area_lanelets; - - util::getObjectiveLanelets( + util::getDetectionLanelets( lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - &conflicting_area_lanelets, &detection_area_lanelets, logger_); - std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( - conflicting_area_lanelets, planner_param_.detection_area_length); - if (conflicting_areas.empty()) { + &detection_area_lanelets); + std::vector detection_areas = + util::getPolygon3dFromLanelets(detection_area_lanelets, planner_param_.detection_area_length); + if (detection_areas.empty()) { RCLCPP_DEBUG(logger_, "no detection area. skip computation."); return true; } - debug_data_.detection_area = conflicting_areas; + debug_data_.detection_area = detection_areas; /* set stop-line and stop-judgement-line for base_link */ util::StopLineIdx stop_line_idxs; const auto private_path = extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m); if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, + lane_id_, detection_areas, planner_data_, planner_param_.stop_line_margin, 0.0 /* unnecessary in merge_from_private */, path, private_path, &stop_line_idxs, logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 2ea4c3b870b87..25d1b6a73ecb8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -309,14 +309,23 @@ bool getStopPoseIndexFromMap( return true; } -bool getObjectiveLanelets( +bool getDetectionLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id, const double detection_area_length, - std::vector * conflicting_lanelets_result, - lanelet::ConstLanelets * objective_lanelets_result, const rclcpp::Logger logger) + lanelet::ConstLanelets * detection_lanelets_result, const bool tl_arrow_solid_on) { const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); + const auto tl_regelems = assigned_lanelet.regulatoryElementsAs(); + bool has_tl = false; + if (tl_regelems.size() != 0) { + const auto tl_regelem = tl_regelems.front(); + const auto stop_line_opt = tl_regelem->stopLine(); + if (!!stop_line_opt) has_tl = true; + } + + const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); + lanelet::ConstLanelets yield_lanelets; // for low priority lane @@ -351,72 +360,49 @@ bool getObjectiveLanelets( const auto & conflicting_lanelets = lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); - std::vector // conflicting lanes with "lane_id" - conflicting_lanelets_ex_yield_ego; // excluding ego lanes and yield lanes - std::vector objective_lanelets; // final objective lanelets + lanelet::ConstLanelets detection_lanelets; // final objective lanelets - // exclude yield lanelets and ego lanelets from objective_lanelets - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { - continue; - } - if (lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { - continue; + // exclude yield lanelets and ego lanelets from detection_lanelets + // if assigned lanelet is "straight" with traffic light, detection area is not necessary + if (turn_direction == std::string("straight") && has_tl) { + } else { + // otherwise we need to know the priority from RightOfWay + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { + continue; + } + if (lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); } - conflicting_lanelets_ex_yield_ego.push_back({conflicting_lanelet}); - objective_lanelets.push_back({conflicting_lanelet}); } // get possible lanelet path that reaches conflicting_lane longer than given length - const double length = detection_area_length; - lanelet::ConstLanelets objective_and_preceding_lanelets; - std::set objective_ids; - for (const auto & ll : objective_lanelets) { - // Preceding lanes does not include objective_lane so add them at the end - for (const auto & l : ll) { - const auto & inserted = objective_ids.insert(l.id()); - if (inserted.second) objective_and_preceding_lanelets.push_back(l); - } - // get preceding lanelets without ego_lanelets - // to prevent the detection area from including the ego lanes and its' preceding lanes. - const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( - routing_graph_ptr, ll.front(), length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = objective_ids.insert(l.id()); - if (inserted.second) objective_and_preceding_lanelets.push_back(l); + // if traffic light arrow is active, this process is unnecessary + if (!tl_arrow_solid_on) { + const double length = detection_area_length; + lanelet::ConstLanelets detection_and_preceding_lanelets; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(l); + } } } + *detection_lanelets_result = detection_and_preceding_lanelets; + } else { + *detection_lanelets_result = detection_lanelets; } - - *conflicting_lanelets_result = conflicting_lanelets_ex_yield_ego; - *objective_lanelets_result = objective_and_preceding_lanelets; - - // set this flag true when debugging - const bool is_debug = false; - if (!is_debug) return true; - std::stringstream ss_c, ss_y, ss_e, ss_o, ss_os; - for (const auto & l : conflicting_lanelets) { - ss_c << l.id() << ", "; - } - for (const auto & l : yield_lanelets) { - ss_y << l.id() << ", "; - } - for (const auto & l : ego_lanelets) { - ss_e << l.id() << ", "; - } - for (const auto & l : objective_lanelets) { - ss_o << l.front().id() << ", "; - } - for (const auto & l : objective_and_preceding_lanelets) { - ss_os << l.id() << ", "; - } - RCLCPP_INFO( - logger, "getObjectiveLanelets() conflict = %s yield = %s ego = %s", ss_c.str().c_str(), - ss_y.str().c_str(), ss_e.str().c_str()); - RCLCPP_INFO( - logger, "getObjectiveLanelets() object = %s object_sequences = %s", ss_o.str().c_str(), - ss_os.str().c_str()); return true; } @@ -676,5 +662,40 @@ std::optional getIntersectionArea( return std::make_optional(poly); } +bool isTrafficLightArrowActivated( + lanelet::ConstLanelet lane, + const std::map & tl_infos) +{ + const auto & turn_direction = lane.attributeOr("turn_direction", "else"); + boost::optional tl_id = boost::none; + for (auto && tl_regelem : lane.regulatoryElementsAs()) { + tl_id = tl_regelem->id(); + break; + } + if (!tl_id) { + // this lane has no traffic light + return false; + } + const auto tl_info_it = tl_infos.find(tl_id.get()); + if (tl_info_it == tl_infos.end()) { + // the info of this traffic light is not available + return false; + } + const auto & tl_info = tl_info_it->second; + for (auto && tl_light : tl_info.signal.lights) { + if (tl_light.color != autoware_auto_perception_msgs::msg::TrafficLight::GREEN) continue; + if (tl_light.status != autoware_auto_perception_msgs::msg::TrafficLight::SOLID_ON) continue; + if ( + turn_direction == std::string("left") && + tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW) + return true; + if ( + turn_direction == std::string("right") && + tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW) + return true; + } + return false; +} + } // namespace util } // namespace behavior_velocity_planner