feat(behavior_path_planner): revise lane change module

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

planning/behavior_path_planner/CMakeLists.txt

@@ -49,6 +49,13 @@ if(BUILD_TESTING)
   target_link_libraries(test_${PROJECT_NAME}_utilities
     behavior_path_planner_node
   )
+
+  ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_lane_change_utils
+    test/test_lane_change_utils.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}_lane_change_utils
+    behavior_path_planner_node
+  )
 endif()
 
 ament_auto_package(

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

@@ -12,9 +12,15 @@
       prediction_time_resolution: 0.5
       static_obstacle_velocity_thresh: 1.5
       maximum_deceleration: 1.0
+      lane_change_sampling_num: 10
       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_search_distance: 30.0
+      safety_time_margin_for_control: 2.0
+      rear_vehicle_reaction_time: 1.0
+      lateral_distance_threshold: 5.0
+      expected_front_deceleration: -1.0
+      expected_rear_deceleration: -1.0

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

@@ -61,6 +61,11 @@ 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
@@ -172,7 +177,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,10 +31,9 @@
 #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;
@@ -59,23 +58,51 @@ 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 Twist & current_twist, const double & vehicle_width, const double & vehicle_length,
   const behavior_path_planner::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 Twist & current_twist, const double & vehicle_width, const double & vehicle_length,
   const behavior_path_planner::LaneChangeParameters & ros_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);
-}  // namespace lane_change_utils
-}  // namespace behavior_path_planner
+
+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 isUnderThresholdDistanceSafe(
+  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);
+
+}  // 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

@@ -125,6 +125,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;
@@ -137,7 +139,6 @@ 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;
 
 struct FrenetCoordinate3d
@@ -346,6 +347,13 @@ bool checkLaneIsInIntersection(
 // object label
 std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification);
 
+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);
 }  // namespace util
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -334,6 +334,11 @@ 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

@@ -115,7 +115,7 @@ bool LaneChangeModule::isExecutionReady() const
   std::tie(found_valid_path, found_safe_path) =
     getSafePath(lane_change_lanes, check_distance_, selected_path);
 
-  return found_safe_path && !isLaneBlocked(lane_change_lanes);
+  return found_safe_path;
 }
 
 BT::NodeStatus LaneChangeModule::updateState()
@@ -385,7 +385,8 @@ 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, parameters_, &safe_path);
+      current_twist, common_parameters.vehicle_width, common_parameters.vehicle_length, parameters_,
+      &safe_path);
     return std::make_pair(true, found_safe_path);
   }
 
@@ -411,54 +412,6 @@ bool LaneChangeModule::isCurrentSpeedLow() const
   return util::l2Norm(current_twist.linear) < threshold_kmph * 1000 / 3600;
 }
 
-bool LaneChangeModule::isLaneBlocked(const lanelet::ConstLanelets & lanes) const
-{
-  const auto & route_handler = planner_data_->route_handler;
-  const auto current_pose = planner_data_->self_pose->pose;
-
-  const auto current_lanes = getCurrentLanes();
-
-  const auto arc = lanelet::utils::getArcCoordinates(lanes, current_pose);
-  constexpr double max_check_distance = 100;
-  double static_obj_velocity_thresh = parameters_.static_obstacle_velocity_thresh;
-  const double lane_changeable_distance_left =
-    route_handler->getLaneChangeableDistance(current_pose, LaneChangeDirection::LEFT);
-  const double lane_changeable_distance_right =
-    route_handler->getLaneChangeableDistance(current_pose, LaneChangeDirection::RIGHT);
-  const double lane_changeable_distance =
-    std::max(lane_changeable_distance_left, lane_changeable_distance_right);
-  const double check_distance = std::min(max_check_distance, lane_changeable_distance);
-  const auto polygon =
-    lanelet::utils::getPolygonFromArcLength(lanes, arc.length, arc.length + check_distance);
-
-  if (polygon.size() < 3) {
-    RCLCPP_WARN_STREAM(
-      getLogger(), "could not get polygon from lanelet with arc lengths: "
-                     << arc.length << " to " << arc.length + check_distance);
-    return false;
-  }
-
-  for (const auto & obj : planner_data_->dynamic_object->objects) {
-    const auto label = util::getHighestProbLabel(obj.classification);
-    if (
-      label == ObjectClassification::CAR || label == ObjectClassification::TRUCK ||
-      label == ObjectClassification::BUS || label == ObjectClassification::MOTORCYCLE) {
-      const auto velocity = util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear);
-      if (velocity < static_obj_velocity_thresh) {
-        const auto position = lanelet::utils::conversion::toLaneletPoint(
-          obj.kinematics.initial_pose_with_covariance.pose.position);
-        const auto distance = boost::geometry::distance(
-          lanelet::utils::to2D(position).basicPoint(),
-          lanelet::utils::to2D(polygon).basicPolygon());
-        if (distance < std::numeric_limits<double>::epsilon()) {
-          return true;
-        }
-      }
-    }
-  }
-  return false;
-}
-
 bool LaneChangeModule::isAbortConditionSatisfied() const
 {
   const auto & route_handler = planner_data_->route_handler;
@@ -501,7 +454,8 @@ 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, parameters_, false, status_.lane_change_path.acceleration);
+      common_parameters.vehicle_width, common_parameters.vehicle_length, parameters_, false,
+      status_.lane_change_path.acceleration);
   }
 
   // check vehicle velocity thresh