fix(drivable_area_expansion): stabler expansion

planning/behavior_path_planner/config/drivable_area_expansion.param.yaml

@@ -8,25 +8,28 @@
     # 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
           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
       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

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 <rclcpp/rclcpp.hpp>
 #include <route_handler/route_handler.hpp>
@@ -29,6 +30,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>
@@ -148,6 +150,8 @@ 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 drivable_area_expansion::ReplanChecker drivable_area_expansion_replan_checker{};
   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,15 +48,21 @@ 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";
   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";
 
   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<std::string> 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<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);
@@ -118,6 +129,9 @@ struct DrivableAreaExpansionParameters
     compensate_uncrossable_lines = node.declare_parameter<bool>(COMPENSATE_PARAM);
     compensate_extra_dist = node.declare_parameter<double>(EXTRA_COMPENSATE_PARAM);
     expansion_method = node.declare_parameter<std::string>(EXPANSION_METHOD_PARAM);
+    replan_enable = node.declare_parameter<bool>(REPLAN_ENABLE_PARAM);
+    replan_max_deviation = node.declare_parameter<double>(REPLAN_MAX_DEVIATION_PARAM);
+    debug_print = node.declare_parameter<bool>(DEBUG_PRINT_PARAM);
 
     const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
     ego_left_offset = vehicle_info.max_lateral_offset_m;

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 <boost/geometry.hpp>
+
+#include <vector>
+
+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_

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

@@ -990,69 +990,83 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam(
     // Drivable area expansion parameters
     using drivable_area_expansion::DrivableAreaExpansionParameters;
     const std::lock_guard<std::mutex> 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();