From 7dee9e7152104f48c65f0a89667472817a22e8d5 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 25 Jul 2024 12:14:25 +0900 Subject: [PATCH] fix(lane_change): filtering object ahead of terminal (#8093) * employ lanelet based filtering before distance based filtering Signed-off-by: Zulfaqar Azmi * use distance based to terminal check instead Signed-off-by: Zulfaqar Azmi * remove RCLCPP INFO Signed-off-by: Muhammad Zulfaqar Azmi * update flow chart Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../README.md | 84 +++++----------- .../scene.hpp | 6 +- .../utils/data_structs.hpp | 1 + .../utils/utils.hpp | 12 +++ .../src/scene.cpp | 99 ++++++------------- .../src/utils/utils.cpp | 80 +++++++++++++++ 6 files changed, 153 insertions(+), 129 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 5790be377b7aa..63f5a2ec05bf1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -331,8 +331,7 @@ title NormalLaneChange::filterObjects Method Execution Flow start group "Filter Objects by Class" { -:Iterate through each object in objects list; -while (has not finished iterating through object list) is (TRUE) +while (has not finished iterating through predicted object list) is (TRUE) if (current object type != param.object_types_to_check?) then (TRUE) #LightPink:Remove current object; else (FALSE) @@ -341,17 +340,15 @@ endif end while end group -if (object list is empty?) then (TRUE) +if (predicted object list is empty?) then (TRUE) :Return empty result; stop else (FALSE) endif group "Filter Oncoming Objects" #PowderBlue { -:Iterate through each object in target lane objects list; -while (has not finished iterating through object list?) is (TRUE) -:check object's yaw with reference to ego's yaw.; -if (yaw difference < 90 degree?) then (TRUE) +while (has not finished iterating through predicted object list?) is (TRUE) +if (object's yaw with reference to ego's yaw difference < 90 degree?) then (TRUE) :Keep current object; else (FALSE) if (object is stopping?) then (TRUE) @@ -363,31 +360,7 @@ endif endwhile end group -if (object list is empty?) then (TRUE) - :Return empty result; - stop -else (FALSE) -endif - -group "Filter Objects Ahead Terminal" #Beige { -:Calculate lateral distance from ego to current lanes center; - -:Iterate through each object in objects list; -while (has not finished iterating through object list) is (TRUE) - :Get current object's polygon; - :Initialize distance to terminal from object to max; - while (has not finished iterating through object polygon's vertices) is (TRUE) - :Calculate object's lateral distance to end of lane; - :Update minimum distance to terminal from object; - end while - if (Is object's distance to terminal exceeds minimum lane change length?) then (TRUE) - #LightPink:Remove current object; - else (FALSE) - endif -end while -end group - -if (object list is empty?) then (TRUE) +if (predicted object list is empty?) then (TRUE) :Return empty result; stop else (FALSE) @@ -395,21 +368,27 @@ endif group "Filter Objects By Lanelets" #LightGreen { -:Iterate through each object in objects list; -while (has not finished iterating through object list) is (TRUE) - :lateral distance diff = difference between object's lateral distance and ego's lateral distance to the current lanes' centerline.; - if (Object in target lane polygon, and lateral distance diff is more than half of ego's width?) then (TRUE) - :Add to target_lane_objects; - else (FALSE) - if (Object overlaps with backward target lanes?) then (TRUE) +while (has not finished iterating through predicted object list) is (TRUE) + :Calculate lateral distance diff; + if (Object in target lane polygon?) then (TRUE) + if (lateral distance diff > half of ego's width?) then (TRUE) + if (Object's physical position is before terminal point?) then (TRUE) :Add to target_lane_objects; else (FALSE) - if (Object in current lane polygon?) then (TRUE) - :Add to current_lane_objects; - else (FALSE) - :Add to other_lane_objects; - endif endif + else (FALSE) + endif + else (FALSE) + endif + + if (Object overlaps with backward target lanes?) then (TRUE) + :Add to target_lane_objects; + else (FALSE) + if (Object in current lane polygon?) then (TRUE) + :Add to current_lane_objects; + else (FALSE) + :Add to other_lane_objects; + endif endif end while @@ -426,13 +405,10 @@ endif group "Filter Target Lanes' objects" #LightCyan { -:Iterate through each object in target lane objects list; -while (has not finished iterating through object list) is (TRUE) - :check object's velocity; +while (has not finished iterating through target lanes' object list) is (TRUE) if(velocity is within threshold?) then (TRUE) :Keep current object; else (FALSE) - :check whether object is ahead of ego; if(object is ahead of ego?) then (TRUE) :keep current object; else (FALSE) @@ -444,11 +420,8 @@ end group group "Filter Current Lanes' objects" #LightYellow { -:Iterate through each object in current lane objects list; -while (has not finished iterating through object list) is (TRUE) - :check object's velocity; +while (has not finished iterating through current lanes' object list) is (TRUE) if(velocity is within threshold?) then (TRUE) - :check whether object is ahead of ego; if(object is ahead of ego?) then (TRUE) :keep current object; else (FALSE) @@ -462,11 +435,8 @@ end group group "Filter Other Lanes' objects" #Lavender { -:Iterate through each object in other lane objects list; -while (has not finished iterating through object list) is (TRUE) - :check object's velocity; +while (has not finished iterating through other lanes' object list) is (TRUE) if(velocity is within threshold?) then (TRUE) - :check whether object is ahead of ego; if(object is ahead of ego?) then (TRUE) :keep current object; else (FALSE) @@ -478,7 +448,7 @@ while (has not finished iterating through object list) is (TRUE) endwhile end group -:Trasform the objects into extended predicted object and return them as lane_change_target_objects; +:Transform the objects into extended predicted object and return them as lane_change_target_objects; stop @enduml diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index dad96d5f7371a..a1284f355a87d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -128,12 +128,8 @@ class NormalLaneChange : public LaneChangeBase void filterOncomingObjects(PredictedObjects & objects) const; - void filterAheadTerminalObjects( - PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const; - void filterObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, + const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path, std::vector & current_lane_objects, std::vector & target_lane_objects, std::vector & other_lane_objects) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 1bb4dfdeb59dc..495fe9012ecd0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -185,6 +185,7 @@ struct PhaseInfo struct Lanes { + bool current_lane_in_goal_section{false}; lanelet::ConstLanelets current; lanelet::ConstLanelets target; std::vector preceding_target; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 703d882794ee4..6dbed08ea651d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -302,6 +302,18 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr); bool is_same_lane_with_prev_iteration( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); + +Pose to_pose( + const autoware::universe_utils::Point2d & point, + const geometry_msgs::msg::Quaternion & orientation); + +bool is_ahead_of_ego( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, + const PredictedObject & object); + +bool is_before_terminal( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, + const PredictedObject & object); } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 7a4b72631629d..0969103ff585b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -83,8 +83,11 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->lanes_ptr->current = current_lanes; common_data_ptr_->lanes_ptr->target = target_lanes; + const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; + common_data_ptr_->lanes_ptr->current_lane_in_goal_section = + route_handler_ptr->isInGoalRouteSection(current_lanes.back()); common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( - *common_data_ptr_->route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), + *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), common_data_ptr_->lc_param_ptr->backward_lane_length); *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); @@ -983,7 +986,6 @@ ExtendedPredictedObjects NormalLaneChange::getTargetObjects( LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const { - const auto & current_pose = getEgoPose(); const auto & route_handler = getRouteHandler(); const auto & common_parameters = planner_data_->parameters; auto objects = *planner_data_->dynamic_object; @@ -1006,12 +1008,6 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const return {}; } - filterAheadTerminalObjects(objects, current_lanes); - - if (objects.objects.empty()) { - return {}; - } - std::vector target_lane_objects; std::vector current_lane_objects; std::vector other_lane_objects; @@ -1022,9 +1018,11 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const return {}; } + const auto path = + route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + filterObjectsByLanelets( - objects, current_lanes, target_lanes, current_lane_objects, target_lane_objects, - other_lane_objects); + objects, path, current_lane_objects, target_lane_objects, other_lane_objects); const auto is_within_vel_th = [](const auto & object) -> bool { constexpr double min_vel_th = 1.0; @@ -1032,38 +1030,25 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); }; - const auto path = - route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - - if (path.points.empty()) { - return {}; - } - - const auto is_ahead_of_ego = [&path, ¤t_pose](const auto & object) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); - - double max_dist_ego_to_obj = std::numeric_limits::lowest(); - for (const auto & polygon_p : obj_polygon) { - const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const auto dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); - max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); - } - return max_dist_ego_to_obj >= 0.0; - }; - utils::path_safety_checker::filterObjects( target_lane_objects, [&](const PredictedObject & object) { - return (is_within_vel_th(object) || is_ahead_of_ego(object)); + const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); + return is_within_vel_th(object) || ahead_of_ego; }); - utils::path_safety_checker::filterObjects( - other_lane_objects, [&](const PredictedObject & object) { - return is_within_vel_th(object) && is_ahead_of_ego(object); - }); + if (lane_change_parameters_->check_objects_on_other_lanes) { + utils::path_safety_checker::filterObjects( + other_lane_objects, [&](const PredictedObject & object) { + const auto ahead_of_ego = + utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); + return is_within_vel_th(object) && ahead_of_ego; + }); + } utils::path_safety_checker::filterObjects( current_lane_objects, [&](const PredictedObject & object) { - return is_within_vel_th(object) && is_ahead_of_ego(object); + const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); + return is_within_vel_th(object) && ahead_of_ego; }); LaneChangeLanesFilteredObjects lane_change_target_objects; @@ -1119,42 +1104,15 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const }); } -void NormalLaneChange::filterAheadTerminalObjects( - PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const -{ - const auto & current_pose = getEgoPose(); - const auto & route_handler = getRouteHandler(); - const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( - route_handler, current_lanes.back(), *lane_change_parameters_, direction_); - - const auto dist_ego_to_current_lanes_center = - lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - - // ignore object that are ahead of terminal lane change start - utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); - // ignore object that are ahead of terminal lane change start - auto distance_to_terminal_from_object = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon) { - const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - Pose polygon_pose; - polygon_pose.position = obj_p; - polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; - const auto dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes); - distance_to_terminal_from_object = std::min(dist_ego_to_current_lanes_center, dist); - } - - return (minimum_lane_change_length > distance_to_terminal_from_object); - }); -} - void NormalLaneChange::filterObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, std::vector & current_lane_objects, + const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path, + std::vector & current_lane_objects, std::vector & target_lane_objects, std::vector & other_lane_objects) const { const auto & current_pose = getEgoPose(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto & route_handler = getRouteHandler(); const auto & common_parameters = planner_data_->parameters; const auto check_optional_polygon = [](const auto & object, const auto & polygon) { @@ -1199,7 +1157,14 @@ void NormalLaneChange::filterObjectsByLanelets( return std::abs(lateral) > (common_parameters.vehicle_width / 2); }); - if (check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far) { + const auto is_before_terminal = [&]() { + return utils::lane_change::is_before_terminal( + common_data_ptr_, current_lanes_ref_path, object); + }; + + if ( + check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far && + is_before_terminal()) { target_lane_objects.push_back(object); continue; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 6ea280817f39b..2ef7bd0a41cc3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -1216,6 +1216,7 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) lanes_polygon.current = utils::lane_change::createPolygon(lanes->current, 0.0, std::numeric_limits::max()); + lanes_polygon.target = utils::lane_change::createPolygon(lanes->target, 0.0, std::numeric_limits::max()); @@ -1263,6 +1264,85 @@ bool is_same_lane_with_prev_iteration( return (prev_target_lanes.front().id() == target_lanes.front().id()) && (prev_target_lanes.back().id() == prev_target_lanes.back().id()); } + +Pose to_pose( + const autoware::universe_utils::Point2d & point, + const geometry_msgs::msg::Quaternion & orientation) +{ + Pose pose; + pose.position = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); + pose.orientation = orientation; + return pose; +} + +bool is_ahead_of_ego( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, + const PredictedObject & object) +{ + const auto & current_ego_pose = common_data_ptr->get_ego_pose(); + + const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; + + const auto dist_to_base_link = autoware::motion_utils::calcSignedArcLength( + path.points, current_ego_pose.position, obj_position); + const auto & ego_info = common_data_ptr->bpp_param_ptr->vehicle_info; + const auto lon_dev = std::max( + ego_info.max_longitudinal_offset_m + ego_info.rear_overhang_m, object.shape.dimensions.x); + + // we don't always have to check the distance accurately. + if (std::abs(dist_to_base_link) > lon_dev) { + return dist_to_base_link >= 0.0; + } + + const auto ego_polygon = getEgoCurrentFootprint(current_ego_pose, ego_info).outer(); + auto ego_min_dist_to_end = std::numeric_limits::max(); + for (const auto & ego_edge_point : ego_polygon) { + const auto ego_edge = + autoware::universe_utils::createPoint(ego_edge_point.x(), ego_edge_point.y(), 0.0); + const auto dist_to_end = autoware::motion_utils::calcSignedArcLength( + path.points, ego_edge, path.points.back().point.pose.position); + ego_min_dist_to_end = std::min(dist_to_end, ego_min_dist_to_end); + } + + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); + auto current_min_dist_to_end = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon) { + const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto dist_ego_to_obj = autoware::motion_utils::calcSignedArcLength( + path.points, obj_p, path.points.back().point.pose.position); + current_min_dist_to_end = std::min(dist_ego_to_obj, current_min_dist_to_end); + } + return ego_min_dist_to_end - current_min_dist_to_end >= 0.0; +} + +bool is_before_terminal( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, + const PredictedObject & object) +{ + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + const auto & lanes_ptr = common_data_ptr->lanes_ptr; + const auto terminal_position = (lanes_ptr->current_lane_in_goal_section) + ? route_handler_ptr->getGoalPose().position + : path.points.back().point.pose.position; + double current_max_dist = std::numeric_limits::lowest(); + + const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; + const auto dist_to_base_link = + autoware::motion_utils::calcSignedArcLength(path.points, obj_position, terminal_position); + // we don't always have to check the distance accurately. + if (std::abs(dist_to_base_link) > object.shape.dimensions.x) { + return dist_to_base_link >= 0.0; + } + + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); + for (const auto & polygon_p : obj_polygon) { + const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto dist_obj_to_terminal = + autoware::motion_utils::calcSignedArcLength(path.points, obj_p, terminal_position); + current_max_dist = std::max(dist_obj_to_terminal, current_max_dist); + } + return current_max_dist >= 0.0; +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug