feat(behavior_path_planner): revise lane change module (tier4#1139)

* feat(behavior_path_planner): revise lane change module

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

Refactoring and renaming

Remove some of the unused variables

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

Add longitudinal threshold and modify default param

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

fix error after rebase

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

* refactor and copy predicted if not empty

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

* Include abort parameters and reorganize parameters declaration

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

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

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -20,3 +20,10 @@
     path_interval: 2.0
 
     visualize_drivable_area_for_shared_linestrings_lanelet: true
+
+    lateral_distance_max_threshold: 5.0
+    longitudinal_distance_min_threshold: 3.0
+    expected_front_deceleration: -1.0
+    expected_rear_deceleration: -1.0
+    rear_vehicle_reaction_time: 2.0
+    rear_vehicle_safety_time_margin: 2.0

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -1,19 +1,19 @@
 /**:
   ros__parameters:
     lane_change:
-      min_stop_distance: 5.0
-      stop_time: 2.0
-      hysteresis_buffer_distance: 2.0
-      lane_change_prepare_duration: 4.0
-      lane_changing_duration: 8.0
-      lane_change_finish_judge_buffer: 3.0
-      minimum_lane_change_velocity: 5.6
-      prediction_duration: 8.0
-      prediction_time_resolution: 0.5
-      static_obstacle_velocity_thresh: 1.5
-      maximum_deceleration: 1.0
-      enable_abort_lane_change: false
-      enable_collision_check_at_prepare_phase: false
-      use_predicted_path_outside_lanelet: false
-      use_all_predicted_path: false
-      enable_blocked_by_obstacle: false
+      lane_change_prepare_duration: 4.0         # [s]
+      lane_changing_duration: 8.0               # [s]
+      minimum_lane_change_prepare_distance: 4.0 # [m]
+      lane_change_finish_judge_buffer: 3.0      # [m]
+      minimum_lane_change_velocity: 5.6         # [m/s]
+      prediction_time_resolution: 0.5           # [s]
+      maximum_deceleration: 1.0                 # [m/s2]
+      lane_change_sampling_num: 10
+      abort_lane_change_velocity_thresh: 0.5
+      abort_lane_change_angle_thresh: 10.0      # [deg]
+      abort_lane_change_distance_thresh: 0.3    # [m]
+      enable_abort_lane_change: true
+      enable_collision_check_at_prepare_phase: true
+      use_predicted_path_outside_lanelet: true
+      use_all_predicted_path: true
+      publish_debug_marker: true

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -56,6 +56,14 @@ struct BehaviorPathPlannerParameters
 
   // drivable area visualization
   bool visualize_drivable_area_for_shared_linestrings_lanelet;
+
+  // collision check
+  double lateral_distance_max_threshold;
+  double longitudinal_distance_min_threshold;
+  double expected_front_deceleration;
+  double expected_rear_deceleration;
+  double rear_vehicle_reaction_time;
+  double rear_vehicle_safety_time_margin;
 };
 
 #endif  // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_

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

@@ -34,16 +34,12 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId;
 
 struct LaneChangeParameters
 {
-  double min_stop_distance;
-  double stop_time;
-  double hysteresis_buffer_distance;
   double lane_change_prepare_duration;
   double lane_changing_duration;
+  double minimum_lane_change_prepare_distance;
   double lane_change_finish_judge_buffer;
   double minimum_lane_change_velocity;
-  double prediction_duration;
   double prediction_time_resolution;
-  double static_obstacle_velocity_thresh;
   double maximum_deceleration;
   int lane_change_sampling_num;
   double abort_lane_change_velocity_thresh;
@@ -53,7 +49,6 @@ struct LaneChangeParameters
   bool enable_collision_check_at_prepare_phase;
   bool use_predicted_path_outside_lanelet;
   bool use_all_predicted_path;
-  bool enable_blocked_by_obstacle;
 };
 
 struct LaneChangeStatus
@@ -167,7 +162,6 @@ class LaneChangeModule : public SceneModuleInterface
   void updateLaneChangeStatus();
 
   bool isSafe() const;
-  bool isLaneBlocked(const lanelet::ConstLanelets & lanes) const;
   bool isNearEndOfLane() const;
   bool isCurrentSpeedLow() const;
   bool isAbortConditionSatisfied() const;

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

@@ -31,25 +31,30 @@
 #include <memory>
 #include <vector>
 
-namespace behavior_path_planner
-{
-namespace lane_change_utils
+namespace behavior_path_planner::lane_change_utils
 {
+using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
+using tier4_autoware_utils::Polygon2d;
 
 PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2);
 bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets);
+double getExpectedVelocityWhenDecelerate(
+  const double & current_velocity, const double & expected_acceleration,
+  const double & lane_change_prepare_duration);
+double getDistanceWhenDecelerate(
+  const double & velocity, const double & expected_acceleration, const double & duration,
+  const double & minimum_distance);
 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,23 +64,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 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 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);
-bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose);
-}  // namespace lane_change_utils
-}  // namespace behavior_path_planner
+}  // 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

@@ -54,6 +54,8 @@ using autoware_auto_perception_msgs::msg::ObjectClassification;
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_perception_msgs::msg::PredictedPath;
+
+using autoware_auto_perception_msgs::msg::Shape;
 using autoware_auto_planning_msgs::msg::Path;
 using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
@@ -66,14 +68,31 @@ using geometry_msgs::msg::Vector3;
 using nav_msgs::msg::OccupancyGrid;
 using route_handler::RouteHandler;
 using tier4_autoware_utils::LineString2d;
+using tier4_autoware_utils::Point2d;
 using tier4_autoware_utils::Polygon2d;
+namespace bg = boost::geometry;
+using geometry_msgs::msg::Pose;
 
 struct FrenetCoordinate3d
 {
   double length{0.0};    // longitudinal
   double distance{0.0};  // lateral
 };
 
+struct ProjectedDistancePoint
+{
+  Point2d projected_point;
+  double distance{0.0};
+};
+
+template <typename Pythagoras = bg::strategy::distance::pythagoras<> >
+ProjectedDistancePoint pointToSegment(
+  const Point2d & reference_point, const Point2d & point_from_ego,
+  const Point2d & point_from_object);
+
+void getProjectedDistancePointFromPolygons(
+  const Polygon2d & ego_polygon, const Polygon2d & object_polygon, Pose & point_on_ego,
+  Pose & point_on_object);
 // data conversions
 
 Path convertToPathFromPathWithLaneId(const PathWithLaneId & path_with_lane_id);
@@ -86,7 +105,8 @@ PoseArray convertToGeometryPoseArray(const PathWithLaneId & path);
 
 PredictedPath convertToPredictedPath(
   const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & vehicle_pose,
-  const double duration, const double resolution, const double acceleration);
+  const double duration, const double resolution, const double acceleration,
+  double min_speed = 1.0);
 
 FrenetCoordinate3d convertToFrenetCoordinate3d(
   const std::vector<Point> & linestring, const Point & search_point_geom);
@@ -285,7 +305,67 @@ lanelet::ConstLanelets getExtendedCurrentLanes(
 lanelet::ConstLanelets calcLaneAroundPose(
   const std::shared_ptr<RouteHandler> route_handler, const geometry_msgs::msg::Pose & pose,
   const double forward_length, const double backward_length);
+Polygon2d convertBoundingBoxObjectToGeometryPolygon(
+  const Pose & current_pose, const double & length, const double & width);
+
+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 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 & rear_vehicle_safety_time_margin);
+
+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 behavior_path_planner::util
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -204,6 +204,13 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.visualize_drivable_area_for_shared_linestrings_lanelet =
     declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true);
 
+  p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold", 3.0);
+  p.longitudinal_distance_min_threshold =
+    declare_parameter("longitudinal_distance_min_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", 2.0);
+  p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin", 2.0);
   return p;
 }
 
@@ -307,27 +314,22 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   };
 
   LaneChangeParameters p{};
-  p.min_stop_distance = dp("min_stop_distance", 5.0);
-  p.stop_time = dp("stop_time", 2.0);
-  p.hysteresis_buffer_distance = dp("hysteresis_buffer_distance", 2.0);
   p.lane_change_prepare_duration = dp("lane_change_prepare_duration", 2.0);
   p.lane_changing_duration = dp("lane_changing_duration", 4.0);
+  p.minimum_lane_change_prepare_distance = dp("minimum_lane_change_prepare_distance", 4.0);
   p.lane_change_finish_judge_buffer = dp("lane_change_finish_judge_buffer", 3.0);
   p.minimum_lane_change_velocity = dp("minimum_lane_change_velocity", 8.3);
-  p.prediction_duration = dp("prediction_duration", 8.0);
   p.prediction_time_resolution = dp("prediction_time_resolution", 0.5);
-  p.static_obstacle_velocity_thresh = dp("static_obstacle_velocity_thresh", 0.1);
   p.maximum_deceleration = dp("maximum_deceleration", 1.0);
   p.lane_change_sampling_num = dp("lane_change_sampling_num", 10);
-  p.enable_abort_lane_change = dp("enable_abort_lane_change", true);
-  p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true);
-  p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true);
-  p.use_all_predicted_path = dp("use_all_predicted_path", false);
   p.abort_lane_change_velocity_thresh = dp("abort_lane_change_velocity_thresh", 0.5);
   p.abort_lane_change_angle_thresh =
     dp("abort_lane_change_angle_thresh", tier4_autoware_utils::deg2rad(10.0));
   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.enable_abort_lane_change = dp("enable_abort_lane_change", true);
+  p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true);
+  p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true);
+  p.use_all_predicted_path = dp("use_all_predicted_path", false);
 
   // validation of parameters
   if (p.lane_change_sampling_num < 1) {