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 eb07deaa2ce23..4b66e25f738a5 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 @@ -3,6 +3,8 @@ intersection: state_transit_margin_time: 0.5 stop_line_margin: 3.0 + keep_detection_line_margin: 1.0 # distance (toward path end) from genearted 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_vel_thr: 0.833 # 0.833m/s = 3.0km/h diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index eb07deaa2ce23..4b66e25f738a5 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -3,6 +3,8 @@ intersection: state_transit_margin_time: 0.5 stop_line_margin: 3.0 + keep_detection_line_margin: 1.0 # distance (toward path end) from genearted 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_vel_thr: 0.833 # 0.833m/s = 3.0km/h 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 1879b7178e120..f1cdcdbbdfecd 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 @@ -105,6 +105,10 @@ class IntersectionModule : public SceneModuleInterface { double state_transit_margin_time; double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary + double keep_detection_line_margin; //! distance (toward path end) from genearted stop line. + //! keep detection if ego is before this line and ego.vel < + //! keep_detection_vel_thr + double keep_detection_vel_thr; double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check double stuck_vehicle_ignore_dist; //! distance from intersection start to start stuck vehicle check 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 e228deec9d704..94a5d2fac2095 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -39,7 +39,7 @@ int insertPoint( autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path); bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const int id); -bool hasDuplicatedPoint( +bool getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point, int * duplicated_point_idx); @@ -54,6 +54,14 @@ bool getObjectiveLanelets( std::vector * objective_lanelets_with_margin_result, const rclcpp::Logger logger); +struct StopLineIdx +{ + int first_idx_inside_lane = -1; + int pass_judge_line_idx = -1; + int stop_line_idx = -1; + int keep_detection_line_idx = -1; +}; + /** * @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. @@ -68,9 +76,10 @@ bool getObjectiveLanelets( bool generateStopLine( const int lane_id, const std::vector detection_areas, const std::shared_ptr & planner_data, const double stop_line_margin, + const double keep_detection_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, - int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger); + const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, + StopLineIdx * stop_line_idxs, const rclcpp::Logger logger); /** * @brief If use_stuck_stopline is true, a stop line is generated before the intersection. @@ -118,6 +127,22 @@ std::vector getLaneletIdsFromLaneletsVec(const std::vector(static_cast(closest_idx) > pass_judge_line_idx); - if (static_cast(closest_idx) == pass_judge_line_idx) { - geometry_msgs::msg::Pose pass_judge_line = path->points.at(pass_judge_line_idx).point.pose; - is_over_pass_judge_line = planning_utils::isAheadOf(current_pose.pose, pass_judge_line); - } - if (is_go_out_ && is_over_pass_judge_line && !external_stop) { - RCLCPP_DEBUG(logger_, "over the pass judge line. 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)); - return true; // no plan needed. + const bool is_over_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx); + if (is_over_pass_judge_line) { + /* + in case ego could not stop exactly before the stop line, but with some overshoot, keep + detection within some margin and low veloctiy threshold + */ + const bool is_before_keep_detection_line = + util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx); + if ( + is_before_keep_detection_line && std::fabs(planner_data_->current_velocity->twist.linear.x) < + planner_param_.keep_detection_vel_thr) { + 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_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)); + return true; // no plan needed. + } } /* get dynamic object */ @@ -201,22 +214,24 @@ bool IntersectionModule::modifyPathVelocity( if (!isActivated()) { constexpr double v = 0.0; is_go_out_ = false; + int stop_line_idx_stop = stop_line_idx; + int pass_judge_line_idx_stop = pass_judge_line_idx; if (planner_param_.use_stuck_stopline && is_stuck) { int stuck_stop_line_idx = -1; int stuck_pass_judge_line_idx = -1; if (util::generateStopLineBeforeIntersection( lane_id_, lanelet_map_ptr, planner_data_, *path, path, &stuck_stop_line_idx, &stuck_pass_judge_line_idx, logger_.get_child("util"))) { - stop_line_idx = stuck_stop_line_idx; - pass_judge_line_idx = stuck_pass_judge_line_idx; + stop_line_idx_stop = stuck_stop_line_idx; + pass_judge_line_idx_stop = stuck_pass_judge_line_idx; } } - planning_utils::setVelocityFromIndex(stop_line_idx, v, path); + planning_utils::setVelocityFromIndex(stop_line_idx_stop, v, path); debug_data_.stop_required = true; debug_data_.stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, base_link2front, *path); - debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; - debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx).point.pose; + planning_utils::getAheadPose(stop_line_idx_stop, base_link2front, *path); + debug_data_.stop_point_pose = path->points.at(stop_line_idx_stop).point.pose; + debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx_stop).point.pose; /* get stop point and stop factor */ tier4_planning_msgs::msg::StopFactor stop_factor; 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 eb1de17673ae6..02f992743ed33 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 @@ -83,27 +83,27 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( debug_data_.detection_area = conflicting_areas; /* set stop-line and stop-judgement-line for base_link */ - int stop_line_idx = -1; - int judge_line_idx = -1; - int first_idx_inside_lane = -1; + util::StopLineIdx stop_line_idxs; const auto private_path = extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m); if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, - private_path, &stop_line_idx, &judge_line_idx, &first_idx_inside_lane, + lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, + 0.0 /* unnecessary in merge_from_private */, path, private_path, &stop_line_idxs, logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); return false; } - if (stop_line_idx <= 0 || judge_line_idx <= 0) { - RCLCPP_DEBUG(logger_, "stop line or judge line is at path[0], ignore planning."); + const int stop_line_idx = stop_line_idxs.stop_line_idx; + if (stop_line_idx <= 0) { + RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); return true; } 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 int first_idx_inside_lane = stop_line_idxs.first_idx_inside_lane; if (first_idx_inside_lane != -1) { debug_data_.first_collision_point = path->points.at(first_idx_inside_lane).point.pose.position; } 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 1d3e703d5ae39..c821d1044d72c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -73,7 +73,7 @@ bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, return false; } -bool hasDuplicatedPoint( +bool getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point, int * duplicated_point_idx) { @@ -116,9 +116,10 @@ int getFirstPointInsidePolygons( bool generateStopLine( const int lane_id, const std::vector detection_areas, const std::shared_ptr & planner_data, const double stop_line_margin, + const double keep_detection_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, - int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger) + const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, + StopLineIdx * stop_line_idxs, const rclcpp::Logger logger) { /* set judge line dist */ const double current_vel = planner_data->current_velocity->twist.linear.x; @@ -132,7 +133,8 @@ bool generateStopLine( /* set parameters */ constexpr double interval = 0.2; - const int margin_idx_dist = std::ceil(stop_line_margin / interval); + const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval); + const int keep_detection_line_margin_idx_dist = std::ceil(keep_detection_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); @@ -143,6 +145,10 @@ bool generateStopLine( return false; } + int first_idx_inside_lane = -1; + int pass_judge_line_idx = -1; + int stop_line_idx = -1; + int keep_detection_line_idx = -1; /* generate stop point */ // If a stop_line is defined in lanelet_map, use it. // else, generates a local stop_line with considering the lane conflicts. @@ -163,32 +169,45 @@ bool generateStopLine( const auto first_idx_inside_lane_opt = motion_utils::findNearestIndex(original_path->points, first_inside_point, 10.0, M_PI_4); if (first_idx_inside_lane_opt) { - *first_idx_inside_lane = first_idx_inside_lane_opt.get(); + first_idx_inside_lane = first_idx_inside_lane_opt.get(); } - if (*first_idx_inside_lane == 0) { + if (first_idx_inside_lane == 0) { RCLCPP_DEBUG( logger, "path[0] is already in the detection area. This happens if you have " "already crossed the stop line or are very far from the intersection. Ignore computation."); - *stop_line_idx = 0; - *pass_judge_line_idx = 0; + stop_line_idxs->first_idx_inside_lane = 0; + stop_line_idxs->pass_judge_line_idx = 0; + stop_line_idxs->stop_line_idx = 0; + stop_line_idxs->keep_detection_line_idx = 0; return true; } - stop_idx_ip = std::max(first_idx_ip_inside_lane - 1 - margin_idx_dist - base2front_idx_dist, 0); + stop_idx_ip = + std::max(first_idx_ip_inside_lane - 1 - stop_line_margin_idx_dist - base2front_idx_dist, 0); + } + + /* insert keep_detection_line */ + const int keep_detection_idx_ip = std::min( + stop_idx_ip + keep_detection_line_margin_idx_dist, static_cast(path_ip.points.size()) - 1); + const auto inserted_keep_detection_point = path_ip.points.at(keep_detection_idx_ip).point.pose; + if (!util::getDuplicatedPointIdx( + *original_path, inserted_keep_detection_point.position, &keep_detection_line_idx)) { + keep_detection_line_idx = util::insertPoint(inserted_keep_detection_point, original_path); } /* insert stop_point */ const auto inserted_stop_point = path_ip.points.at(stop_idx_ip).point.pose; // if path has too close (= duplicated) point to the stop point, do not insert it // and consider the index of the duplicated point as stop_line_idx - if (!util::hasDuplicatedPoint(*original_path, inserted_stop_point.position, stop_line_idx)) { - *stop_line_idx = util::insertPoint(inserted_stop_point, original_path); + if (!util::getDuplicatedPointIdx(*original_path, inserted_stop_point.position, &stop_line_idx)) { + stop_line_idx = util::insertPoint(inserted_stop_point, original_path); + ++keep_detection_line_idx; // the index is incremented by judge stop line insertion } /* if another stop point exist before intersection stop_line, disable judge_line. */ bool has_prior_stopline = false; - for (int i = 0; i < *stop_line_idx; ++i) { + for (int i = 0; i < stop_line_idx; ++i) { if (std::fabs(original_path->points.at(i).point.longitudinal_velocity_mps) < 0.1) { has_prior_stopline = true; break; @@ -199,23 +218,29 @@ bool generateStopLine( const int pass_judge_idx_ip = std::min( static_cast(path_ip.points.size()) - 1, std::max(stop_idx_ip - pass_judge_idx_dist, 0)); if (has_prior_stopline || stop_idx_ip == pass_judge_idx_ip) { - *pass_judge_line_idx = *stop_line_idx; + pass_judge_line_idx = stop_line_idx; } else { const auto inserted_pass_judge_point = path_ip.points.at(pass_judge_idx_ip).point.pose; // if path has too close (= duplicated) point to the pass judge point, do not insert it // and consider the index of the duplicated point as pass_judge_line_idx - if (!util::hasDuplicatedPoint( - *original_path, inserted_pass_judge_point.position, pass_judge_line_idx)) { - *pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, original_path); - ++(*stop_line_idx); // stop index is incremented by judge line insertion + if (!util::getDuplicatedPointIdx( + *original_path, inserted_pass_judge_point.position, &pass_judge_line_idx)) { + pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, original_path); + ++stop_line_idx; // stop index is incremented by judge line insertion + ++keep_detection_line_idx; // same. } } + stop_line_idxs->first_idx_inside_lane = first_idx_inside_lane; + stop_line_idxs->pass_judge_line_idx = pass_judge_line_idx; + stop_line_idxs->stop_line_idx = stop_line_idx; + stop_line_idxs->keep_detection_line_idx = keep_detection_line_idx; + RCLCPP_DEBUG( logger, "generateStopLine() : stop_idx = %d, pass_judge_idx = %d, stop_idx_ip = " "%d, pass_judge_idx_ip = %d, has_prior_stopline = %d", - *stop_line_idx, *pass_judge_line_idx, stop_idx_ip, pass_judge_idx_ip, has_prior_stopline); + stop_line_idx, pass_judge_line_idx, stop_idx_ip, pass_judge_idx_ip, has_prior_stopline); return true; } @@ -507,7 +532,7 @@ bool generateStopLineBeforeIntersection( const auto inserted_stop_point = path_ip.points.at(stop_idx_ip).point.pose; // if path has too close (= duplicated) point to the stop point, do not insert it // and consider the index of the duplicated point as *stuck_stop_line_idx - if (!util::hasDuplicatedPoint( + if (!util::getDuplicatedPointIdx( *output_path, inserted_stop_point.position, stuck_stop_line_idx)) { *stuck_stop_line_idx = util::insertPoint(inserted_stop_point, output_path); } @@ -531,7 +556,7 @@ bool generateStopLineBeforeIntersection( const auto inserted_pass_judge_point = path_ip.points.at(pass_judge_idx_ip).point.pose; // if path has too close (= duplicated) point to the pass judge point, do not insert it // and consider the index of the duplicated point as pass_judge_line_idx - if (!util::hasDuplicatedPoint( + if (!util::getDuplicatedPointIdx( *output_path, inserted_pass_judge_point.position, pass_judge_line_idx)) { *pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, output_path); ++(*stuck_stop_line_idx); // stop index is incremented by judge line insertion @@ -556,5 +581,28 @@ geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p) pose.position = p; return pose; } + +bool isOverTargetIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & current_pose, const int target_idx) +{ + if (closest_idx == target_idx) { + const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; + return planning_utils::isAheadOf(current_pose, target_pose); + } + return static_cast(closest_idx > target_idx); +} + +bool isBeforeTargetIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & current_pose, const int target_idx) +{ + if (closest_idx == target_idx) { + const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; + return planning_utils::isAheadOf(target_pose, current_pose); + } + return static_cast(target_idx > closest_idx); +} + } // namespace util } // namespace behavior_velocity_planner