diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index a5baa2ffa30ed..6ab8a54235853 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -195,6 +195,11 @@ 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 1acdaf30016c1..abd08e7662e06 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -112,6 +112,7 @@ 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; @@ -180,6 +181,19 @@ 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); @@ -191,20 +205,20 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } const size_t closest_idx = closest_idx_opt.get(); - 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) { + if (static_pass_judge_line_opt && default_stop_line_idx_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_pass_judge_line && is_go_out_ && !keep_detection) { + 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) { RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); RCLCPP_DEBUG(logger_, "===== plan end ====="); return true; @@ -236,12 +250,6 @@ 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 - @@ -407,7 +415,6 @@ 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 0cc88ad4b1b7b..fdead61936024 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -54,6 +54,7 @@ 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 4270427821dd1..aa131fb793cfe 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -136,28 +136,19 @@ std::optional generateStaticPassJudgeLine( const std::pair lane_interval, const std::shared_ptr & planner_data) { - 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; - } + 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; } std::optional generatePeekingLimitLine(