From 7fd41ab21d532fe30db963ca981cd3c6e8d0936a Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 22 May 2023 11:38:01 +0900 Subject: [PATCH] Preempt PR3711(2) (#13) * fix(intersection): use 0 offset peeking line Signed-off-by: Mamoru Sobue * use braking distance Signed-off-by: Mamoru Sobue * add pass_judge_wall_pose marker Signed-off-by: Mamoru Sobue * keep detection if ego is between stopline and pass judge line and velocity is below threshold Signed-off-by: Mamoru Sobue * modify condition Signed-off-by: Mamoru Sobue * use 0.0s for delay response time Signed-off-by: Mamoru Sobue * right turn scenario passes Signed-off-by: Mamoru Sobue * find all stop line candidates at first in order Signed-off-by: Mamoru Sobue * use keep_detection_vel for staticPassJudgeLine Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../src/scene_intersection.cpp | 47 ++++++++++++------- .../src/util.cpp | 12 ++--- .../src/util.hpp | 2 +- 3 files changed, 36 insertions(+), 25 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 9a51224de63c0..de42c415aba15 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -181,10 +181,12 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0)); } - const auto static_pass_judge_line_opt = - first_detection_area - ? util::generateStaticPassJudgeLine( - first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_) + const std::optional stuck_line_idx_opt = + first_conflicting_area + ? util::generateStuckStopLine( + first_conflicting_area.value(), planner_data_, planner_param_.common.stop_line_margin, + planner_param_.stuck_vehicle.use_stuck_stopline, path, path_ip, interval, + lane_interval_ip, logger_.get_child("util")) : std::nullopt; const auto default_stop_line_idx_opt = @@ -194,6 +196,21 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * lane_interval_ip, logger_.get_child("util")) : std::nullopt; + const auto static_pass_judge_line_opt = + first_detection_area + ? util::generateStaticPassJudgeLine( + first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_, + planner_param_.collision_detection.keep_detection_vel_thr) + : std::nullopt; + + const auto occlusion_peeking_line_idx_opt = + first_detection_area + ? util::generatePeekingLimitLine( + first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_, + planner_param_.occlusion.peeking_offset) + : std::nullopt; + + // TODO(Mamoru Sobue): check the ordering of these stop lines and refactor /* calc closest index */ const auto closest_idx_opt = motion_utils::findNearestIndex(path->points, current_pose, 3.0, M_PI_4); @@ -218,7 +235,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // if ego is over the pass judge line and not stopped if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { /* do nothing */ - } else if (is_over_default_stop_line && is_over_pass_judge_line) { + } else if (is_over_default_stop_line && is_over_pass_judge_line && !keep_detection) { RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); RCLCPP_DEBUG(logger_, "===== plan end ====="); return true; @@ -240,13 +257,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * generateStuckVehicleDetectAreaPolygon(*path, ego_lane_with_next_lane, closest_idx); const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - const std::optional stuck_line_idx_opt = - first_conflicting_area - ? util::generateStuckStopLine( - first_conflicting_area.value(), planner_data_, planner_param_.common.stop_line_margin, - planner_param_.stuck_vehicle.use_stuck_stopline, path, path_ip, interval, - lane_interval_ip, logger_.get_child("util")) - : std::nullopt; /* calculate dynamic collision around detection area */ /* set stop lines for base_link */ @@ -269,12 +279,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_detection_area.value(), path_ip, lane_interval_ip, detection_divisions_.value(), occlusion_dist_thr) : true; - const auto occlusion_peeking_line_idx_opt = - first_detection_area - ? util::generatePeekingLimitLine( - first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_, - planner_param_.occlusion.peeking_offset) - : std::nullopt; /* calculate final stop lines */ std::optional stop_line_idx = default_stop_line_idx_opt; @@ -414,6 +418,13 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * setSafe(collision_state_machine_.getState() == StateMachine::State::GO); } + RCLCPP_DEBUG( + logger_, + "has_collision = %d, is_occlusion_cleared = %d, collision_stop_required = %d, " + "first_phase_stop_required = %d, occlusion_stop_required = %d", + has_collision, is_occlusion_cleared, collision_stop_required, first_phase_stop_required, + occlusion_stop_required); + /* make decision */ if (!occlusion_activated_) { is_go_out_ = false; diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index aa131fb793cfe..3277b68608302 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -134,15 +134,15 @@ std::optional generateStaticPassJudgeLine( autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, const std::pair lane_interval, - const std::shared_ptr & planner_data) + const std::shared_ptr & planner_data, const double velocity) { - const double velocity = planner_data->current_velocity->twist.linear.x; - const double acceleration = planner_data->current_acceleration->accel.accel.linear.x; + // const double velocity = planner_data->current_velocity->twist.linear.x; + // const double acceleration = planner_data->current_acceleration->accel.accel.linear.x; const double max_stop_acceleration = planner_data->max_stop_acceleration_threshold; - const double max_stop_jerk = planner_data->max_stop_jerk_threshold; + // const double max_stop_jerk = planner_data->max_stop_jerk_threshold; // const double delay_response_time = planner_data->delay_response_time; - const double offset = -planning_utils::calcJudgeLineDistWithJerkLimit( - velocity, acceleration, max_stop_acceleration, max_stop_jerk, 0.0); + const double offset = + -planning_utils::calcJudgeLineDistWithAccLimit(velocity, max_stop_acceleration, 0.0); const auto pass_judge_line_idx = generatePeekingLimitLine( first_detection_area, original_path, path_ip, ip_interval, lane_interval, planner_data, offset); if (pass_judge_line_idx) { diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 980abf2cd005b..3aa370322c45a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -108,7 +108,7 @@ std::optional generateStaticPassJudgeLine( autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, const std::pair lane_interval, - const std::shared_ptr & planner_data); + const std::shared_ptr & planner_data, const double velocity); std::optional generatePeekingLimitLine( const lanelet::CompoundPolygon3d & first_detection_area,