diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 90bdbea51297a..80280a0946c0a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -114,6 +115,7 @@ class IntersectionModule : public SceneModuleInterface bool is_go_out_; // Parameter PlannerParam planner_param_; + std::optional intersection_lanelets_; /** * @brief check collision for all lanelet area & predicted objects (call checkPathCollision() as diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp index a2d884240747d..6db961152d80e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp @@ -95,6 +95,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface // Parameter PlannerParam planner_param_; + std::optional intersection_lanelets_; StateMachine state_machine_; //! for state 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 085de0c834ab7..6fa4d184239da 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -17,6 +17,7 @@ #include #include +#include #include @@ -52,17 +53,10 @@ std::optional getDuplicatedPointIdx( /** * @brief get objective polygons for detection area */ -std::tuple getObjectiveLanelets( +IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id, const double detection_area_length, const bool tl_arrow_solid_on = false); -struct StopLineIdx -{ - size_t first_inside_lane = 0; - size_t pass_judge_line = 0; - size_t stop_line = 0; -}; - /** * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map, * read it from the map; otherwise, generate a stop line at a position where it will not collide. @@ -73,13 +67,27 @@ struct StopLineIdx " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane * @return nullopt if path is not intersecting with detection areas */ -std::pair, std::optional> generateStopLine( +std::optional generateStopLine( const int lane_id, const std::vector & detection_areas, - const std::vector & conflicting_areas, + const std::shared_ptr & planner_data, const double stop_line_margin, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, + const std::pair lane_interaval, const rclcpp::Logger logger); + +/** + * @brief Generate a stop line for stuck vehicle + * @param conflicting_areas used to generate stop line for stuck vehicle + * @param original_path ego-car lane + * @param target_path target lane to insert stop point (part of ego-car lane or same to ego-car + * lane) + " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane + */ +std::optional generateStuckStopLine( + const int lane_id, const std::vector & conflicting_areas, const std::shared_ptr & planner_data, const double stop_line_margin, const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, + const std::pair lane_interval, const rclcpp::Logger logger); /** * @brief Calculate first path index that is in the polygon. @@ -136,7 +144,7 @@ bool isBeforeTargetIndex( const geometry_msgs::msg::Pose & current_pose, const int target_idx); lanelet::ConstLanelets extendedAdjacentDirectionLanes( - const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, + lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane); std::optional getIntersectionArea( diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp new file mode 100644 index 0000000000000..604cefd373496 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp @@ -0,0 +1,43 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ +#define SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ + +#include + +#include + +namespace behavior_velocity_planner::util +{ +struct IntersectionLanelets +{ + bool tl_arrow_solid_on; + lanelet::ConstLanelets attention; + lanelet::ConstLanelets conflicting; + lanelet::ConstLanelets adjacent; + std::vector attention_area; + std::vector conflicting_area; + std::vector adjacent_area; +}; + +struct StopLineIdx +{ + size_t pass_judge_line = 0; + size_t collision_stop_line = 0; +}; + +} // namespace behavior_velocity_planner::util + +#endif // SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ 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 0096b922ea75f..83099c46a6336 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 @@ -96,16 +96,19 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * /* get detection area*/ /* dynamically change detection area based on tl_arrow_solid_on */ - [[maybe_unused]] const bool has_tl = util::hasAssociatedTrafficLight(assigned_lanelet); const bool tl_arrow_solid_on = util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map); - auto && [detection_lanelets, conflicting_lanelets] = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - tl_arrow_solid_on); - const std::vector detection_area = - util::getPolygon3dFromLanelets(detection_lanelets, planner_param_.detection_area_length); - const std::vector conflicting_area = - util::getPolygon3dFromLanelets(conflicting_lanelets); + if ( + !intersection_lanelets_.has_value() || + intersection_lanelets_.value().tl_arrow_solid_on != tl_arrow_solid_on) { + intersection_lanelets_ = util::getObjectiveLanelets( + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, + tl_arrow_solid_on); + } + const auto & detection_lanelets = intersection_lanelets_.value().attention; + const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent; + const auto & detection_area = intersection_lanelets_.value().attention_area; + const auto & conflicting_area = intersection_lanelets_.value().conflicting_area; debug_data_.detection_area = detection_area; /* get intersection area */ @@ -115,25 +118,34 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } - /* get adjacent lanelets */ - const auto adjacent_lanelets = - util::extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); - debug_data_.adjacent_area = util::getPolygon3dFromLanelets(adjacent_lanelets); - - /* set stop lines for base_link */ - const auto [stuck_line_idx_opt, stop_lines_idx_opt] = util::generateStopLine( - lane_id_, detection_area, conflicting_area, planner_data_, planner_param_.stop_line_margin, - planner_param_.use_stuck_stopline, path, *path, logger_.get_child("util"), clock_); - if (!stuck_line_idx_opt.has_value()) { - // returns here if path is not intersecting with conflicting areas - RCLCPP_DEBUG_SKIPFIRST_THROTTLE( - logger_, *clock_, 1000 /* ms */, "setStopLineIdx for stuck line fail"); + /* spline interpolation */ + constexpr double interval = 0.2; + autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; + if (!splineInterpolate(*path, interval, path_ip, logger_)) { + RCLCPP_DEBUG_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "splineInterpolate failed"); RCLCPP_DEBUG(logger_, "===== plan end ====="); setSafe(true); setDistance(std::numeric_limits::lowest()); return false; } - const auto stuck_line_idx = stuck_line_idx_opt.value(); + const auto lane_interval_ip_opt = util::findLaneIdInterval(path_ip, lane_id_); + if (!lane_interval_ip_opt.has_value()) { + RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; + } + + const auto stuck_line_idx_opt = util::generateStuckStopLine( + lane_id_, conflicting_area, planner_data_, planner_param_.stop_line_margin, + planner_param_.use_stuck_stopline, path, path_ip, interval, lane_interval_ip_opt.value(), + logger_.get_child("util")); + + /* set stop lines for base_link */ + const auto stop_lines_idx_opt = util::generateStopLine( + lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin, path, path_ip, + interval, lane_interval_ip_opt.value(), logger_.get_child("util")); /* calc closest index */ const auto closest_idx_opt = @@ -149,7 +161,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const size_t closest_idx = closest_idx_opt.get(); if (stop_lines_idx_opt.has_value()) { - const auto stop_line_idx = stop_lines_idx_opt.value().stop_line; + const auto stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; const auto pass_judge_line_idx = stop_lines_idx_opt.value().pass_judge_line; const bool is_over_pass_judge_line = @@ -189,29 +201,35 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * closest_idx, stuck_vehicle_detect_area, time_delay); /* calculate final stop lines */ - int stop_line_idx_final = - stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().stop_line : -1; + std::optional stop_line_idx = std::nullopt; if (external_go) { is_entry_prohibited = false; } else if (external_stop) { is_entry_prohibited = true; - } else if (is_stuck || has_collision) { + stop_line_idx = stop_lines_idx_opt.has_value() + ? std::make_optional(stop_lines_idx_opt.value().collision_stop_line) + : std::nullopt; + } else if (is_stuck && stuck_line_idx_opt.has_value()) { is_entry_prohibited = true; const double dist_stuck_stopline = motion_utils::calcSignedArcLength( - path->points, path->points.at(stuck_line_idx).point.pose.position, + path->points, path->points.at(stuck_line_idx_opt.value()).point.pose.position, path->points.at(closest_idx).point.pose.position); const bool is_over_stuck_stopline = - util::isOverTargetIndex(*path, closest_idx, current_pose, stuck_line_idx) && + util::isOverTargetIndex(*path, closest_idx, current_pose, stuck_line_idx_opt.value()) && dist_stuck_stopline > planner_param_.stop_overshoot_margin; - if (is_stuck && !is_over_stuck_stopline) { - stop_line_idx_final = stuck_line_idx; - } else if ( - ((is_stuck && is_over_stuck_stopline) || has_collision) && stop_lines_idx_opt.has_value()) { - stop_line_idx_final = stop_lines_idx_opt.value().stop_line; + if (!is_over_stuck_stopline) { + stop_line_idx = stuck_line_idx_opt.value(); + } else if (is_over_stuck_stopline && stop_lines_idx_opt.has_value()) { + stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; } + } else if (has_collision) { + is_entry_prohibited = true; + stop_line_idx = stop_lines_idx_opt.has_value() + ? std::make_optional(stop_lines_idx_opt.value().collision_stop_line) + : std::nullopt; } - if (stop_line_idx_final == -1) { + if (!stop_line_idx.has_value()) { RCLCPP_DEBUG(logger_, "detection_area is empty, no plan needed"); RCLCPP_DEBUG(logger_, "===== plan end ====="); setSafe(true); @@ -226,23 +244,23 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * setSafe(state_machine_.getState() == StateMachine::State::GO); setDistance(motion_utils::calcSignedArcLength( path->points, planner_data_->current_odometry->pose.position, - path->points.at(stop_line_idx_final).point.pose.position)); + path->points.at(stop_line_idx.value()).point.pose.position)); if (!isActivated()) { // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block is_go_out_ = false; constexpr double v = 0.0; - planning_utils::setVelocityFromIndex(stop_line_idx_final, v, path); + planning_utils::setVelocityFromIndex(stop_line_idx.value(), v, path); debug_data_.stop_required = true; const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx_final, base_link2front, *path); + planning_utils::getAheadPose(stop_line_idx.value(), base_link2front, *path); // Get stop point and stop factor { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx_final).point.pose; + stop_factor.stop_pose = path->points.at(stop_line_idx.value()).point.pose; const auto stop_factor_conflict = planning_utils::toRosPoints(debug_data_.conflicting_targets); const auto stop_factor_stuck = planning_utils::toRosPoints(debug_data_.stuck_targets); @@ -250,7 +268,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * planning_utils::concatVector(stop_factor_conflict, stop_factor_stuck); planning_utils::appendStopReason(stop_factor, stop_reason); - const auto & stop_pose = path->points.at(stop_line_idx_final).point.pose; + const auto & stop_pose = path->points.at(stop_line_idx.value()).point.pose; velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); } 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 d6084a5d8ce92..a395fc00d5153 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 @@ -64,32 +64,42 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* get detection area */ - auto && [detection_lanelets, conflicting_lanelets] = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - false /* tl_arrow_solid on does not matter here*/); - if (detection_lanelets.empty()) { - RCLCPP_DEBUG(logger_, "no detection area. skip computation."); - return true; + if (!intersection_lanelets_.has_value()) { + intersection_lanelets_ = util::getObjectiveLanelets( + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, + false /* tl_arrow_solid on does not matter here*/); } - const auto detection_area = - util::getPolygon3dFromLanelets(detection_lanelets, planner_param_.detection_area_length); - const std::vector conflicting_area = - util::getPolygon3dFromLanelets(conflicting_lanelets); - debug_data_.detection_area = detection_area; + const auto & detection_area = intersection_lanelets_.value().attention_area; /* set stop-line and stop-judgement-line for base_link */ - const auto private_path = - extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m); - const auto [stuck_line_idx_opt, stop_lines_idx_opt] = util::generateStopLine( - lane_id_, detection_area, conflicting_area, planner_data_, planner_param_.stop_line_margin, - false /* same */, path, *path, logger_.get_child("util"), clock_); + /* spline interpolation */ + constexpr double interval = 0.2; + autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; + if (!splineInterpolate(*path, interval, path_ip, logger_)) { + RCLCPP_DEBUG_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "splineInterpolate failed"); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; + } + const auto lane_interval_ip_opt = util::findLaneIdInterval(path_ip, lane_id_); + if (!lane_interval_ip_opt.has_value()) { + RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; + } + + const auto stop_lines_idx_opt = util::generateStopLine( + lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin, path, path_ip, + interval, lane_interval_ip_opt.value(), logger_.get_child("util")); if (!stop_lines_idx_opt.has_value()) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); return false; } - const auto & stop_lines_idx = stop_lines_idx_opt.value(); - const size_t stop_line_idx = stop_lines_idx.stop_line; + const size_t stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; if (stop_line_idx == 0) { RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); return true; @@ -98,8 +108,6 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR debug_data_.virtual_wall_pose = planning_utils::getAheadPose( stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; - const size_t first_inside_lane_idx = stop_lines_idx.first_inside_lane; - debug_data_.first_collision_point = path->points.at(first_inside_lane_idx).point.pose.position; /* set stop speed */ if (state_machine_.getState() == StateMachine::State::STOP) { @@ -109,7 +117,6 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR /* get stop point and stop factor */ tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = debug_data_.stop_point_pose; - stop_factor.stop_factor_points.emplace_back(debug_data_.first_collision_point); planning_utils::appendStopReason(stop_factor, stop_reason); const auto & stop_pose = path->points.at(stop_line_idx).point.pose; velocity_factor_.set( 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 c047c7814e018..e4ae13c6ce7ce 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -141,13 +141,12 @@ std::optional getFirstPointInsidePolygons( return first_idx_inside_lanelet; } -std::pair, std::optional> generateStopLine( +std::optional generateStopLine( const int lane_id, const std::vector & detection_areas, - const std::vector & conflicting_areas, const std::shared_ptr & planner_data, const double stop_line_margin, - const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger, - [[maybe_unused]] const rclcpp::Clock::SharedPtr clock) + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, + const std::pair lane_interval_ip, const rclcpp::Logger logger) { /* set judge line dist */ const double current_vel = planner_data->current_velocity->twist.linear.x; @@ -158,81 +157,15 @@ std::pair, std::optional> generateStopLine( const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithJerkLimit( current_vel, current_acc, max_acc, max_jerk, delay_response_time); - // first inside lane idx - const auto first_inside_lane_it = - std::find_if(original_path->points.begin(), original_path->points.end(), [&](const auto & p) { - return std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end(); - }); - if (first_inside_lane_it == original_path->points.end()) { - RCLCPP_ERROR(logger, "No points on intersection lane %d", lane_id); - return {std::nullopt, std::nullopt}; - } - const size_t first_inside_lane_idx = - std::distance(original_path->points.begin(), first_inside_lane_it); - - /* spline interpolation */ - constexpr double interval = 0.2; - autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; - if (!splineInterpolate(target_path, interval, path_ip, logger)) { - return {std::nullopt, std::nullopt}; - } - const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval); - const auto lane_interval_ip_opt = util::findLaneIdInterval(path_ip, lane_id); - if (!lane_interval_ip_opt.has_value()) { - RCLCPP_WARN(logger, "Path has no interval on intersection lane %d", lane_id); - return {std::nullopt, std::nullopt}; - } - const auto [lane_interval_ip_start, lane_interval_ip_end] = lane_interval_ip_opt.value(); - - /* generate stuck stop line */ - size_t stuck_stop_line_idx_ip = 0; - if (use_stuck_stopline) { - // the first point in intersection lane - stuck_stop_line_idx_ip = lane_interval_ip_start; - } else { - const auto stuck_stop_line_idx_ip_opt = util::getFirstPointInsidePolygons( - path_ip, lane_interval_ip_start, lane_interval_ip_end, lane_id, conflicting_areas); - if (!stuck_stop_line_idx_ip_opt.has_value()) { - RCLCPP_DEBUG( - logger, - "Path is not intersecting with conflicting area, not generating stuck_stop_line. start = " - "%ld, end = %ld", - lane_interval_ip_start, lane_interval_ip_end); - return {std::nullopt, std::nullopt}; - } - stuck_stop_line_idx_ip = stuck_stop_line_idx_ip_opt.value(); - } - - size_t stuck_stop_line_idx = 0; - { - /* insert stuck stop line */ - const size_t insert_idx_ip = static_cast(std::max( - static_cast(stuck_stop_line_idx_ip) - 1 - stop_line_margin_idx_dist - - base2front_idx_dist, - 0)); - const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose; - const auto duplicate_idx_opt = - util::getDuplicatedPointIdx(*original_path, insert_point.position); - if (duplicate_idx_opt.has_value()) { - stuck_stop_line_idx = duplicate_idx_opt.value(); - } else { - const auto insert_idx_opt = util::insertPoint(insert_point, original_path); - if (!insert_idx_opt.has_value()) { - RCLCPP_WARN(logger, "insertPoint failed for stuck stop line"); - return {std::nullopt, std::nullopt}; - } - stuck_stop_line_idx = insert_idx_opt.value(); - } - } + const auto [lane_interval_ip_start, lane_interval_ip_end] = lane_interval_ip; /* generate stop points */ util::StopLineIdx idxs; - idxs.first_inside_lane = first_inside_lane_idx; // If a stop_line tag is defined on lanelet_map, use it. // else generate a stop_line behind the intersection of path and detection area @@ -251,7 +184,7 @@ std::pair, std::optional> generateStopLine( if (!first_inside_detection_idx_ip_opt.has_value()) { RCLCPP_DEBUG( logger, "Path is not intersecting with detection_area, not generating stop_line"); - return {stuck_stop_line_idx, std::nullopt}; + return std::nullopt; } const auto first_inside_detection_idx_ip = first_inside_detection_idx_ip_opt.value(); @@ -262,7 +195,7 @@ std::pair, std::optional> generateStopLine( } if (stop_idx_ip == 0) { RCLCPP_DEBUG(logger, "stop line is at path[0], ignore planning\n===== plan end ====="); - return {stuck_stop_line_idx, std::nullopt}; + return std::nullopt; } { @@ -271,19 +204,19 @@ std::pair, std::optional> generateStopLine( const auto duplicate_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position); if (duplicate_idx_opt.has_value()) { - idxs.stop_line = duplicate_idx_opt.value(); + idxs.collision_stop_line = duplicate_idx_opt.value(); } else { const auto insert_idx_opt = util::insertPoint(insert_point, original_path); if (!insert_idx_opt.has_value()) { RCLCPP_WARN(logger, "insertPoint failed for stop line"); - return {stuck_stop_line_idx, std::nullopt}; + return std::nullopt; } - idxs.stop_line = insert_idx_opt.value(); + idxs.collision_stop_line = insert_idx_opt.value(); } } const bool has_prior_stopline = std::any_of( - original_path->points.begin(), original_path->points.begin() + idxs.stop_line, + original_path->points.begin(), original_path->points.begin() + idxs.collision_stop_line, [](const auto & p) { return std::fabs(p.point.longitudinal_velocity_mps) < 0.1; }); /* insert judge point */ @@ -292,7 +225,7 @@ std::pair, std::optional> generateStopLine( std::max(static_cast(stop_idx_ip) - pass_judge_idx_dist, 0))); /* if another stop point exist before intersection stop_line, disable judge_line. */ if (has_prior_stopline || pass_judge_idx_ip == stop_idx_ip) { - idxs.pass_judge_line = idxs.stop_line; + idxs.pass_judge_line = idxs.collision_stop_line; } else { const auto & insert_point = path_ip.points.at(pass_judge_idx_ip).point.pose; const auto duplicate_idx_opt = @@ -303,24 +236,68 @@ std::pair, std::optional> generateStopLine( const auto insert_idx_opt = util::insertPoint(insert_point, original_path); if (!insert_idx_opt.has_value()) { RCLCPP_WARN(logger, "insertPoint failed to pass judge line"); - return {stuck_stop_line_idx, std::nullopt}; + return std::nullopt; } idxs.pass_judge_line = insert_idx_opt.value(); - idxs.stop_line = std::min(idxs.stop_line + 1, original_path->points.size() - 1); - if (stuck_stop_line_idx >= idxs.pass_judge_line) { - stuck_stop_line_idx = - std::min(stuck_stop_line_idx + 1, original_path->points.size() - 1); - } + idxs.collision_stop_line = + std::min(idxs.collision_stop_line + 1, original_path->points.size() - 1); } } RCLCPP_DEBUG( logger, "generateStopLine() : stop_idx = %ld, pass_judge_idx = %ld" - ", stuck_stop_idx = %ld, has_prior_stopline = %d", - idxs.stop_line, idxs.pass_judge_line, stuck_stop_line_idx, has_prior_stopline); + ", has_prior_stopline = %d", + idxs.collision_stop_line, idxs.pass_judge_line, has_prior_stopline); + + return std::make_optional(idxs); +} + +std::optional generateStuckStopLine( + const int lane_id, const std::vector & conflicting_areas, + const std::shared_ptr & planner_data, const double stop_line_margin, + const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, + const std::pair lane_interval_ip, const rclcpp::Logger logger) +{ + size_t stuck_stop_line_idx_ip = 0; + const auto [lane_interval_ip_start, lane_interval_ip_end] = lane_interval_ip; + if (use_stuck_stopline) { + stuck_stop_line_idx_ip = lane_interval_ip_start; + } else { + const auto stuck_stop_line_idx_ip_opt = util::getFirstPointInsidePolygons( + path_ip, lane_interval_ip_start, lane_interval_ip_end, lane_id, conflicting_areas); + if (!stuck_stop_line_idx_ip_opt.has_value()) { + RCLCPP_DEBUG( + logger, + "Path is not intersecting with conflicting area, not generating stuck_stop_line. start = " + "%ld, end = %ld", + lane_interval_ip_start, lane_interval_ip_end); + return std::nullopt; + } + stuck_stop_line_idx_ip = stuck_stop_line_idx_ip_opt.value(); + } - return {stuck_stop_line_idx, std::make_optional(idxs)}; + const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval); + const int base2front_idx_dist = + std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); + size_t stuck_stop_line_idx = 0; + const size_t insert_idx_ip = static_cast(std::max( + static_cast(stuck_stop_line_idx_ip) - 1 - stop_line_margin_idx_dist - base2front_idx_dist, + 0)); + const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose; + const auto duplicate_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position); + if (duplicate_idx_opt.has_value()) { + stuck_stop_line_idx = duplicate_idx_opt.value(); + return std::make_optional(stuck_stop_line_idx); + } else { + const auto insert_idx_opt = util::insertPoint(insert_point, original_path); + if (!insert_idx_opt.has_value()) { + RCLCPP_WARN(logger, "insertPoint failed for stuck stop line"); + return std::nullopt; + } + return std::make_optional(insert_idx_opt.value()); + } } bool getStopLineIndexFromMap( @@ -387,7 +364,7 @@ bool getStopLineIndexFromMap( return true; } -std::tuple getObjectiveLanelets( +IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id, const double detection_area_length, const bool tl_arrow_solid_on) { @@ -461,9 +438,9 @@ std::tuple getObjectiveLanelets( // get possible lanelet path that reaches conflicting_lane longer than given length // if traffic light arrow is active, this process is unnecessary + lanelet::ConstLanelets detection_and_preceding_lanelets; 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 @@ -480,10 +457,22 @@ std::tuple getObjectiveLanelets( } } } - return {std::move(detection_and_preceding_lanelets), std::move(conflicting_ex_ego_lanelets)}; - } else { - return {std::move(detection_lanelets), std::move(conflicting_ex_ego_lanelets)}; } + + IntersectionLanelets result; + if (!tl_arrow_solid_on) { + result.attention = std::move(detection_and_preceding_lanelets); + } else { + result.attention = std::move(detection_lanelets); + } + result.conflicting = std::move(conflicting_ex_ego_lanelets); + result.adjacent = + extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); + // compoundPolygon3d + result.attention_area = getPolygon3dFromLanelets(result.attention); + result.conflicting_area = getPolygon3dFromLanelets(result.conflicting); + result.adjacent_area = getPolygon3dFromLanelets(result.adjacent); + return result; } std::vector getPolygon3dFromLanelets( @@ -574,7 +563,7 @@ static std::vector getAllAdjacentLanelets( } lanelet::ConstLanelets extendedAdjacentDirectionLanes( - const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, + lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane) { // some of the intersections are not well-formed, and "adjacent" turning