From b4966b860756e91c4daeffe14ed0312f34e07fe0 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 6 Dec 2022 19:41:32 +0900 Subject: [PATCH 1/4] use ignore dist before the exist of ego lane, and extend_dist after the ego lane Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.hpp | 2 +- .../intersection/scene_intersection.cpp | 25 +++++++++++-------- 2 files changed, 15 insertions(+), 12 deletions(-) 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 8decae844adb7..c1097f39bbf01 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 @@ -166,7 +166,7 @@ class IntersectionModule : public SceneModuleInterface Polygon2d generateEgoIntersectionLanePolygon( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const int start_idx, const double extra_dist, const double ignore_dist) const; + const double extra_dist, const double ignore_dist) const; /** * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. 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 d3c20427f118e..e944d707ae699 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 @@ -188,11 +188,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * /* calculate final stop lines */ bool is_entry_prohibited = false; - const double detect_length = + const double ignore_length = planner_param_.stuck_vehicle_detect_dist + planner_data_->vehicle_info_.vehicle_length_m; const auto stuck_vehicle_detect_area = generateEgoIntersectionLanePolygon( - lanelet_map_ptr, *path, closest_idx, stuck_line_idx, detect_length, - planner_param_.stuck_vehicle_detect_dist); + lanelet_map_ptr, *path, closest_idx, planner_param_.stuck_vehicle_detect_dist, ignore_length); const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); int stop_line_idx_final = stuck_line_idx; int pass_judge_line_idx_final = stuck_line_idx; @@ -318,8 +317,13 @@ bool IntersectionModule::checkCollision( using lanelet::utils::getPolygonFromArcLength; /* generate ego-lane polygon */ - const auto ego_poly = - generateEgoIntersectionLanePolygon(lanelet_map_ptr, path, closest_idx, closest_idx, 0.0, 0.0); + const auto ego_lane_poly = lanelet_map_ptr->laneletLayer.get(module_id_).polygon2d(); + Polygon2d ego_poly{}; + for (const auto & p : ego_lane_poly) { + ego_poly.outer().emplace_back(p.x(), p.y()); + } + // const auto ego_poly = + // generateEgoIntersectionLanePolygon(lanelet_map_ptr, path, closest_idx, closest_idx, 0.0, 0.0); lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); lanelet::ConstLanelet closest_lanelet; lanelet::utils::query::getClosestLanelet( @@ -479,7 +483,7 @@ bool IntersectionModule::checkCollision( Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const int start_idx, const double extra_dist, const double ignore_dist) const + const double extra_dist, const double ignore_dist) const { using lanelet::utils::getArcCoordinates; using lanelet::utils::getLaneletLength3d; @@ -488,15 +492,14 @@ Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); - const auto start_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(start_idx).point)); + const double intersection_exit_length = getLaneletLength3d(ego_lane_with_next_lane.front()); const auto closest_arc_coords = getArcCoordinates( ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - const double start_arc_length = start_arc_coords.length + ignore_dist < closest_arc_coords.length - ? closest_arc_coords.length - : start_arc_coords.length + ignore_dist; + const double start_arc_length = intersection_exit_length - ignore_dist > closest_arc_coords.length + ? intersection_exit_length - ignore_dist + : closest_arc_coords.length; const double end_arc_length = getLaneletLength3d(ego_lane_with_next_lane.front()) + extra_dist; From d08b631902a8b27e46138646655803dbaa12d142 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 19 Dec 2022 11:53:35 +0900 Subject: [PATCH 2/4] do not generate stopline overly before ego Signed-off-by: Mamoru Sobue --- .../intersection.param.yaml | 2 +- .../config/intersection.param.yaml | 2 +- .../intersection/scene_intersection.cpp | 122 ++++++++++-------- 3 files changed, 67 insertions(+), 59 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 79eb257de6889..220c087d18de0 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -6,7 +6,7 @@ keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr keep_detection_vel_thr: 0.833 # == 3.0km/h stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) + stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h intersection_velocity: 2.778 # 2.778m/s = 10.0km/h intersection_max_accel: 0.5 # m/ss diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 79eb257de6889..220c087d18de0 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -6,7 +6,7 @@ keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr keep_detection_vel_thr: 0.833 # == 3.0km/h stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) + stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h intersection_velocity: 2.778 # 2.778m/s = 10.0km/h intersection_max_accel: 0.5 # m/ss 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 e944d707ae699..ee217c696b60b 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 @@ -137,6 +137,15 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } const auto stuck_line_idx = stuck_line_idx_opt.value(); + if (!stop_lines_idx_opt.has_value()) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, *clock_, 1000 /* ms*/, + "generateStopLine failed, maybe the lane is not intersecting with any other lanes"); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; + } + /* calc closest index */ const auto closest_idx_opt = motion_utils::findNearestIndex(path->points, current_pose.pose, 3.0, M_PI_4); @@ -150,49 +159,23 @@ 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_lines = stop_lines_idx_opt.value(); - const size_t stop_line_idx = stop_lines.stop_line; - const size_t pass_judge_line_idx = stop_lines.pass_judge_line; - const size_t keep_detection_line_idx = stop_lines.keep_detection_line; - - const bool is_over_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx); - const bool is_before_keep_detection_line = - util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx); - const bool keep_detection = is_before_keep_detection_line && - std::fabs(current_vel) < planner_param_.keep_detection_vel_thr; - - if (is_over_pass_judge_line && keep_detection) { - // in case ego could not stop exactly before the stop line, but with some overshoot, - // keep detection within some margin under low velocity threshold - RCLCPP_DEBUG( - logger_, - "over the pass judge line, but before keep detection line and low speed, " - "continue planning"); - // no return here, keep planning - } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) { - RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed."); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_pose.pose.position, - path->points.at(stop_line_idx).point.pose.position)); - // no plan needed. - return true; - } - } + /* collision checking */ + bool is_entry_prohibited = false; /* get dynamic object */ const auto objects_ptr = planner_data_->predicted_objects; - /* calculate final stop lines */ - bool is_entry_prohibited = false; + /* check stuck vehicle */ const double ignore_length = + planner_param_.stuck_vehicle_ignore_dist + planner_data_->vehicle_info_.vehicle_length_m; + const double detect_dist = planner_param_.stuck_vehicle_detect_dist + planner_data_->vehicle_info_.vehicle_length_m; const auto stuck_vehicle_detect_area = generateEgoIntersectionLanePolygon( - lanelet_map_ptr, *path, closest_idx, planner_param_.stuck_vehicle_detect_dist, ignore_length); + lanelet_map_ptr, *path, closest_idx, detect_dist, ignore_length); const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); + + /* calculate final stop lines */ + const auto & stop_lines_idx = stop_lines_idx_opt.value(); int stop_line_idx_final = stuck_line_idx; int pass_judge_line_idx_final = stuck_line_idx; if (external_go) { @@ -201,33 +184,54 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * is_entry_prohibited = true; } else if (is_stuck) { is_entry_prohibited = true; - stop_line_idx_final = stuck_line_idx; - pass_judge_line_idx_final = stuck_line_idx; + const double dist_stuck_stopline = motion_utils::calcSignedArcLength( + path->points, current_pose.pose.position, + path->points.at(stuck_line_idx).point.pose.position); + const bool is_over_stuck_stopline = + util::isOverTargetIndex(*path, closest_idx, current_pose.pose, stuck_line_idx) && + dist_stuck_stopline > 5e-2; + if (is_over_stuck_stopline) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, *clock_, 1000, + "detected stuck vehicle after passing stuck_vehicle_stopline, stop in the intersection"); + stop_line_idx_final = stop_lines_idx.stop_line; + pass_judge_line_idx_final = stop_lines_idx.pass_judge_line; + std::cout << "is_over_stuck_stopline" << std::endl; + } else { + stop_line_idx_final = stuck_line_idx; + pass_judge_line_idx_final = stuck_line_idx; + std::cout << "not is_over_stuck_stopline" << std::endl; + } } else { /* calculate dynamic collision around detection area */ const bool has_collision = checkCollision( lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr, closest_idx, stuck_vehicle_detect_area); - is_entry_prohibited = has_collision; - if (stop_lines_idx_opt.has_value()) { - const auto & stop_lines_idx = stop_lines_idx_opt.value(); - stop_line_idx_final = stop_lines_idx.stop_line; - pass_judge_line_idx_final = stop_lines_idx.pass_judge_line; - } else { - if (has_collision) { - RCLCPP_ERROR(logger_, "generateStopLine() failed for detected objects"); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return false; - } else { - RCLCPP_DEBUG(logger_, "no need to stop"); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return true; - } - } + is_entry_prohibited |= has_collision; + stop_line_idx_final = stop_lines_idx.stop_line; + pass_judge_line_idx_final = stop_lines_idx.pass_judge_line; + } + + const bool is_over_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx_final); + const bool is_before_keep_detection_line = util::isBeforeTargetIndex( + *path, closest_idx, current_pose.pose, stop_lines_idx.keep_detection_line); + const bool keep_detection = + is_before_keep_detection_line && std::fabs(current_vel) < planner_param_.keep_detection_vel_thr; + + if (is_over_pass_judge_line && keep_detection) { + // in case ego could not stop exactly before the stop line, but with some overshoot, + // keep detection within some margin under low velocity threshold + std::cout << "is_over_pass_judge_line && keep_detection" << std::endl; + } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) { + std::cout << "is_over_pass_judge_line && is_go_out_ && !external_stop" << std::endl; + RCLCPP_WARN(logger_, "over the keep_detection line and not low speed. no plan needed."); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(motion_utils::calcSignedArcLength( + path->points, planner_data_->current_pose.pose.position, + path->points.at(stop_line_idx_final).point.pose.position)); + return true; } state_machine_.setStateWithMarginTime( @@ -235,6 +239,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * logger_.get_child("state_machine"), *clock_); setSafe(state_machine_.getState() == StateMachine::State::GO); + std::cout << "state_machine_.getState == GO: " + << (state_machine_.getState() == StateMachine::State::GO) << std::endl; if (is_entry_prohibited) { setDistance(motion_utils::calcSignedArcLength( path->points, planner_data_->current_pose.pose.position, @@ -242,8 +248,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } else { setDistance(std::numeric_limits::lowest()); } + std::cout << "is_entry_prohibited: " << is_entry_prohibited << std::endl; if (!isActivated()) { + std::cout << "!isActivated()" << std::endl; // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block is_go_out_ = false; From ce0500c030af319abc66afc7e85a1c8f74b8f922 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 19 Dec 2022 17:48:32 +0900 Subject: [PATCH 3/4] (1) fixed stuck vehicle detect size to smaller (2) do not generate stuck stopline way before ego, and generate it in the intersection instead Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.cpp | 85 ++++++++----------- 1 file changed, 34 insertions(+), 51 deletions(-) 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 ee217c696b60b..01a38f42f4d49 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 @@ -137,15 +137,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } const auto stuck_line_idx = stuck_line_idx_opt.value(); - if (!stop_lines_idx_opt.has_value()) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger_, *clock_, 1000 /* ms*/, - "generateStopLine failed, maybe the lane is not intersecting with any other lanes"); - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return false; - } - /* calc closest index */ const auto closest_idx_opt = motion_utils::findNearestIndex(path->points, current_pose.pose, 3.0, M_PI_4); @@ -174,58 +165,60 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * lanelet_map_ptr, *path, closest_idx, detect_dist, ignore_length); const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); + /* calculate dynamic collision around detection area */ + const bool has_collision = checkCollision( + lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr, + closest_idx, stuck_vehicle_detect_area); + /* calculate final stop lines */ - const auto & stop_lines_idx = stop_lines_idx_opt.value(); - int stop_line_idx_final = stuck_line_idx; - int pass_judge_line_idx_final = stuck_line_idx; + int stop_line_idx_final = -1; + int pass_judge_line_idx_final = -1; if (external_go) { is_entry_prohibited = false; } else if (external_stop) { is_entry_prohibited = true; - } else if (is_stuck) { + } else if (is_stuck || has_collision) { is_entry_prohibited = true; const double dist_stuck_stopline = motion_utils::calcSignedArcLength( - path->points, current_pose.pose.position, - path->points.at(stuck_line_idx).point.pose.position); + path->points, path->points.at(stuck_line_idx).point.pose.position, + path->points.at(closest_idx).point.pose.position); + const double eps = 1e-1; // NOTE: check if sufficiently over the stuck stopline const bool is_over_stuck_stopline = util::isOverTargetIndex(*path, closest_idx, current_pose.pose, stuck_line_idx) && - dist_stuck_stopline > 5e-2; - if (is_over_stuck_stopline) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger_, *clock_, 1000, - "detected stuck vehicle after passing stuck_vehicle_stopline, stop in the intersection"); - stop_line_idx_final = stop_lines_idx.stop_line; - pass_judge_line_idx_final = stop_lines_idx.pass_judge_line; - std::cout << "is_over_stuck_stopline" << std::endl; - } else { + dist_stuck_stopline > eps; + if (is_stuck && !is_over_stuck_stopline) { stop_line_idx_final = stuck_line_idx; pass_judge_line_idx_final = stuck_line_idx; - std::cout << "not is_over_stuck_stopline" << std::endl; + } 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; + pass_judge_line_idx_final = stop_lines_idx_opt.value().pass_judge_line; } - } else { - /* calculate dynamic collision around detection area */ - const bool has_collision = checkCollision( - lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr, - closest_idx, stuck_vehicle_detect_area); - is_entry_prohibited |= has_collision; - stop_line_idx_final = stop_lines_idx.stop_line; - pass_judge_line_idx_final = stop_lines_idx.pass_judge_line; + } + + if (stop_line_idx_final == -1) { + RCLCPP_DEBUG(logger_, "detection_area is empty, no plan needed"); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; } const bool is_over_pass_judge_line = util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx_final); - const bool is_before_keep_detection_line = util::isBeforeTargetIndex( - *path, closest_idx, current_pose.pose, stop_lines_idx.keep_detection_line); + const bool is_before_keep_detection_line = + stop_lines_idx_opt.has_value() + ? util::isBeforeTargetIndex( + *path, closest_idx, current_pose.pose, stop_lines_idx_opt.value().keep_detection_line) + : false; const bool keep_detection = is_before_keep_detection_line && std::fabs(current_vel) < planner_param_.keep_detection_vel_thr; if (is_over_pass_judge_line && keep_detection) { // in case ego could not stop exactly before the stop line, but with some overshoot, // keep detection within some margin under low velocity threshold - std::cout << "is_over_pass_judge_line && keep_detection" << std::endl; } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) { - std::cout << "is_over_pass_judge_line && is_go_out_ && !external_stop" << std::endl; - RCLCPP_WARN(logger_, "over the keep_detection line and not low speed. no plan needed."); + RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed."); RCLCPP_DEBUG(logger_, "===== plan end ====="); setSafe(true); setDistance(motion_utils::calcSignedArcLength( @@ -239,19 +232,11 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * logger_.get_child("state_machine"), *clock_); setSafe(state_machine_.getState() == StateMachine::State::GO); - std::cout << "state_machine_.getState == GO: " - << (state_machine_.getState() == StateMachine::State::GO) << std::endl; - if (is_entry_prohibited) { - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_pose.pose.position, - path->points.at(stop_line_idx_final).point.pose.position)); - } else { - setDistance(std::numeric_limits::lowest()); - } - std::cout << "is_entry_prohibited: " << is_entry_prohibited << std::endl; + setDistance(motion_utils::calcSignedArcLength( + path->points, planner_data_->current_pose.pose.position, + path->points.at(stop_line_idx_final).point.pose.position)); if (!isActivated()) { - std::cout << "!isActivated()" << std::endl; // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block is_go_out_ = false; @@ -330,8 +315,6 @@ bool IntersectionModule::checkCollision( for (const auto & p : ego_lane_poly) { ego_poly.outer().emplace_back(p.x(), p.y()); } - // const auto ego_poly = - // generateEgoIntersectionLanePolygon(lanelet_map_ptr, path, closest_idx, closest_idx, 0.0, 0.0); lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); lanelet::ConstLanelet closest_lanelet; lanelet::utils::query::getClosestLanelet( From c39c3d6ef106b11d01091dafd2a71b8d8df4d381 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 19 Dec 2022 20:15:54 +0900 Subject: [PATCH 4/4] also tested with use_stuck_stopline = true flag Signed-off-by: Mamoru Sobue --- .../src/scene_module/intersection/util.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) 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 a7e1739c38119..1f4cba618fe28 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -148,7 +148,7 @@ std::pair, std::optional> generateStopLine( const double keep_detection_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) + [[maybe_unused]] const rclcpp::Clock::SharedPtr clock) { /* set judge line dist */ const double current_vel = planner_data->current_velocity->twist.linear.x; @@ -196,13 +196,6 @@ std::pair, std::optional> generateStopLine( if (use_stuck_stopline) { // the first point in intersection lane stuck_stop_line_idx_ip = lane_interval_ip_start; - if (stuck_stop_line_idx_ip == 0) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger, *clock, 1000 /* ms */, - "use_stuck_stopline, but ego is already in the intersection, not generating stuck stop " - "line"); - return {std::nullopt, std::nullopt}; - } } 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);