From c9279b468a3deb4adf390072fb2ea86eb0dd4037 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 25 Sep 2023 11:28:11 +0900 Subject: [PATCH] fix(drivable_area_expansion): stabler expansion Signed-off-by: Takayuki Murooka --- .../config/drivable_area_expansion.param.yaml | 11 +- .../behavior_path_planner/data_manager.hpp | 4 + .../drivable_area_expansion.hpp | 13 ++- .../drivable_area_expansion/footprints.hpp | 2 +- .../drivable_area_expansion/parameters.hpp | 16 ++- .../replan_checker.hpp | 92 +++++++++++++++ .../utils/drivable_area_expansion/types.hpp | 1 + .../src/behavior_path_planner_node.cpp | 56 +++++---- .../drivable_area_expansion.cpp | 107 +++++++++++++++--- .../drivable_area_expansion/expansion.cpp | 15 ++- .../drivable_area_expansion/footprints.cpp | 32 +++--- .../behavior_path_planner/src/utils/utils.cpp | 4 +- .../test/test_drivable_area_expansion.cpp | 55 ++++++++- 13 files changed, 330 insertions(+), 78 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/replan_checker.hpp 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 1609cdbc60b7a..706a4fc2e45c1 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -8,6 +8,7 @@ # Dynamic expansion by projecting the ego footprint along the path dynamic_expansion: enabled: false + debug_print: false # if true, print some debug runtime measurements ego: extra_footprint_offset: front: 0.5 # [m] extra length to add to the front of the ego footprint @@ -15,18 +16,20 @@ 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 dynamic_objects: - avoid: true # if true, the drivable area is not expanded in the predicted path of dynamic objects + avoid: false # if true, the drivable area is not expanded in the predicted path of dynamic objects extra_footprint_offset: front: 0.5 # [m] extra length to add to the front of the dynamic object footprint rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint left: 0.5 # [m] extra length to add to the left of the dynamic object footprint right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint + 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) - max_path_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area @@ -35,3 +38,7 @@ 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 5b252cef92714..5b61efd1fbb96 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,6 +18,7 @@ #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 @@ -29,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -148,6 +150,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 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 53ef91473fe24..8293e0a1d10a9 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 @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" @@ -22,17 +23,17 @@ #include +#include + namespace drivable_area_expansion { /// @brief Expand the drivable area based on the projected ego footprint along the path -/// @param[in] path path whose drivable area will be expanded -/// @param[in] params expansion parameters -/// @param[in] dynamic_objects dynamic objects -/// @param[in] route_handler route handler +/// @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( - PathWithLaneId & path, const DrivableAreaExpansionParameters & params, - const PredictedObjects & dynamic_objects, const route_handler::RouteHandler & route_handler, + 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 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 e5e1c76f2521f..8fc0157710dc8 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 @@ -64,6 +64,6 @@ multi_polygon_t createObjectFootprints( /// @param[in] params expansion parameters defining how to create the footprint /// @return footprint polygons of the path multi_polygon_t createPathFootprints( - const PathWithLaneId & path, const DrivableAreaExpansionParameters & params); + 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/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index a4c11c68d13ec..d93cb65d0554d 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 @@ -48,8 +48,10 @@ struct DrivableAreaExpansionParameters "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 RESAMPLE_INTERVAL_PARAM = + "dynamic_expansion.path_preprocessing.resample_interval"; static constexpr auto MAX_PATH_ARC_LENGTH_PARAM = - "dynamic_expansion.expansion.max_path_arc_length"; + "dynamic_expansion.path_preprocessing.max_arc_length"; static constexpr auto EXTRA_ARC_LENGTH_PARAM = "dynamic_expansion.expansion.extra_arc_length"; static constexpr auto AVOID_DYN_OBJECTS_PARAM = "dynamic_expansion.dynamic_objects.avoid"; static constexpr auto AVOID_LINESTRING_TYPES_PARAM = "dynamic_expansion.avoid_linestring.types"; @@ -57,6 +59,10 @@ struct DrivableAreaExpansionParameters 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"; double drivable_area_right_bound_offset; double drivable_area_left_bound_offset; @@ -78,11 +84,15 @@ struct DrivableAreaExpansionParameters double dynamic_objects_extra_front_offset{}; double max_expansion_distance{}; double max_path_arc_length{}; + double resample_interval{}; double extra_arc_length{}; bool avoid_dynamic_objects{}; std::vector avoid_linestring_types{}; bool compensate_uncrossable_lines = false; double compensate_extra_dist{}; + bool replan_enable{}; + double replan_max_deviation{}; + bool debug_print{}; DrivableAreaExpansionParameters() = default; explicit DrivableAreaExpansionParameters(rclcpp::Node & node) { init(node); } @@ -98,6 +108,7 @@ struct DrivableAreaExpansionParameters enabled = node.declare_parameter(ENABLED_PARAM); max_expansion_distance = node.declare_parameter(MAX_EXP_DIST_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); @@ -118,6 +129,9 @@ struct DrivableAreaExpansionParameters 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); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ego_left_offset = vehicle_info.max_lateral_offset_m; 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 new file mode 100644 index 0000000000000..278893ccae55c --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/replan_checker.hpp @@ -0,0 +1,92 @@ +// 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 daabfa97598fd..e300a347e47a8 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 @@ -26,6 +26,7 @@ namespace drivable_area_expansion { using autoware_auto_perception_msgs::msg::PredictedObjects; 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; 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 f86b2ff2c9c61..c2f122220fe48 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -990,69 +990,83 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( // Drivable area expansion parameters using drivable_area_expansion::DrivableAreaExpansionParameters; const std::lock_guard lock(mutex_pd_); // for planner_data_ - updateParam( + bool updated = false; + updated |= updateParam( parameters, DrivableAreaExpansionParameters::DRIVABLE_AREA_RIGHT_BOUND_OFFSET_PARAM, planner_data_->drivable_area_expansion_parameters.drivable_area_right_bound_offset); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM, planner_data_->drivable_area_expansion_parameters.drivable_area_left_bound_offset); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::DRIVABLE_AREA_TYPES_TO_SKIP_PARAM, planner_data_->drivable_area_expansion_parameters.drivable_area_types_to_skip); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::ENABLED_PARAM, planner_data_->drivable_area_expansion_parameters.enabled); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::AVOID_DYN_OBJECTS_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_dynamic_objects); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::EXPANSION_METHOD_PARAM, planner_data_->drivable_area_expansion_parameters.expansion_method); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_TYPES_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_types); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_DIST_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_dist); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_FRONT, planner_data_->drivable_area_expansion_parameters.ego_extra_front_offset); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_REAR, planner_data_->drivable_area_expansion_parameters.ego_extra_rear_offset); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_LEFT, planner_data_->drivable_area_expansion_parameters.ego_extra_left_offset); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_RIGHT, planner_data_->drivable_area_expansion_parameters.ego_extra_right_offset); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_FRONT, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_front_offset); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_REAR, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_rear_offset); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_LEFT, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_left_offset); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_RIGHT, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_right_offset); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::MAX_EXP_DIST_PARAM, planner_data_->drivable_area_expansion_parameters.max_expansion_distance); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::MAX_PATH_ARC_LENGTH_PARAM, planner_data_->drivable_area_expansion_parameters.max_path_arc_length); - updateParam( + updated |= updateParam( + 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); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::COMPENSATE_PARAM, planner_data_->drivable_area_expansion_parameters.compensate_uncrossable_lines); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::EXTRA_COMPENSATE_PARAM, planner_data_->drivable_area_expansion_parameters.compensate_extra_dist); + updated |= updateParam( + parameters, DrivableAreaExpansionParameters::REPLAN_ENABLE_PARAM, + planner_data_->drivable_area_expansion_parameters.replan_enable); + 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(); } 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 2b62d5e7c3b53..102ed5b8c5fd1 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 @@ -19,20 +19,62 @@ #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/types.hpp" -#include "interpolation/linear_interpolation.hpp" #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) +{ + 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 DrivableAreaExpansionParameters & params, - const PredictedObjects & dynamic_objects, const route_handler::RouteHandler & route_handler, + PathWithLaneId & path, + const std::shared_ptr planner_data, const lanelet::ConstLanelets & path_lanes) { + 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; @@ -40,16 +82,50 @@ void expandDrivableArea( 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 path_footprints = createPathFootprints(path, params); - const auto predicted_paths = createObjectFootprints(dynamic_objects, params); - const auto expansion_polygons = + 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); + + 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); + + 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; + } } point_t convert_point(const Point & p) @@ -113,8 +189,8 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ { const auto original_left_bound = path.left_bound; const auto original_right_bound = path.right_bound; - const auto is_left_of_segment = [](const point_t & a, const point_t & b, const point_t & p) { - return (b.x() - a.x()) * (p.y() - a.y()) - (b.y() - a.y()) * (p.x() - a.x()) > 0; + 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 = @@ -178,12 +254,10 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ *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_of_path_start = is_left_of_segment( - convert_point(path.points[0].point.pose.position), - convert_point(path.points[1].point.pose.position), *inter_start); - if (is_left_of_path_start && dist < start_left.distance) + 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_of_path_start && dist < start_right.distance) + else if (!is_left && dist < start_right.distance) start_right.update(*inter_start, it, dist); } const auto inter_end = @@ -192,11 +266,10 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ : 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_of_path_end = is_left_of_segment( - convert_point(path.points.back().point.pose.position), end_segment_center, *inter_end); - if (is_left_of_path_end && dist < end_left.distance) + 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_of_path_end && dist < end_right.distance) + else if (!is_left && dist < end_right.distance) end_right.update(*inter_end, it, dist); } } 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 index 035cc579d2a82..828fdc2f17a51 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp @@ -25,10 +25,15 @@ double calculateDistanceLimit( const multi_linestring_t & limit_lines) { auto dist_limit = std::numeric_limits::max(); - 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 & 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; } @@ -80,7 +85,7 @@ std::array calculate_arc_length_range_and_distance( } 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) continue; + 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); 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 0c31093a83c0e..ae12ac438caf3 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 @@ -44,26 +44,24 @@ multi_polygon_t createObjectFootprints( const DrivableAreaExpansionParameters & params) { multi_polygon_t 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; - 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)); - } + 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 PathWithLaneId & path, const DrivableAreaExpansionParameters & params) + 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; @@ -75,9 +73,9 @@ multi_polygon_t createPathFootprints( point_t{front, left}}; multi_polygon_t footprints; // skip the last footprint as its orientation is usually wrong - footprints.reserve(path.points.size() - 1); + footprints.reserve(points.size() - 1); double arc_length = 0.0; - for (auto it = path.points.begin(); std::next(it) != path.points.end(); ++it) { + 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); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index f6d6edc6920a7..c7fde9dada4ec 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1532,9 +1532,7 @@ void generateDrivableArea( } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { - drivable_area_expansion::expandDrivableArea( - path, expansion_params, *planner_data->dynamic_object, *planner_data->route_handler, - transformed_lanes); + drivable_area_expansion::expandDrivableArea(path, planner_data, transformed_lanes); } // 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 605b3b96ee9b9..7a5bb68decdfa 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#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" @@ -257,6 +258,7 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) 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 @@ -265,9 +267,15 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.ego_left_offset = 2.0; params.ego_right_offset = -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.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, params, dynamic_objects, route_handler, path_lanes); + path, std::make_shared(planner_data), path_lanes); // unchanged path points ASSERT_EQ(path.points.size(), 3ul); for (auto i = 0.0; i < path.points.size(); ++i) { @@ -276,13 +284,15 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) } // expanded left bound - ASSERT_EQ(path.left_bound.size(), 3ul); + ASSERT_EQ(path.left_bound.size(), 4ul); 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[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); // expanded right bound ASSERT_EQ(path.right_bound.size(), 3ul); EXPECT_NEAR(path.right_bound[0].x, 0.0, eps); @@ -304,7 +314,7 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit) 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}, {10.0, -4.0}}, {}}; + {{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); @@ -313,7 +323,7 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit) 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}, {10.0, -4.0}}, {}}; + {{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); @@ -323,9 +333,44 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit) 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}, {10.0, -4.0}}, {}}; + {{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; + + 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); + } +}