From a7de50420109a0a669dd5e4dbb7237bb45911032 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 24 Jan 2023 20:14:29 +0900 Subject: [PATCH] fix(behavior_path_planner): reduce obj indices call in lane change (#2726) * fix(behavior_path_planner): reduce obj indices call in lane change Signed-off-by: Muhammad Zulfaqar Azmi * remove unused functions Signed-off-by: Muhammad Zulfaqar Azmi * add continue for the intersect in current * replace target and current lanes Signed-off-by: Muhammad Zulfaqar Azmi * change isLaneChangePathSafe arguments Signed-off-by: Muhammad Zulfaqar Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar --- .../scene_module/lane_change/util.hpp | 2 +- .../behavior_path_planner/utilities.hpp | 4 - .../external_request_lane_change_module.cpp | 5 +- .../lane_change/lane_change_module.cpp | 5 +- .../src/scene_module/lane_change/util.cpp | 116 +++++++++++++++--- .../behavior_path_planner/src/utilities.cpp | 24 ---- 6 files changed, 101 insertions(+), 55 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index dfc5708f10480..d0b83171771ba 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -90,7 +90,7 @@ bool selectSafePath( std::unordered_map & debug_data); bool isLaneChangePathSafe( - const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, + const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const size_t current_seg_idx, const Twist & current_twist, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index afefd8985ecb2..6292867db7053 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -320,10 +320,6 @@ std::pair, std::vector> separateObjectIndicesByLanel std::pair separateObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); -std::vector filterObjectsIndicesByPath( - const PredictedObjects & objects, const std::vector & object_indices, - const PathWithLaneId & ego_path, const double vehicle_width); - PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); PredictedObjects filterObjectsByVelocity( diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index ec000759aa002..786eb0b512696 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -695,9 +695,8 @@ bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_ path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); return lane_change_utils::isLaneChangePathSafe( - path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, - current_twist, common_parameters, *parameters_, - common_parameters.expected_front_deceleration_for_abort, + path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, + common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort, common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, false, status_.lane_change_path.acceleration); } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 1203cf6139eea..67c2aa296b1d8 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -694,9 +694,8 @@ bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) cons path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); return lane_change_utils::isLaneChangePathSafe( - path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, - current_twist, common_parameters, *parameters_, - common_parameters.expected_front_deceleration_for_abort, + path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, + common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort, common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, false, status_.lane_change_path.acceleration); } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 8b9cfb504e72f..a1c59c38365ed 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp" +#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" #include "behavior_path_planner/utilities.hpp" @@ -34,6 +35,85 @@ #include #include +namespace +{ + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::util::calcObjectPolygon; +using behavior_path_planner::util::getHighestProbLabel; +using geometry_msgs::msg::Pose; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +void filterObjectIndices( + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & ego_path, + const Pose & current_pose, const double forward_path_length, const double filter_width, + std::vector & current_lane_obj_indices, std::vector & target_lane_obj_indices, + std::vector & others_obj_indices, const bool ignore_unknown_obj = false) +{ + // Reserve maximum amount possible + current_lane_obj_indices.reserve(objects.objects.size()); + target_lane_obj_indices.reserve(objects.objects.size()); + others_obj_indices.reserve(objects.objects.size()); + + const auto get_basic_polygon = + [](const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist) { + const auto polygon_3d = lanelet::utils::getPolygonFromArcLength(lanes, start_dist, end_dist); + return lanelet::utils::to2D(polygon_3d).basicPolygon(); + }; + const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); + const auto current_polygon = + get_basic_polygon(current_lanes, arc.length, arc.length + forward_path_length); + const auto target_polygon = + get_basic_polygon(target_lanes, 0.0, std::numeric_limits::max()); + LineString2d ego_path_linestring; + ego_path_linestring.reserve(ego_path.points.size()); + for (const auto & pt : ego_path.points) { + const auto & position = pt.point.pose.position; + boost::geometry::append(ego_path_linestring, Point2d(position.x, position.y)); + } + + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & obj = objects.objects.at(i); + + if (ignore_unknown_obj) { + const auto t = getHighestProbLabel(obj.classification); + if (t == ObjectClassification::UNKNOWN) { + continue; + } + } + + Polygon2d obj_polygon; + if (!calcObjectPolygon(obj, &obj_polygon)) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change"), + "Failed to calcObjectPolygon...!!!"); + continue; + } + + if (boost::geometry::intersects(current_polygon, obj_polygon)) { + const double distance = boost::geometry::distance(obj_polygon, ego_path_linestring); + + if (distance < filter_width) { + current_lane_obj_indices.push_back(i); + continue; + } + } + + const bool is_intersect_with_target = boost::geometry::intersects(target_polygon, obj_polygon); + if (is_intersect_with_target) { + target_lane_obj_indices.push_back(i); + } else { + others_obj_indices.push_back(i); + } + } +} +} // namespace + namespace behavior_path_planner::lane_change_utils { using autoware_auto_planning_msgs::msg::PathPointWithLaneId; @@ -337,7 +417,7 @@ bool selectSafePath( common_parameters.ego_nearest_yaw_threshold); Pose ego_pose_before_collision; if (isLaneChangePathSafe( - path.path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx, + path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, common_parameters, ros_parameters, common_parameters.expected_front_deceleration, common_parameters.expected_rear_deceleration, ego_pose_before_collision, debug_data, true, @@ -403,7 +483,7 @@ bool hasEnoughDistance( } bool isLaneChangePathSafe( - const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, + const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const size_t current_seg_idx, const Twist & current_twist, @@ -417,6 +497,7 @@ bool isLaneChangePathSafe( return true; } + const auto & path = lane_change_path.path; if (path.points.empty() || target_lanes.empty() || current_lanes.empty()) { return false; } @@ -434,25 +515,20 @@ bool isLaneChangePathSafe( time_resolution, acceleration, min_lc_speed); const auto prepare_phase_ignore_target_speed_thresh = lane_change_parameters.prepare_phase_ignore_target_speed_thresh; - - const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); - - // find obstacle in lane change target lanes - // retrieve lanes that are merging target lanes as well - const auto target_lane_object_indices = - util::filterObjectIndicesByLanelets(*dynamic_objects, target_lanes); - - // find objects in current lane - const double check_distance = common_parameters.forward_path_length; - const auto current_lane_object_indices_lanelet = util::filterObjectIndicesByLanelets( - *dynamic_objects, current_lanes, arc.length, arc.length + check_distance); - - const double lateral_buffer = (use_buffer) ? 0.5 : 0.0; const auto & vehicle_info = common_parameters.vehicle_info; - const auto & vehicle_width = common_parameters.vehicle_width; - const auto current_lane_object_indices = util::filterObjectsIndicesByPath( - *dynamic_objects, current_lane_object_indices_lanelet, path, - vehicle_width / 2 + lateral_buffer); + + std::vector current_lane_object_indices{}; + std::vector target_lane_object_indices{}; + std::vector other_lane_object_indices{}; + { + const auto lateral_buffer = (use_buffer) ? 0.5 : 0.0; + const auto current_obj_filtering_buffer = lateral_buffer + common_parameters.vehicle_width / 2; + + filterObjectIndices( + *dynamic_objects, lane_change_path.reference_lanelets, lane_change_path.target_lanelets, path, + current_pose, common_parameters.forward_path_length, current_obj_filtering_buffer, + current_lane_object_indices, target_lane_object_indices, other_lane_object_indices, true); + } const auto assignDebugData = [](const PredictedObject & obj) { CollisionCheckDebug debug; diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 5370556f81c37..9a111b358f41e 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -720,30 +720,6 @@ std::vector calcObjectsDistanceToPath( return distance_array; } -std::vector filterObjectsIndicesByPath( - const PredictedObjects & objects, const std::vector & object_indices, - const PathWithLaneId & ego_path, const double vehicle_width) -{ - std::vector indices; - const auto ego_path_point_array = convertToGeometryPointArray(ego_path); - for (const auto & i : object_indices) { - Polygon2d obj_polygon; - if (!calcObjectPolygon(objects.objects.at(i), &obj_polygon)) { - continue; - } - LineString2d ego_path_line; - ego_path_line.reserve(ego_path_point_array.size()); - for (const auto & ego_path_point : ego_path_point_array) { - boost::geometry::append(ego_path_line, Point2d(ego_path_point.x, ego_path_point.y)); - } - const double distance = boost::geometry::distance(obj_polygon, ego_path_line); - if (distance < vehicle_width) { - indices.push_back(i); - } - } - return indices; -} - PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) { PathWithLaneId filtered_path;