From d612716c11e0d64938556fdfb1f36c9367eaecc4 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 26 Dec 2023 09:23:43 +0900 Subject: [PATCH] feat(drivable area expansion): do not over expand beyond the desired width (#5640) Signed-off-by: Maxime CLEMENT --- .../config/drivable_area_expansion.param.yaml | 2 +- .../src/behavior_path_planner_node.cpp | 4 +- .../drivable_area_expansion.hpp | 50 +++- .../drivable_area_expansion/parameters.hpp | 8 +- .../path_projection.hpp | 18 +- .../utils/drivable_area_expansion/types.hpp | 26 ++- .../drivable_area_expansion.cpp | 221 ++++++++++++------ .../test/test_drivable_area_expansion.cpp | 6 +- 8 files changed, 233 insertions(+), 102 deletions(-) diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml index c0b6f259c7726..bb897db393b65 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -13,7 +13,7 @@ smoothing: curvature_average_window: 3 # window size used for smoothing the curvatures using a moving window average max_bound_rate: 1.0 # [m/m] maximum rate of change of the bound lateral distance over its arc length - extra_arc_length: 2.0 # [m] extra arc length where an expansion distance is initially applied + arc_length_range: 2.0 # [m] arc length range where an expansion distance is initially applied ego: extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase extra_front_overhang: 0.5 # [m] extra length to add to the front overhang diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 85abf774d159e..93747fc6e9aa3 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -929,8 +929,8 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM, planner_data_->drivable_area_expansion_parameters.max_bound_rate); updateParam( - parameters, DrivableAreaExpansionParameters::SMOOTHING_EXTRA_ARC_LENGTH_PARAM, - planner_data_->drivable_area_expansion_parameters.extra_arc_length); + parameters, DrivableAreaExpansionParameters::SMOOTHING_ARC_LENGTH_RANGE_PARAM, + planner_data_->drivable_area_expansion_parameters.arc_length_range); updateParam( parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM, planner_data_->drivable_area_expansion_parameters.print_runtime); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp index d0eec8f2b8901..7e4cf94c890d8 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -35,14 +35,14 @@ void expand_drivable_area( PathWithLaneId & path, const std::shared_ptr planner_data); -/// @brief prepare path poses and try to reuse their previously calculated curvatures +/// @brief try to reuse the previous path poses and their previously calculated curvatures /// @details poses are reused if they do not deviate too much from the current path /// @param [in] path input path /// @param [inout] prev_poses previous poses to reuse /// @param [inout] prev_curvatures previous curvatures to reuse /// @param [in] ego_point current ego point /// @param [in] params parameters for reuse criteria and resampling interval -void update_path_poses_and_previous_curvatures( +void reuse_previous_poses( const PathWithLaneId & path, std::vector & prev_poses, std::vector & prev_curvatures, const Point & ego_point, const DrivableAreaExpansionParameters & params); @@ -57,6 +57,36 @@ void update_path_poses_and_previous_curvatures( double calculate_minimum_lane_width( const double curvature_radius, const DrivableAreaExpansionParameters & params); +/// @brief calculate mappings between path poses and the given drivable area bound +/// @param [inout] expansion expansion data to update with the mapping +/// @param [in] path_poses path poses +/// @param [in] bound drivable area bound +/// @param [in] Side left or right side +void calculate_bound_index_mappings( + Expansion & expansion, const std::vector & path_poses, const std::vector & bound, + const Side side); + +/// @brief apply expansion distances to all bound points within the given arc length range +/// @param [inout] expansion expansion data to update +/// @param [in] bound drivable area bound +/// @param [in] arc_length_range [m] arc length range where the expansion distances are also applied +/// @param [in] Side left or right side +void apply_arc_length_range_smoothing( + Expansion & expansion, const std::vector & bound, const double arc_length_range, + const Side side); + +/// @brief calculate minimum lane widths and mappings between path and and drivable area bounds +/// @param [in] path_poses path poses +/// @param [in] left_bound left drivable area bound +/// @param [in] right_bound right drivable area bound +/// @param [in] curvatures curvatures at each path point +/// @param [in] params parameters with the vehicle dimensions used to calculate the min lane width +/// @return expansion data (path->bound mappings, min lane widths, ...) +Expansion calculate_expansion( + const std::vector & path_poses, const std::vector & left_bound, + const std::vector & right_bound, const std::vector & curvatures, + const DrivableAreaExpansionParameters & params); + /// @brief smooth the bound by applying a limit on its rate of change /// @details rate of change is the lateral distance from the path over the arc length along the path /// @param [inout] bound_distances bound distances (lateral distance from the path) @@ -66,16 +96,16 @@ void apply_bound_change_rate_limit( std::vector & distances, const std::vector & bound, const double max_rate); /// @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_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 +/// @param [in] Side left or right side std::vector calculate_maximum_distance( - const std::vector & path_poses, const std::vector bound, - const SegmentRtree & uncrossable_lines, const std::vector & uncrossable_polygons, - const DrivableAreaExpansionParameters & params); + const std::vector & bound, const SegmentRtree & uncrossable_lines, + const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params, const Side side); /// @brief expand a bound by the given lateral distances away from the path /// @param [inout] bound bound points to expand @@ -85,6 +115,14 @@ void expand_bound( std::vector & bound, const std::vector & path_poses, const std::vector & distances); +/// @brief calculate the expansion distances of the left and right drivable area bounds +/// @param [inout] expansion expansion data to be updated with the left/right expansion distances +/// @param [in] max_left_expansions maximum left expansion distances +/// @param [in] max_right_expansions maximum right expansion distances +void calculate_expansion_distances( + Expansion & expansion, const std::vector & max_left_expansions, + const std::vector & max_right_expansions); + /// @brief calculate smoothed curvatures /// @details smoothing is done using a moving average /// @param [in] poses input poses used to calculate the curvatures diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp index 6d1497397ad27..055e20a4cda02 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp @@ -55,8 +55,8 @@ struct DrivableAreaExpansionParameters "dynamic_expansion.smoothing.curvature_average_window"; static constexpr auto SMOOTHING_MAX_BOUND_RATE_PARAM = "dynamic_expansion.smoothing.max_bound_rate"; - static constexpr auto SMOOTHING_EXTRA_ARC_LENGTH_PARAM = - "dynamic_expansion.smoothing.extra_arc_length"; + static constexpr auto SMOOTHING_ARC_LENGTH_RANGE_PARAM = + "dynamic_expansion.smoothing.arc_length_range"; static constexpr auto PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime"; // static expansion @@ -78,7 +78,7 @@ struct DrivableAreaExpansionParameters double max_expansion_distance{}; double max_path_arc_length{}; double resample_interval{}; - double extra_arc_length{}; + double arc_length_range{}; double max_reuse_deviation{}; bool avoid_dynamic_objects{}; bool print_runtime{}; @@ -103,7 +103,7 @@ struct DrivableAreaExpansionParameters extra_width = node.declare_parameter(EGO_EXTRA_WIDTH); curvature_average_window = node.declare_parameter(SMOOTHING_CURVATURE_WINDOW_PARAM); max_bound_rate = node.declare_parameter(SMOOTHING_MAX_BOUND_RATE_PARAM); - extra_arc_length = node.declare_parameter(SMOOTHING_EXTRA_ARC_LENGTH_PARAM); + arc_length_range = node.declare_parameter(SMOOTHING_ARC_LENGTH_RANGE_PARAM); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); resample_interval = node.declare_parameter(RESAMPLE_INTERVAL_PARAM); max_reuse_deviation = node.declare_parameter(MAX_REUSE_DEVIATION_PARAM); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp index e60b35b95515f..2aeac123623ce 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp @@ -26,7 +26,6 @@ namespace drivable_area_expansion { /// @brief project a point to a segment -/// @details the distance is signed based on the side of the point: left=positive, right=negative /// @param p point to project on the segment /// @param p1 first segment point /// @param p2 second segment point @@ -37,22 +36,18 @@ inline PointDistance point_to_segment_projection( const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()}; - const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x(); - const auto dist_sign = cross < 0.0 ? -1.0 : 1.0; - const auto c1 = boost::geometry::dot_product(p_vec, p2_vec); - if (c1 <= 0) return {p1, boost::geometry::distance(p, p1) * dist_sign}; + if (c1 <= 0) return {p1, boost::geometry::distance(p, p1)}; const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec); - if (c2 <= c1) return {p2, boost::geometry::distance(p, p2) * dist_sign}; + if (c2 <= c1) return {p2, boost::geometry::distance(p, p2)}; const auto projection = p1 + (p2_vec * c1 / c2); const auto projection_point = Point2d{projection.x(), projection.y()}; - return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; + return {projection_point, boost::geometry::distance(p, projection_point)}; } /// @brief project a point to a line -/// @details the distance is signed based on the side of the point: left=positive, right=negative /// @param p point to project on the line /// @param p1 first line point /// @param p2 second line point @@ -63,14 +58,11 @@ inline PointDistance point_to_line_projection( const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()}; - const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x(); - const auto dist_sign = cross < 0.0 ? -1.0 : 1.0; - const auto c1 = boost::geometry::dot_product(p_vec, p2_vec); const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec); const auto projection = p1 + (p2_vec * c1 / c2); const auto projection_point = Point2d{projection.x(), projection.y()}; - return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; + return {projection_point, boost::geometry::distance(p, projection_point)}; } /// @brief project a point to a linestring @@ -84,7 +76,7 @@ inline Projection point_to_linestring_projection(const Point2d & p, const LineSt auto arc_length = 0.0; for (auto ls_it = ls.begin(); std::next(ls_it) != ls.end(); ++ls_it) { const auto pd = point_to_segment_projection(p, *ls_it, *(ls_it + 1)); - if (std::abs(pd.distance) < std::abs(closest.distance)) { + if (pd.distance < closest.distance) { closest.projected_point = pd.point; closest.distance = pd.distance; closest.arc_length = arc_length + boost::geometry::distance(*ls_it, pd.point); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp index a7b91e5f7a8c9..e438105c6645e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp @@ -24,6 +24,9 @@ #include +#include +#include + namespace drivable_area_expansion { using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -45,16 +48,27 @@ using SegmentRtree = boost::geometry::index::rtree left_distances; + std::vector right_distances; + // mappings from path index + std::vector left_bound_indexes; + std::vector left_projections; + std::vector right_bound_indexes; + std::vector right_projections; + std::vector min_lane_widths; +}; } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 866d114815a86..da7f0fbb327d7 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -120,52 +120,78 @@ double calculate_minimum_lane_width( return (a * a + 2.0 * a * l + 2.0 * k * w + l * l + w * w) / (2.0 * k + w); } -std::vector calculate_minimum_expansions( - const std::vector & path_poses, const std::vector bound, - const std::vector curvatures, const Side side, - const DrivableAreaExpansionParameters & params) +void calculate_bound_index_mappings( + Expansion & expansion, const std::vector & path_poses, const std::vector & bound, + const Side side) { - std::vector minimum_expansions(bound.size()); size_t lb_idx = 0; + auto & bound_indexes = + (side == LEFT ? expansion.left_bound_indexes : expansion.right_bound_indexes); + auto & bound_projections = + (side == LEFT ? expansion.left_projections : expansion.right_projections); + bound_indexes.resize(path_poses.size(), 0LU); + bound_projections.resize(path_poses.size(), {{}, std::numeric_limits::max()}); for (auto path_idx = 0UL; path_idx < path_poses.size(); ++path_idx) { - const auto & path_pose = path_poses[path_idx]; - if (curvatures[path_idx] == 0.0) continue; - const auto curvature_radius = 1 / curvatures[path_idx]; - const auto min_lane_width = calculate_minimum_lane_width(curvature_radius, params); - const auto side_distance = min_lane_width / 2.0 * (side == LEFT ? 1.0 : -1.0); - const auto offset_point = - tier4_autoware_utils::calcOffsetPose(path_pose, 0.0, side_distance, 0.0).position; + const auto path_p = convert_point(path_poses[path_idx].position); for (auto bound_idx = lb_idx; bound_idx + 1 < bound.size(); ++bound_idx) { - const auto & prev_p = bound[bound_idx]; - const auto & next_p = bound[bound_idx + 1]; - const auto intersection_point = - tier4_autoware_utils::intersect(offset_point, path_pose.position, prev_p, next_p); - if (intersection_point) { - lb_idx = bound_idx; - const auto dist = tier4_autoware_utils::calcDistance2d(*intersection_point, offset_point); - minimum_expansions[bound_idx] = std::max(minimum_expansions[bound_idx], dist); - minimum_expansions[bound_idx + 1] = std::max(minimum_expansions[bound_idx + 1], dist); - // apply the expansion to all bound points within the extra arc length - auto arc_length = - tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]); - for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { - arc_length += - tier4_autoware_utils::calcDistance2d(bound[up_bound_idx - 1], bound[up_bound_idx]); - if (arc_length > params.extra_arc_length) break; - minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist); - } - arc_length = tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx]); - for (auto down_offset_idx = 1LU; down_offset_idx < bound_idx; ++down_offset_idx) { - const auto idx = bound_idx - down_offset_idx; - arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]); - if (arc_length > params.extra_arc_length) break; - minimum_expansions[idx] = std::max(minimum_expansions[idx], dist); - } - break; + const auto prev_p = convert_point(bound[bound_idx]); + const auto next_p = convert_point(bound[bound_idx + 1]); + const auto projection = point_to_segment_projection(path_p, prev_p, next_p); + if (projection.distance < bound_projections[path_idx].distance) { + bound_indexes[path_idx] = bound_idx; + bound_projections[path_idx] = projection; } } + lb_idx = bound_indexes[path_idx]; + } +} + +void apply_arc_length_range_smoothing( + Expansion & expansion, const std::vector & bound, const double arc_length_range, + const Side side) +{ + const auto & bound_indexes = + side == LEFT ? expansion.left_bound_indexes : expansion.right_bound_indexes; + const auto & bound_projections = + side == LEFT ? expansion.left_projections : expansion.right_projections; + auto & bound_expansions = side == LEFT ? expansion.left_distances : expansion.right_distances; + const auto original_expansions = bound_expansions; + for (auto path_idx = 0UL; path_idx < bound_indexes.size(); ++path_idx) { + const auto bound_idx = bound_indexes[path_idx]; + auto arc_length = boost::geometry::distance( + bound_projections[path_idx].point, convert_point(bound[bound_idx + 1])); + const auto update_arc_length_and_bound_expansions = [&](auto idx) { + arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]); + bound_expansions[idx] = std::max(bound_expansions[idx], original_expansions[bound_idx]); + }; + for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { + update_arc_length_and_bound_expansions(up_bound_idx); + if (arc_length > arc_length_range) break; + } + arc_length = + boost::geometry::distance(bound_projections[path_idx].point, convert_point(bound[bound_idx])); + for (auto down_offset_idx = 1LU; down_offset_idx < bound_idx; ++down_offset_idx) { + update_arc_length_and_bound_expansions(bound_idx - down_offset_idx); + if (arc_length > arc_length_range) break; + } } - return minimum_expansions; +} + +Expansion calculate_expansion( + const std::vector & path_poses, const std::vector & left_bound, + const std::vector & right_bound, const std::vector & curvatures, + const DrivableAreaExpansionParameters & params) +{ + Expansion expansion; + expansion.min_lane_widths.resize(path_poses.size(), 0.0); + for (auto path_idx = 0UL; path_idx < path_poses.size(); ++path_idx) { + if (curvatures[path_idx] == 0.0) continue; + const auto curvature_radius = 1 / curvatures[path_idx]; + expansion.min_lane_widths[path_idx] = calculate_minimum_lane_width(curvature_radius, params); + } + calculate_bound_index_mappings(expansion, path_poses, left_bound, LEFT); + calculate_bound_index_mappings(expansion, path_poses, right_bound, RIGHT); + return expansion; } void apply_bound_change_rate_limit( @@ -184,21 +210,30 @@ void apply_bound_change_rate_limit( } std::vector calculate_maximum_distance( - const std::vector & path_poses, const std::vector bound, - const SegmentRtree & uncrossable_segments, const std::vector & uncrossable_polygons, - const DrivableAreaExpansionParameters & params) + const std::vector & bound, const SegmentRtree & uncrossable_segments, + const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params, const Side side) { - // TODO(Maxime): improve performances (dont use bg::distance ? use rtree ?) std::vector maximum_distances(bound.size(), std::numeric_limits::max()); - LineString2d path_ls; LineString2d bound_ls; 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 Segment2d segment_ls = {bound_ls[i], bound_ls[i + 1]}; + const auto segment_vector = segment_ls.second - segment_ls.first; + const auto is_point_on_correct_side = [&](const Point2d & p) { + const auto point_vector = p - segment_ls.first; + const auto cross_product = + (segment_vector.x() * point_vector.y() - segment_vector.y() * point_vector.x()); + return cross_product * (side == LEFT ? -1.0 : 1.0) <= 0.0; + }; + const auto is_on_correct_side = [&](const Segment2d & segment) { + return is_point_on_correct_side(segment.first) || is_point_on_correct_side(segment.second); + }; std::vector query_result; boost::geometry::index::query( - uncrossable_segments, boost::geometry::index::nearest(segment_ls, 1), + uncrossable_segments, + boost::geometry::index::nearest(segment_ls, 1) && + boost::geometry::index::satisfies(is_on_correct_side), std::back_inserter(query_result)); if (!query_result.empty()) { const auto bound_to_line_dist = boost::geometry::distance(segment_ls, query_result.front()); @@ -207,9 +242,18 @@ std::vector calculate_maximum_distance( maximum_distances[i + 1] = std::min(maximum_distances[i + 1], dist_limit); } for (const auto & uncrossable_poly : uncrossable_polygons) { - const auto bound_to_poly_dist = boost::geometry::distance(segment_ls, uncrossable_poly); - maximum_distances[i] = std::min(maximum_distances[i], bound_to_poly_dist); - maximum_distances[i + 1] = std::min(maximum_distances[i + 1], bound_to_poly_dist); + if (boost::geometry::intersects(uncrossable_poly.outer(), segment_ls)) { + maximum_distances[i] = 0.0; + maximum_distances[i + 1] = 0.0; + break; + } + if (std::all_of( + uncrossable_poly.outer().begin(), uncrossable_poly.outer().end(), + is_point_on_correct_side)) { + const auto bound_to_poly_dist = boost::geometry::distance(segment_ls, uncrossable_poly); + maximum_distances[i] = std::min(maximum_distances[i], bound_to_poly_dist); + maximum_distances[i + 1] = std::min(maximum_distances[i + 1], bound_to_poly_dist); + } } } if (params.max_expansion_distance > 0.0) @@ -227,8 +271,7 @@ void expand_bound( if (expansions[idx] > 0.0) { const auto bound_p = convert_point(bound[idx]); const auto projection = point_to_linestring_projection(bound_p, path_ls); - const auto expansion_ratio = - (expansions[idx] + std::abs(projection.distance)) / std::abs(projection.distance); + const auto expansion_ratio = (expansions[idx] + projection.distance) / projection.distance; const auto & path_p = projection.projected_point; const auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio); bound[idx].x = expanded_p.x(); @@ -270,6 +313,49 @@ std::vector calculate_smoothed_curvatures( } return smoothed_curvatures; } +void calculate_expansion_distances( + Expansion & expansion, const std::vector & max_left_expansions, + const std::vector & max_right_expansions) +{ + expansion.left_distances.resize(max_left_expansions.size(), 0.0); + expansion.right_distances.resize(max_right_expansions.size(), 0.0); + for (auto path_idx = 0UL; path_idx < expansion.min_lane_widths.size(); ++path_idx) { + const auto left_bound_idx = expansion.left_bound_indexes[path_idx]; + const auto right_bound_idx = expansion.right_bound_indexes[path_idx]; + const auto original_width = expansion.left_projections[path_idx].distance + + expansion.right_projections[path_idx].distance; + const auto expansion_dist = expansion.min_lane_widths[path_idx] - original_width; + if (expansion_dist <= 0.0) continue; + const auto left_limit = + std::min(max_left_expansions[left_bound_idx], max_left_expansions[left_bound_idx + 1]); + const auto right_limit = + std::min(max_right_expansions[right_bound_idx], max_right_expansions[right_bound_idx + 1]); + const auto expansion_fits_on_left_side = expansion_dist / 2.0 < left_limit; + const auto expansion_fits_on_right_side = expansion_dist / 2.0 < right_limit; + if (expansion_fits_on_left_side && expansion_fits_on_right_side) { + expansion.left_distances[left_bound_idx] = expansion_dist / 2.0; + expansion.left_distances[left_bound_idx + 1] = expansion_dist / 2.0; + expansion.right_distances[right_bound_idx] = expansion_dist / 2.0; + expansion.right_distances[right_bound_idx + 1] = expansion_dist / 2.0; + } else if (!expansion_fits_on_left_side) { + expansion.left_distances[left_bound_idx] = left_limit; + expansion.left_distances[left_bound_idx + 1] = left_limit; + const auto compensation_dist = expansion_dist / 2.0 - left_limit; + expansion.right_distances[right_bound_idx] = + std::min(right_limit, expansion_dist / 2.0 + compensation_dist); + expansion.right_distances[right_bound_idx + 1] = + std::min(right_limit, expansion_dist / 2.0 + compensation_dist); + } else if (!expansion_fits_on_right_side) { + expansion.right_distances[right_bound_idx] = right_limit; + expansion.right_distances[right_bound_idx + 1] = right_limit; + const auto compensation_dist = expansion_dist / 2.0 - right_limit; + expansion.left_distances[left_bound_idx] = + std::min(left_limit, expansion_dist / 2.0 + compensation_dist); + expansion.left_distances[left_bound_idx + 1] = + std::min(left_limit, expansion_dist / 2.0 + compensation_dist); + } + } +} void expand_drivable_area( PathWithLaneId & path, @@ -280,7 +366,6 @@ void expand_drivable_area( tier4_autoware_utils::StopWatch stop_watch; stop_watch.tic("overall"); stop_watch.tic("preprocessing"); - // 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_segments = extract_uncrossable_segments( @@ -303,32 +388,34 @@ void expand_drivable_area( const auto first_new_point_idx = curvatures.size(); curvatures.insert( curvatures.end(), new_curvatures.begin() + first_new_point_idx, new_curvatures.end()); - auto left_expansions = - calculate_minimum_expansions(path_poses, path.left_bound, curvatures, LEFT, params); - auto right_expansions = - calculate_minimum_expansions(path_poses, path.right_bound, curvatures, RIGHT, params); + auto expansion = + calculate_expansion(path_poses, path.left_bound, path.right_bound, curvatures, params); const auto curvature_expansion_ms = stop_watch.toc("curvatures_expansion"); stop_watch.tic("max_dist"); const auto max_left_expansions = calculate_maximum_distance( - path_poses, path.left_bound, uncrossable_segments, uncrossable_polygons, params); + path.left_bound, uncrossable_segments, uncrossable_polygons, params, LEFT); const auto max_right_expansions = calculate_maximum_distance( - 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) - right_expansions[i] = std::min(right_expansions[i], max_right_expansions[i]); + path.right_bound, uncrossable_segments, uncrossable_polygons, params, RIGHT); const auto max_dist_ms = stop_watch.toc("max_dist"); + calculate_expansion_distances(expansion, max_left_expansions, max_right_expansions); + apply_arc_length_range_smoothing(expansion, path.left_bound, params.arc_length_range, LEFT); + apply_arc_length_range_smoothing(expansion, path.right_bound, params.arc_length_range, RIGHT); + stop_watch.tic("smooth"); - apply_bound_change_rate_limit(left_expansions, path.left_bound, params.max_bound_rate); - apply_bound_change_rate_limit(right_expansions, path.right_bound, params.max_bound_rate); + apply_bound_change_rate_limit(expansion.left_distances, path.left_bound, params.max_bound_rate); + apply_bound_change_rate_limit(expansion.right_distances, path.right_bound, params.max_bound_rate); const auto smooth_ms = stop_watch.toc("smooth"); + // reapply expansion limits that may have been broken by the previous smoothing + for (auto i = 0LU; i < expansion.left_distances.size(); ++i) + expansion.left_distances[i] = std::min(expansion.left_distances[i], max_left_expansions[i]); + for (auto i = 0LU; i < expansion.right_distances.size(); ++i) + expansion.right_distances[i] = std::min(expansion.right_distances[i], max_right_expansions[i]); - // TODO(Maxime): limit the distances based on the total width (left + right < min_lane_width) stop_watch.tic("expand"); - expand_bound(path.left_bound, path_poses, left_expansions); - expand_bound(path.right_bound, path_poses, right_expansions); + expand_bound(path.left_bound, path_poses, expansion.left_distances); + expand_bound(path.right_bound, path_poses, expansion.right_distances); const auto expand_ms = stop_watch.toc("expand"); const auto total_ms = stop_watch.toc("overall"); diff --git a/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp index 2cbaaf46e78cb..77728cc87604d 100644 --- a/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp @@ -58,7 +58,7 @@ TEST(DrivableAreaExpansionProjection, PointToSegment) Point2d query(5.0, -5.0); Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); - EXPECT_NEAR(projection.distance, -5.0, eps); + EXPECT_NEAR(projection.distance, 5.0, eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } @@ -82,7 +82,7 @@ TEST(DrivableAreaExpansionProjection, PointToSegment) Point2d query(0.0, 0.0); Segment2d segment(Point2d(2.5, 7.5), Point2d(7.5, 2.5)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); - EXPECT_NEAR(projection.distance, -std::sqrt(50), eps); + EXPECT_NEAR(projection.distance, std::sqrt(50), eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 5.0, eps); } @@ -115,7 +115,7 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) Point2d query(0.0, 5.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 30.0 + std::sqrt(2.5 * 2.5 * 2), eps); - EXPECT_NEAR(projection.distance, -std::sqrt(2.5 * 2.5 * 2), eps); + EXPECT_NEAR(projection.distance, std::sqrt(2.5 * 2.5 * 2), eps); EXPECT_NEAR(projection.projected_point.x(), 2.5, eps); EXPECT_NEAR(projection.projected_point.y(), 7.5, eps); }