From b56a7fbad2e717f4ace3d596cb4e5ae6e4566a05 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 23 Aug 2023 10:00:22 +0900 Subject: [PATCH] fix(intersection): modify ego polygon calculation (#4694) Signed-off-by: Mamoru Sobue --- .../src/scene_intersection.cpp | 185 +++++++++--------- .../src/scene_intersection.hpp | 4 +- .../src/util.cpp | 79 +++++--- .../src/util.hpp | 9 +- .../src/util_type.hpp | 12 ++ 5 files changed, 165 insertions(+), 124 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 7f56afde2f4ea..d9ebafbe682a0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -719,9 +719,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } const auto & current_pose = planner_data_->current_odometry->pose; + const auto lanelets_on_path = + planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); if (!intersection_lanelets_) { - const auto lanelets_on_path = - planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); intersection_lanelets_ = util::getObjectiveLanelets( lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, planner_param_.common.attention_area_length, @@ -756,8 +756,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( [closest_idx, stuck_stop_line_idx, default_stop_line_idx, occlusion_peeking_stop_line_idx, pass_judge_line_idx] = intersection_stop_lines; - const auto ego_lane_with_next_lane = util::getEgoLaneWithNextLane( - *path, associative_ids_, planner_data_->vehicle_info_.vehicle_width_m); + const auto path_lanelets_opt = util::generatePathLanelets( + lanelets_on_path, *path, associative_ids_, closest_idx, + planner_data_->vehicle_info_.vehicle_width_m); + if (!path_lanelets_opt.has_value()) { + RCLCPP_DEBUG(logger_, "failed to generate PathLanelets"); + return IntersectionModule::Indecisive{}; + } + const auto path_lanelets = path_lanelets_opt.value(); + + const auto ego_lane_with_next_lane = + path_lanelets.next.has_value() + ? std::vector< + lanelet::ConstLanelet>{path_lanelets.ego_or_entry2exit, path_lanelets.next.value()} + : std::vector{path_lanelets.ego_or_entry2exit}; const bool stuck_detected = checkStuckVehicle(planner_data_, ego_lane_with_next_lane, *path, intersection_stop_lines); @@ -846,8 +858,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getDuration()); const auto target_objects = filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); + const bool has_collision = checkCollision( - *path, target_objects, ego_lane_with_next_lane, closest_idx, time_delay, tl_arrow_solid_on); + *path, target_objects, path_lanelets, closest_idx, time_delay, tl_arrow_solid_on); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -1026,8 +1039,8 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & objects, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double time_delay, const bool tl_arrow_solid_on) + const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, + const bool tl_arrow_solid_on) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1042,12 +1055,10 @@ bool IntersectionModule::checkCollision( auto target_objects = objects; util::cutPredictPathWithDuration(&target_objects, clock_, passing_time); + const auto & concat_lanelets = path_lanelets.all; const auto closest_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - const auto & ego_lane = ego_lane_with_next_lane.front(); - const double distance_until_intersection = - util::calcDistanceUntilIntersectionLanelet(ego_lane, path, closest_idx); - const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); + const auto & ego_lane = path_lanelets.ego_or_entry2exit; const auto ego_poly = ego_lane.polygon2d().basicPolygon(); // check collision between predicted_path and ego_area @@ -1059,7 +1070,6 @@ bool IntersectionModule::checkCollision( : planner_param_.collision_detection.normal.collision_end_margin_time; bool collision_detected = false; for (const auto & object : target_objects.objects) { - bool has_collision = false; for (const auto & predicted_path : object.kinematics.predicted_paths) { if ( predicted_path.confidence < @@ -1068,97 +1078,82 @@ bool IntersectionModule::checkCollision( continue; } - std::vector predicted_poses; - for (const auto & pose : predicted_path.path) { - predicted_poses.push_back(pose); - } - has_collision = bg::intersects(ego_poly, to_bg2d(predicted_poses)); - if (has_collision) { - 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)}); - }); - 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)}); - }); - const double ref_object_enter_time = - static_cast(first_itr - predicted_path.path.begin()) * - rclcpp::Duration(predicted_path.time_step).seconds(); - auto start_time_distance_itr = time_distance_array.begin(); - if (ref_object_enter_time - collision_start_margin_time > 0) { - start_time_distance_itr = std::lower_bound( - time_distance_array.begin(), time_distance_array.end(), - ref_object_enter_time - collision_start_margin_time, - [](const auto & a, const double b) { return a.first < b; }); - if (start_time_distance_itr == time_distance_array.end()) { - continue; - } - } - const double ref_object_exit_time = - static_cast(last_itr.base() - predicted_path.path.begin()) * - rclcpp::Duration(predicted_path.time_step).seconds(); - auto end_time_distance_itr = std::lower_bound( + // 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)}); + }); + 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)}); + }); + if (last_itr == predicted_path.path.crend()) continue; + + // possible collision time interval + const double ref_object_enter_time = + static_cast(first_itr - predicted_path.path.begin()) * + rclcpp::Duration(predicted_path.time_step).seconds(); + auto start_time_distance_itr = time_distance_array.begin(); + if (ref_object_enter_time - collision_start_margin_time > 0) { + // start of possible ego position in the intersection + start_time_distance_itr = std::lower_bound( time_distance_array.begin(), time_distance_array.end(), - ref_object_exit_time + collision_end_margin_time, + ref_object_enter_time - collision_start_margin_time, [](const auto & a, const double b) { return a.first < b; }); - if (end_time_distance_itr == time_distance_array.end()) { - end_time_distance_itr = time_distance_array.end() - 1; - } - const double start_arc_length = std::max( - 0.0, closest_arc_coords.length + (*start_time_distance_itr).second - - distance_until_intersection); - const double end_arc_length = std::max( - 0.0, closest_arc_coords.length + (*end_time_distance_itr).second + base_link2front - - distance_until_intersection); - - long double lanes_length = 0.0; - std::vector ego_lane_with_next_lanes; - - const auto lanelets_on_path = planning_utils::getLaneletsOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), - planner_data_->current_odometry->pose); - for (const auto & lane : lanelets_on_path) { - lanes_length += bg::length(lane.centerline()); - ego_lane_with_next_lanes.push_back(lane); - if (lanes_length > start_arc_length && lanes_length < end_arc_length) { - break; - } - } - const auto trimmed_ego_polygon = - getPolygonFromArcLength(ego_lane_with_next_lanes, start_arc_length, end_arc_length); - - if (trimmed_ego_polygon.empty()) { + if (start_time_distance_itr == time_distance_array.end()) { + // ego is already at the exit of intersection when npc is at collision point even if npc + // accelerates so ego's position interval is empty continue; } + } + const double ref_object_exit_time = + static_cast(last_itr.base() - predicted_path.path.begin()) * + rclcpp::Duration(predicted_path.time_step).seconds(); + auto end_time_distance_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + ref_object_exit_time + collision_end_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (end_time_distance_itr == time_distance_array.end()) { + // ego is already passing the intersection, when npc is is at collision point + // so ego's position interval is up to the end of intersection lane + end_time_distance_itr = time_distance_array.end() - 1; + } + const double start_arc_length = std::max( + 0.0, closest_arc_coords.length + (*start_time_distance_itr).second - + planner_data_->vehicle_info_.rear_overhang_m); + const double end_arc_length = std::min( + closest_arc_coords.length + (*end_time_distance_itr).second + + planner_data_->vehicle_info_.max_longitudinal_offset_m, + lanelet::utils::getLaneletLength2d(concat_lanelets)); + + const auto trimmed_ego_polygon = + getPolygonFromArcLength(concat_lanelets, start_arc_length, end_arc_length); + + if (trimmed_ego_polygon.empty()) { + continue; + } - Polygon2d polygon{}; - for (const auto & p : trimmed_ego_polygon) { - polygon.outer().emplace_back(p.x(), p.y()); - } - - polygon.outer().emplace_back(polygon.outer().front()); - - bg::correct(polygon); - - debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); + Polygon2d polygon{}; + for (const auto & p : trimmed_ego_polygon) { + polygon.outer().emplace_back(p.x(), p.y()); + } + bg::correct(polygon); + debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); - for (auto itr = first_itr; itr != last_itr.base(); ++itr) { - const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape); - debug_data_.candidate_collision_object_polygons.emplace_back( - toGeomPoly(footprint_polygon)); - if (bg::intersects(polygon, footprint_polygon)) { - collision_detected = true; - break; - } - } - if (collision_detected) { - debug_data_.conflicting_targets.objects.push_back(object); + for (auto itr = first_itr; itr != last_itr.base(); ++itr) { + const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape); + if (bg::intersects(polygon, footprint_polygon)) { + collision_detected = true; break; } } + if (collision_detected) { + debug_data_.conflicting_targets.objects.push_back(object); + break; + } } } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index a427c4930b6d5..9d1fab1339d0e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -258,8 +258,8 @@ class IntersectionModule : public SceneModuleInterface bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double time_delay, const bool tl_arrow_solid_on); + const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, + const bool tl_arrow_solid_on); bool isOcclusionCleared( const nav_msgs::msg::OccupancyGrid & occ_grid, diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index f0343891c6051..7fdae3df2d9d2 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -872,29 +872,6 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( return obj_pose; } -lanelet::ConstLanelets getEgoLaneWithNextLane( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const double width) -{ - // NOTE: findLaneIdsInterval returns (start, end) of associative_ids - const auto ego_lane_interval_opt = findLaneIdsInterval(path, associative_ids); - if (!ego_lane_interval_opt) { - return lanelet::ConstLanelets({}); - } - const auto [ego_start, ego_end] = ego_lane_interval_opt.value(); - if (ego_end < path.points.size() - 1) { - const int next_id = path.points.at(ego_end).lane_ids.at(0); - const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); - if (next_lane_interval_opt) { - const auto [next_start, next_end] = next_lane_interval_opt.value(); - return { - planning_utils::generatePathLanelet(path, ego_start, next_start + 1, width), - planning_utils::generatePathLanelet(path, next_start + 1, next_end, width)}; - } - } - return {planning_utils::generatePathLanelet(path, ego_start, ego_end, width)}; -} - static bool isTargetStuckVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) { @@ -1138,5 +1115,61 @@ void IntersectionLanelets::update( } } +static lanelet::ConstLanelets getPrevLanelets( + const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) +{ + lanelet::ConstLanelets prevs; + for (const auto & ll : lanelets_on_path) { + if (associative_ids.find(ll.id()) != associative_ids.end()) { + return prevs; + } + prevs.push_back(ll); + } + return prevs; +} + +std::optional generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::set & associative_ids, const size_t closest_idx, const double width) +{ + const auto assigned_lane_interval_opt = findLaneIdsInterval(path, associative_ids); + if (!assigned_lane_interval_opt) { + return std::nullopt; + } + + PathLanelets path_lanelets; + // prev + path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids); + path_lanelets.all = path_lanelets.prev; + + // entry2ego if exist + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval_opt.value(); + if (closest_idx > assigned_lane_start) { + path_lanelets.all.push_back( + planning_utils::generatePathLanelet(path, assigned_lane_start, closest_idx, width)); + } + + // ego_or_entry2exit + const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); + path_lanelets.ego_or_entry2exit = + planning_utils::generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width); + path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); + + // next + if (assigned_lane_end < path.points.size() - 1) { + const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); + const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); + if (next_lane_interval_opt) { + const auto [next_start, next_end] = next_lane_interval_opt.value(); + path_lanelets.next = + planning_utils::generatePathLanelet(path, next_start + 1, next_end, width); + path_lanelets.all.push_back(path_lanelets.next.value()); + } + } + + return path_lanelets; +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 081ade30f8f7d..e125f485a65e8 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -126,10 +126,6 @@ std::optional generateInterpolatedPath( geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); -lanelet::ConstLanelets getEgoLaneWithNextLane( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const double width); - bool checkStuckVehicleInIntersection( const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, @@ -160,6 +156,11 @@ double calcDistanceUntilIntersectionLanelet( const lanelet::ConstLanelet & assigned_lanelet, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx); +std::optional generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::set & associative_ids, const size_t closest_idx, const double width); + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 55e6308d7e67b..4d2cf9695b2db 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -132,6 +132,18 @@ struct IntersectionStopLines size_t pass_judge_line; }; +struct PathLanelets +{ + lanelet::ConstLanelets prev; + // lanelet::Constlanelet entry2ego; this is included in `all` if exists + lanelet::ConstLanelet + ego_or_entry2exit; // this is `assigned lane` part of the path(not from + // ego) if ego is before the intersection, otherwise from ego to exit + std::optional next = + std::nullopt; // this is nullopt is the goal is inside intersection + lanelet::ConstLanelets all; +}; + } // namespace behavior_velocity_planner::util #endif // UTIL_TYPE_HPP_