From 6047f1ca0c8c2fdfc5961413076c74a824c98688 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 22 May 2023 17:56:52 +0900 Subject: [PATCH] Revert "preempt PR3711 (#11)" This reverts commit a6d7fb68798b785d431d31831385efcf8597ed69. --- .../src/debug.cpp | 5 --- .../src/scene_intersection.cpp | 37 ++++++++----------- .../src/scene_intersection.hpp | 1 - .../src/util.cpp | 35 +++++++++++------- 4 files changed, 37 insertions(+), 41 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 6ab8a54235853..a5baa2ffa30ed 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -195,11 +195,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.predicted_obj_pose, "predicted_obj_pose", module_id_, 0.7, 0.85, 0.9), &debug_marker_array, now); - appendMarkerArray( - createPoseMarkerArray( - debug_data_.pass_judge_wall_pose, "pass_judge_wall_pose", module_id_, 0.7, 0.85, 0.9), - &debug_marker_array, now); - return debug_marker_array; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 3dbb8d0eaae9d..529564847ffcb 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -112,7 +112,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * occlusion_stop_distance_ = std::numeric_limits::lowest(); occlusion_first_stop_required_ = false; - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; /* get current pose */ const geometry_msgs::msg::Pose current_pose = planner_data_->current_odometry->pose; @@ -183,19 +182,6 @@ 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_) - : std::nullopt; - - const auto default_stop_line_idx_opt = - first_detection_area ? util::generateCollisionStopLine( - lane_id_, first_detection_area.value(), planner_data_, - planner_param_.common.stop_line_margin, path, path_ip, interval, - lane_interval_ip, logger_.get_child("util")) - : std::nullopt; - /* calc closest index */ const auto closest_idx_opt = motion_utils::findNearestIndex(path->points, current_pose, 3.0, M_PI_4); @@ -207,20 +193,20 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } const size_t closest_idx = closest_idx_opt.get(); - if (static_pass_judge_line_opt && default_stop_line_idx_opt) { + 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_) + : std::nullopt; + + if (static_pass_judge_line_opt) { const auto pass_judge_line_idx = static_pass_judge_line_opt.value(); - debug_data_.pass_judge_wall_pose = - planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); const bool is_over_pass_judge_line = util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); - const bool is_over_default_stop_line = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx_opt.value()); const double vel = std::fabs(planner_data_->current_velocity->twist.linear.x); const bool keep_detection = (vel < planner_param_.collision_detection.keep_detection_vel_thr); // 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) { + if (is_over_pass_judge_line && is_go_out_ && !keep_detection) { RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); RCLCPP_DEBUG(logger_, "===== plan end ====="); return true; @@ -252,6 +238,12 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * /* calculate dynamic collision around detection area */ /* set stop lines for base_link */ + const auto default_stop_line_idx_opt = + first_detection_area ? util::generateCollisionStopLine( + lane_id_, first_detection_area.value(), planner_data_, + planner_param_.common.stop_line_margin, path, path_ip, interval, + lane_interval_ip, logger_.get_child("util")) + : std::nullopt; const double time_delay = is_go_out_ ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - @@ -427,6 +419,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } /* make decision */ + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; if (!occlusion_activated_) { is_go_out_ = false; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 2a9e6dc678005..0a1697d4a832d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -54,7 +54,6 @@ class IntersectionModule : public SceneModuleInterface geometry_msgs::msg::Pose collision_stop_wall_pose; geometry_msgs::msg::Pose occlusion_stop_wall_pose; geometry_msgs::msg::Pose occlusion_first_stop_wall_pose; - geometry_msgs::msg::Pose pass_judge_wall_pose; geometry_msgs::msg::Polygon stuck_vehicle_detect_area; geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon; std::vector candidate_collision_object_polygons; diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index aa131fb793cfe..4270427821dd1 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -136,19 +136,28 @@ std::optional generateStaticPassJudgeLine( const std::pair lane_interval, const std::shared_ptr & planner_data) { - 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 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 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) { - return pass_judge_line_idx; - } - return 0; + const auto pass_judge_line_idx_ip = + util::getFirstPointInsidePolygon(path_ip, lane_interval, first_detection_area); + if (!pass_judge_line_idx_ip) { + return std::nullopt; + } + const int base2front_idx_dist = + std::ceil(planner_data->vehicle_info_.vehicle_length_m / ip_interval); + const int idx = static_cast(pass_judge_line_idx_ip.value()) - base2front_idx_dist; + if (idx < 0) { + return std::nullopt; + } + const auto & insert_point = path_ip.points.at(static_cast(idx)).point.pose; + const auto duplicate_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position); + if (duplicate_idx_opt) { + return duplicate_idx_opt; + } else { + const auto insert_idx_opt = util::insertPoint(insert_point, original_path); + if (!insert_idx_opt) { + return std::nullopt; + } + return insert_idx_opt; + } } std::optional generatePeekingLimitLine(