From 2ad1d816fa1bd31e0976fc9679c98ae2a9d56d4b Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Wed, 18 Oct 2023 23:23:25 +0900 Subject: [PATCH] perf(drivable area expansion): use faster lateral offset and nearest line calculations (#5349) Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.hpp | 5 ++- .../drivable_area_expansion/map_utils.hpp | 6 ++-- .../utils/drivable_area_expansion/types.hpp | 4 +++ .../drivable_area_expansion.cpp | 35 +++++++++++-------- .../drivable_area_expansion/map_utils.cpp | 14 +++++--- 5 files changed, 39 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index 19ea89a3ce3c7..3f6f107cdce51 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -68,14 +68,13 @@ void apply_bound_change_rate_limit( /// @brief calculate the maximum distance by which a bound can be expanded /// @param [in] path_poses input path /// @param [in] bound bound points -/// @param [in] uncrossable_lines lines that limit the bound expansion +/// @param [in] uncrossable_segments segments that limit the bound expansion, indexed in a Rtree /// @param [in] uncrossable_polygons polygons that limit the bound expansion /// @param [in] params parameters with the buffer distance to keep with lines, /// and the static maximum expansion distance std::vector calculate_maximum_distance( const std::vector & path_poses, const std::vector bound, - const std::vector & uncrossable_lines, - const std::vector & uncrossable_polygons, + const SegmentRtree & uncrossable_lines, const std::vector & uncrossable_polygons, const DrivableAreaExpansionParameters & params); /// @brief expand a bound by the given lateral distances away from the path diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp index 6f96b83237310..8c6bdb8a6943b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp @@ -25,12 +25,12 @@ namespace drivable_area_expansion { -/// @brief Extract uncrossable linestrings from the lanelet map that are in range of ego +/// @brief Extract uncrossable segments from the lanelet map that are in range of ego /// @param[in] lanelet_map lanelet map /// @param[in] ego_point point of the current ego position /// @param[in] params parameters with linestring types that cannot be crossed and maximum range -/// @return the uncrossable linestrings -MultiLineString2d extract_uncrossable_lines( +/// @return the uncrossable segments stored in a rtree +SegmentRtree extract_uncrossable_segments( const lanelet::LaneletMap & lanelet_map, const Point & ego_point, const DrivableAreaExpansionParameters & params); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index 7db92c163f567..8da1521db6c28 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -22,6 +22,8 @@ #include #include +#include + namespace drivable_area_expansion { using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -39,6 +41,8 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::Segment2d; +typedef boost::geometry::index::rtree> SegmentRtree; + struct PointDistance { Point2d point; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 25cf917d27135..3008f98331c92 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -36,12 +36,10 @@ namespace drivable_area_expansion namespace { - Point2d convert_point(const Point & p) { return Point2d{p.x, p.y}; } - } // namespace void reuse_previous_poses( @@ -61,11 +59,17 @@ void reuse_previous_poses( const auto deviation = motion_utils::calcLateralOffset(prev_poses, path.points.front().point.pose.position); if (first_idx && deviation < params.max_reuse_deviation) { + LineString2d path_ls; + for (const auto & p : path.points) path_ls.push_back(convert_point(p.point.pose.position)); for (auto idx = *first_idx; idx < prev_poses.size(); ++idx) { - if ( - motion_utils::calcLateralOffset(path.points, prev_poses[idx].position) > - params.max_reuse_deviation) - break; + double lateral_offset = std::numeric_limits::max(); + for (auto segment_idx = 0LU; segment_idx + 1 < path_ls.size(); ++segment_idx) { + const auto projection = point_to_line_projection( + convert_point(prev_poses[idx].position), path_ls[segment_idx], + path_ls[segment_idx + 1]); + lateral_offset = std::min(projection.distance, lateral_offset); + } + if (lateral_offset > params.max_reuse_deviation) break; cropped_poses.push_back(prev_poses[idx]); cropped_curvatures.push_back(prev_curvatures[idx]); } @@ -172,8 +176,7 @@ void apply_bound_change_rate_limit( std::vector calculate_maximum_distance( const std::vector & path_poses, const std::vector bound, - const std::vector & uncrossable_lines, - const std::vector & uncrossable_polygons, + const SegmentRtree & uncrossable_segments, const std::vector & uncrossable_polygons, const DrivableAreaExpansionParameters & params) { // TODO(Maxime): improve performances (dont use bg::distance ? use rtree ?) @@ -183,9 +186,13 @@ std::vector calculate_maximum_distance( for (const auto & p : bound) bound_ls.push_back(convert_point(p)); for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); for (auto i = 0UL; i + 1 < bound_ls.size(); ++i) { - const LineString2d segment_ls = {bound_ls[i], bound_ls[i + 1]}; - for (const auto & uncrossable_line : uncrossable_lines) { - const auto bound_to_line_dist = boost::geometry::distance(segment_ls, uncrossable_line); + const Segment2d segment_ls = {bound_ls[i], bound_ls[i + 1]}; + std::vector query_result; + boost::geometry::index::query( + uncrossable_segments, boost::geometry::index::nearest(segment_ls, 1), + std::back_inserter(query_result)); + if (!query_result.empty()) { + const auto bound_to_line_dist = boost::geometry::distance(segment_ls, query_result.front()); const auto dist_limit = std::max(0.0, bound_to_line_dist - params.avoid_linestring_dist); maximum_distances[i] = std::min(maximum_distances[i], dist_limit); maximum_distances[i + 1] = std::min(maximum_distances[i + 1], dist_limit); @@ -267,7 +274,7 @@ void expand_drivable_area( // crop first/last non deviating path_poses const auto & params = planner_data->drivable_area_expansion_parameters; const auto & route_handler = *planner_data->route_handler; - const auto uncrossable_lines = extract_uncrossable_lines( + const auto uncrossable_segments = extract_uncrossable_segments( *route_handler.getLaneletMapPtr(), planner_data->self_odometry->pose.pose.position, params); const auto uncrossable_polygons = create_object_footprints(*planner_data->dynamic_object, params); const auto preprocessing_ms = stop_watch.toc("preprocessing"); @@ -295,9 +302,9 @@ void expand_drivable_area( stop_watch.tic("max_dist"); const auto max_left_expansions = calculate_maximum_distance( - path_poses, path.left_bound, uncrossable_lines, uncrossable_polygons, params); + path_poses, path.left_bound, uncrossable_segments, uncrossable_polygons, params); const auto max_right_expansions = calculate_maximum_distance( - path_poses, path.right_bound, uncrossable_lines, uncrossable_polygons, params); + path_poses, path.right_bound, uncrossable_segments, uncrossable_polygons, params); for (auto i = 0LU; i < left_expansions.size(); ++i) left_expansions[i] = std::min(left_expansions[i], max_left_expansions[i]); for (auto i = 0LU; i < right_expansions.size(); ++i) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp index deeb787cf39f6..6ed14138c62e4 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp @@ -24,22 +24,26 @@ namespace drivable_area_expansion { -MultiLineString2d extract_uncrossable_lines( +SegmentRtree extract_uncrossable_segments( const lanelet::LaneletMap & lanelet_map, const Point & ego_point, const DrivableAreaExpansionParameters & params) { - MultiLineString2d uncrossable_lines_in_range; + SegmentRtree uncrossable_segments_in_range; LineString2d line; const auto ego_p = Point2d{ego_point.x, ego_point.y}; for (const auto & ls : lanelet_map.lineStringLayer) { if (has_types(ls, params.avoid_linestring_types)) { line.clear(); for (const auto & p : ls) line.push_back(Point2d{p.x(), p.y()}); - if (boost::geometry::distance(line, ego_p) < params.max_path_arc_length) - uncrossable_lines_in_range.push_back(line); + for (auto segment_idx = 0LU; segment_idx + 1 < line.size(); ++segment_idx) { + Segment2d segment = {line[segment_idx], line[segment_idx + 1]}; + if (boost::geometry::distance(segment, ego_p) < params.max_path_arc_length) { + uncrossable_segments_in_range.insert(segment); + } + } } } - return uncrossable_lines_in_range; + return uncrossable_segments_in_range; } bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types)