move collision check to utilities.

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -18,3 +18,9 @@
     intersection_search_distance: 30.0
 
     visualize_drivable_area_for_shared_linestrings_lanelet: true
+
+    lateral_distance_max_threshold: 5.0
+    expected_front_deceleration: -1.0
+    expected_rear_deceleration: -1.0
+    rear_vehicle_reaction_time: 1.0
+    safety_time_margin_for_control: 2.0

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -51,6 +51,13 @@ struct BehaviorPathPlannerParameters
 
   // drivable area visualization
   bool visualize_drivable_area_for_shared_linestrings_lanelet;
+
+  // collision check
+  double lateral_distance_max_threshold;
+  double expected_front_deceleration;
+  double expected_rear_deceleration;
+  double rear_vehicle_reaction_time;
+  double safety_time_margin_for_control;
 };
 
 #endif  // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp

@@ -61,11 +61,6 @@ struct LaneChangeParameters
   bool use_all_predicted_path;
   bool enable_blocked_by_obstacle;
   double lane_change_search_distance;
-  double safety_time_margin_for_control;
-  double rear_vehicle_reaction_time;
-  double lateral_distance_threshold;
-  double expected_front_deceleration;
-  double expected_rear_deceleration;
 };
 
 struct LaneChangeStatus

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp

@@ -48,8 +48,7 @@ bool isPathInLanelets(
 std::vector<LaneChangePath> getLaneChangePaths(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist,
-  const BehaviorPathPlannerParameters & common_parameter,
-  const behavior_path_planner::LaneChangeParameters & parameter);
+  const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter);
 std::vector<LaneChangePath> selectValidPaths(
   const std::vector<LaneChangePath> & paths, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
@@ -59,55 +58,20 @@ bool selectSafePath(
   const std::vector<LaneChangePath> & paths, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
-  const Twist & current_twist, const double & vehicle_width, const double & vehicle_length,
-  const behavior_path_planner::LaneChangeParameters & ros_parameters,
-  LaneChangePath * selected_path);
+  const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters,
+  const LaneChangeParameters & ros_parameters, LaneChangePath * selected_path);
 bool isLaneChangePathSafe(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
-  const Twist & current_twist, const double & vehicle_width, const double & vehicle_length,
-  const behavior_path_planner::LaneChangeParameters & ros_parameters, const bool use_buffer = true,
+  const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters,
+  const LaneChangeParameters & lane_change_parameters, const bool use_buffer = true,
   const double acceleration = 0.0);
 bool hasEnoughDistance(
   const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes, const Pose & current_pose,
   const bool isInGoalRouteSection, const Pose & goal_pose,
   const lanelet::routing::RoutingGraphContainer & overall_graphs);
-
-std::vector<PredictedPath> getPredictedPathFromObj(
-  const PredictedObject & obj, const bool & is_use_all_predicted_path);
-
-Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target_object);
-
-bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose);
-
-bool isObjectFront(const Pose & projected_ego_pose);
-
-double stoppingDistance(const double & vehicle_velocity, const double & vehicle_accel);
-
-double stoppingDistance(
-  const double & rear_vehicle_velocity, const double & rear_vehicle_accel,
-  const double & rear_vehicle_reaction_time);
-
-double frontVehicleStopDistance(
-  const double & front_vehicle_velocity, const double & front_vehicle_accel,
-  const double & distance_to_collision);
-
-double rearVehicleStopDistance(
-  const double & rear_vehicle_velocity, const double & rear_vehicle_accel,
-  const double & rear_vehicle_reaction_time, const double & safety_time_margin_for_control);
-
-bool isLongitudinalDistanceEnough(
-  const double & rear_vehicle_stop_threshold, const double & front_vehicle_stop_threshold);
-
-bool hasEnoughDistance(
-  const Pose & expected_ego_pose, const Twist & ego_current_twist,
-  const Pose & expected_object_pose, const Twist & object_current_twist,
-  const LaneChangeParameters & param);
-
-bool isLateralDistanceEnough(
-  const double & relative_lateral_distance, const double & lateral_distance_threshold);
 }  // namespace behavior_path_planner::lane_change_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -311,6 +311,64 @@ Polygon2d convertCylindricalObjectToGeometryPolygon(
   const Pose & current_pose, const Shape & obj_shape);
 
 Polygon2d convertPolygonObjectToGeometryPolygon(const Pose & current_pose, const Shape & obj_shape);
+
+std::vector<PredictedPath> getPredictedPathFromObj(
+  const PredictedObject & obj, const bool & is_use_all_predicted_path);
+
+Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target_object);
+
+bool getEgoExpectedPoseAndConvertToPolygon(
+  const Pose & current_pose, const PredictedPath & pred_path, Pose & expected_pose,
+  tier4_autoware_utils::Polygon2d & ego_polygon, const double & check_current_time,
+  const double & length, const double & width);
+
+bool getObjectExpectedPoseAndConvertToPolygon(
+  const PredictedPath & pred_path, const PredictedObject & object, Pose & expected_pose,
+  Polygon2d & obj_polygon, const double & check_current_time);
+
+bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose);
+
+bool isObjectFront(const Pose & projected_ego_pose);
+
+double stoppingDistance(const double & vehicle_velocity, const double & vehicle_accel);
+
+double stoppingDistance(
+  const double & rear_vehicle_velocity, const double & rear_vehicle_accel,
+  const double & rear_vehicle_reaction_time);
+
+double frontVehicleStopDistance(
+  const double & front_vehicle_velocity, const double & front_vehicle_accel,
+  const double & distance_to_collision);
+
+double rearVehicleStopDistance(
+  const double & rear_vehicle_velocity, const double & rear_vehicle_accel,
+  const double & rear_vehicle_reaction_time, const double & safety_time_margin_for_control);
+
+bool isLongitudinalDistanceEnough(
+  const double & rear_vehicle_stop_threshold, const double & front_vehicle_stop_threshold);
+
+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);
+
+bool isLateralDistanceEnough(
+  const double & relative_lateral_distance, const double & lateral_distance_threshold);
+
+bool isSafeInLaneletCollisionCheck(
+  const Pose & ego_current_pose, const Twist & ego_current_twist,
+  const PredictedPath & ego_predicted_path, const double & ego_vehicle_length,
+  const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time,
+  const double & check_time_resolution, const PredictedObject & target_object,
+  const PredictedPath & target_object_path,
+  const BehaviorPathPlannerParameters & common_parameters);
+
+bool isSafeInFreeSpaceCollisionCheck(
+  const Pose & ego_current_pose, const Twist & ego_current_twist,
+  const PredictedPath & ego_predicted_path, const double & ego_vehicle_length,
+  const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time,
+  const double & check_time_resolution, const PredictedObject & target_object,
+  const BehaviorPathPlannerParameters & common_parameters);
 }  // namespace util
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -209,6 +209,11 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.base_link2front = vehicle_info.max_longitudinal_offset_m;
   p.base_link2rear = p.rear_overhang;
 
+  p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold", 3.0);
+  p.expected_front_deceleration = declare_parameter("expected_front_deceleration", -1.0);
+  p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration", -1.0);
+  p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time", 1.0);
+  p.safety_time_margin_for_control = declare_parameter("safety_time_margin_for_control", 2.0);
   return p;
 }
 
@@ -335,11 +340,6 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.abort_lane_change_distance_thresh = dp("abort_lane_change_distance_thresh", 0.3);
   p.enable_blocked_by_obstacle = dp("enable_blocked_by_obstacle", false);
   p.lane_change_search_distance = dp("lane_change_search_distance", 30.0);
-  p.safety_time_margin_for_control = dp("safety_time_margin_for_control", 2.0);
-  p.rear_vehicle_reaction_time = dp("rear_vehicle_reaction_time", 1.0);
-  p.lateral_distance_threshold = dp("lateral_distance_threshold", 5.0);
-  p.expected_front_deceleration = dp("expected_front_deceleration", -1.0);
-  p.expected_rear_deceleration = dp("expected_rear_deceleration", -1.0);
 
   // validation of parameters
   if (p.lane_change_sampling_num < 1) {

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -394,8 +394,7 @@ std::pair<bool, bool> LaneChangeModule::getSafePath(
     // select safe path
     bool found_safe_path = lane_change_utils::selectSafePath(
       valid_paths, current_lanes, check_lanes, planner_data_->dynamic_object, current_pose,
-      current_twist, common_parameters.vehicle_width, common_parameters.vehicle_length, parameters_,
-      &safe_path);
+      current_twist, common_parameters, parameters_, &safe_path);
     return std::make_pair(true, found_safe_path);
   }
 
@@ -463,8 +462,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() const
 
     is_path_safe = lane_change_utils::isLaneChangePathSafe(
       path.path, current_lanes, check_lanes, objects, current_pose, current_twist,
-      common_parameters.vehicle_width, common_parameters.vehicle_length, parameters_, false,
-      status_.lane_change_path.acceleration);
+      common_parameters, parameters_, false, status_.lane_change_path.acceleration);
   }
 
   // check vehicle velocity thresh