feat(lane_change): cherry-pick LC PRs

common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml

@@ -14,6 +14,10 @@ behavior_path_planner_avoidance:
   - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
     logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance
 
+behavior_path_planner_lane_change:
+  - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+    logger_name: lane_change
+
 behavior_velocity_planner:
   - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner
     logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner

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

@@ -45,6 +45,14 @@
           lateral_distance_max_threshold: 1.0
           longitudinal_distance_min_threshold: 2.5
           longitudinal_velocity_delta_time: 0.6
+        stuck:
+          expected_front_deceleration: -1.0
+          expected_rear_deceleration: -1.0
+          rear_vehicle_reaction_time: 2.0
+          rear_vehicle_safety_time_margin: 1.0
+          lateral_distance_max_threshold: 2.0
+          longitudinal_distance_min_threshold: 3.0
+          longitudinal_velocity_delta_time: 0.8
 
         # lane expansion for object filtering
         lane_expansion:

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

@@ -230,7 +230,8 @@ class LaneChangeBase
   virtual bool getLaneChangePaths(
     const lanelet::ConstLanelets & original_lanelets,
     const lanelet::ConstLanelets & target_lanelets, Direction direction,
-    LaneChangePaths * candidate_paths, const bool check_safety) const = 0;
+    LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params,
+    const bool is_stuck, const bool check_safety) const = 0;
 
   virtual TurnSignalInfo calcTurnSignalInfo() = 0;
 
@@ -270,6 +271,9 @@ class LaneChangeBase
   mutable LaneChangeTargetObjects debug_filtered_objects_{};
   mutable double object_debug_lifetime_{0.0};
   mutable StopWatch<std::chrono::milliseconds> stop_watch_;
+
+  rclcpp::Logger logger_ = rclcpp::get_logger("lane_change");
+  mutable rclcpp::Clock clock_{RCL_ROS_TIME};
 };
 }  // namespace behavior_path_planner
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_

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

@@ -138,6 +138,7 @@ class NormalLaneChange : public LaneChangeBase
   bool getLaneChangePaths(
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
     Direction direction, LaneChangePaths * candidate_paths,
+    const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck,
     const bool check_safety = true) const override;
 
   TurnSignalInfo calcTurnSignalInfo() override;
@@ -146,20 +147,33 @@ class NormalLaneChange : public LaneChangeBase
 
   PathSafetyStatus isLaneChangePathSafe(
     const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects,
-    const utils::path_safety_checker::RSSparams & rss_params,
+    const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck,
     CollisionCheckDebugMap & debug_data) const;
 
   LaneChangeTargetObjectIndices filterObject(
-    const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
+    const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
+    const lanelet::ConstLanelets & target_lanes,
     const lanelet::ConstLanelets & target_backward_lanes) const;
 
+  //! @brief Check if the ego vehicle is in stuck by a stationary obstacle.
+  //! @param obstacle_check_distance Distance to check ahead for any objects that might be
+  //! obstructing ego path. It makes sense to use values like the maximum lane change distance.
+  bool isVehicleStuckByObstacle(
+    const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const;
+
+  bool isVehicleStuckByObstacle(const lanelet::ConstLanelets & current_lanes) const;
+
+  double calcMaximumLaneChangeLength(
+    const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const;
+
+  std::pair<double, double> calcCurrentMinMaxAcceleration() const;
+
   void setStopPose(const Pose & stop_pose);
 
   void updateStopTime();
 
   double getStopTime() const { return stop_time_; }
 
-  rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr());
   double stop_time_{0.0};
 };
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -83,6 +83,7 @@ struct LaneChangeParameters
   bool allow_loose_check_for_cancel{true};
   utils::path_safety_checker::RSSparams rss_params{};
   utils::path_safety_checker::RSSparams rss_params_for_abort{};
+  utils::path_safety_checker::RSSparams rss_params_for_stuck{};
 
   // abort
   LaneChangeCancelParameters cancel;

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp

@@ -179,18 +179,19 @@ struct SafetyCheckParams
 
 struct CollisionCheckDebug
 {
-  std::string unsafe_reason;                ///< Reason indicating unsafe situation.
-  Twist current_twist{};                    ///< Ego vehicle's current velocity and rotation.
-  Pose expected_ego_pose{};                 ///< Predicted future pose of ego vehicle.
-  Pose current_obj_pose{};                  ///< Detected object's current pose.
-  Twist object_twist{};                     ///< Detected object's velocity and rotation.
-  Pose expected_obj_pose{};                 ///< Predicted future pose of object.
-  double rss_longitudinal{0.0};             ///< Longitudinal RSS measure.
-  double inter_vehicle_distance{0.0};       ///< Distance between ego vehicle and object.
-  double extended_polygon_lon_offset{0.0};  ///< Longitudinal offset for extended polygon.
-  double extended_polygon_lat_offset{0.0};  ///< Lateral offset for extended polygon.
-  bool is_front{false};                     ///< True if object is in front of ego vehicle.
-  bool is_safe{false};                      ///< True if situation is deemed safe.
+  std::string unsafe_reason;           ///< Reason indicating unsafe situation.
+  Twist current_twist{};               ///< Ego vehicle's current velocity and rotation.
+  Pose expected_ego_pose{};            ///< Predicted future pose of ego vehicle.
+  Pose current_obj_pose{};             ///< Detected object's current pose.
+  Twist object_twist{};                ///< Detected object's velocity and rotation.
+  Pose expected_obj_pose{};            ///< Predicted future pose of object.
+  double rss_longitudinal{0.0};        ///< Longitudinal RSS measure.
+  double inter_vehicle_distance{0.0};  ///< Distance between ego vehicle and object.
+  double forward_lon_offset{0.0};      ///< Forward longitudinal offset for extended polygon.
+  double backward_lon_offset{0.0};     ///< Backward longitudinal offset for extended polygon.
+  double lat_offset{0.0};              ///< Lateral offset for extended polygon.
+  bool is_front{false};                ///< True if object is in front of ego vehicle.
+  bool is_safe{false};                 ///< True if situation is deemed safe.
   std::vector<PoseWithVelocityStamped> ego_predicted_path;  ///< ego vehicle's predicted path.
   std::vector<PoseWithVelocityAndPolygonStamped> obj_predicted_path;  ///< object's predicted path.
   Polygon2d extended_ego_polygon{};  ///< Ego vehicle's extended collision polygon.

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp

@@ -61,10 +61,11 @@ bool isTargetObjectFront(
 
 Polygon2d createExtendedPolygon(
   const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info,
-  const double lon_length, const double lat_margin, CollisionCheckDebug & debug);
+  const double lon_length, const double lat_margin, const double is_stopped_obj,
+  CollisionCheckDebug & debug);
 Polygon2d createExtendedPolygon(
   const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin,
-  CollisionCheckDebug & debug);
+  const double is_stopped_obj, CollisionCheckDebug & debug);
 
 PredictedPath convertToPredictedPath(
   const std::vector<PoseWithVelocityStamped> & path, const double time_resolution);
@@ -75,7 +76,7 @@ double calcRssDistance(
 
 double calcMinimumLongitudinalLength(
   const double front_object_velocity, const double rear_object_velocity,
-  const BehaviorPathPlannerParameters & params);
+  const RSSparams & rss_params);
 
 boost::optional<PoseWithVelocityStamped> calcInterpolatedPoseWithVelocity(
   const std::vector<PoseWithVelocityStamped> & path, const double relative_time);

planning/behavior_path_planner/src/marker_utils/utils.cpp

@@ -636,8 +636,9 @@ MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, st
        << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal
        << "\nEgo to obj: " << info.inter_vehicle_distance
        << "\nExtended polygon: " << (info.is_front ? "ego" : "object")
-       << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset
-       << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset
+       << "\nExtended polygon lateral offset: " << info.lat_offset
+       << "\nExtended polygon forward longitudinal offset: " << info.forward_lon_offset
+       << "\nExtended polygon backward longitudinal offset: " << info.backward_lon_offset
        << "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego")
        << "\nSafe: " << (info.is_safe ? "Yes" : "No");
 

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

@@ -125,6 +125,21 @@ LaneChangeModuleManager::LaneChangeModuleManager(
   p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.cancel.lateral_distance_max_threshold"));
 
+  p.rss_params_for_stuck.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.longitudinal_distance_min_threshold"));
+  p.rss_params_for_stuck.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.longitudinal_velocity_delta_time"));
+  p.rss_params_for_stuck.front_vehicle_deceleration = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.expected_front_deceleration"));
+  p.rss_params_for_stuck.rear_vehicle_deceleration = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.expected_rear_deceleration"));
+  p.rss_params_for_stuck.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.rear_vehicle_reaction_time"));
+  p.rss_params_for_stuck.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.rear_vehicle_safety_time_margin"));
+  p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.stuck.lateral_distance_max_threshold"));
+
   // target object
   {
     std::string ns = "lane_change.target_object.";