feat(behavior_path_planner): more stable dynamic drivable area expans…

…ion (autowarefoundation#4677)

Signed-off-by: Maxime CLEMENT <[email protected]>

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

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -29,6 +29,7 @@
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
 #include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -147,6 +148,7 @@ struct PlannerData
   BehaviorPathPlannerParameters parameters{};
   drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{};
 
+  mutable std::optional<geometry_msgs::msg::Pose> drivable_area_expansion_prev_crop_pose;
   mutable TurnSignalDecider turn_signal_decider;
 
   TurnIndicatorsCommand getTurnSignal(

planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp

@@ -15,24 +15,25 @@
 #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"
 
 #include <route_handler/route_handler.hpp>
 
 #include <lanelet2_core/Forward.h>
 
+#include <memory>
+
 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<const behavior_path_planner::PlannerData> planner_data,
   const lanelet::ConstLanelets & path_lanes);
 
 /// @brief Create a polygon combining the drivable area of a path and some expansion polygons

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<PathPointWithLaneId> & path, const DrivableAreaExpansionParameters & params);
 }  // namespace drivable_area_expansion
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_

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<std::string> avoid_linestring_types{};
@@ -98,6 +101,7 @@ struct DrivableAreaExpansionParameters
     enabled = node.declare_parameter<bool>(ENABLED_PARAM);
     max_expansion_distance = node.declare_parameter<double>(MAX_EXP_DIST_PARAM);
     max_path_arc_length = node.declare_parameter<double>(MAX_PATH_ARC_LENGTH_PARAM);
+    resample_interval = node.declare_parameter<double>(RESAMPLE_INTERVAL_PARAM);
     ego_extra_front_offset = node.declare_parameter<double>(EGO_EXTRA_OFFSET_FRONT);
     ego_extra_rear_offset = node.declare_parameter<double>(EGO_EXTRA_OFFSET_REAR);
     ego_extra_left_offset = node.declare_parameter<double>(EGO_EXTRA_OFFSET_LEFT);

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;
 

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);

planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp

@@ -19,28 +19,80 @@
 #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 <Eigen/Geometry>
+#include <interpolation/linear_interpolation.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 
 #include <boost/geometry.hpp>
 
 namespace drivable_area_expansion
 {
 
+std::vector<PathPointWithLaneId> crop_and_resample(
+  const std::vector<PathPointWithLaneId> & points,
+  const std::shared_ptr<const behavior_path_planner::PlannerData> 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<const behavior_path_planner::PlannerData> 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;
   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 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"

planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp

@@ -85,7 +85,7 @@ std::array<double, 3> 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);

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<PathPointWithLaneId> & 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);

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

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<drivable_area_expansion::PathWithLaneId>(path);
+  planner_data.dynamic_object =
+    std::make_shared<drivable_area_expansion::PredictedObjects>(dynamic_objects);
+  planner_data.route_handler = std::make_shared<route_handler::RouteHandler>(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<behavior_path_planner::PlannerData>(planner_data), path_lanes);
   // unchanged path points
   ASSERT_EQ(path.points.size(), 3ul);
   for (auto i = 0.0; i < path.points.size(); ++i) {