feat(goal_planner): secure lateral distance for path planning with corresponding to the centrifugal force (autowarefoundation#5196)

* add feature to plan the path with considering latral overshoot

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

* separate a function in "safety_check" in order to that "goal_planner_module" can handle the ego_polygons

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

* add visualization codes
fix the handling miss of sign of value

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

* refactor codes based on comments in the PR

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

---------

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

Author: yuki-takagi-66

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

@@ -106,6 +106,7 @@ struct FreespacePlannerDebugData
 struct GoalPlannerDebugData
 {
   FreespacePlannerDebugData freespace_planner{};
+  std::vector<Polygon2d> ego_polygons_expanded{};
 };
 
 class GoalPlannerModule : public SceneModuleInterface

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

@@ -110,6 +110,10 @@ bool checkCollision(
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
   const double hysteresis_factor, CollisionCheckDebug & debug);
 
+std::vector<Polygon2d> generatePolygonsWithStoppingAndInertialMargin(
+  const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear,
+  const double width, const double maximum_deceleration, const double max_extra_stopping_margin);
+
 /**
  * @brief Iterate the points in the ego and target's predicted path and
  *        perform safety check for each of the iterated points.
@@ -133,14 +137,12 @@ std::vector<Polygon2d> getCollidedPolygons(
   const double hysteresis_factor, CollisionCheckDebug & debug);
 
 /**
- * @brief Check collision between ego path footprints with extra longitudinal stopping margin and
- * objects.
+ * @brief Check collision between ego polygons with margin and objects.
  * @return Has collision or not
  */
-bool checkCollisionWithExtraStoppingMargin(
-  const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects,
-  const double base_to_front, const double base_to_rear, const double width,
-  const double maximum_deceleration, const double margin, const double max_stopping_margin);
+bool checkCollisionWithMargin(
+  const std::vector<Polygon2d> & ego_polygons, const PredictedObjects & dynamic_objects,
+  const double collision_check_margin);
 }  // namespace behavior_path_planner::utils::path_safety_checker
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -1296,28 +1296,32 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const
     }
   }
 
-  if (parameters_->use_object_recognition) {
-    const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
-      *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,
-      parameters_->forward_goal_search_length);
-    const auto [pull_over_lane_objects, others] =
-      utils::path_safety_checker::separateObjectsByLanelets(
-        *(planner_data_->dynamic_object), pull_over_lanes);
-    const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
-      pull_over_lane_objects, parameters_->th_moving_object_velocity);
-    const auto common_parameters = planner_data_->parameters;
-    const double base_link2front = common_parameters.base_link2front;
-    const double base_link2rear = common_parameters.base_link2rear;
-    const double vehicle_width = common_parameters.vehicle_width;
-    if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin(
-          path, pull_over_lane_stop_objects, base_link2front, base_link2rear, vehicle_width,
-          parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin,
-          parameters_->object_recognition_collision_check_max_extra_stopping_margin)) {
-      return true;
-    }
+  if (!parameters_->use_object_recognition) {
+    return false;
   }
 
-  return false;
+  const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
+    *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,
+    parameters_->forward_goal_search_length);
+  const auto [pull_over_lane_objects, others] =
+    utils::path_safety_checker::separateObjectsByLanelets(
+      *(planner_data_->dynamic_object), pull_over_lanes);
+  const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
+    pull_over_lane_objects, parameters_->th_moving_object_velocity);
+  const auto common_parameters = planner_data_->parameters;
+  const double base_link2front = common_parameters.base_link2front;
+  const double base_link2rear = common_parameters.base_link2rear;
+  const double vehicle_width = common_parameters.vehicle_width;
+
+  const auto ego_polygons_expanded =
+    utils::path_safety_checker::generatePolygonsWithStoppingAndInertialMargin(
+      path, base_link2front, base_link2rear, vehicle_width, parameters_->maximum_deceleration,
+      parameters_->object_recognition_collision_check_max_extra_stopping_margin);
+  debug_data_.ego_polygons_expanded = ego_polygons_expanded;
+
+  return utils::path_safety_checker::checkCollisionWithMargin(
+    ego_polygons_expanded, pull_over_lane_stop_objects,
+    parameters_->object_recognition_collision_check_margin);
 }
 
 bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const
@@ -1686,6 +1690,24 @@ void GoalPlannerModule::setDebugData()
       add(
         createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9));
     }
+
+    auto marker = tier4_autoware_utils::createDefaultMarker(
+      "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST,
+      tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0),
+      tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999));
+
+    for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) {
+      for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) {
+        const auto & current_point = ego_polygon.outer().at(ep_idx);
+        const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size());
+
+        marker.points.push_back(
+          tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), 0.0));
+        marker.points.push_back(
+          tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), 0.0));
+      }
+    }
+    debug_marker_.markers.push_back(marker);
   }
   // safety check
   if (parameters_->safety_check_params.enable_safety_check) {
@@ -1695,7 +1717,6 @@ void GoalPlannerModule::setDebugData()
       add(createPredictedPathMarkerArray(
         ego_predicted_path, vehicle_info_, "ego_predicted_path_goal_planner", 0, 0.0, 0.5, 0.9));
     }
-
     if (goal_planner_data_.filtered_objects.objects.size() > 0) {
       add(createObjectsMarkerArray(
         goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9));

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

@@ -364,27 +364,49 @@ std::vector<Polygon2d> getCollidedPolygons(
 
   return collided_polygons;
 }
-bool checkCollisionWithExtraStoppingMargin(
-  const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects,
-  const double base_to_front, const double base_to_rear, const double width,
-  const double maximum_deceleration, const double collision_check_margin,
-  const double max_extra_stopping_margin)
+
+std::vector<Polygon2d> generatePolygonsWithStoppingAndInertialMargin(
+  const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear,
+  const double width, const double maximum_deceleration, const double max_extra_stopping_margin)
 {
-  for (const auto & p : ego_path.points) {
+  std::vector<Polygon2d> polygons;
+  const auto curvatures = motion_utils::calcCurvature(ego_path.points);
+
+  for (size_t i = 0; i < ego_path.points.size(); ++i) {
+    const auto p = ego_path.points.at(i);
+
     const double extra_stopping_margin = std::min(
       std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration,
       max_extra_stopping_margin);
 
+    double extra_lateral_margin = (-1) * curvatures[i] * p.point.longitudinal_velocity_mps *
+                                  std::abs(p.point.longitudinal_velocity_mps);
+    extra_lateral_margin =
+      std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin);
+
+    const auto lateral_offset_pose =
+      tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0);
     const auto ego_polygon = tier4_autoware_utils::toFootprint(
-      p.point.pose, base_to_front + extra_stopping_margin, base_to_rear, width);
+      lateral_offset_pose, base_to_front + extra_stopping_margin, base_to_rear,
+      width + std::abs(extra_lateral_margin));
+    polygons.push_back(ego_polygon);
+  }
+  return polygons;
+}
 
+bool checkCollisionWithMargin(
+  const std::vector<Polygon2d> & ego_polygons, const PredictedObjects & dynamic_objects,
+  const double collision_check_margin)
+{
+  for (const auto & ego_polygon : ego_polygons) {
     for (const auto & object : dynamic_objects.objects) {
       const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);
       const double distance = boost::geometry::distance(obj_polygon, ego_polygon);
-      if (distance < collision_check_margin) return true;
+      if (distance < collision_check_margin) {
+        return true;
+      }
     }
   }
-
   return false;
 }
 }  // namespace behavior_path_planner::utils::path_safety_checker