feat(dynamic_avoidance): object polygon based drivable area generation (

autowarefoundation#4598)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp

@@ -74,6 +74,7 @@ struct DynamicAvoidanceParameters
   double max_oncoming_crossing_object_angle{0.0};
 
   // drivable area generation
+  std::string polygon_generation_method{};
   double lat_offset_from_obstacle{0.0};
   double max_lat_offset_to_avoid{0.0};
   double max_time_for_lat_shift{0.0};
@@ -335,7 +336,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface
 
   std::pair<lanelet::ConstLanelets, lanelet::ConstLanelets> getAdjacentLanes(
     const double forward_distance, const double backward_distance) const;
-  std::optional<tier4_autoware_utils::Polygon2d> calcDynamicObstaclePolygon(
+  std::optional<tier4_autoware_utils::Polygon2d> calcEgoPathBasedDynamicObstaclePolygon(
+    const DynamicAvoidanceObject & object) const;
+  std::optional<tier4_autoware_utils::Polygon2d> calcObjectPathBasedDynamicObstaclePolygon(
     const DynamicAvoidanceObject & object) const;
 
   void printIgnoreReason(const std::string & obj_uuid, const std::string & reason)

planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp

@@ -274,7 +274,15 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan()
   // create obstacles to avoid (= extract from the drivable area)
   std::vector<DrivableAreaInfo::Obstacle> obstacles_for_drivable_area;
   for (const auto & object : target_objects_) {
-    const auto obstacle_poly = calcDynamicObstaclePolygon(object);
+    const auto obstacle_poly = [&]() {
+      if (parameters_->polygon_generation_method == "ego_path_base") {
+        return calcEgoPathBasedDynamicObstaclePolygon(object);
+      }
+      if (parameters_->polygon_generation_method == "object_path_base") {
+        return calcObjectPathBasedDynamicObstaclePolygon(object);
+      }
+      throw std::logic_error("The polygon_generation_method's string is invalid.");
+    }();
     if (obstacle_poly) {
       obstacles_for_drivable_area.push_back(
         {object.pose, obstacle_poly.value(), object.is_collision_left});
@@ -836,7 +844,8 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid(
 }
 
 // NOTE: object does not have const only to update min_bound_lat_offset.
-std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynamicObstaclePolygon(
+std::optional<tier4_autoware_utils::Polygon2d>
+DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon(
   const DynamicAvoidanceObject & object) const
 {
   if (!object.lon_offset_to_avoid || !object.lat_offset_to_avoid) {
@@ -896,4 +905,52 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
   boost::geometry::correct(obj_poly);
   return obj_poly;
 }
+
+std::optional<tier4_autoware_utils::Polygon2d>
+DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon(
+  const DynamicAvoidanceObject & object) const
+{
+  const auto obj_path = *std::max_element(
+    object.predicted_paths.begin(), object.predicted_paths.end(),
+    [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; });
+
+  // calculate left and right bound
+  std::vector<geometry_msgs::msg::Point> obj_left_bound_points;
+  std::vector<geometry_msgs::msg::Point> obj_right_bound_points;
+  for (size_t i = 0; i < obj_path.path.size(); ++i) {
+    const double lon_offset = [&]() {
+      if (i == 0) return -object.shape.dimensions.x / 2.0 - parameters_->lat_offset_from_obstacle;
+      if (i == obj_path.path.size() - 1)
+        return object.shape.dimensions.x / 2.0 + parameters_->lat_offset_from_obstacle;
+      return 0.0;
+    }();
+
+    const auto & obj_pose = obj_path.path.at(i);
+    obj_left_bound_points.push_back(
+      tier4_autoware_utils::calcOffsetPose(
+        obj_pose, lon_offset,
+        object.shape.dimensions.y / 2.0 + parameters_->lat_offset_from_obstacle, 0.0)
+        .position);
+    obj_right_bound_points.push_back(
+      tier4_autoware_utils::calcOffsetPose(
+        obj_pose, lon_offset,
+        -object.shape.dimensions.y / 2.0 - parameters_->lat_offset_from_obstacle, 0.0)
+        .position);
+  }
+
+  // create obj_polygon from inner/outer bound points
+  tier4_autoware_utils::Polygon2d obj_poly;
+  for (const auto & bound_point : obj_right_bound_points) {
+    const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y);
+    obj_poly.outer().push_back(obj_poly_point);
+  }
+  std::reverse(obj_left_bound_points.begin(), obj_left_bound_points.end());
+  for (const auto & bound_point : obj_left_bound_points) {
+    const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y);
+    obj_poly.outer().push_back(obj_poly_point);
+  }
+
+  boost::geometry::correct(obj_poly);
+  return obj_poly;
+}
 }  // namespace behavior_path_planner

planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp

@@ -81,6 +81,8 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager(
 
   {  // drivable_area_generation
     std::string ns = "dynamic_avoidance.drivable_area_generation.";
+    p.polygon_generation_method =
+      node->declare_parameter<std::string>(ns + "polygon_generation_method");
     p.lat_offset_from_obstacle = node->declare_parameter<double>(ns + "lat_offset_from_obstacle");
     p.max_lat_offset_to_avoid = node->declare_parameter<double>(ns + "max_lat_offset_to_avoid");
     p.max_time_for_lat_shift =
@@ -178,6 +180,8 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
   {  // drivable_area_generation
     const std::string ns = "dynamic_avoidance.drivable_area_generation.";
 
+    updateParam<std::string>(
+      parameters, ns + "polygon_generation_method", p->polygon_generation_method);
     updateParam<double>(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle);
     updateParam<double>(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid);
     updateParam<double>(