fix(lane_change): fix chattering for stopped objects (#5302)

Update planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp

fix(path_safety_checker): remove redundant parameter name

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

Author: kosuke55

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

@@ -182,18 +182,19 @@ struct SafetyCheckParams
 
 struct CollisionCheckDebug
 {
-  std::string unsafe_reason;                ///< Reason indicating unsafe situation.
-  Twist current_twist{};                    ///< Ego vehicle's current velocity and rotation.
-  Pose expected_ego_pose{};                 ///< Predicted future pose of ego vehicle.
-  Pose current_obj_pose{};                  ///< Detected object's current pose.
-  Twist object_twist{};                     ///< Detected object's velocity and rotation.
-  Pose expected_obj_pose{};                 ///< Predicted future pose of object.
-  double rss_longitudinal{0.0};             ///< Longitudinal RSS measure.
-  double inter_vehicle_distance{0.0};       ///< Distance between ego vehicle and object.
-  double extended_polygon_lon_offset{0.0};  ///< Longitudinal offset for extended polygon.
-  double extended_polygon_lat_offset{0.0};  ///< Lateral offset for extended polygon.
-  bool is_front{false};                     ///< True if object is in front of ego vehicle.
-  bool is_safe{false};                      ///< True if situation is deemed safe.
+  std::string unsafe_reason;           ///< Reason indicating unsafe situation.
+  Twist current_twist{};               ///< Ego vehicle's current velocity and rotation.
+  Pose expected_ego_pose{};            ///< Predicted future pose of ego vehicle.
+  Pose current_obj_pose{};             ///< Detected object's current pose.
+  Twist object_twist{};                ///< Detected object's velocity and rotation.
+  Pose expected_obj_pose{};            ///< Predicted future pose of object.
+  double rss_longitudinal{0.0};        ///< Longitudinal RSS measure.
+  double inter_vehicle_distance{0.0};  ///< Distance between ego vehicle and object.
+  double forward_lon_offset{0.0};      ///< Forward longitudinal offset for extended polygon.
+  double backward_lon_offset{0.0};     ///< Backward longitudinal offset for extended polygon.
+  double lat_offset{0.0};              ///< Lateral offset for extended polygon.
+  bool is_front{false};                ///< True if object is in front of ego vehicle.
+  bool is_safe{false};                 ///< True if situation is deemed safe.
   std::vector<PoseWithVelocityStamped> ego_predicted_path;  ///< ego vehicle's predicted path.
   std::vector<PoseWithVelocityAndPolygonStamped> obj_predicted_path;  ///< object's predicted path.
   Polygon2d extended_ego_polygon{};  ///< Ego vehicle's extended collision polygon.

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp

@@ -66,10 +66,11 @@ bool isTargetObjectFront(
 
 Polygon2d createExtendedPolygon(
   const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info,
-  const double lon_length, const double lat_margin, CollisionCheckDebug & debug);
+  const double lon_length, const double lat_margin, const double is_stopped_obj,
+  CollisionCheckDebug & debug);
 Polygon2d createExtendedPolygon(
   const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin,
-  CollisionCheckDebug & debug);
+  const double is_stopped_obj, CollisionCheckDebug & debug);
 
 PredictedPath convertToPredictedPath(
   const std::vector<PoseWithVelocityStamped> & path, const double time_resolution);

planning/behavior_path_planner/src/marker_utils/utils.cpp

@@ -641,8 +641,9 @@ MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, st
        << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal
        << "\nEgo to obj: " << info.inter_vehicle_distance
        << "\nExtended polygon: " << (info.is_front ? "ego" : "object")
-       << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset
-       << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset
+       << "\nExtended polygon lateral offset: " << info.lat_offset
+       << "\nExtended polygon forward longitudinal offset: " << info.forward_lon_offset
+       << "\nExtended polygon backward longitudinal offset: " << info.backward_lon_offset
        << "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego")
        << "\nSafe: " << (info.is_safe ? "Yes" : "No");
 

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

@@ -82,28 +82,33 @@ bool isTargetObjectFront(
 
 Polygon2d createExtendedPolygon(
   const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info,
-  const double lon_length, const double lat_margin, CollisionCheckDebug & debug)
+  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;
 
-  const double lon_offset = std::max(lon_length + base_to_front, base_to_front);
-
+  // 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.extended_polygon_lon_offset = lon_offset;
-    debug.extended_polygon_lat_offset = lat_offset;
+    debug.forward_lon_offset = forward_lon_offset;
+    debug.backward_lon_offset = backward_lon_offset;
+    debug.lat_offset = lat_offset;
   }
 
-  const auto p1 = tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, lat_offset, 0.0);
+  const auto p1 =
+    tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0);
   const auto p2 =
-    tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, -lat_offset, 0.0);
+    tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0);
   const auto p3 =
-    tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, -lat_offset, 0.0);
+    tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0);
   const auto p4 =
-    tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, lat_offset, 0.0);
+    tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0);
 
   Polygon2d polygon;
   appendPointToPolygon(polygon, p1.position);
@@ -118,7 +123,7 @@ Polygon2d createExtendedPolygon(
 
 Polygon2d createExtendedPolygon(
   const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin,
-  CollisionCheckDebug & debug)
+  const double is_stopped_obj, CollisionCheckDebug & debug)
 {
   const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, shape);
   if (obj_polygon.outer().empty()) {
@@ -139,19 +144,27 @@ Polygon2d createExtendedPolygon(
     min_y = std::min(transformed_p.y, min_y);
   }
 
-  const double lon_offset = max_x + lon_length;
+  // if stationary object, extend forward and backward by the half of lon length
+  const double forward_lon_offset = max_x + (is_stopped_obj ? lon_length / 2 : lon_length);
+  const double backward_lon_offset = min_x - (is_stopped_obj ? lon_length / 2 : 0);  // minus value
+
   const double left_lat_offset = max_y + lat_margin;
   const double right_lat_offset = min_y - lat_margin;
 
   {
-    debug.extended_polygon_lon_offset = lon_offset;
-    debug.extended_polygon_lat_offset = (left_lat_offset + right_lat_offset) / 2;
+    debug.forward_lon_offset = forward_lon_offset;
+    debug.backward_lon_offset = backward_lon_offset;
+    debug.lat_offset = (left_lat_offset + right_lat_offset) / 2;
   }
 
-  const auto p1 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, left_lat_offset, 0.0);
-  const auto p2 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, right_lat_offset, 0.0);
-  const auto p3 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, right_lat_offset, 0.0);
-  const auto p4 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, left_lat_offset, 0.0);
+  const auto p1 =
+    tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, left_lat_offset, 0.0);
+  const auto p2 =
+    tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, right_lat_offset, 0.0);
+  const auto p3 =
+    tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, right_lat_offset, 0.0);
+  const auto p4 =
+    tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0);
 
   Polygon2d polygon;
   appendPointToPolygon(polygon, p1.position);
@@ -338,14 +351,17 @@ std::vector<Polygon2d> getCollidedPolygons(
 
     const auto & lon_offset = std::max(rss_dist, min_lon_length) * hysteresis_factor;
     const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor;
-    const auto & extended_ego_polygon =
-      is_object_front
-        ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug)
-        : ego_polygon;
+    // 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_obj_polygon =
       is_object_front
         ? obj_polygon
-        : createExtendedPolygon(obj_pose, target_object.shape, lon_offset, lat_margin, debug);
+        : createExtendedPolygon(
+            obj_pose, target_object.shape, lon_offset, lat_margin, is_stopped_object, debug);
 
     {
       debug.rss_longitudinal = rss_dist;

planning/behavior_path_planner/test/test_safety_check.cpp

@@ -52,9 +52,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon)
 
     const double lon_length = 10.0;
     const double lat_margin = 2.0;
+    const bool is_stopped_object = false;
 
-    const auto polygon =
-      createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug);
+    const auto polygon = createExtendedPolygon(
+      ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug);
 
     EXPECT_EQ(polygon.outer().size(), static_cast<unsigned int>(5));
 
@@ -79,9 +80,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon)
 
     const double lon_length = 10.0;
     const double lat_margin = 2.0;
+    const bool is_stopped_object = false;
 
-    const auto polygon =
-      createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug);
+    const auto polygon = createExtendedPolygon(
+      ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug);
 
     EXPECT_EQ(polygon.outer().size(), static_cast<unsigned int>(5));
 
@@ -107,9 +109,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon)
 
     const double lon_length = 10.0;
     const double lat_margin = 2.0;
+    const bool is_stopped_object = false;
 
-    const auto polygon =
-      createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug);
+    const auto polygon = createExtendedPolygon(
+      ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug);
 
     EXPECT_EQ(polygon.outer().size(), static_cast<unsigned int>(5));
 
@@ -155,9 +158,11 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon)
 
     const double lon_length = 10.0;
     const double lat_margin = 2.0;
+    const bool is_stopped_object = false;
 
     CollisionCheckDebug debug;
-    const auto polygon = createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, debug);
+    const auto polygon =
+      createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, is_stopped_object, debug);
 
     EXPECT_EQ(polygon.outer().size(), static_cast<unsigned int>(5));