feat(behavior_path_planner): check goal to objects logitudinal distan…

…ce for pull_over (autowarefoundation#1796)

* feat(behavior_path_planner): check goal to objects logitudinal distance for pull_over

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Update planning/behavior_path_planner/src/utilities.cpp

Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com>
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* rename to goal_to_obstacle_margin

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix rear check

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com>

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml

@@ -16,7 +16,7 @@
       forward_goal_search_length: 20.0
       backward_goal_search_length: 20.0
       goal_search_interval: 1.0
-      goal_to_obj_margin: 2.0
+      goal_to_obstacle_margin: 2.0
       # occupancy grid map
       use_occupancy_grid: true
       occupancy_grid_collision_check_margin: 0.0

planning/behavior_path_planner/README.md

@@ -268,7 +268,7 @@ searched for in certain range of the shoulder lane.
 | forward_goal_search_length  | [m]  | double | length of forward range to be explored from the original goal                                                                                                                                                            | 20.0           |
 | backward_goal_search_length | [m]  | double | length of backward range to be explored from the original goal                                                                                                                                                           | 20.0           |
 | goal_search_interval        | [m]  | double | distance interval for goal search                                                                                                                                                                                        | 1.0            |
-| goal_to_obj_margin          | [m]  | double | margin between ego-vehicle and obstacles when arriving the goal                                                                                                                                                          | 2.0            |
+| goal_to_obstacle_margin     | [m]  | double | margin between ego-vehicle at the goal position and obstacles                                                                                                                                                            | 2.0            |
 
 #### **Path Generation**
 

planning/behavior_path_planner/config/pull_over/pull_over.param.yaml

@@ -16,7 +16,7 @@
       forward_goal_search_length: 20.0
       backward_goal_search_length: 20.0
       goal_search_interval: 1.0
-      goal_to_obj_margin: 2.0
+      goal_to_obstacle_margin: 2.0
       # occupancy grid map
       use_occupancy_grid: true
       occupancy_grid_collision_check_margin: 0.0

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp

@@ -55,7 +55,7 @@ struct PullOverParameters
   double forward_goal_search_length;
   double backward_goal_search_length;
   double goal_search_interval;
-  double goal_to_obj_margin;
+  double goal_to_obstacle_margin;
   // occupancy grid map
   bool use_occupancy_grid;
   double occupancy_grid_collision_check_margin;
@@ -208,6 +208,8 @@ class PullOverModule : public SceneModuleInterface
   void updateOccupancyGrid();
   void researchGoal();
   void resetStatus();
+  bool checkCollisionWtihLongitudinalDistance(
+    const Pose & ego_pose, const PredictedObjects & dynamic_objects) const;
   bool checkCollisionWithPose(const Pose & pose) const;
   bool checkCollisionWithPath(const PathWithLaneId & path) const;
 

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -174,6 +174,22 @@ bool checkCollisionBetweenFootprintAndObjects(
   const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const Pose & ego_pose,
   const PredictedObjects & dynamic_objects, const double margin);
 
+/**
+ * @brief calculate longitudinal distance from ego pose to object
+ * @return distance from ego pose to object
+ */
+double calcLongitudinalDistanceFromEgoToObject(
+  const Pose & ego_pose, double base_link2front, double base_link2rear,
+  const PredictedObject & dynamic_object);
+
+/**
+ * @brief calculate minimum longitudinal distance from ego pose to objects
+ * @return minimum distance from ego pose to objects
+ */
+double calcLongitudinalDistanceFromEgoToObjects(
+  const Pose & ego_pose, double base_link2front, double base_link2rear,
+  const PredictedObjects & dynamic_objects);
+
 /**
  * @brief Get index of the obstacles inside the lanelets with start and end length
  * @return Indices corresponding to the obstacle inside the lanelets

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -374,7 +374,7 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam()
   p.forward_goal_search_length = dp("forward_goal_search_length", 20.0);
   p.backward_goal_search_length = dp("backward_goal_search_length", 20.0);
   p.goal_search_interval = dp("goal_search_interval", 5.0);
-  p.goal_to_obj_margin = dp("goal_to_obj_margin", 2.0);
+  p.goal_to_obstacle_margin = dp("goal_to_obstacle_margin", 2.0);
   // occupancy grid map
   p.use_occupancy_grid = dp("use_occupancy_grid", true);
   p.occupancy_grid_collision_check_margin = dp("occupancy_grid_collision_check_margin", 0.0);

planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp

@@ -260,6 +260,12 @@ void PullOverModule::researchGoal()
       continue;
     }
 
+    const auto objects_in_shoulder_lane =
+      util::filterObjectsByLanelets(*(planner_data_->dynamic_object), status_.pull_over_lanes);
+    if (checkCollisionWtihLongitudinalDistance(search_pose, objects_in_shoulder_lane)) {
+      continue;
+    }
+
     GoalCandidate goal_candidate;
     goal_candidate.goal_pose = search_pose;
     goal_candidate.distance_from_original_goal =
@@ -307,6 +313,49 @@ bool PullOverModule::isLongEnoughToParkingStart(
   return *dist_to_parking_start_pose > current_to_stop_distance;
 }
 
+bool PullOverModule::checkCollisionWtihLongitudinalDistance(
+  const Pose & ego_pose, const PredictedObjects & dynamic_objects) const
+{
+  if (parameters_.use_occupancy_grid) {
+    bool check_out_of_range = false;
+    const double offset = std::max(
+      parameters_.goal_to_obstacle_margin - parameters_.occupancy_grid_collision_check_margin, 0.0);
+
+    // check forward collison
+    const Pose ego_pose_moved_forward = calcOffsetPose(ego_pose, offset, 0, 0);
+    const Pose forward_pose_grid_coords =
+      global2local(occupancy_grid_map_.getMap(), ego_pose_moved_forward);
+    const auto forward_idx = pose2index(
+      occupancy_grid_map_.getMap(), forward_pose_grid_coords,
+      occupancy_grid_map_.getParam().theta_size);
+    if (occupancy_grid_map_.detectCollision(forward_idx, check_out_of_range)) {
+      return true;
+    }
+
+    // check backward collison
+    const Pose ego_pose_moved_backward = calcOffsetPose(ego_pose, -offset, 0, 0);
+    const Pose backward_pose_grid_coords =
+      global2local(occupancy_grid_map_.getMap(), ego_pose_moved_backward);
+    const auto backward_idx = pose2index(
+      occupancy_grid_map_.getMap(), backward_pose_grid_coords,
+      occupancy_grid_map_.getParam().theta_size);
+    if (occupancy_grid_map_.detectCollision(backward_idx, check_out_of_range)) {
+      return true;
+    }
+  }
+
+  if (parameters_.use_object_recognition) {
+    if (
+      util::calcLongitudinalDistanceFromEgoToObjects(
+        ego_pose, planner_data_->parameters.base_link2front,
+        planner_data_->parameters.base_link2rear,
+        dynamic_objects) < parameters_.goal_to_obstacle_margin) {
+      return true;
+    }
+  }
+  return false;
+}
+
 bool PullOverModule::checkCollisionWithPose(const Pose & pose) const
 {
   if (parameters_.use_occupancy_grid) {

planning/behavior_path_planner/src/utilities.cpp

@@ -575,6 +575,54 @@ bool checkCollisionBetweenFootprintAndObjects(
   return false;
 }
 
+double calcLongitudinalDistanceFromEgoToObject(
+  const Pose & ego_pose, double base_link2front, double base_link2rear,
+  const PredictedObject & dynamic_object)
+{
+  double min_distance = std::numeric_limits<double>::max();
+  Polygon2d obj_polygon;
+  if (!calcObjectPolygon(dynamic_object, &obj_polygon)) {
+    return min_distance;
+  }
+
+  const auto vehicle_front_pose =
+    tier4_autoware_utils::calcOffsetPose(ego_pose, base_link2front, 0, 0);
+  const auto vehicle_rear_pose =
+    tier4_autoware_utils::calcOffsetPose(ego_pose, base_link2rear, 0, 0);
+
+  for (const auto & p : obj_polygon.outer()) {
+    const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0);
+
+    const double signed_distance_from_front =
+      tier4_autoware_utils::calcLongitudinalDeviation(vehicle_front_pose, point);
+    const double signed_distance_from_rear =
+      -tier4_autoware_utils::calcLongitudinalDeviation(vehicle_rear_pose, point);
+
+    if (signed_distance_from_front < 0.0 && signed_distance_from_rear < 0.0) {
+      // point is between front and rear
+      return 0.0;
+    }
+
+    const double distance_from_ego =
+      std::min(std::abs(signed_distance_from_front), std::abs(signed_distance_from_rear));
+    min_distance = std::min(min_distance, distance_from_ego);
+  }
+  return min_distance;
+}
+
+double calcLongitudinalDistanceFromEgoToObjects(
+  const Pose & ego_pose, double base_link2front, double base_link2rear,
+  const PredictedObjects & dynamic_objects)
+{
+  double min_distance = std::numeric_limits<double>::max();
+  for (const auto & object : dynamic_objects.objects) {
+    min_distance = std::min(
+      min_distance,
+      calcLongitudinalDistanceFromEgoToObject(ego_pose, base_link2front, base_link2rear, object));
+  }
+  return min_distance;
+}
+
 // only works with consecutive lanes
 std::vector<size_t> filterObjectIndicesByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets,