feat(behavior_path_planner): renew lane change collision check functi…

…ons (#3609) * feat(behavior_path_planner): renew lane change collision check Signed-off-by: yutaka <[email protected]> * update Signed-off-by: yutaka <[email protected]> --------- Signed-off-by: yutaka <[email protected]>
autowarefoundation · May 8, 2023 · 52127f0 · 52127f0
1 parent ccdcb5c
commit 52127f0
Show file tree

Hide file tree

Showing 4 changed files with 198 additions and 200 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp
@@ -42,6 +42,7 @@ namespace behavior_path_planner::utils::safety_check
 
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedPath;
+using autoware_auto_perception_msgs::msg::Shape;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 using marker_utils::CollisionCheckDebug;
@@ -59,32 +60,6 @@ bool isTargetObjectFront(
   const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose,
   const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon);
 
-/**
- * @brief Project nearest point on a line segment.
- * @param [in] reference_point point to project
- * @param [in] line segment
- * @return nearest point on the line segment
- */
-template <typename Pythagoras = bg::strategy::distance::pythagoras<>>
-ProjectedDistancePoint pointToSegment(
-  const Point2d & reference_point, const Point2d & polygon_segment_start,
-  const Point2d & polygon_segment_end);
-
-/**
- * @brief Find nearest points between two polygon.
- */
-void getProjectedDistancePointFromPolygons(
-  const Polygon2d & ego_polygon, const Polygon2d & object_polygon, Pose & point_on_ego,
-  Pose & point_on_object);
-
-/**
- * @brief get relative pose with reference to the target object.
- * @param [in] absolute pose desired_pose reference pose
- * @param [in] absolute pose target_pose target pose to check
- * @return relative pose of the target
- */
-Pose projectCurrentPoseToTarget(const Pose & reference_pose, const Pose & target_pose);
-
 /**
  * @brief find which vehicle is front and rear and check for lateral,
  *        longitudinal physical and longitudinal expected stopping distance between two points
@@ -99,17 +74,30 @@ Pose projectCurrentPoseToTarget(const Pose & reference_pose, const Pose & target
  * @return true if distance is safe.
  */
 bool hasEnoughDistance(
-  const Pose & expected_ego_pose, const Twist & ego_current_twist,
-  const Pose & expected_object_pose, const Twist & object_current_twist,
-  const BehaviorPathPlannerParameters & param, const double front_decel, const double rear_decel,
-  CollisionCheckDebug & debug);
+  const Polygon2d & front_object_polygon, const double front_object_velocity,
+  const Polygon2d & rear_object_polygon, const double rear_object_velocity,
+  const bool is_object_front, const BehaviorPathPlannerParameters & param,
+  const double front_object_deceleration, const double rear_object_deceleration);
+
+void getTransformedPolygon(
+  const Pose & front_object_pose, const Pose & rear_object_pose,
+  const vehicle_info_util::VehicleInfo & ego_vehicle_info, const Shape & object_shape,
+  const bool is_object_front, Polygon2d & transformed_front_object_polygon,
+  Polygon2d & transformed_rear_object_polygon);
+
+double calcLateralDistance(
+  const Polygon2d & front_object_polygon, const Polygon2d & rear_object_polygon);
+
+double calcLongitudinalDistance(
+  const Polygon2d & front_object_polygon, const Polygon2d & rear_object_polygon);
 
 /**
  * @brief Iterate the points in the ego and target's predicted path and
  *        perform safety check for each of the iterated points.
  * @return true if distance is safe.
  */
 bool isSafeInLaneletCollisionCheck(
+  const PathWithLaneId & path,
   const std::vector<std::pair<Pose, tier4_autoware_utils::Polygon2d>> & interpolated_ego,
   const Twist & ego_current_twist, const std::vector<double> & check_duration,
   const double prepare_duration, const PredictedObject & target_object,
@@ -123,12 +111,13 @@ bool isSafeInLaneletCollisionCheck(
  * @return true if distance is safe.
  */
 bool isSafeInFreeSpaceCollisionCheck(
+  const PathWithLaneId & path,
   const std::vector<std::pair<Pose, tier4_autoware_utils::Polygon2d>> & interpolated_ego,
   const Twist & ego_current_twist, const std::vector<double> & check_duration,
   const double prepare_duration, const PredictedObject & target_object,
   const BehaviorPathPlannerParameters & common_parameters,
-  const double prepare_phase_ignore_target_speed_thresh, const double front_decel,
-  const double rear_decel, CollisionCheckDebug & debug);
+  const double prepare_phase_ignore_target_velocity_thresh, const double front_object_deceleration,
+  const double rear_object_deceleration, CollisionCheckDebug & debug);
 
 }  // namespace behavior_path_planner::utils::safety_check
 

diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp
@@ -326,8 +326,8 @@ bool isLaneChangePathSafe(
       utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path);
     for (const auto & obj_path : obj_predicted_paths) {
       if (!utils::safety_check::isSafeInLaneletCollisionCheck(
-            interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare,
-            obj, obj_path, common_parameter,
+            path, interpolated_ego, current_twist, check_durations,
+            lane_change_path.duration.prepare, obj, obj_path, common_parameter,
             lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, front_decel,
             rear_decel, ego_pose_before_collision, current_debug_data.second)) {
         appendDebugInfo(current_debug_data, false);
@@ -350,9 +350,10 @@ bool isLaneChangePathSafe(
       utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path);
 
     if (!utils::safety_check::isSafeInFreeSpaceCollisionCheck(
-          interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, obj,
-          common_parameter, lane_change_parameter.prepare_segment_ignore_object_velocity_thresh,
-          front_decel, rear_decel, current_debug_data.second)) {
+          path, interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare,
+          obj, common_parameter,
+          lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, front_decel,
+          rear_decel, current_debug_data.second)) {
       appendDebugInfo(current_debug_data, false);
       return false;
     }