fix(goal_planner): change debug polygons to contain the nominal margin (

#5365)

* fixed so that the visualize polygons contain the nominal margin, with some refactoring

---------

Signed-off-by: Yuki Takagi <[email protected]>
Co-authored-by: Kyoichi Sugahara <[email protected]>

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

@@ -107,10 +107,6 @@ 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,13 +129,9 @@ std::vector<Polygon2d> getCollidedPolygons(
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
   const double hysteresis_factor, CollisionCheckDebug & debug);
 
-/**
- * @brief Check collision between ego polygons with margin and objects.
- * @return Has collision or not
- */
-bool checkCollisionWithMargin(
-  const std::vector<Polygon2d> & ego_polygons, const PredictedObjects & dynamic_objects,
-  const double collision_check_margin);
+bool checkPolygonsIntersects(
+  const std::vector<Polygon2d> & polys_1, const std::vector<Polygon2d> & polys_2);
+
 }  // 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

@@ -22,6 +22,7 @@
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
 #include "tier4_autoware_utils/math/unit_conversion.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
@@ -1314,7 +1315,6 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const
       return true;
     }
   }
-
   if (!parameters_->use_object_recognition) {
     return false;
   }
@@ -1328,20 +1328,41 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const
       utils::path_safety_checker::isPolygonOverlapLanelet);
   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,
+  std::vector<Polygon2d> obj_polygons;
+  for (const auto & object : pull_over_lane_stop_objects.objects) {
+    obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object));
+  }
+
+  std::vector<Polygon2d> ego_polygons_expanded;
+  const auto curvatures = motion_utils::calcCurvature(path.points);
+  for (size_t i = 0; i < path.points.size(); ++i) {
+    const auto p = path.points.at(i);
+
+    const double extra_stopping_margin = std::min(
+      std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters_->maximum_deceleration,
       parameters_->object_recognition_collision_check_max_extra_stopping_margin);
+
+    double extra_lateral_margin = (-1) * curvatures.at(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(
+      lateral_offset_pose,
+      planner_data_->parameters.base_link2front +
+        parameters_->object_recognition_collision_check_margin + extra_stopping_margin,
+      planner_data_->parameters.base_link2rear +
+        parameters_->object_recognition_collision_check_margin,
+      planner_data_->parameters.vehicle_width +
+        parameters_->object_recognition_collision_check_margin * 2.0 +
+        std::abs(extra_lateral_margin));
+    ego_polygons_expanded.push_back(ego_polygon);
+  }
   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);
+  return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons);
 }
 
 bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const

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

@@ -18,7 +18,7 @@
 #include "motion_utils/trajectory/trajectory.hpp"
 #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
 
-#include <boost/geometry/algorithms/distance.hpp>
+#include <boost/geometry/algorithms/intersects.hpp>
 #include <boost/geometry/algorithms/overlaps.hpp>
 #include <boost/geometry/strategies/strategies.hpp>
 
@@ -380,44 +380,12 @@ std::vector<Polygon2d> getCollidedPolygons(
   return collided_polygons;
 }
 
-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)
+bool checkPolygonsIntersects(
+  const std::vector<Polygon2d> & polys_1, const std::vector<Polygon2d> & polys_2)
 {
-  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(
-      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) {
+  for (const auto & poly_1 : polys_1) {
+    for (const auto & poly_2 : polys_2) {
+      if (boost::geometry::intersects(poly_1, poly_2)) {
         return true;
       }
     }