feat(safety_check): add option to use polygon along path in safety check (autowarefoundation#6336)

* feat(safety_check): add new function to create polygon along path

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(avoidance): use free steer policy for safety check

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(safety_check): fix polygon edge

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(safety_check): fix param name

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -187,6 +187,7 @@
         time_horizon_for_rear_object: 10.0               # [s]
         delay_until_departure: 0.0                       # [s]
         # rss parameters
+        extended_polygon_policy: "along_path"            # [-] select "rectangle" or "along_path"
         expected_front_deceleration: -1.0                # [m/ss]
         expected_rear_deceleration: -1.0                 # [m/ss]
         rear_vehicle_reaction_time: 2.0                  # [s]

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -223,6 +223,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
   // safety check rss params
   {
     const std::string ns = "avoidance.safety_check.";
+    p.rss_params.extended_polygon_policy =
+      getOrDeclareParameter<std::string>(*node, ns + "extended_polygon_policy");
     p.rss_params.longitudinal_distance_min_threshold =
       getOrDeclareParameter<double>(*node, ns + "longitudinal_distance_min_threshold");
     p.rss_params.longitudinal_velocity_delta_time =

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp

@@ -126,6 +126,9 @@ struct TargetObjectsOnLane
  */
 struct RSSparams
 {
+  std::string extended_polygon_policy{
+    "rectangle"};                               ///< Policy to create collision check polygon.
+                                                ///< possible values: ["rectangle", "along_path"]
   double rear_vehicle_reaction_time{0.0};       ///< Reaction time of the rear vehicle.
   double rear_vehicle_safety_time_margin{0.0};  ///< Safety time margin for the rear vehicle.
   double lateral_distance_max_threshold{0.0};   ///< Maximum threshold for lateral distance.

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

@@ -184,6 +184,88 @@ Polygon2d createExtendedPolygon(
            : tier4_autoware_utils::inverseClockwise(polygon);
 }
 
+Polygon2d createExtendedPolygonAlongPath(
+  const PathWithLaneId & planned_path, const Pose & base_link_pose,
+  const vehicle_info_util::VehicleInfo & vehicle_info, const double lon_length,
+  const double lat_margin, const double is_stopped_obj, CollisionCheckDebug & debug)
+{
+  const double & base_to_front = vehicle_info.max_longitudinal_offset_m;
+  const double & width = vehicle_info.vehicle_width_m;
+  const double & base_to_rear = vehicle_info.rear_overhang_m;
+
+  // if stationary object, extend forward and backward by the half of lon length
+  const double forward_lon_offset = base_to_front + (is_stopped_obj ? lon_length / 2 : lon_length);
+  const double backward_lon_offset =
+    -base_to_rear - (is_stopped_obj ? lon_length / 2 : 0);  // minus value
+  const double lat_offset = width / 2.0 + lat_margin;
+
+  {
+    debug.forward_lon_offset = forward_lon_offset;
+    debug.backward_lon_offset = backward_lon_offset;
+    debug.lat_offset = lat_offset;
+  }
+
+  const auto lon_offset_pose = motion_utils::calcLongitudinalOffsetPose(
+    planned_path.points, base_link_pose.position, lon_length);
+  if (!lon_offset_pose.has_value()) {
+    return createExtendedPolygon(
+      base_link_pose, vehicle_info, lon_length, lat_margin, is_stopped_obj, debug);
+  }
+
+  const size_t start_idx =
+    motion_utils::findNearestSegmentIndex(planned_path.points, base_link_pose.position);
+  const size_t end_idx =
+    motion_utils::findNearestSegmentIndex(planned_path.points, lon_offset_pose.value().position);
+
+  Polygon2d polygon;
+
+  {
+    const auto p_offset =
+      tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0);
+    appendPointToPolygon(polygon, p_offset.position);
+  }
+
+  for (size_t i = start_idx + 1; i < end_idx + 1; ++i) {
+    const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i));
+    const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, lat_offset, 0.0);
+    appendPointToPolygon(polygon, p_offset.position);
+  }
+
+  {
+    const auto p_offset =
+      tier4_autoware_utils::calcOffsetPose(lon_offset_pose.value(), base_to_front, lat_offset, 0.0);
+    appendPointToPolygon(polygon, p_offset.position);
+  }
+
+  {
+    const auto p_offset = tier4_autoware_utils::calcOffsetPose(
+      lon_offset_pose.value(), base_to_front, -lat_offset, 0.0);
+    appendPointToPolygon(polygon, p_offset.position);
+  }
+
+  for (size_t i = end_idx; i > start_idx; --i) {
+    const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i));
+    const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, -lat_offset, 0.0);
+    appendPointToPolygon(polygon, p_offset.position);
+  }
+
+  {
+    const auto p_offset =
+      tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0);
+    appendPointToPolygon(polygon, p_offset.position);
+  }
+
+  {
+    const auto p_offset =
+      tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0);
+    appendPointToPolygon(polygon, p_offset.position);
+  }
+
+  return tier4_autoware_utils::isClockwise(polygon)
+           ? polygon
+           : tier4_autoware_utils::inverseClockwise(polygon);
+}
+
 std::vector<Polygon2d> createExtendedPolygonsFromPoseWithVelocityStamped(
   const std::vector<PoseWithVelocityStamped> & predicted_path, const VehicleInfo & vehicle_info,
   const double forward_margin, const double backward_margin, const double lat_margin)
@@ -552,10 +634,24 @@ std::vector<Polygon2d> getCollidedPolygons(
     const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor;
     // TODO(watanabe) fix hard coding value
     const bool is_stopped_object = object_velocity < 0.3;
-    const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon(
-                                                            ego_pose, ego_vehicle_info, lon_offset,
-                                                            lat_margin, is_stopped_object, debug)
-                                                        : ego_polygon;
+    const auto extended_ego_polygon = [&]() {
+      if (!is_object_front) {
+        return ego_polygon;
+      }
+
+      if (rss_parameters.extended_polygon_policy == "rectangle") {
+        return createExtendedPolygon(
+          ego_pose, ego_vehicle_info, lon_offset, lat_margin, is_stopped_object, debug);
+      }
+
+      if (rss_parameters.extended_polygon_policy == "along_path") {
+        return createExtendedPolygonAlongPath(
+          planned_path, ego_pose, ego_vehicle_info, lon_offset, lat_margin, is_stopped_object,
+          debug);
+      }
+
+      throw std::domain_error("invalid rss parameter. please select 'rectangle' or 'along_path'.");
+    }();
     const auto & extended_obj_polygon =
       is_object_front
         ? obj_polygon