feat(behavior_path_planner): curvature based drivable area expansion (a…

…utowarefoundation#935)

* Switch to new curvature based dynamic drivable area expansion

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Cleanup + remove the old code

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Handle uncrossable lines/polygons (may not be accurate enough)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Add runtime measurements

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* [WIP] Reuse previously calculated raw curvatures

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Fix bug with lateral offset distance and repeating path points

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Remove self crossings in the expanded bounds

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Big cleanup + update parameters

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Remove svg debug output

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Update parameter file

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Fix rebase mistakes

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Add parameter to enable/disable printing the runtime

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Fix append of new path points to satisfy the resampling interval

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Add smoothing.extra_arc_length param

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Code cleanup + add docstrings

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Fix spellcheck

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Fix initial path poses (no longer cropped) and fix test

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

---------

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

planning/behavior_path_planner/CMakeLists.txt

@@ -49,7 +49,6 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/utils/drivable_area_expansion/drivable_area_expansion.cpp
   src/utils/drivable_area_expansion/map_utils.cpp
   src/utils/drivable_area_expansion/footprints.cpp
-  src/utils/drivable_area_expansion/expansion.cpp
   src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp
   src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp
   src/marker_utils/utils.cpp

planning/behavior_path_planner/config/drivable_area_expansion.param.yaml

@@ -5,16 +5,19 @@
     drivable_area_left_bound_offset: 0.0
     drivable_area_types_to_skip: [road_border]
 
-    # Dynamic expansion by projecting the ego footprint along the path
+    # Dynamic expansion by using the path curvature
     dynamic_expansion:
-      enabled: false
-      debug_print: false  # if true, print some debug runtime measurements
+      enabled: true
+      print_runtime: false
+      max_expansion_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit)
+      smoothing:
+        curvature_average_window: 3  # window size used for smoothing the curvatures using a moving window average
+        max_bound_rate: 1.0  # [m/m] maximum rate of change of the bound lateral distance over its arc length
+        extra_arc_length: 2.0  # [m] extra arc length where an expansion distance is initially applied
       ego:
-        extra_footprint_offset:
-          front: 0.5 # [m] extra length to add to the front of the ego footprint
-          rear: 0.5 # [m] extra length to add to the rear of the ego footprint
-          left: 0.5 # [m] extra length to add to the left of the ego footprint
-          right: 0.5 # [m] extra length to add to the rear of the ego footprint
+        extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase
+        extra_front_overhang: 0.5 # [m] extra length to add to the front overhang
+        extra_width: 1.0 # [m] extra length to add to the width
       dynamic_objects:
         avoid: false # if true, the drivable area is not expanded in the predicted path of dynamic objects
         extra_footprint_offset:
@@ -25,20 +28,8 @@
       path_preprocessing:
         max_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit)
         resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used)
-      expansion:
-        method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'.
-                         # 'lanelet': add lanelets overlapped by the ego footprints
-                         # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area
-        max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit)
-        extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint
+        reuse_max_deviation: 0.5 # [m] if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused.
       avoid_linestring:
         types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area
           - road_border
         distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid
-        compensate:
-          enable: true  # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction
-          extra_distance: 3.0 # [m] extra distance to add to the compensation
-      replan_checker:
-        enable: true  # if true, only recalculate the expanded drivable area when the path or its original drivable area change significantly
-                      # not compatible with dynamic_objects.avoid
-        max_deviation: 0.5  # [m] full replan is only done if the path changes by more than this distance

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -18,7 +18,6 @@
 #include "behavior_path_planner/parameters.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
 #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
-#include "behavior_path_planner/utils/drivable_area_expansion/replan_checker.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <route_handler/route_handler.hpp>
@@ -150,8 +149,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 std::vector<geometry_msgs::msg::Pose> drivable_area_expansion_prev_path_poses{};
+  mutable std::vector<double> drivable_area_expansion_prev_curvatures{};
   mutable TurnSignalDecider turn_signal_decider;
 
   TurnIndicatorsCommand getTurnSignal(

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

@@ -24,30 +24,76 @@
 #include <lanelet2_core/Forward.h>
 
 #include <memory>
+#include <vector>
 
 namespace drivable_area_expansion
 {
-/// @brief Expand the drivable area based on the projected ego footprint along the path
+/// @brief Expand the drivable area based on the path curvature and the vehicle dimensions
 /// @param[inout] path path whose drivable area will be expanded
-/// @param[inout] planner_data planning data (params, dynamic objects, route handler, ...)
-/// @param[in] path_lanes lanelets of the path
-void expandDrivableArea(
+/// @param[inout] planner_data planning data (params, dynamic objects, vehicle info, ...)
+void expand_drivable_area(
   PathWithLaneId & path,
-  const std::shared_ptr<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
-/// @param[in] path path and its drivable area
-/// @param[in] expansion_polygons polygons to add to the drivable area
-/// @return expanded drivable area polygon
-polygon_t createExpandedDrivableAreaPolygon(
-  const PathWithLaneId & path, const multi_polygon_t & expansion_polygons);
-
-/// @brief Update the drivable area of the given path with the given polygon
-/// @details this function splits the polygon into a left and right bound and sets it in the path
-/// @param[in] path path whose drivable area will be expanded
-/// @param[in] expanded_drivable_area polygon of the new drivable area
-void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area);
+  const std::shared_ptr<const behavior_path_planner::PlannerData> planner_data);
+
+/// @brief prepare path poses and try to reuse their previously calculated curvatures
+/// @details poses are reused if they do not deviate too much from the current path
+/// @param [in] path input path
+/// @param [inout] prev_poses previous poses to reuse
+/// @param [inout] prev_curvatures previous curvatures to reuse
+/// @param [in] ego_point current ego point
+/// @param [in] params parameters for reuse criteria and resampling interval
+void update_path_poses_and_previous_curvatures(
+  const PathWithLaneId & path, std::vector<Pose> & prev_poses,
+  std::vector<double> & prev_curvatures, const Point & ego_point,
+  const DrivableAreaExpansionParameters & params);
+
+/// @brief calculate the minimum lane width based on the path curvature and the vehicle dimensions
+/// @cite Lim, H., Kim, C., and Jo, A., "Model Predictive Control-Based Lateral Control of
+/// Autonomous Large-Size Bus on Road with Large Curvature," SAE Technical Paper 2021-01-0099, 2021,
+/// https://doi.org/10.4271/2021-01-0099
+/// @param [in] curvature curvature
+/// @param [in] params parameters containing the vehicle dimensions
+/// @return minimum lane width
+double calculate_minimum_lane_width(
+  const double curvature_radius, const DrivableAreaExpansionParameters & params);
+
+/// @brief smooth the bound by applying a limit on its rate of change
+/// @details rate of change is the lateral distance from the path over the arc length along the path
+/// @param [inout] bound_distances bound distances (lateral distance from the path)
+/// @param [in] bound_points bound points
+/// @param [in] max_rate [m/m] maximum rate of lateral deviation over arc length
+void apply_bound_change_rate_limit(
+  std::vector<double> & distances, const std::vector<Point> & bound, const double max_rate);
+
+/// @brief calculate the maximum distance by which a bound can be expanded
+/// @param [in] path_poses input path
+/// @param [in] bound bound points
+/// @param [in] uncrossable_lines lines that limit the bound expansion
+/// @param [in] uncrossable_polygons polygons that limit the bound expansion
+/// @param [in] params parameters with the buffer distance to keep with lines,
+/// and the static maximum expansion distance
+std::vector<double> calculate_maximum_distance(
+  const std::vector<Pose> & path_poses, const std::vector<Point> bound,
+  const std::vector<LineString2d> & uncrossable_lines,
+  const std::vector<Polygon2d> & uncrossable_polygons,
+  const DrivableAreaExpansionParameters & params);
+
+/// @brief expand a bound by the given lateral distances away from the path
+/// @param [inout] bound bound points to expand
+/// @param [in] path_poses input path
+/// @param [in] distances new lateral distances of the bound points away from the path
+void expand_bound(
+  std::vector<Point> & bound, const std::vector<Pose> & path_poses,
+  const std::vector<double> & distances);
+
+/// @brief calculate smoothed curvatures
+/// @details smoothing is done using a moving average
+/// @param [in] poses input poses used to calculate the curvatures
+/// @param [in] smoothing_window_size size of the window used for the moving average
+/// @return smoothed curvatures of the input poses
+std::vector<double> calculate_smoothed_curvatures(
+  const std::vector<Pose> & poses, const size_t smoothing_window_size);
+
 }  // namespace drivable_area_expansion
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_

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

@@ -43,27 +43,20 @@ namespace drivable_area_expansion
 /// @param[in] x translation distance on the x axis
 /// @param[in] y translation distance on the y axis
 /// @return translated polygon
-polygon_t translatePolygon(const polygon_t & polygon, const double x, const double y);
+Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const double y);
 
 /// @brief create the footprint of a pose and its base footprint
 /// @param[in] pose the origin pose of the footprint
 /// @param[in] base_footprint the base axis-aligned footprint
 /// @return footprint polygon
-polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t base_footprint);
+Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint);
 
 /// @brief create footprints of the predicted paths of an object
 /// @param [in] objects objects from which to create polygons
 /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects
 /// @return footprint polygons of the object's predicted paths
-multi_polygon_t createObjectFootprints(
+MultiPolygon2d create_object_footprints(
   const autoware_auto_perception_msgs::msg::PredictedObjects & objects,
   const DrivableAreaExpansionParameters & params);
-
-/// @brief create the footprint polygon from a path
-/// @param[in] path the path for which to create a footprint
-/// @param[in] params expansion parameters defining how to create the footprint
-/// @return footprint polygons of the path
-multi_polygon_t createPathFootprints(
-  const std::vector<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/map_utils.hpp

@@ -15,6 +15,7 @@
 #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_
 #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_
 
+#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
 #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
 
 #include <lanelet2_core/LaneletMap.h>
@@ -24,18 +25,20 @@
 
 namespace drivable_area_expansion
 {
-/// @brief Extract uncrossable linestrings from the lanelet map
+/// @brief Extract uncrossable linestrings from the lanelet map that are in range of ego
 /// @param[in] lanelet_map lanelet map
-/// @param[in] uncrossable_types types that cannot be crossed
+/// @param[in] ego_point point of the current ego position
+/// @param[in] params parameters with linestring types that cannot be crossed and maximum range
 /// @return the uncrossable linestrings
-multi_linestring_t extractUncrossableLines(
-  const lanelet::LaneletMap & lanelet_map, const std::vector<std::string> & uncrossable_types);
+MultiLineString2d extract_uncrossable_lines(
+  const lanelet::LaneletMap & lanelet_map, const Point & ego_point,
+  const DrivableAreaExpansionParameters & params);
 
 /// @brief Determine if the given linestring has one of the given types
 /// @param[in] ls linestring to check
 /// @param[in] types type strings to check
 /// @return true if the linestring has one of the given types
-bool hasTypes(const lanelet::ConstLineString3d & ls, const std::vector<std::string> & types);
+bool has_types(const lanelet::ConstLineString3d & ls, const std::vector<std::string> & types);
 }  // namespace drivable_area_expansion
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_