From 8ed98a994eb29fe0a2f761a4e09012498f6ddac2 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 16 Oct 2023 17:26:16 +0900 Subject: [PATCH] feat(behavior_path_planner): curvature based drivable area expansion (#935) * Switch to new curvature based dynamic drivable area expansion Signed-off-by: Maxime CLEMENT * Cleanup + remove the old code Signed-off-by: Maxime CLEMENT * Handle uncrossable lines/polygons (may not be accurate enough) Signed-off-by: Maxime CLEMENT * Add runtime measurements Signed-off-by: Maxime CLEMENT * [WIP] Reuse previously calculated raw curvatures Signed-off-by: Maxime CLEMENT * Fix bug with lateral offset distance and repeating path points Signed-off-by: Maxime CLEMENT * Remove self crossings in the expanded bounds Signed-off-by: Maxime CLEMENT * Big cleanup + update parameters Signed-off-by: Maxime CLEMENT * Remove svg debug output Signed-off-by: Maxime CLEMENT * Update parameter file Signed-off-by: Maxime CLEMENT * Fix rebase mistakes Signed-off-by: Maxime CLEMENT * Add parameter to enable/disable printing the runtime Signed-off-by: Maxime CLEMENT * Fix append of new path points to satisfy the resampling interval Signed-off-by: Maxime CLEMENT * Add smoothing.extra_arc_length param Signed-off-by: Maxime CLEMENT * Code cleanup + add docstrings Signed-off-by: Maxime CLEMENT * Fix spellcheck Signed-off-by: Maxime CLEMENT * Fix initial path poses (no longer cropped) and fix test Signed-off-by: Maxime CLEMENT --------- Signed-off-by: Maxime CLEMENT --- planning/behavior_path_planner/CMakeLists.txt | 1 - .../config/drivable_area_expansion.param.yaml | 33 +- .../behavior_path_planner/data_manager.hpp | 5 +- .../drivable_area_expansion.hpp | 84 ++- .../drivable_area_expansion/expansion.hpp | 85 --- .../drivable_area_expansion/footprints.hpp | 13 +- .../drivable_area_expansion/map_utils.hpp | 13 +- .../drivable_area_expansion/parameters.hpp | 84 ++- .../path_projection.hpp | 36 +- .../replan_checker.hpp | 92 --- .../utils/drivable_area_expansion/types.hpp | 22 +- .../src/behavior_path_planner_node.cpp | 42 +- .../drivable_area_expansion.cpp | 556 +++++++++--------- .../drivable_area_expansion/expansion.cpp | 237 -------- .../drivable_area_expansion/footprints.cpp | 62 +- .../drivable_area_expansion/map_utils.cpp | 25 +- .../behavior_path_planner/src/utils/utils.cpp | 2 +- .../test/test_drivable_area_expansion.cpp | 221 ++----- 18 files changed, 531 insertions(+), 1082 deletions(-) delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/replan_checker.hpp delete mode 100644 planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index efae65ae48813..5bfcf8ce0415b 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -49,7 +49,6 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/drivable_area_expansion/drivable_area_expansion.cpp src/utils/drivable_area_expansion/map_utils.cpp src/utils/drivable_area_expansion/footprints.cpp - src/utils/drivable_area_expansion/expansion.cpp src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp src/marker_utils/utils.cpp 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 706a4fc2e45c1..c05a874a1ad29 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -5,16 +5,19 @@ drivable_area_left_bound_offset: 0.0 drivable_area_types_to_skip: [road_border] - # Dynamic expansion by projecting the ego footprint along the path + # Dynamic expansion by using the path curvature dynamic_expansion: - enabled: false - debug_print: false # if true, print some debug runtime measurements + enabled: true + print_runtime: false + max_expansion_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) + 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 ego: - extra_footprint_offset: - front: 0.5 # [m] extra length to add to the front of the ego footprint - rear: 0.5 # [m] extra length to add to the rear of the ego footprint - left: 0.5 # [m] extra length to add to the left of the ego footprint - right: 0.5 # [m] extra length to add to the rear of the ego footprint + 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 + extra_width: 1.0 # [m] extra length to add to the width dynamic_objects: avoid: false # if true, the drivable area is not expanded in the predicted path of dynamic objects extra_footprint_offset: @@ -25,20 +28,8 @@ path_preprocessing: max_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used) - expansion: - method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. - # 'lanelet': add lanelets overlapped by the ego footprints - # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area - max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) - extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint + reuse_max_deviation: 0.5 # [m] if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused. avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area - road_border distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid - compensate: - enable: true # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction - extra_distance: 3.0 # [m] extra distance to add to the compensation - replan_checker: - enable: true # if true, only recalculate the expanded drivable area when the path or its original drivable area change significantly - # not compatible with dynamic_objects.avoid - max_deviation: 0.5 # [m] full replan is only done if the path changes by more than this distance diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 5b61efd1fbb96..967607e4c143a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -18,7 +18,6 @@ #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/replan_checker.hpp" #include #include @@ -150,8 +149,8 @@ struct PlannerData BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; - mutable std::optional drivable_area_expansion_prev_crop_pose; - mutable drivable_area_expansion::ReplanChecker drivable_area_expansion_replan_checker{}; + mutable std::vector drivable_area_expansion_prev_path_poses{}; + mutable std::vector drivable_area_expansion_prev_curvatures{}; mutable TurnSignalDecider turn_signal_decider; TurnIndicatorsCommand getTurnSignal( 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 8293e0a1d10a9..19ea89a3ce3c7 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 @@ -24,30 +24,76 @@ #include #include +#include namespace drivable_area_expansion { -/// @brief Expand the drivable area based on the projected ego footprint along the path +/// @brief Expand the drivable area based on the path curvature and the vehicle dimensions /// @param[inout] path path whose drivable area will be expanded -/// @param[inout] planner_data planning data (params, dynamic objects, route handler, ...) -/// @param[in] path_lanes lanelets of the path -void expandDrivableArea( +/// @param[inout] planner_data planning data (params, dynamic objects, vehicle info, ...) +void expand_drivable_area( PathWithLaneId & path, - const std::shared_ptr planner_data, - const lanelet::ConstLanelets & path_lanes); - -/// @brief Create a polygon combining the drivable area of a path and some expansion polygons -/// @param[in] path path and its drivable area -/// @param[in] expansion_polygons polygons to add to the drivable area -/// @return expanded drivable area polygon -polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multi_polygon_t & expansion_polygons); - -/// @brief Update the drivable area of the given path with the given polygon -/// @details this function splits the polygon into a left and right bound and sets it in the path -/// @param[in] path path whose drivable area will be expanded -/// @param[in] expanded_drivable_area polygon of the new drivable area -void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area); + const std::shared_ptr planner_data); + +/// @brief prepare path poses and try to reuse 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( + const PathWithLaneId & path, std::vector & prev_poses, + std::vector & prev_curvatures, const Point & ego_point, + const DrivableAreaExpansionParameters & params); + +/// @brief calculate the minimum lane width based on the path curvature and the vehicle dimensions +/// @cite Lim, H., Kim, C., and Jo, A., "Model Predictive Control-Based Lateral Control of +/// Autonomous Large-Size Bus on Road with Large Curvature," SAE Technical Paper 2021-01-0099, 2021, +/// https://doi.org/10.4271/2021-01-0099 +/// @param [in] curvature curvature +/// @param [in] params parameters containing the vehicle dimensions +/// @return minimum lane width +double calculate_minimum_lane_width( + const double curvature_radius, 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) +/// @param [in] bound_points bound points +/// @param [in] max_rate [m/m] maximum rate of lateral deviation over arc length +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_lines lines that limit the bound expansion +/// @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 DrivableAreaExpansionParameters & params); + +/// @brief expand a bound by the given lateral distances away from the path +/// @param [inout] bound bound points to expand +/// @param [in] path_poses input path +/// @param [in] distances new lateral distances of the bound points away from the path +void expand_bound( + std::vector & bound, const std::vector & path_poses, + const std::vector & distances); + +/// @brief calculate smoothed curvatures +/// @details smoothing is done using a moving average +/// @param [in] poses input poses used to calculate the curvatures +/// @param [in] smoothing_window_size size of the window used for the moving average +/// @return smoothed curvatures of the input poses +std::vector calculate_smoothed_curvatures( + const std::vector & poses, const size_t smoothing_window_size); + } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp deleted file mode 100644 index 70cc8a8bc5925..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ - -#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" - -#include - -#include - -#include -#include - -namespace drivable_area_expansion -{ -/// @brief Calculate the distance limit required for the polygon to not cross the limit lines -/// @details Calculate the minimum distance from base_ls to an intersection of limit_lines and -/// expansion_polygon -/// @param[in] base_ls base linestring from which the distance is calculated -/// @param[in] expansion_polygon polygon to consider -/// @param[in] limit_lines lines we do not want to cross -/// @return distance limit -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_linestring_t & limit_lines); - -/// @brief Calculate the distance limit required for the polygon to not cross the limit polygons. -/// @details Calculate the minimum distance from base_ls to an intersection of limit_polygons and -/// expansion_polygon -/// @param[in] base_ls base linestring from which the distance is calculated -/// @param[in] expansion_polygon polygon to consider -/// @param[in] limit_polygons polygons we do not want to cross -/// @return distance limit -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_polygon_t & limit_polygons); - -/// @brief Create a polygon from a base line with a given expansion distance -/// @param[in] base_ls base linestring from which the polygon is created -/// @param[in] dist desired expansion distance from the base line -/// @param[in] is_left_side desired side of the expansion from the base line -/// @return expansion polygon -polygon_t createExpansionPolygon( - const linestring_t & base_ls, const double dist, const bool is_left_side); - -/// @brief Create polygons for the area where the drivable area should be expanded -/// @param[in] path path and its drivable area -/// @param[in] path_footprints polygons of the ego footprint projected along the path -/// @param[in] predicted_paths polygons of the dynamic objects' predicted paths -/// @param[in] uncrossable_lines lines that should not be crossed by the expanded drivable area -/// @param[in] params expansion parameters -/// @return expansion polygons -multi_polygon_t createExpansionPolygons( - const PathWithLaneId & path, const multi_polygon_t & path_footprints, - const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, - const DrivableAreaExpansionParameters & params); - -/// @brief Create polygons for the area where the drivable area should be expanded -/// @param[in] path_lanes lanelets of the current path -/// @param[in] route_handler route handler -/// @param[in] path_footprints polygons of the ego footprint projected along the path -/// @param[in] predicted_paths polygons of the dynamic objects' predicted paths -/// @param[in] params expansion parameters -/// @return expansion polygons -multi_polygon_t createExpansionLaneletPolygons( - const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, - const DrivableAreaExpansionParameters & params); -} // namespace drivable_area_expansion - -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp index 8fc0157710dc8..418a9a5a68572 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp @@ -43,27 +43,20 @@ namespace drivable_area_expansion /// @param[in] x translation distance on the x axis /// @param[in] y translation distance on the y axis /// @return translated polygon -polygon_t translatePolygon(const polygon_t & polygon, const double x, const double y); +Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const double y); /// @brief create the footprint of a pose and its base footprint /// @param[in] pose the origin pose of the footprint /// @param[in] base_footprint the base axis-aligned footprint /// @return footprint polygon -polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t base_footprint); +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint); /// @brief create footprints of the predicted paths of an object /// @param [in] objects objects from which to create polygons /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects /// @return footprint polygons of the object's predicted paths -multi_polygon_t createObjectFootprints( +MultiPolygon2d create_object_footprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); - -/// @brief create the footprint polygon from a path -/// @param[in] path the path for which to create a footprint -/// @param[in] params expansion parameters defining how to create the footprint -/// @return footprint polygons of the path -multi_polygon_t createPathFootprints( - const std::vector & path, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ 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 4524bd2be2299..6f96b83237310 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 @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include @@ -24,18 +25,20 @@ namespace drivable_area_expansion { -/// @brief Extract uncrossable linestrings from the lanelet map +/// @brief Extract uncrossable linestrings from the lanelet map that are in range of ego /// @param[in] lanelet_map lanelet map -/// @param[in] uncrossable_types types that cannot be crossed +/// @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 -multi_linestring_t extractUncrossableLines( - const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types); +MultiLineString2d extract_uncrossable_lines( + const lanelet::LaneletMap & lanelet_map, const Point & ego_point, + const DrivableAreaExpansionParameters & params); /// @brief Determine if the given linestring has one of the given types /// @param[in] ls linestring to check /// @param[in] types type strings to check /// @return true if the linestring has one of the given types -bool hasTypes(const lanelet::ConstLineString3d & ls, const std::vector & types); +bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index d93cb65d0554d..e7275960b0888 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -32,12 +32,9 @@ struct DrivableAreaExpansionParameters static constexpr auto DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM = "drivable_area_left_bound_offset"; static constexpr auto DRIVABLE_AREA_TYPES_TO_SKIP_PARAM = "drivable_area_types_to_skip"; static constexpr auto ENABLED_PARAM = "dynamic_expansion.enabled"; - static constexpr auto EGO_EXTRA_OFFSET_FRONT = - "dynamic_expansion.ego.extra_footprint_offset.front"; - static constexpr auto EGO_EXTRA_OFFSET_REAR = "dynamic_expansion.ego.extra_footprint_offset.rear"; - static constexpr auto EGO_EXTRA_OFFSET_LEFT = "dynamic_expansion.ego.extra_footprint_offset.left"; - static constexpr auto EGO_EXTRA_OFFSET_RIGHT = - "dynamic_expansion.ego.extra_footprint_offset.right"; + static constexpr auto EGO_EXTRA_FRONT_OVERHANG = "dynamic_expansion.ego.extra_front_overhang"; + static constexpr auto EGO_EXTRA_WHEELBASE = "dynamic_expansion.ego.extra_wheelbase"; + static constexpr auto EGO_EXTRA_WIDTH = "dynamic_expansion.ego.extra_width"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_FRONT = "dynamic_expansion.dynamic_objects.extra_footprint_offset.front"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_REAR = @@ -46,38 +43,36 @@ struct DrivableAreaExpansionParameters "dynamic_expansion.dynamic_objects.extra_footprint_offset.left"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_RIGHT = "dynamic_expansion.dynamic_objects.extra_footprint_offset.right"; - static constexpr auto EXPANSION_METHOD_PARAM = "dynamic_expansion.expansion.method"; - static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.expansion.max_distance"; + static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.max_expansion_distance"; static constexpr auto RESAMPLE_INTERVAL_PARAM = "dynamic_expansion.path_preprocessing.resample_interval"; static constexpr auto MAX_PATH_ARC_LENGTH_PARAM = "dynamic_expansion.path_preprocessing.max_arc_length"; - static constexpr auto EXTRA_ARC_LENGTH_PARAM = "dynamic_expansion.expansion.extra_arc_length"; + static constexpr auto MAX_REUSE_DEVIATION_PARAM = + "dynamic_expansion.path_preprocessing.reuse_max_deviation"; static constexpr auto AVOID_DYN_OBJECTS_PARAM = "dynamic_expansion.dynamic_objects.avoid"; static constexpr auto AVOID_LINESTRING_TYPES_PARAM = "dynamic_expansion.avoid_linestring.types"; static constexpr auto AVOID_LINESTRING_DIST_PARAM = "dynamic_expansion.avoid_linestring.distance"; - static constexpr auto COMPENSATE_PARAM = "dynamic_expansion.avoid_linestring.compensate.enable"; - static constexpr auto EXTRA_COMPENSATE_PARAM = - "dynamic_expansion.avoid_linestring.compensate.extra_distance"; - static constexpr auto REPLAN_ENABLE_PARAM = "dynamic_expansion.replan_checker.enable"; - static constexpr auto REPLAN_MAX_DEVIATION_PARAM = - "dynamic_expansion.replan_checker.max_deviation"; - static constexpr auto DEBUG_PRINT_PARAM = "dynamic_expansion.debug_print"; + static constexpr auto SMOOTHING_CURVATURE_WINDOW_PARAM = + "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 PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime"; - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip; + // static expansion + double drivable_area_right_bound_offset{}; + double drivable_area_left_bound_offset{}; + std::vector drivable_area_types_to_skip{}; + // dynamic expansion bool enabled = false; - std::string expansion_method{}; double avoid_linestring_dist{}; - double ego_left_offset{}; - double ego_right_offset{}; - double ego_rear_offset{}; - double ego_front_offset{}; - double ego_extra_left_offset{}; - double ego_extra_right_offset{}; - double ego_extra_rear_offset{}; - double ego_extra_front_offset{}; + double extra_front_overhang{}; + double extra_wheelbase{}; + double extra_width{}; + int curvature_average_window{}; + double max_bound_rate{}; double dynamic_objects_extra_left_offset{}; double dynamic_objects_extra_right_offset{}; double dynamic_objects_extra_rear_offset{}; @@ -86,13 +81,11 @@ struct DrivableAreaExpansionParameters double max_path_arc_length{}; double resample_interval{}; double extra_arc_length{}; + double max_reuse_deviation{}; bool avoid_dynamic_objects{}; + bool print_runtime{}; std::vector avoid_linestring_types{}; - bool compensate_uncrossable_lines = false; - double compensate_extra_dist{}; - bool replan_enable{}; - double replan_max_deviation{}; - bool debug_print{}; + vehicle_info_util::VehicleInfo vehicle_info; DrivableAreaExpansionParameters() = default; explicit DrivableAreaExpansionParameters(rclcpp::Node & node) { init(node); } @@ -107,12 +100,15 @@ struct DrivableAreaExpansionParameters node.declare_parameter>(DRIVABLE_AREA_TYPES_TO_SKIP_PARAM); enabled = node.declare_parameter(ENABLED_PARAM); max_expansion_distance = node.declare_parameter(MAX_EXP_DIST_PARAM); + extra_front_overhang = node.declare_parameter(EGO_EXTRA_FRONT_OVERHANG); + extra_wheelbase = node.declare_parameter(EGO_EXTRA_WHEELBASE); + 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); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); resample_interval = node.declare_parameter(RESAMPLE_INTERVAL_PARAM); - ego_extra_front_offset = node.declare_parameter(EGO_EXTRA_OFFSET_FRONT); - ego_extra_rear_offset = node.declare_parameter(EGO_EXTRA_OFFSET_REAR); - ego_extra_left_offset = node.declare_parameter(EGO_EXTRA_OFFSET_LEFT); - ego_extra_right_offset = node.declare_parameter(EGO_EXTRA_OFFSET_RIGHT); + max_reuse_deviation = node.declare_parameter(MAX_REUSE_DEVIATION_PARAM); dynamic_objects_extra_front_offset = node.declare_parameter(DYN_OBJECTS_EXTRA_OFFSET_FRONT); dynamic_objects_extra_rear_offset = @@ -125,19 +121,9 @@ struct DrivableAreaExpansionParameters node.declare_parameter>(AVOID_LINESTRING_TYPES_PARAM); avoid_dynamic_objects = node.declare_parameter(AVOID_DYN_OBJECTS_PARAM); avoid_linestring_dist = node.declare_parameter(AVOID_LINESTRING_DIST_PARAM); - extra_arc_length = node.declare_parameter(EXTRA_ARC_LENGTH_PARAM); - compensate_uncrossable_lines = node.declare_parameter(COMPENSATE_PARAM); - compensate_extra_dist = node.declare_parameter(EXTRA_COMPENSATE_PARAM); - expansion_method = node.declare_parameter(EXPANSION_METHOD_PARAM); - replan_enable = node.declare_parameter(REPLAN_ENABLE_PARAM); - replan_max_deviation = node.declare_parameter(REPLAN_MAX_DEVIATION_PARAM); - debug_print = node.declare_parameter(DEBUG_PRINT_PARAM); + print_runtime = node.declare_parameter(PRINT_RUNTIME_PARAM); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - ego_left_offset = vehicle_info.max_lateral_offset_m; - ego_right_offset = vehicle_info.min_lateral_offset_m; - ego_rear_offset = vehicle_info.min_longitudinal_offset_m; - ego_front_offset = vehicle_info.max_longitudinal_offset_m; + vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); } }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp index 3e2b177f59167..93afaad825582 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp @@ -33,10 +33,10 @@ namespace drivable_area_expansion /// @param p2 second segment point /// @return projected point and corresponding distance inline PointDistance point_to_segment_projection( - const point_t & p, const point_t & p1, const point_t & p2) + const Point2d & p, const Point2d & p1, const Point2d & p2) { - const point_t p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; - const point_t p_vec = {p.x() - p1.x(), p.y() - p1.y()}; + 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; @@ -48,7 +48,7 @@ inline PointDistance point_to_segment_projection( if (c2 <= c1) return {p2, boost::geometry::distance(p, p2) * dist_sign}; const auto projection = p1 + (p2_vec * c1 / c2); - const auto projection_point = point_t{projection.x(), projection.y()}; + const auto projection_point = Point2d{projection.x(), projection.y()}; return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; } @@ -59,10 +59,10 @@ inline PointDistance point_to_segment_projection( /// @param p2 second line point /// @return projected point and corresponding distance inline PointDistance point_to_line_projection( - const point_t & p, const point_t & p1, const point_t & p2) + const Point2d & p, const Point2d & p1, const Point2d & p2) { - const point_t p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; - const point_t p_vec = {p.x() - p1.x(), p.y() - p1.y()}; + 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; @@ -70,7 +70,7 @@ inline PointDistance point_to_line_projection( 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 = point_t{projection.x(), projection.y()}; + const auto projection_point = Point2d{projection.x(), projection.y()}; return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; } @@ -78,7 +78,7 @@ inline PointDistance point_to_line_projection( /// @param p point to project /// @param ls linestring /// @return projected point, corresponding distance, and arc length along the linestring -inline Projection point_to_linestring_projection(const point_t & p, const linestring_t & ls) +inline Projection point_to_linestring_projection(const Point2d & p, const LineString2d & ls) { Projection closest; closest.distance = std::numeric_limits::max(); @@ -100,14 +100,14 @@ inline Projection point_to_linestring_projection(const point_t & p, const linest /// @param p2 second vector point /// @param dist distance /// @return point p such that (p1,p) is orthogonal to (p1,p2) at the given distance -inline point_t normal_at_distance(const point_t & p1, const point_t & p2, const double dist) +inline Point2d normal_at_distance(const Point2d & p1, const Point2d & p2, const double dist) { auto base = p1; auto normal_vector = p2; boost::geometry::subtract_point(normal_vector, base); boost::geometry::detail::vec_normalize(normal_vector); boost::geometry::multiply_value(normal_vector, dist); - return point_t{base.x() - normal_vector.y(), base.y() + normal_vector.x()}; + return Point2d{base.x() - normal_vector.y(), base.y() + normal_vector.x()}; } /// @brief interpolate between two points @@ -115,7 +115,7 @@ inline point_t normal_at_distance(const point_t & p1, const point_t & p2, const /// @param b second point /// @param ratio interpolation ratio such that 0 yields a, and 1 yields b /// @return point interpolated between a and b as per the given ratio -inline point_t lerp_point(const point_t & a, const point_t & b, const double ratio) +inline Point2d lerp_point(const Point2d & a, const Point2d & b, const double ratio) { return {interpolation::lerp(a.x(), b.x(), ratio), interpolation::lerp(a.y(), b.y(), ratio)}; } @@ -125,10 +125,10 @@ inline point_t lerp_point(const point_t & a, const point_t & b, const double rat /// @param arc_length arc length along the reference linestring of the resulting point /// @param distance distance from the reference linestring of the resulting point /// @return point at the distance and arc length relative to the reference linestring -inline segment_t linestring_to_point_projection( - const linestring_t & ls, const double arc_length, const double distance) +inline Segment2d linestring_to_point_projection( + const LineString2d & ls, const double arc_length, const double distance) { - if (ls.empty()) return segment_t{}; + if (ls.empty()) return Segment2d{}; if (ls.size() == 1lu) return {ls.front(), ls.front()}; auto ls_iterator = ls.begin(); auto prev_arc_length = 0.0; @@ -156,10 +156,10 @@ inline segment_t linestring_to_point_projection( /// @param from_arc_length arc length of the first point of the sub linestring /// @param to_arc_length arc length of the last point of the sub linestring /// @return sub linestring -inline linestring_t sub_linestring( - const linestring_t & ls, const double from_arc_length, const double to_arc_length) +inline LineString2d sub_linestring( + const LineString2d & ls, const double from_arc_length, const double to_arc_length) { - linestring_t sub_ls; + LineString2d sub_ls; if (from_arc_length >= to_arc_length || ls.empty()) throw(std::runtime_error("sub_linestring: bad inputs")); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/replan_checker.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/replan_checker.hpp deleted file mode 100644 index 278893ccae55c..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/replan_checker.hpp +++ /dev/null @@ -1,92 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__REPLAN_CHECKER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__REPLAN_CHECKER_HPP_ - -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" - -#include - -#include - -namespace drivable_area_expansion -{ -class ReplanChecker -{ -private: - linestring_t prev_path_ls_{}; - polygon_t prev_expanded_drivable_area_{}; - -public: - /// @brief set the previous path and its expanded drivable area - /// @param path previous path - void set_previous(const PathWithLaneId & path) - { - prev_path_ls_.clear(); - for (const auto & p : path.points) - prev_path_ls_.emplace_back(p.point.pose.position.x, p.point.pose.position.y); - boost::geometry::clear(prev_expanded_drivable_area_); - for (const auto & p : path.left_bound) - prev_expanded_drivable_area_.outer().emplace_back(p.x, p.y); - for (auto it = path.right_bound.rbegin(); it != path.right_bound.rend(); ++it) - prev_expanded_drivable_area_.outer().emplace_back(it->x, it->y); - if (!boost::geometry::is_empty(prev_expanded_drivable_area_)) - prev_expanded_drivable_area_.outer().push_back(prev_expanded_drivable_area_.outer().front()); - } - - /// @brief get the previous expanded drivable area - /// @return the previous expanded drivable area - polygon_t get_previous_expanded_drivable_area() { return prev_expanded_drivable_area_; } - - /// @brief reset the previous path and expanded drivable area - void reset() - { - boost::geometry::clear(prev_path_ls_); - boost::geometry::clear(prev_expanded_drivable_area_); - } - - /// @brief calculate the index of the input path from which replanning is necessary (starting from - /// ego pose) - /// @param [in] path input path - /// @param [in] ego_index path index before the current ego pose - /// @param [in] max_deviation [m] replan index will be the first path point that deviates by more - /// than this distance - /// @return path index from which to replan - size_t calculate_replan_index( - const PathWithLaneId & path, const size_t ego_index, const double max_deviation) const - { - linestring_t path_ls; - path_ls.reserve(path.points.size()); - for (const auto & p : path.points) - path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); - // full replan if no prev path or if end of the previous path does not match with the new path - if ( - prev_path_ls_.empty() || - boost::geometry::distance(prev_path_ls_.back(), path_ls) > max_deviation) - return 0; - - for (size_t i = ego_index; i < path_ls.size(); ++i) { - const auto & point = path_ls[i]; - const auto deviation_distance = boost::geometry::distance(point, prev_path_ls_); - if (deviation_distance > max_deviation) { - return i; - } - } - return path_ls.size(); - } -}; -} // namespace drivable_area_expansion - -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__REPLAN_CHECKER_HPP_ 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 e300a347e47a8..7db92c163f567 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 @@ -29,26 +29,28 @@ using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; -using point_t = tier4_autoware_utils::Point2d; -using multi_point_t = tier4_autoware_utils::MultiPoint2d; -using polygon_t = tier4_autoware_utils::Polygon2d; -using ring_t = tier4_autoware_utils::LinearRing2d; -using multi_polygon_t = tier4_autoware_utils::MultiPolygon2d; -using segment_t = tier4_autoware_utils::Segment2d; -using linestring_t = tier4_autoware_utils::LineString2d; -using multi_linestring_t = tier4_autoware_utils::MultiLineString2d; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::MultiLineString2d; +using tier4_autoware_utils::MultiPoint2d; +using tier4_autoware_utils::MultiPolygon2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using tier4_autoware_utils::Segment2d; struct PointDistance { - point_t point; + Point2d point; double distance; }; struct Projection { - point_t projected_point; + Point2d projected_point; double distance; double arc_length; }; +enum Side { LEFT, RIGHT }; + } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ 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 a73ef0a38c997..647ff641dd806 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1008,9 +1008,6 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updated |= updateParam( parameters, DrivableAreaExpansionParameters::AVOID_DYN_OBJECTS_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_dynamic_objects); - updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EXPANSION_METHOD_PARAM, - planner_data_->drivable_area_expansion_parameters.expansion_method); updated |= updateParam( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_TYPES_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_types); @@ -1018,17 +1015,14 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_DIST_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_dist); updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_FRONT, - planner_data_->drivable_area_expansion_parameters.ego_extra_front_offset); - updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_REAR, - planner_data_->drivable_area_expansion_parameters.ego_extra_rear_offset); + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_FRONT_OVERHANG, + planner_data_->drivable_area_expansion_parameters.extra_front_overhang); updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_LEFT, - planner_data_->drivable_area_expansion_parameters.ego_extra_left_offset); + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WHEELBASE, + planner_data_->drivable_area_expansion_parameters.extra_wheelbase); updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_RIGHT, - planner_data_->drivable_area_expansion_parameters.ego_extra_right_offset); + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WIDTH, + planner_data_->drivable_area_expansion_parameters.extra_width); updated |= updateParam( parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_FRONT, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_front_offset); @@ -1051,24 +1045,20 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( parameters, DrivableAreaExpansionParameters::RESAMPLE_INTERVAL_PARAM, planner_data_->drivable_area_expansion_parameters.resample_interval); updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EXTRA_ARC_LENGTH_PARAM, - planner_data_->drivable_area_expansion_parameters.extra_arc_length); + parameters, DrivableAreaExpansionParameters::MAX_REUSE_DEVIATION_PARAM, + planner_data_->drivable_area_expansion_parameters.max_reuse_deviation); updated |= updateParam( - parameters, DrivableAreaExpansionParameters::COMPENSATE_PARAM, - planner_data_->drivable_area_expansion_parameters.compensate_uncrossable_lines); + parameters, DrivableAreaExpansionParameters::SMOOTHING_CURVATURE_WINDOW_PARAM, + planner_data_->drivable_area_expansion_parameters.curvature_average_window); updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EXTRA_COMPENSATE_PARAM, - planner_data_->drivable_area_expansion_parameters.compensate_extra_dist); + parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM, + planner_data_->drivable_area_expansion_parameters.max_bound_rate); updated |= updateParam( - parameters, DrivableAreaExpansionParameters::REPLAN_ENABLE_PARAM, - planner_data_->drivable_area_expansion_parameters.replan_enable); + parameters, DrivableAreaExpansionParameters::SMOOTHING_EXTRA_ARC_LENGTH_PARAM, + planner_data_->drivable_area_expansion_parameters.extra_arc_length); updated |= updateParam( - parameters, DrivableAreaExpansionParameters::REPLAN_MAX_DEVIATION_PARAM, - planner_data_->drivable_area_expansion_parameters.replan_max_deviation); - updateParam( - parameters, DrivableAreaExpansionParameters::DEBUG_PRINT_PARAM, - planner_data_->drivable_area_expansion_parameters.debug_print); - if (updated) planner_data_->drivable_area_expansion_replan_checker.reset(); + parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM, + planner_data_->drivable_area_expansion_parameters.print_runtime); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); 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 102ed5b8c5fd1..decd145f7a57b 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 @@ -14,337 +14,315 @@ #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/footprints.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include #include #include +#include #include +#include + namespace drivable_area_expansion { -std::vector crop_and_resample( - const std::vector & points, const DrivableAreaExpansionParameters & params, - const size_t first_idx) +namespace { - if (first_idx >= points.size()) return points; - const auto & crop_pose = points[first_idx].point.pose; - const auto & crop_distance = motion_utils::calcSignedArcLength(points, 0, first_idx); - // crop - const auto crop_seg_idx = motion_utils::findNearestSegmentIndex(points, crop_pose.position); - const auto cropped_points = motion_utils::cropPoints( - points, crop_pose.position, crop_seg_idx, params.max_path_arc_length, - params.max_path_arc_length - crop_distance); - // resample - PathWithLaneId cropped_path; - cropped_path.points = cropped_points; - const auto resampled_path = - motion_utils::resamplePath(cropped_path, params.resample_interval, true, true, false); - - return resampled_path.points; -} -void expandDrivableArea( - PathWithLaneId & path, - const std::shared_ptr planner_data, - const lanelet::ConstLanelets & path_lanes) +Point2d convert_point(const Point & p) { - if (path.points.empty() || path.left_bound.empty() || path.right_bound.empty()) return; - - tier4_autoware_utils::StopWatch stopwatch; - const auto & params = planner_data->drivable_area_expansion_parameters; - const auto & dynamic_objects = *planner_data->dynamic_object; - const auto & route_handler = *planner_data->route_handler; - - stopwatch.tic("calc_replan"); - const auto ego_index = planner_data->findEgoIndex(path.points); - auto & replan_checker = planner_data->drivable_area_expansion_replan_checker; - const auto replan_index = params.replan_enable ? replan_checker.calculate_replan_index( - path, ego_index, params.replan_max_deviation) - : 0; - const auto & prev_expanded_drivable_area = replan_checker.get_previous_expanded_drivable_area(); - const auto is_replanning = - params.expansion_method == "polygon" && !params.avoid_dynamic_objects && - !boost::geometry::is_empty(prev_expanded_drivable_area) && replan_index > ego_index; - const auto calc_replan_duration = stopwatch.toc("calc_replan"); - - stopwatch.tic("extract_uncrossable_lines"); - const auto uncrossable_lines = - extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types); - multi_linestring_t uncrossable_lines_in_range; - const auto & p = path.points.front().point.pose.position; - for (const auto & line : uncrossable_lines) - if (boost::geometry::distance(line, point_t{p.x, p.y}) < params.max_path_arc_length) - uncrossable_lines_in_range.push_back(line); - const auto extra_uncrossable_lines_duration = stopwatch.toc("extract_uncrossable_lines"); - - stopwatch.tic("crop"); - const auto points = crop_and_resample(path.points, params, replan_index); - const auto crop_duration = stopwatch.toc("crop"); - stopwatch.tic("footprints"); - const auto path_footprints = createPathFootprints(points, params); - const auto footprints_duration = stopwatch.toc("footprints"); - const auto predicted_paths = params.avoid_dynamic_objects - ? createObjectFootprints(dynamic_objects, params) - : multi_polygon_t{}; - stopwatch.tic("exp_polys"); - auto expansion_polygons = - params.expansion_method == "lanelet" - ? createExpansionLaneletPolygons( - path_lanes, route_handler, path_footprints, predicted_paths, params) - : createExpansionPolygons( - path, path_footprints, predicted_paths, uncrossable_lines_in_range, params); - if (is_replanning) expansion_polygons.push_back(prev_expanded_drivable_area); - const auto exp_polygons_duration = stopwatch.toc("exp_polys"); - stopwatch.tic("exp_da"); - const auto expanded_drivable_area = createExpandedDrivableAreaPolygon(path, expansion_polygons); - const auto exp_da_duration = stopwatch.toc("exp_da"); - - linestring_t path_ls; - for (const auto & p : path.points) - path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); + return Point2d{p.x, p.y}; +} - stopwatch.tic("update"); - updateDrivableAreaBounds(path, expanded_drivable_area); - const auto update_duration = stopwatch.toc("update"); - planner_data->drivable_area_expansion_replan_checker.set_previous(path); +} // namespace - if (params.debug_print) { - std::printf("Dynamic drivable area expansion runtime: %2.2fms\n", stopwatch.toc()); - std::printf("\tcalc_replan: %2.2fms\n", calc_replan_duration); - std::printf("\textract_lines: %2.2fms\n", extra_uncrossable_lines_duration); - std::printf("\tcrop: %2.2fms\n", crop_duration); - std::printf("\tfootprints: %2.2fms\n", footprints_duration); - std::printf("\texp_polys: %2.2fms\n", exp_polygons_duration); - std::printf("\texp_da: %2.2fms\n", exp_da_duration); - std::printf("\tupdate: %2.2fms\n", update_duration); - std::cout << std::endl; +void reuse_previous_poses( + const PathWithLaneId & path, std::vector & prev_poses, + std::vector & prev_curvatures, const Point & ego_point, + const DrivableAreaExpansionParameters & params) +{ + std::vector cropped_poses; + std::vector cropped_curvatures; + const auto ego_is_behind = prev_poses.size() > 1 && motion_utils::calcLongitudinalOffsetToSegment( + prev_poses, 0, ego_point) < 0.0; + const auto ego_is_far = !prev_poses.empty() && + tier4_autoware_utils::calcDistance2d(ego_point, prev_poses.front()) < 0.0; + if (!ego_is_behind && !ego_is_far && prev_poses.size() > 1) { + const auto first_idx = + motion_utils::findNearestSegmentIndex(prev_poses, path.points.front().point.pose); + const auto deviation = + motion_utils::calcLateralOffset(prev_poses, path.points.front().point.pose.position); + if (first_idx && deviation < params.max_reuse_deviation) { + 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; + cropped_poses.push_back(prev_poses[idx]); + cropped_curvatures.push_back(prev_curvatures[idx]); + } + } + } + if (cropped_poses.empty()) { + const auto resampled_path_points = + motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; + for (const auto & p : resampled_path_points) cropped_poses.push_back(p.point.pose); + } else if (!path.points.empty()) { + const auto initial_arc_length = motion_utils::calcArcLength(cropped_poses); + const auto max_path_arc_length = motion_utils::calcArcLength(path.points); + const auto first_arc_length = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, cropped_poses.back().position); + for (auto arc_length = first_arc_length + params.resample_interval; + initial_arc_length + (arc_length - first_arc_length) <= params.max_path_arc_length && + arc_length <= max_path_arc_length; + arc_length += params.resample_interval) + cropped_poses.push_back(motion_utils::calcInterpolatedPose(path.points, arc_length)); } + prev_poses = motion_utils::removeOverlapPoints(cropped_poses); + prev_curvatures = cropped_curvatures; } -point_t convert_point(const Point & p) +double calculate_minimum_lane_width( + const double curvature_radius, const DrivableAreaExpansionParameters & params) { - return point_t{p.x, p.y}; + const auto k = curvature_radius; + const auto a = params.vehicle_info.front_overhang_m + params.extra_front_overhang; + const auto w = params.vehicle_info.vehicle_width_m + params.extra_width; + const auto l = params.vehicle_info.wheel_base_m + params.extra_wheelbase; + return (a * a + 2.0 * a * l + 2.0 * k * w + l * l + w * w) / (2.0 * k + w); } -Point convert_point(const point_t & p) +std::vector calculate_minimum_expansions( + const std::vector & path_poses, const std::vector bound, + const std::vector curvatures, const Side side, + const DrivableAreaExpansionParameters & params) { - return Point().set__x(p.x()).set__y(p.y()); + std::vector minimum_expansions(bound.size()); + size_t lb_idx = 0; + 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; + 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 + if (bound_idx + 2 < bound.size()) { + auto up_arc_length = + tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]) + + tier4_autoware_utils::calcDistance2d(bound[bound_idx + 1], bound[bound_idx + 2]); + for (auto up_bound_idx = bound_idx + 2; + bound_idx < bound.size() && up_arc_length <= params.extra_arc_length; + ++up_bound_idx) { + minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist); + if (up_bound_idx + 1 < bound.size()) + up_arc_length += + tier4_autoware_utils::calcDistance2d(bound[up_bound_idx], bound[up_bound_idx + 1]); + } + } + if (bound_idx > 0) { + auto down_arc_length = + tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx]) + + tier4_autoware_utils::calcDistance2d(bound[bound_idx - 1], bound[bound_idx]); + for (auto down_bound_idx = bound_idx - 1; + down_bound_idx > 0 && down_arc_length <= params.extra_arc_length; --down_bound_idx) { + minimum_expansions[down_bound_idx] = std::max(minimum_expansions[down_bound_idx], dist); + if (down_bound_idx > 1) + down_arc_length += tier4_autoware_utils::calcDistance2d( + bound[down_bound_idx], bound[down_bound_idx - 1]); + } + } + break; + } + } + } + return minimum_expansions; } -polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multi_polygon_t & expansion_polygons) +void apply_bound_change_rate_limit( + std::vector & distances, const std::vector & bound, const double max_rate) { - polygon_t original_da_poly; - original_da_poly.outer().reserve(path.left_bound.size() + path.right_bound.size() + 1); - for (const auto & p : path.left_bound) original_da_poly.outer().push_back(convert_point(p)); - for (auto it = path.right_bound.rbegin(); it != path.right_bound.rend(); ++it) - original_da_poly.outer().push_back(convert_point(*it)); - original_da_poly.outer().push_back(original_da_poly.outer().front()); - - multi_polygon_t unions; - auto expanded_da_poly = original_da_poly; - for (const auto & p : expansion_polygons) { - unions.clear(); - boost::geometry::union_(expanded_da_poly, p, unions); - if (unions.size() == 1) // union of overlapping polygons should produce a single polygon - expanded_da_poly = unions[0]; - } - return expanded_da_poly; + if (distances.empty()) return; + const auto apply_max_vel = [&](auto & exp, const auto from, const auto to) { + if (exp[from] > exp[to]) { + const auto arc_length = tier4_autoware_utils::calcDistance2d(bound[from], bound[to]); + const auto smoothed_dist = exp[from] - arc_length * max_rate; + exp[to] = std::max(exp[to], smoothed_dist); + } + }; + for (auto idx = 0LU; idx + 1 < distances.size(); ++idx) apply_max_vel(distances, idx, idx + 1); + for (auto idx = distances.size() - 1; idx > 0; --idx) apply_max_vel(distances, idx, idx - 1); } -void copy_z_over_arc_length( - const std::vector & from, std::vector & to) +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 DrivableAreaExpansionParameters & params) { - if (from.empty() || to.empty()) return; - to.front().z = from.front().z; - if (from.size() < 2 || to.size() < 2) return; - to.back().z = from.back().z; - auto i_from = 1lu; - auto s_from = tier4_autoware_utils::calcDistance2d(from[0], from[1]); - auto s_to = 0.0; - auto s_from_prev = 0.0; - for (auto i_to = 1lu; i_to + 1 < to.size(); ++i_to) { - s_to += tier4_autoware_utils::calcDistance2d(to[i_to - 1], to[i_to]); - for (; s_from < s_to && i_from + 1 < from.size(); ++i_from) { - s_from_prev = s_from; - s_from += tier4_autoware_utils::calcDistance2d(from[i_from], from[i_from + 1]); + // 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 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 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); } - if (s_from - s_from_prev != 0.0) { - const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); - to[i_to].z = interpolation::lerp(from[i_from - 1].z, from[i_from].z, ratio); - } else { - to[i_to].z = to[i_to - 1].z; + 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 (params.max_expansion_distance > 0.0) + for (auto & d : maximum_distances) d = std::min(params.max_expansion_distance, d); + return maximum_distances; } -void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area) +void expand_bound( + std::vector & bound, const std::vector & path_poses, + const std::vector & expansions) { - const auto original_left_bound = path.left_bound; - const auto original_right_bound = path.right_bound; - const auto is_left_of_path = [&](const point_t & p) { - return motion_utils::calcLateralOffset(path.points, convert_point(p)) > 0.0; - }; - // prepare delimiting lines: start and end of the original expanded drivable area - const auto start_segment = - segment_t{convert_point(path.left_bound.front()), convert_point(path.right_bound.front())}; - const auto end_segment = - segment_t{convert_point(path.left_bound.back()), convert_point(path.right_bound.back())}; - point_t start_segment_center; - boost::geometry::centroid(start_segment, start_segment_center); - const auto path_start_segment = - segment_t{start_segment_center, convert_point(path.points[1].point.pose.position)}; - point_t end_segment_center; - boost::geometry::centroid(end_segment, end_segment_center); - const auto path_end_segment = - segment_t{convert_point(path.points.back().point.pose.position), end_segment_center}; - const auto segment_to_line_intersection = - [](const auto p1, const auto p2, const auto q1, const auto q2) -> std::optional { - const auto line = Eigen::Hyperplane::Through(q1, q2); - const auto segment = Eigen::Hyperplane::Through(p1, p2); - const auto intersection = line.intersection(segment); - std::optional result; - const auto is_on_segment = - (p1.x() <= p2.x() ? intersection.x() >= p1.x() && intersection.x() <= p2.x() - : intersection.x() <= p1.x() && intersection.x() >= p2.x()) && - (p1.y() <= p2.y() ? intersection.y() >= p1.y() && intersection.y() <= p2.y() - : intersection.y() <= p1.y() && intersection.y() >= p2.y()); - if (is_on_segment) result = point_t{intersection.x(), intersection.y()}; - return result; - }; - // find intersection between the expanded drivable area and the delimiting lines - const auto & da = expanded_drivable_area.outer(); - struct Intersection - { - point_t intersection_point; - ring_t::const_iterator segment_it; - double distance = std::numeric_limits::max(); - explicit Intersection(ring_t::const_iterator it) : segment_it(it) {} - void update(const point_t & p, const ring_t::const_iterator & it, const double dist) - { - intersection_point = p; - segment_it = it; - distance = dist; + LineString2d path_ls; + for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); + for (auto idx = 0LU; idx < bound.size(); ++idx) { + 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); + if (expansion_ratio > 1.0) { + 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(); + bound[idx].y = expanded_p.y(); } - }; - Intersection start_left(da.end()); - Intersection end_left(da.end()); - Intersection start_right(da.end()); - Intersection end_right(da.end()); - for (auto it = da.begin(); it != da.end(); ++it) { - if (boost::geometry::distance(*it, start_segment.first) < 1e-3) - start_left.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, start_segment.second) < 1e-3) - start_right.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, end_segment.first) < 1e-3) - end_left.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, end_segment.second) < 1e-3) - end_right.update(*it, it, 0.0); - const auto inter_start = - std::next(it) == da.end() - ? segment_to_line_intersection(*it, da.front(), start_segment.first, start_segment.second) - : segment_to_line_intersection( - *it, *std::next(it), start_segment.first, start_segment.second); - if (inter_start) { - const auto dist = boost::geometry::distance(*inter_start, path_start_segment); - const auto is_left = is_left_of_path(*inter_start); - if (is_left && dist < start_left.distance) - start_left.update(*inter_start, it, dist); - else if (!is_left && dist < start_right.distance) - start_right.update(*inter_start, it, dist); - } - const auto inter_end = - std::next(it) == da.end() - ? segment_to_line_intersection(*it, da.front(), end_segment.first, end_segment.second) - : segment_to_line_intersection(*it, *std::next(it), end_segment.first, end_segment.second); - if (inter_end) { - const auto dist = boost::geometry::distance(*inter_end, path_end_segment); - const auto is_left = is_left_of_path(*inter_end); - if (is_left && dist < end_left.distance) - end_left.update(*inter_end, it, dist); - else if (!is_left && dist < end_right.distance) - end_right.update(*inter_end, it, dist); - } - } - if (start_left.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, start_segment.first) < - boost::geometry::distance(b, start_segment.first); - }); - start_left.update(*closest_it, closest_it, 0.0); - } - if (start_right.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, start_segment.second) < - boost::geometry::distance(b, start_segment.second); - }); - start_right.update(*closest_it, closest_it, 0.0); - } - if (end_left.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, end_segment.first) < - boost::geometry::distance(b, end_segment.first); - }); - end_left.update(*closest_it, closest_it, 0.0); - } - if (end_right.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, end_segment.second) < - boost::geometry::distance(b, end_segment.second); - }); - end_right.update(*closest_it, closest_it, 0.0); } - // extract the expanded left and right bound from the expanded drivable area - path.left_bound.clear(); - path.right_bound.clear(); - path.left_bound.push_back(convert_point(start_left.intersection_point)); - path.right_bound.push_back(convert_point(start_right.intersection_point)); - if (!boost::geometry::equals(start_right.intersection_point, *start_right.segment_it)) - path.right_bound.push_back(convert_point(*start_right.segment_it)); - if (start_left.segment_it < end_left.segment_it) { - for (auto it = std::next(start_left.segment_it); it <= end_left.segment_it; ++it) - path.left_bound.push_back(convert_point(*it)); - } else { - for (auto it = std::next(start_left.segment_it); it < da.end(); ++it) - path.left_bound.push_back(convert_point(*it)); - for (auto it = da.begin(); it <= end_left.segment_it; ++it) - path.left_bound.push_back(convert_point(*it)); + // remove any self intersection by skipping the points inside of the loop + std::vector no_loop_bound = {bound.front()}; + for (auto idx = 1LU; idx < bound.size(); ++idx) { + bool is_intersecting = false; + for (auto succ_idx = idx + 1; succ_idx < bound.size(); ++succ_idx) { + const auto intersection = tier4_autoware_utils::intersect( + bound[idx - 1], bound[idx], bound[succ_idx - 1], bound[succ_idx]); + if ( + intersection && + tier4_autoware_utils::calcDistance2d(*intersection, bound[idx - 1]) < 1e-3 && + tier4_autoware_utils::calcDistance2d(*intersection, bound[idx]) < 1e-3) { + idx = succ_idx; + is_intersecting = true; + } + } + if (!is_intersecting) no_loop_bound.push_back(bound[idx]); } - if (!boost::geometry::equals(end_left.intersection_point, *end_left.segment_it)) - path.left_bound.push_back(convert_point(end_left.intersection_point)); - if (start_right.segment_it < end_right.segment_it) { - for (auto it = std::prev(start_right.segment_it); it >= da.begin(); --it) - path.right_bound.push_back(convert_point(*it)); - for (auto it = std::prev(da.end()); it > end_right.segment_it; --it) - path.right_bound.push_back(convert_point(*it)); - } else { - for (auto it = std::prev(start_right.segment_it); it > end_right.segment_it; --it) - path.right_bound.push_back(convert_point(*it)); + bound = no_loop_bound; +} + +std::vector calculate_smoothed_curvatures( + const std::vector & poses, const size_t smoothing_window_size) +{ + const auto curvatures = motion_utils::calcCurvature(poses); + std::vector smoothed_curvatures(curvatures.size()); + for (auto i = 0UL; i < curvatures.size(); ++i) { + auto sum = 0.0; + const auto from_idx = (i >= smoothing_window_size ? i - smoothing_window_size : 0); + const auto to_idx = std::min(i + smoothing_window_size, curvatures.size() - 1); + for (auto j = from_idx; j <= to_idx; ++j) sum += std::abs(curvatures[j]); + smoothed_curvatures[i] = sum / static_cast(to_idx - from_idx + 1); } - if (!boost::geometry::equals(end_right.intersection_point, *std::next(end_right.segment_it))) - path.right_bound.push_back(convert_point(end_right.intersection_point)); - // remove possible duplicated points - const auto point_cmp = [](const auto & p1, const auto & p2) { - return p1.x == p2.x && p1.y == p2.y; - }; - path.left_bound.erase( - std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp), path.left_bound.end()); - path.right_bound.erase( - std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp), - path.right_bound.end()); - copy_z_over_arc_length(original_left_bound, path.left_bound); - copy_z_over_arc_length(original_right_bound, path.right_bound); + return smoothed_curvatures; +} + +void expand_drivable_area( + PathWithLaneId & path, + const std::shared_ptr planner_data) +{ + // skip if no bounds or not enough points to calculate path curvature + if (path.points.size() < 3 || path.left_bound.empty() || path.right_bound.empty()) return; + 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_lines = extract_uncrossable_lines( + *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"); + + stop_watch.tic("crop"); + std::vector path_poses = planner_data->drivable_area_expansion_prev_path_poses; + std::vector curvatures = planner_data->drivable_area_expansion_prev_curvatures; + reuse_previous_poses( + path, path_poses, curvatures, planner_data->self_odometry->pose.pose.position, params); + const auto crop_ms = stop_watch.toc("crop"); + + stop_watch.tic("curvatures_expansion"); + // Only add curvatures for the new points. Curvatures of reused path points are not updated. + const auto new_curvatures = + calculate_smoothed_curvatures(path_poses, params.curvature_average_window); + 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); + 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_lines, uncrossable_polygons, params); + const auto max_right_expansions = calculate_maximum_distance( + path_poses, path.right_bound, uncrossable_lines, 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]); + const auto max_dist_ms = stop_watch.toc("max_dist"); + + 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); + const auto smooth_ms = stop_watch.toc("smooth"); + // 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); + const auto expand_ms = stop_watch.toc("expand"); + + const auto total_ms = stop_watch.toc("overall"); + if (params.print_runtime) + std::printf( + "Total runtime(ms): %2.2f\n\tPreprocessing: %2.2f\n\tCrop: %2.2f\n\tCurvature expansion: " + "%2.2f\n\tMaximum expansion: %2.2f\n\tSmoothing: %2.2f\n\tExpansion: %2.2f\n\n", + total_ms, preprocessing_ms, crop_ms, curvature_expansion_ms, max_dist_ms, smooth_ms, + expand_ms); + + planner_data->drivable_area_expansion_prev_path_poses = path_poses; + planner_data->drivable_area_expansion_prev_curvatures = curvatures; } } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp deleted file mode 100644 index 8fe8b12252aea..0000000000000 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ /dev/null @@ -1,237 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" - -#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" - -namespace drivable_area_expansion -{ - -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_linestring_t & limit_lines) -{ - auto dist_limit = std::numeric_limits::max(); - for (const auto & line : limit_lines) { - multi_point_t intersections; - boost::geometry::intersection(expansion_polygon, limit_lines, intersections); - for (const auto & p : intersections) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - for (const auto & p : line) - if (boost::geometry::within(p, expansion_polygon)) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - } - return dist_limit; -} - -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_polygon_t & limit_polygons) -{ - auto dist_limit = std::numeric_limits::max(); - for (const auto & polygon : limit_polygons) { - multi_point_t intersections; - boost::geometry::intersection(expansion_polygon, polygon, intersections); - for (const auto & p : intersections) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - } - return dist_limit; -} - -polygon_t createExpansionPolygon( - const linestring_t & base_ls, const double dist, const bool is_left_side) -{ - namespace strategy = boost::geometry::strategy::buffer; - multi_polygon_t polygons; - // set a non 0 value for the buffer as it sometimes causes no polygon to be returned by bg:buffer - constexpr auto zero = 0.1; - const auto left_dist = is_left_side ? dist : zero; - const auto right_dist = !is_left_side ? dist : zero; - const auto distance_strategy = strategy::distance_asymmetric(left_dist, right_dist); - boost::geometry::buffer( - base_ls, polygons, distance_strategy, strategy::side_straight(), strategy::join_miter(), - strategy::end_flat(), strategy::point_square()); - return polygons.empty() ? polygon_t{} : polygons.front(); -} - -std::array calculate_arc_length_range_and_distance( - const linestring_t & path_ls, const polygon_t & footprint, const linestring_t & bound, - const bool is_left, const double path_length) -{ - multi_point_t intersections; - double expansion_dist = 0.0; - double from_arc_length = std::numeric_limits::max(); - double to_arc_length = std::numeric_limits::min(); - boost::geometry::intersection(footprint, bound, intersections); - if (!intersections.empty()) { - for (const auto & intersection : intersections) { - const auto projection = point_to_linestring_projection(intersection, path_ls); - if (projection.arc_length <= 0.0 || projection.arc_length >= path_length) continue; - from_arc_length = std::min(from_arc_length, projection.arc_length); - to_arc_length = std::max(to_arc_length, projection.arc_length); - } - for (const auto & p : footprint.outer()) { - const auto projection = point_to_linestring_projection(p, path_ls); - if (projection.arc_length <= 0.0 || projection.arc_length >= path_length - 1e-3) continue; - if (is_left == (projection.distance > 0) && std::abs(projection.distance) > expansion_dist) { - expansion_dist = std::abs(projection.distance); - from_arc_length = std::min(from_arc_length, projection.arc_length); - to_arc_length = std::max(to_arc_length, projection.arc_length); - } - } - } - return std::array({from_arc_length, to_arc_length, expansion_dist}); -} - -polygon_t create_compensation_polygon( - const linestring_t & base_ls, const double compensation_dist, const bool is_left, - const multi_linestring_t uncrossable_lines, const multi_polygon_t & predicted_paths) -{ - polygon_t compensation_polygon = createExpansionPolygon(base_ls, compensation_dist, !is_left); - double dist_limit = std::min( - compensation_dist, calculateDistanceLimit(base_ls, compensation_polygon, uncrossable_lines)); - if (!predicted_paths.empty()) - dist_limit = - std::min(dist_limit, calculateDistanceLimit(base_ls, compensation_polygon, predicted_paths)); - if (dist_limit < compensation_dist) - compensation_polygon = createExpansionPolygon(base_ls, dist_limit, !is_left); - return compensation_polygon; -} - -multi_polygon_t createExpansionPolygons( - const PathWithLaneId & path, const multi_polygon_t & path_footprints, - const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, - const DrivableAreaExpansionParameters & params) -{ - linestring_t path_ls; - linestring_t left_ls; - linestring_t right_ls; - for (const auto & p : path.points) - path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); - for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y); - for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y); - // extend the path linestring to the beginning and end of the drivable area - if (!right_ls.empty() && !left_ls.empty() && path_ls.size() > 2) { - const auto left_proj_begin = point_to_line_projection(left_ls.front(), path_ls[0], path_ls[1]); - const auto right_proj_begin = - point_to_line_projection(right_ls.front(), path_ls[0], path_ls[1]); - const auto left_ls_proj_begin = point_to_linestring_projection(left_proj_begin.point, path_ls); - const auto right_ls_proj_begin = - point_to_linestring_projection(right_proj_begin.point, path_ls); - if (left_ls_proj_begin.arc_length < right_ls_proj_begin.arc_length) - path_ls.insert(path_ls.begin(), left_proj_begin.point); - else - path_ls.insert(path_ls.begin(), right_proj_begin.point); - const auto left_proj_end = - point_to_line_projection(left_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); - const auto right_proj_end = - point_to_line_projection(right_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); - const auto left_ls_proj_end = point_to_linestring_projection(left_proj_end.point, path_ls); - const auto right_ls_proj_end = point_to_linestring_projection(right_proj_end.point, path_ls); - if (left_ls_proj_end.arc_length > right_ls_proj_end.arc_length) - path_ls.push_back(left_proj_end.point); - else - path_ls.push_back(right_proj_end.point); - } - const auto path_length = static_cast(boost::geometry::length(path_ls)); - - multi_polygon_t expansion_polygons; - for (const auto & footprint : path_footprints) { - bool is_left = true; - for (const auto & bound : {left_ls, right_ls}) { - auto [from_arc_length, to_arc_length, footprint_dist] = - calculate_arc_length_range_and_distance(path_ls, footprint, bound, is_left, path_length); - if (footprint_dist > 0.0) { - from_arc_length -= params.extra_arc_length; - to_arc_length += params.extra_arc_length; - from_arc_length = std::max(0.0, from_arc_length); - to_arc_length = std::min(path_length, to_arc_length); - const auto base_ls = sub_linestring(path_ls, from_arc_length, to_arc_length); - const auto expansion_dist = params.max_expansion_distance != 0.0 - ? std::min(params.max_expansion_distance, footprint_dist) - : footprint_dist; - auto expansion_polygon = createExpansionPolygon(base_ls, expansion_dist, is_left); - auto limited_dist = expansion_dist; - const auto uncrossable_dist_limit = std::max( - 0.0, calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines) - - params.avoid_linestring_dist); - if (uncrossable_dist_limit < limited_dist) { - limited_dist = uncrossable_dist_limit; - if (params.compensate_uncrossable_lines) { - const auto compensation_dist = - footprint_dist - limited_dist + params.compensate_extra_dist; - expansion_polygons.push_back(create_compensation_polygon( - base_ls, compensation_dist, is_left, uncrossable_lines, predicted_paths)); - } - } - limited_dist = std::min( - limited_dist, calculateDistanceLimit(base_ls, expansion_polygon, predicted_paths)); - if (limited_dist < expansion_dist) - expansion_polygon = createExpansionPolygon(base_ls, limited_dist, is_left); - expansion_polygons.push_back(expansion_polygon); - } - is_left = false; - } - } - return expansion_polygons; -} - -multi_polygon_t createExpansionLaneletPolygons( - const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, - const DrivableAreaExpansionParameters & params) -{ - multi_polygon_t expansion_polygons; - lanelet::ConstLanelets candidates; - const auto already_added = [&](const auto & ll) { - return std::find_if(candidates.begin(), candidates.end(), [&](const auto & l) { - return ll.id() == l.id(); - }) != candidates.end(); - }; - const auto add_if_valid = [&](const auto & ll, const auto is_left) { - const auto bound_to_check = is_left ? ll.rightBound() : ll.leftBound(); - if (std::find_if(path_lanes.begin(), path_lanes.end(), [&](const auto & l) { - return ll.id() == l.id(); - }) == path_lanes.end()) - if (!already_added(ll) && !hasTypes(bound_to_check, params.avoid_linestring_types)) - candidates.push_back(ll); - }; - for (const auto & current_ll : path_lanes) { - for (const auto & left_ll : - route_handler.getLaneletsFromPoint(current_ll.leftBound3d().front())) - add_if_valid(left_ll, true); - for (const auto & left_ll : route_handler.getLaneletsFromPoint(current_ll.leftBound3d().back())) - add_if_valid(left_ll, true); - for (const auto & right_ll : - route_handler.getLaneletsFromPoint(current_ll.rightBound3d().front())) - add_if_valid(right_ll, false); - for (const auto & right_ll : - route_handler.getLaneletsFromPoint(current_ll.rightBound3d().back())) - add_if_valid(right_ll, false); - } - for (const auto & candidate : candidates) { - polygon_t candidate_poly; - for (const auto & p : candidate.polygon2d()) candidate_poly.outer().emplace_back(p.x(), p.y()); - boost::geometry::correct(candidate_poly); - if ( - !boost::geometry::overlaps(candidate_poly, predicted_paths) && - boost::geometry::overlaps(path_footprints, candidate_poly)) - expansion_polygons.push_back(candidate_poly); - } - return expansion_polygons; -} - -} // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp index ae12ac438caf3..c03adf6c8a76e 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp @@ -25,61 +25,39 @@ namespace drivable_area_expansion { -polygon_t translatePolygon(const polygon_t & polygon, const double x, const double y) +Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const double y) { - polygon_t translated_polygon; + Polygon2d translated_polygon; const boost::geometry::strategy::transform::translate_transformer translation(x, y); boost::geometry::transform(polygon, translated_polygon, translation); return translated_polygon; } -polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t base_footprint) +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint) { const auto angle = tf2::getYaw(pose.orientation); - return translatePolygon(rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); + return translate_polygon( + tier4_autoware_utils::rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); } -multi_polygon_t createObjectFootprints( +MultiPolygon2d create_object_footprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params) { - multi_polygon_t footprints; - for (const auto & object : objects.objects) { - const auto front = object.shape.dimensions.x / 2 + params.dynamic_objects_extra_front_offset; - const auto rear = -object.shape.dimensions.x / 2 - params.dynamic_objects_extra_rear_offset; - const auto left = object.shape.dimensions.y / 2 + params.dynamic_objects_extra_left_offset; - const auto right = -object.shape.dimensions.y / 2 - params.dynamic_objects_extra_right_offset; - polygon_t base_footprint; - base_footprint.outer() = { - point_t{front, left}, point_t{front, right}, point_t{rear, right}, point_t{rear, left}, - point_t{front, left}}; - for (const auto & path : object.kinematics.predicted_paths) - for (const auto & pose : path.path) - footprints.push_back(createFootprint(pose, base_footprint)); - } - return footprints; -} - -multi_polygon_t createPathFootprints( - const std::vector & points, const DrivableAreaExpansionParameters & params) -{ - const auto left = params.ego_left_offset + params.ego_extra_left_offset; - const auto right = params.ego_right_offset - params.ego_extra_right_offset; - const auto rear = params.ego_rear_offset - params.ego_extra_rear_offset; - const auto front = params.ego_front_offset + params.ego_extra_front_offset; - polygon_t base_footprint; - base_footprint.outer() = { - point_t{front, left}, point_t{front, right}, point_t{rear, right}, point_t{rear, left}, - point_t{front, left}}; - multi_polygon_t footprints; - // skip the last footprint as its orientation is usually wrong - footprints.reserve(points.size() - 1); - double arc_length = 0.0; - for (auto it = points.begin(); std::next(it) != points.end(); ++it) { - footprints.push_back(createFootprint(it->point.pose, base_footprint)); - if (params.max_path_arc_length > 0.0) { - arc_length += tier4_autoware_utils::calcDistance2d(it->point.pose, std::next(it)->point.pose); - if (arc_length > params.max_path_arc_length) break; + MultiPolygon2d footprints; + if (params.avoid_dynamic_objects) { + for (const auto & object : objects.objects) { + const auto front = object.shape.dimensions.x / 2 + params.dynamic_objects_extra_front_offset; + const auto rear = -object.shape.dimensions.x / 2 - params.dynamic_objects_extra_rear_offset; + const auto left = object.shape.dimensions.y / 2 + params.dynamic_objects_extra_left_offset; + const auto right = -object.shape.dimensions.y / 2 - params.dynamic_objects_extra_right_offset; + Polygon2d base_footprint; + base_footprint.outer() = { + Point2d{front, left}, Point2d{front, right}, Point2d{rear, right}, Point2d{rear, left}, + Point2d{front, left}}; + for (const auto & path : object.kinematics.predicted_paths) + for (const auto & pose : path.path) + footprints.push_back(create_footprint(pose, base_footprint)); } } return footprints; 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 ded67c251f7ae..deeb787cf39f6 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 @@ -15,33 +15,34 @@ #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" -#include "lanelet2_core/primitives/LineString.h" - #include #include +#include #include namespace drivable_area_expansion { -multi_linestring_t extractUncrossableLines( - const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types) +MultiLineString2d extract_uncrossable_lines( + const lanelet::LaneletMap & lanelet_map, const Point & ego_point, + const DrivableAreaExpansionParameters & params) { - multi_linestring_t lines; - linestring_t line; + MultiLineString2d uncrossable_lines_in_range; + LineString2d line; + const auto ego_p = Point2d{ego_point.x, ego_point.y}; for (const auto & ls : lanelet_map.lineStringLayer) { - if (hasTypes(ls, uncrossable_types)) { + if (has_types(ls, params.avoid_linestring_types)) { line.clear(); - for (const auto & p : ls) line.push_back(point_t{p.x(), p.y()}); - lines.push_back(line); + 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); } } - return lines; + return uncrossable_lines_in_range; } -bool hasTypes(const lanelet::ConstLineString3d & ls, const std::vector & types) +bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types) { constexpr auto no_type = ""; const auto type = ls.attributeOr(lanelet::AttributeName::Type, no_type); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index bae591ca16d40..ed2979bd1d5a1 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1532,7 +1532,7 @@ void generateDrivableArea( } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { - drivable_area_expansion::expandDrivableArea(path, planner_data, transformed_lanes); + drivable_area_expansion::expand_drivable_area(path, planner_data); } // make bound longitudinally monotonic diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index 7a5bb68decdfa..1cade95a96681 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -14,16 +14,15 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include -using drivable_area_expansion::linestring_t; -using drivable_area_expansion::point_t; -using drivable_area_expansion::segment_t; +using drivable_area_expansion::LineString2d; +using drivable_area_expansion::Point2d; +using drivable_area_expansion::Segment2d; constexpr auto eps = 1e-9; TEST(DrivableAreaExpansionProjection, PointToSegment) @@ -31,56 +30,56 @@ TEST(DrivableAreaExpansionProjection, PointToSegment) using drivable_area_expansion::point_to_segment_projection; { - point_t query(1.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(1.0, 1.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, 1.0, eps); EXPECT_NEAR(projection.point.x(), 1.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(-1.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(-1.0, 1.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, std::sqrt(2), eps); EXPECT_NEAR(projection.point.x(), 0.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(11.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(11.0, 1.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, std::sqrt(2), eps); EXPECT_NEAR(projection.point.x(), 10.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(5.0, -5.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + 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.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(5.0, -5.0); - segment_t segment(point_t(0.0, 0.0), point_t(0.0, -10.0)); + Point2d query(5.0, -5.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(0.0, -10.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, 5.0, eps); EXPECT_NEAR(projection.point.x(), 0.0, eps); EXPECT_NEAR(projection.point.y(), -5.0, eps); } { - point_t query(5.0, 5.0); - segment_t segment(point_t(2.5, 7.5), point_t(7.5, 2.5)); + Point2d query(5.0, 5.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, 0.0, eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 5.0, eps); } { - point_t query(0.0, 0.0); - segment_t segment(point_t(2.5, 7.5), point_t(7.5, 2.5)); + 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.point.x(), 5.0, eps); @@ -92,11 +91,11 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) { using drivable_area_expansion::point_to_linestring_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; { - point_t query(0.0, 0.0); + Point2d query(0.0, 0.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 0.0, eps); EXPECT_NEAR(projection.distance, 0.0, eps); @@ -104,7 +103,7 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) EXPECT_NEAR(projection.projected_point.y(), 0.0, eps); } { - point_t query(2.0, 1.0); + Point2d query(2.0, 1.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 2.0, eps); EXPECT_NEAR(projection.distance, 1.0, eps); @@ -112,7 +111,7 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) EXPECT_NEAR(projection.projected_point.y(), 0.0, eps); } { - point_t query(0.0, 5.0); + 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); @@ -125,9 +124,9 @@ TEST(DrivableAreaExpansionProjection, LinestringToPoint) { using drivable_area_expansion::linestring_to_point_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; for (auto arc_length = 0.0; arc_length <= 10.0; arc_length += 1.0) { const auto projection = linestring_to_point_projection(ls, arc_length, 0.0); EXPECT_NEAR(projection.first.x(), arc_length, eps); @@ -151,58 +150,18 @@ TEST(DrivableAreaExpansionProjection, LinestringToPoint) } } -TEST(DrivableAreaExpansionProjection, SubLinestring) -{ - using drivable_area_expansion::sub_linestring; - - const linestring_t ls = { - point_t{0.0, 0.0}, point_t{1.0, 0.0}, point_t{2.0, 0.0}, point_t{3.0, 0.0}, - point_t{4.0, 0.0}, point_t{5.0, 0.0}, point_t{6.0, 0.0}, - }; - { - // arc lengths equal to the original range: same linestring is returned - const auto sub = sub_linestring(ls, 0.0, 6.0); - ASSERT_EQ(ls.size(), sub.size()); - for (auto i = 0lu; i < ls.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i], sub[i])); - } - { - // arc lengths equal to existing point: sub-linestring with same points - const auto sub = sub_linestring(ls, 1.0, 5.0); - ASSERT_EQ(ls.size() - 2lu, sub.size()); - for (auto i = 0lu; i < sub.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i + 1], sub[i])); - } - { - // arc lengths inside the original: sub-linestring with some interpolated points - const auto sub = sub_linestring(ls, 1.5, 2.5); - ASSERT_EQ(sub.size(), 3lu); - EXPECT_NEAR(sub[0].x(), 1.5, eps); - EXPECT_NEAR(sub[1].x(), 2.0, eps); - EXPECT_NEAR(sub[2].x(), 2.5, eps); - for (const auto & p : sub) EXPECT_NEAR(p.y(), 0.0, eps); - } - { - // arc length outside of the original range: first & last point are replaced by interpolations - const auto sub = sub_linestring(ls, -0.5, 8.5); - ASSERT_EQ(sub.size(), ls.size()); - EXPECT_NEAR(sub.front().x(), -0.5, eps); - for (auto i = 1lu; i + 1 < ls.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i], sub[i])); - EXPECT_NEAR(sub.back().x(), 8.5, eps); - for (const auto & p : sub) EXPECT_NEAR(p.y(), 0.0, eps); - } -} - TEST(DrivableAreaExpansionProjection, InverseProjection) { using drivable_area_expansion::linestring_to_point_projection; using drivable_area_expansion::point_to_linestring_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; for (auto x = 0.0; x < 10.0; x += 0.1) { for (auto y = 0.0; x < 10.0; x += 0.1) { - point_t p(x, y); + Point2d p(x, y); const auto projection = point_to_linestring_projection(p, ls); const auto inverse = linestring_to_point_projection(ls, projection.arc_length, projection.distance); @@ -212,7 +171,7 @@ TEST(DrivableAreaExpansionProjection, InverseProjection) } } -TEST(DrivableAreaExpansionProjection, expandDrivableArea) +TEST(DrivableAreaExpansionProjection, expand_drivable_area) { drivable_area_expansion::DrivableAreaExpansionParameters params; drivable_area_expansion::PredictedObjects dynamic_objects; @@ -255,122 +214,60 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.avoid_dynamic_objects = false; params.avoid_linestring_dist = 0.0; params.avoid_linestring_types = {}; - params.compensate_extra_dist = false; params.max_expansion_distance = 0.0; // means no limit params.max_path_arc_length = 0.0; // means no limit params.resample_interval = 1.0; - params.extra_arc_length = 1.0; - params.expansion_method = "polygon"; // 2m x 4m ego footprint - params.ego_front_offset = 1.0; - params.ego_rear_offset = -1.0; - params.ego_left_offset = 2.0; - params.ego_right_offset = -2.0; + params.vehicle_info.front_overhang_m = 0.0; + params.vehicle_info.wheel_base_m = 2.0; + params.vehicle_info.vehicle_width_m = 2.0; } behavior_path_planner::PlannerData planner_data; planner_data.drivable_area_expansion_parameters = params; planner_data.reference_path = std::make_shared(path); planner_data.dynamic_object = std::make_shared(dynamic_objects); + planner_data.self_odometry = std::make_shared(); planner_data.route_handler = std::make_shared(route_handler); - // we expect the drivable area to be expanded by 1m on each side - drivable_area_expansion::expandDrivableArea( - path, std::make_shared(planner_data), path_lanes); + drivable_area_expansion::expand_drivable_area( + path, std::make_shared(planner_data)); // unchanged path points ASSERT_EQ(path.points.size(), 3ul); for (auto i = 0.0; i < path.points.size(); ++i) { EXPECT_NEAR(path.points[i].point.pose.position.x, i, eps); EXPECT_NEAR(path.points[i].point.pose.position.y, 0.0, eps); } - + // straight path: no expansion // expanded left bound - ASSERT_EQ(path.left_bound.size(), 4ul); + ASSERT_EQ(path.left_bound.size(), 3ul); EXPECT_NEAR(path.left_bound[0].x, 0.0, eps); EXPECT_NEAR(path.left_bound[0].y, 1.0, eps); - EXPECT_NEAR(path.left_bound[1].x, 0.0, eps); - EXPECT_NEAR(path.left_bound[1].y, 2.0, eps); + EXPECT_NEAR(path.left_bound[1].x, 1.0, eps); + EXPECT_NEAR(path.left_bound[1].y, 1.0, eps); EXPECT_NEAR(path.left_bound[2].x, 2.0, eps); - EXPECT_NEAR(path.left_bound[2].y, 2.0, eps); - EXPECT_NEAR(path.left_bound[3].x, 2.0, eps); - EXPECT_NEAR(path.left_bound[3].y, 1.0, eps); + EXPECT_NEAR(path.left_bound[2].y, 1.0, eps); // expanded right bound ASSERT_EQ(path.right_bound.size(), 3ul); EXPECT_NEAR(path.right_bound[0].x, 0.0, eps); - EXPECT_NEAR(path.right_bound[0].y, -2.0, eps); - EXPECT_NEAR(path.right_bound[1].x, 2.0, eps); - EXPECT_NEAR(path.right_bound[1].y, -2.0, eps); + EXPECT_NEAR(path.right_bound[0].y, -1.0, eps); + EXPECT_NEAR(path.right_bound[1].x, 1.0, eps); + EXPECT_NEAR(path.right_bound[1].y, -1.0, eps); EXPECT_NEAR(path.right_bound[2].x, 2.0, eps); EXPECT_NEAR(path.right_bound[2].y, -1.0, eps); -} - -TEST(DrivableAreaExpansion, calculateDistanceLimit) -{ - using drivable_area_expansion::calculateDistanceLimit; - using drivable_area_expansion::linestring_t; - using drivable_area_expansion::multi_linestring_t; - using drivable_area_expansion::polygon_t; - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multi_linestring_t uncrossable_lines = {}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); - EXPECT_NEAR(limit_distance, std::numeric_limits::max(), 1e-9); - } - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const linestring_t uncrossable_line = {{0.0, 2.0}, {10.0, 2.0}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_line}); - EXPECT_NEAR(limit_distance, 2.0, 1e-9); - } - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multi_linestring_t uncrossable_lines = { - {{0.0, 2.0}, {10.0, 2.0}}, {{0.0, 1.5}, {10.0, 1.0}}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); - EXPECT_NEAR(limit_distance, 1.0, 1e-9); - } -} - -TEST(DrivableAreaExpansion, calculateDistanceLimitEdgeCases) -{ - using drivable_area_expansion::calculateDistanceLimit; - using drivable_area_expansion::linestring_t; - using drivable_area_expansion::polygon_t; + // add some curvature + path.points[1].point.pose.position.y = 0.5; - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - { // intersection points further than the line point inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 5.0}, {6.0, 3.0}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 3.0, 1e-9); - } - { // intersection points further than the line point inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 5.0}, {5.0, 2.0}, {6.0, 4.5}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 2.0, 1e-9); - } - { // line completely inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 2.0}, {6.0, 3.0}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 2.0, 1e-9); - } - { // line completely inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 3.5}, {6.0, 3.0}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 3.0, 1e-9); - } + drivable_area_expansion::expand_drivable_area( + path, std::make_shared(planner_data)); + // expanded left bound + ASSERT_EQ(path.left_bound.size(), 3ul); + EXPECT_GT(path.left_bound[0].y, 1.0); + EXPECT_GT(path.left_bound[1].y, 1.0); + EXPECT_GT(path.left_bound[2].y, 1.0); + // expanded right bound + ASSERT_EQ(path.right_bound.size(), 3ul); + EXPECT_LT(path.right_bound[0].y, -1.0); + EXPECT_LT(path.right_bound[1].y, -1.0); + EXPECT_LT(path.right_bound[2].y, -1.0); }