diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 972f572890d16..352b18f8b49b6 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -13,6 +13,10 @@ path_interpolation_ds: 0.1 # [m] consider_wrong_direction_vehicle: false stuck_vehicle: + turn_direction: + left: true + right: true + straight: true use_stuck_stopline: true # stopline generated before the first conflicting area stuck_vehicle_detect_dist: 5.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_vel_thr: 0.833 # 0.833m/s = 3.0km/h diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 4137090a5b6ae..3cca1f42115d1 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -63,6 +63,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.common.consider_wrong_direction_vehicle = getOrDeclareParameter(node, ns + ".common.consider_wrong_direction_vehicle"); + ip.stuck_vehicle.turn_direction.left = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); + ip.stuck_vehicle.turn_direction.right = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.right"); + ip.stuck_vehicle.turn_direction.straight = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.straight"); ip.stuck_vehicle.use_stuck_stopline = getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); ip.stuck_vehicle.stuck_vehicle_detect_dist = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 75bb4e861a60f..ee541a061ce0e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -43,6 +44,32 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +namespace +{ +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware_auto_perception_msgs::msg::Shape & shape) +{ + const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + + Polygon2d one_step_poly; + for (const auto & point : prev_poly.outer()) { + one_step_poly.outer().push_back(point); + } + for (const auto & point : next_poly.outer()) { + one_step_poly.outer().push_back(point); + } + + bg::correct(one_step_poly); + + Polygon2d convex_one_step_poly; + bg::convex_hull(one_step_poly, convex_one_step_poly); + + return convex_one_step_poly; +} +} // namespace + static bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) { @@ -877,7 +904,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); const bool is_prioritized = traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; - intersection_lanelets.update(is_prioritized, interpolated_path_info); + const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint); // this is abnormal const auto & conflicting_lanelets = intersection_lanelets.conflicting(); @@ -1185,6 +1213,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( bool IntersectionModule::checkStuckVehicle( const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) { + const bool stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || + (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || + (turn_direction_ == "straight" && planner_param_.stuck_vehicle.turn_direction.straight); + }(); + if (!stuck_detection_direction) { + return false; + } + const auto & objects_ptr = planner_data->predicted_objects; // considering lane change in the intersection, these lanelets are generated from the path @@ -1312,14 +1349,14 @@ bool IntersectionModule::checkCollision( // collision point const auto first_itr = std::adjacent_find( predicted_path.path.cbegin(), predicted_path.path.cend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); }); if (first_itr == predicted_path.path.cend()) continue; const auto last_itr = std::adjacent_find( predicted_path.path.crbegin(), predicted_path.path.crend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); }); if (last_itr == predicted_path.path.crend()) continue; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 439614de2a57c..f7b7e1903ca67 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -62,6 +62,12 @@ class IntersectionModule : public SceneModuleInterface } common; struct StuckVehicle { + struct TurnDirection + { + bool left; + bool right; + bool straight; + } turn_direction; bool use_stuck_stopline; //! stopline generate before the intersection lanelet when is_stuck. double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check double stuck_vehicle_vel_thr; //! Threshold of the speed to be recognized as stopped diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 5cedb18e8f172..9411d6664a8af 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -93,8 +93,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR planner_param_.attention_area_length, planner_param_.occlusion_attention_area_length, planner_param_.consider_wrong_direction_vehicle); } - intersection_lanelets_.value().update(false, interpolated_path_info); - const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area(); + auto & intersection_lanelets = intersection_lanelets_.value(); + const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + intersection_lanelets.update(false, interpolated_path_info, local_footprint); + const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area(); if (!first_conflicting_area) { return false; } diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 722bf696ef55b..e83472adc4ccf 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -217,6 +217,50 @@ static std::optional getStopLineIndexFromMap( return stop_idx_ip_opt.get(); } +static std::optional getFirstPointInsidePolygonByFootprint( + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + + const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); + for (auto i = lane_start; i <= lane_end; ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, area_2d)) { + return std::make_optional(i); + } + } + return std::nullopt; +} + +static std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + + for (size_t i = lane_start; i <= lane_end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (const auto & polygon : polygons) { + const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); + const bool is_in_polygon = bg::intersects(area_2d, path_footprint); + if (is_in_polygon) { + return std::make_optional>( + i, polygon); + } + } + } + return std::nullopt; +} + std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::CompoundPolygon3d & first_detection_area, @@ -233,6 +277,7 @@ std::optional generateIntersectionStopLines( const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); + /* // find the index of the first point that intersects with detection_areas const auto first_inside_detection_idx_ip_opt = getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_detection_area); @@ -241,6 +286,16 @@ std::optional generateIntersectionStopLines( return std::nullopt; } const auto first_inside_detection_ip = first_inside_detection_idx_ip_opt.value(); + */ + // find the index of the first point whose vehicle footprint on it intersects with detection_area + const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); + std::optional first_footprint_inside_detection_ip_opt = + getFirstPointInsidePolygonByFootprint( + first_detection_area, interpolated_path_info, local_footprint); + if (!first_footprint_inside_detection_ip_opt) { + return std::nullopt; + } + const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); // (1) default stop line position on interpolated path bool default_stop_line_valid = true; @@ -251,8 +306,7 @@ std::optional generateIntersectionStopLines( stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; } if (stop_idx_ip_int < 0) { - stop_idx_ip_int = static_cast(first_inside_detection_ip) - stop_line_margin_idx_dist - - base2front_idx_dist; + stop_idx_ip_int = first_footprint_inside_detection_ip - stop_line_margin_idx_dist; } if (stop_idx_ip_int < 0) { default_stop_line_valid = false; @@ -269,8 +323,6 @@ std::optional generateIntersectionStopLines( const auto closest_idx_ip = closest_idx_ip_opt.value(); // (3) occlusion peeking stop line position on interpolated path - const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); - const auto area_2d = lanelet::utils::to2D(first_detection_area).basicPolygon(); int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); bool occlusion_peeking_line_valid = true; { @@ -278,20 +330,13 @@ std::optional generateIntersectionStopLines( const auto & base_pose0 = path_ip.points.at(default_stop_line_ip).point.pose; const auto path_footprint0 = tier4_autoware_utils::transformVector( local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); - if (bg::intersects(path_footprint0, area_2d)) { + if (bg::intersects( + path_footprint0, lanelet::utils::to2D(first_detection_area).basicPolygon())) { occlusion_peeking_line_valid = false; } } if (occlusion_peeking_line_valid) { - for (size_t i = default_stop_line_ip + 1; i <= std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, area_2d)) { - occlusion_peeking_line_ip_int = i; - break; - } - } + occlusion_peeking_line_ip_int = first_footprint_inside_detection_ip; } const auto first_attention_stop_line_ip = static_cast(occlusion_peeking_line_ip_int); const bool first_attention_stop_line_valid = occlusion_peeking_line_valid; @@ -307,8 +352,8 @@ std::optional generateIntersectionStopLines( const double delay_response_time = planner_data->delay_response_time; const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( velocity, acceleration, max_stop_acceleration, max_stop_jerk, delay_response_time); - int pass_judge_ip_int = static_cast(first_inside_detection_ip) - base2front_idx_dist - - std::ceil(braking_dist / ds); + int pass_judge_ip_int = + static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); const auto pass_judge_line_ip = static_cast( std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); @@ -318,18 +363,18 @@ std::optional generateIntersectionStopLines( if (use_stuck_stopline) { // NOTE: when ego vehicle is approaching detection area and already passed // first_conflicting_area, this could be null. - const auto stuck_stop_line_idx_ip_opt = - getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_conflicting_area); + const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_area, interpolated_path_info, local_footprint); if (!stuck_stop_line_idx_ip_opt) { stuck_stop_line_valid = false; stuck_stop_line_ip_int = 0; } else { - stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); + stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value() - stop_line_margin_idx_dist; } } else { - stuck_stop_line_ip_int = std::get<0>(lane_interval_ip); + stuck_stop_line_ip_int = + std::get<0>(lane_interval_ip) - (stop_line_margin_idx_dist + base2front_idx_dist); } - stuck_stop_line_ip_int -= (stop_line_margin_idx_dist + base2front_idx_dist); if (stuck_stop_line_ip_int < 0) { stuck_stop_line_valid = false; } @@ -427,6 +472,9 @@ std::optional getFirstPointInsidePolygon( if (is_in_lanelet) { return std::make_optional(i); } + if (i == 0) { + break; + } } } return std::nullopt; @@ -469,6 +517,9 @@ getFirstPointInsidePolygons( if (is_in_lanelet) { break; } + if (i == 0) { + break; + } } } return std::nullopt; @@ -1240,21 +1291,22 @@ double calcDistanceUntilIntersectionLanelet( } void IntersectionLanelets::update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info) + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) { is_prioritized_ = is_prioritized; // find the first conflicting/detection area polygon intersecting the path - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - { - auto first = getFirstPointInsidePolygons(path, lane_interval, conflicting_area_); - if (first && !first_conflicting_area_) { + if (!first_conflicting_area_) { + auto first = + getFirstPointInsidePolygonsByFootprint(conflicting_area_, interpolated_path_info, footprint); + if (first) { first_conflicting_area_ = first.value().second; } } - { - auto first = getFirstPointInsidePolygons(path, lane_interval, attention_area_); - if (first && !first_attention_area_) { + if (!first_attention_area_) { + auto first = + getFirstPointInsidePolygonsByFootprint(attention_area_, interpolated_path_info, footprint); + if (first) { first_attention_area_ = first.value().second; } } diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index aed81d771e480..581f22904fd2b 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -15,6 +15,8 @@ #ifndef UTIL_TYPE_HPP_ #define UTIL_TYPE_HPP_ +#include + #include #include #include @@ -67,7 +69,9 @@ struct InterpolatedPathInfo struct IntersectionLanelets { public: - void update(const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info); + void update( + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint); const lanelet::ConstLanelets & attention() const { return is_prioritized_ ? attention_non_preceding_ : attention_;