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..160ebdc180020 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -21,12 +21,14 @@ 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 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 b682d28efeaa5..ee44c4f509963 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 @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -147,6 +148,7 @@ struct PlannerData BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; + mutable std::optional drivable_area_expansion_prev_crop_pose; 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..81eab9560b736 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"; @@ -78,6 +80,7 @@ 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{}; @@ -98,6 +101,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); 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 84393848267b9..4a3c582242cf9 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1731,6 +1731,9 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updateParam( parameters, DrivableAreaExpansionParameters::MAX_PATH_ARC_LENGTH_PARAM, planner_data_->drivable_area_expansion_parameters.max_path_arc_length); + updateParam( + parameters, DrivableAreaExpansionParameters::RESAMPLE_INTERVAL_PARAM, + planner_data_->drivable_area_expansion_parameters.resample_interval); updateParam( parameters, DrivableAreaExpansionParameters::EXTRA_ARC_LENGTH_PARAM, planner_data_->drivable_area_expansion_parameters.extra_arc_length); 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 837b8f0dfb77b..8e15848f22382 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,71 @@ #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 std::shared_ptr planner_data, + const double resample_interval) +{ + auto lon_offset = 0.0; + auto crop_pose = *planner_data->drivable_area_expansion_prev_crop_pose; + // reuse or update the previous crop point + if (planner_data->drivable_area_expansion_prev_crop_pose) { + const auto lon_offset = motion_utils::calcSignedArcLength( + points, points.front().point.pose.position, crop_pose.position); + if (lon_offset < 0.0) { + planner_data->drivable_area_expansion_prev_crop_pose.reset(); + } else { + const auto is_behind_ego = + motion_utils::calcSignedArcLength( + points, crop_pose.position, planner_data->self_odometry->pose.pose.position) > 0.0; + const auto is_too_far = motion_utils::calcLateralOffset(points, crop_pose.position) > 0.1; + if (!is_behind_ego || is_too_far) + planner_data->drivable_area_expansion_prev_crop_pose.reset(); + } + } + if (!planner_data->drivable_area_expansion_prev_crop_pose) { + crop_pose = planner_data->drivable_area_expansion_prev_crop_pose.value_or( + motion_utils::calcInterpolatedPose(points, resample_interval - lon_offset)); + } + // 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 + 1, + planner_data->drivable_area_expansion_parameters.max_path_arc_length, 0.0); + planner_data->drivable_area_expansion_prev_crop_pose = crop_pose; + // resample + PathWithLaneId cropped_path; + if (tier4_autoware_utils::calcDistance2d(crop_pose, cropped_points.front()) > 1e-3) { + PathPointWithLaneId crop_path_point; + crop_path_point.point.pose = crop_pose; + cropped_path.points.push_back(crop_path_point); + } + cropped_path.points.insert( + cropped_path.points.end(), cropped_points.begin(), cropped_points.end()); + const auto resampled_path = + motion_utils::resamplePath(cropped_path, 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) { + 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; const auto uncrossable_lines = extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types); multi_linestring_t uncrossable_lines_in_range; @@ -40,7 +91,8 @@ 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 points = crop_and_resample(path.points, planner_data, params.resample_interval); + const auto path_footprints = createPathFootprints(points, params); const auto predicted_paths = createObjectFootprints(dynamic_objects, params); const auto expansion_polygons = params.expansion_method == "lanelet" 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 5304985b1cbf2..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 @@ -85,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..dd305cdfdb760 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 @@ -63,7 +63,7 @@ multi_polygon_t createObjectFootprints( } 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 +75,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 36e3014486d74..a2a487e573aec 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1500,9 +1500,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 fe33d48f74fe5..6c02609b474c4 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) {