feat(goal_planner): add extra front margin for collision check consid…

…ering stopping distance Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Aug 20, 2023 · 84fbda8 · 84fbda8
1 parent 9dc104f
commit 84fbda8
Show file tree

Hide file tree

Showing 7 changed files with 55 additions and 9 deletions.
diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml
@@ -31,7 +31,8 @@
       # object recognition
       object_recognition:
         use_object_recognition: true
-        object_recognition_collision_check_margin: 1.0
+        object_recognition_collision_check_margin: 0.5
+        object_recognition_collision_check_extra_max_stopping_margin: 1.0
 
       # pull over
       pull_over:

diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md
@@ -150,10 +150,11 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio
 
 #### Parameters for object recognition based collision check
 
-| Name                                      | Unit | Type   | Description                                                | Default value |
-| :---------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ |
-| use_object_recognition                    | [-]  | bool   | flag whether to use object recognition for collision check | true          |
-| object_recognition_collision_check_margin | [m]  | double | margin to calculate ego-vehicle cells from footprint.      | 1.0           |
+| Name                                                         | Unit | Type   | Description                                                | Default value |
+| :----------------------------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | ---------------------------------------------------------------------------------------------------------- |
+| use_object_recognition                                       | [-]  | bool   | flag whether to use object recognition for collision check | true          |
+| object_recognition_collision_check_margin                    | [m]  | double | margin to calculate ego-vehicle cells from footprint.      | 0.5           |
+| object_recognition_collision_check_extra_max_stopping_margin | [m]  | double |                                                            | 1.0           | 　maximum value when adding longitudinal distance margin for collision check considering stopping distance |
 
 ## **Goal Search**
 

diff --git a/...path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/...path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp
@@ -68,6 +68,7 @@ struct GoalPlannerParameters
   // object recognition
   bool use_object_recognition;
   double object_recognition_collision_check_margin;
+  double object_recognition_collision_check_extra_max_stopping_margin;
 
   // pull over general params
   double pull_over_velocity;

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
@@ -164,6 +164,16 @@ double getDistanceBetweenPredictedPathAndObject(
   const PredictedObject & object, const PredictedPath & path, const double start_time,
   const double end_time, const double resolution);
 
+/**
+ * @brief Check collision between ego path footprints with extra longitudinal stopping 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);
+
 /**
  * @brief Check collision between ego path footprints and objects.
  * @return Has collision or not

diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -553,7 +553,9 @@ void GoalPlannerModule::selectSafePullOverPath()
     }
 
     // check if path is valid and safe
-    if (!hasEnoughDistance(pull_over_path) || checkCollision(pull_over_path.getParkingPath())) {
+    if (
+      !hasEnoughDistance(pull_over_path) ||
+      checkCollision(utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5))) {
       continue;
     }
 
@@ -1178,9 +1180,14 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const
   }
 
   if (parameters_->use_object_recognition) {
-    if (utils::checkCollisionBetweenPathFootprintsAndObjects(
-          vehicle_footprint_, path, *(planner_data_->dynamic_object),
-          parameters_->object_recognition_collision_check_margin)) {
+    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::checkCollisionWithExtraStoppingMargin(
+          path, *(planner_data_->dynamic_object), base_link2front, base_link2rear, vehicle_width,
+          parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin,
+          parameters_->object_recognition_collision_check_extra_max_stopping_margin)) {
       return true;
     }
   }

diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp
@@ -88,6 +88,9 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
     p.use_object_recognition = node->declare_parameter<bool>(ns + "use_object_recognition");
     p.object_recognition_collision_check_margin =
       node->declare_parameter<double>(ns + "object_recognition_collision_check_margin");
+    p.object_recognition_collision_check_extra_max_stopping_margin =
+      node->declare_parameter<double>(
+        ns + "object_recognition_collision_check_extra_max_stopping_margin");
   }
 
   // pull over general params

diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -621,6 +621,29 @@ bool checkCollisionBetweenPathFootprintsAndObjects(
   return false;
 }
 
+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_extra_stopping_margin)
+{
+  for (const auto & p : ego_path.points) {
+    const double extra_stopping_margin = std::min(
+      std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration,
+      max_extra_stopping_margin);
+
+    const auto ego_polygon = tier4_autoware_utils::toFootprint(
+      p.point.pose, base_to_front + extra_stopping_margin, base_to_rear, width);
+
+    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 < margin) return true;
+    }
+  }
+
+  return false;
+}
+
 bool checkCollisionBetweenFootprintAndObjects(
   const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose,
   const PredictedObjects & dynamic_objects, const double margin)