feat(behavior_path_planner): abort lane change function (autowarefoun…

…dation#2359)

* feat(behavior_path_planner): abort lane change function

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

* change Revert -> Cancel

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

* Remove some unwanted functions and and STOP state

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

* update steering factor (accidentally removed)

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

* include is_abort_condition_satisfied_ flag

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

* use only check ego in current lane

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

* Revert "use only check ego in current lane"

This reverts commit 4f97408.

* ci(pre-commit): autofix

* use only check ego in current lane

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

* improve isAbortConditionSatisfied by using ego polygon check

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

* add lateral jerk and path doesn't keep on updating anymore

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

* parameterized all abort related values

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

* rename abort_end -> abort_return

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

* fix some parameter issue

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

* check if lane change distance is enough after abort

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

* improve the code flow of isAbortConditionSatisfied

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

* Place warning message in corresponding states.

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

* fix clock and rebase

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

* remove accel and jerk parameters

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

* remove unnecessary parameters

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

* fix param file in config

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

* Update planning/behavior_path_planner/src/scene_module/lane_change/util.cpp

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove isStopState and refactoring

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

* Fixed CANCEL when ego is out of lane

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

* fix path reset during abort

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

* fix abort path exceed goal

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

* fix logger to debug

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

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

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

@@ -17,13 +17,18 @@
       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]
+      # collision check
+      enable_collision_check_at_prepare_phase: false
       prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s]
-
-      enable_abort_lane_change: true
-      enable_collision_check_at_prepare_phase: true
       use_predicted_path_outside_lanelet: true
-      use_all_predicted_path: false
+      use_all_predicted_path: true
+
+      # abort
+      enable_cancel_lane_change: true
+      enable_abort_lane_change: false
+
+      abort_delta_time: 3.0                      # [s]
+      abort_max_lateral_jerk: 5.0                # [m/s3]
+
+      # debug
       publish_debug_marker: false

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

@@ -54,8 +54,6 @@ class LaneChangeModule : public SceneModuleInterface
     const std::string & name, rclcpp::Node & node,
     std::shared_ptr<LaneChangeParameters> parameters);
 
-  BehaviorModuleOutput run() override;
-
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
   BT::NodeStatus updateState() override;
@@ -103,16 +101,24 @@ class LaneChangeModule : public SceneModuleInterface
   LaneChangeStatus status_;
   PathShifter path_shifter_;
   mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_;
+  LaneChangeStates current_lane_change_state_;
+  std::shared_ptr<LaneChangePath> abort_path_;
+  PathWithLaneId prev_approved_path_;
 
   double lane_change_lane_length_{200.0};
   double check_distance_{100.0};
+  bool is_abort_path_approved_{false};
+  bool is_abort_approval_requested_{false};
+  bool is_abort_condition_satisfied_{false};
+  bool is_activated_ = false;
 
   RTCInterface rtc_interface_left_;
   RTCInterface rtc_interface_right_;
   UUID uuid_left_;
   UUID uuid_right_;
+  UUID candidate_uuid_;
 
-  bool is_activated_ = false;
+  void resetParameters();
 
   void waitApprovalLeft(const double start_distance, const double finish_distance)
   {
@@ -134,12 +140,14 @@ class LaneChangeModule : public SceneModuleInterface
       rtc_interface_left_.updateCooperateStatus(
         uuid_left_, isExecutionReady(), candidate.start_distance_to_path_change,
         candidate.finish_distance_to_path_change, clock_->now());
+      candidate_uuid_ = uuid_left_;
       return;
     }
     if (candidate.lateral_shift < 0.0) {
       rtc_interface_right_.updateCooperateStatus(
         uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change,
         candidate.finish_distance_to_path_change, clock_->now());
+      candidate_uuid_ = uuid_right_;
       return;
     }
 
@@ -154,6 +162,21 @@ class LaneChangeModule : public SceneModuleInterface
     rtc_interface_right_.clearCooperateStatus();
   }
 
+  void removePreviousRTCStatusLeft()
+  {
+    if (rtc_interface_left_.isRegistered(uuid_left_)) {
+      rtc_interface_left_.removeCooperateStatus(uuid_left_);
+    }
+  }
+
+  void removePreviousRTCStatusRight()
+  {
+    if (rtc_interface_right_.isRegistered(uuid_right_)) {
+      rtc_interface_right_.removeCooperateStatus(uuid_right_);
+    }
+  }
+
+  lanelet::ConstLanelets get_original_lanes() const;
   PathWithLaneId getReferencePath() const;
   lanelet::ConstLanelets getLaneChangeLanes(
     const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const;
@@ -165,27 +188,28 @@ class LaneChangeModule : public SceneModuleInterface
   void generateExtendedDrivableArea(PathWithLaneId & path);
   void updateOutputTurnSignal(BehaviorModuleOutput & output);
   void updateSteeringFactorPtr(const BehaviorModuleOutput & output);
+  bool isApprovedPathSafe(Pose & ego_pose_before_collision) const;
 
   void updateSteeringFactorPtr(
     const CandidateOutput & output, const LaneChangePath & selected_path) const;
   bool isSafe() const;
   bool isValidPath(const PathWithLaneId & path) const;
   bool isNearEndOfLane() const;
   bool isCurrentSpeedLow() const;
-  bool isAbortConditionSatisfied() const;
+  bool isAbortConditionSatisfied();
   bool hasFinishedLaneChange() const;
-  void resetParameters();
+  bool isAbortState() const;
 
   // getter
   Pose getEgoPose() const;
   Twist getEgoTwist() const;
   std_msgs::msg::Header getRouteHeader() const;
+  void resetPathIfAbort();
 
   // debug
+  void setObjectDebugVisualization() const;
   mutable std::unordered_map<std::string, CollisionCheckDebug> object_debug_;
   mutable std::vector<LaneChangePath> debug_valid_path_;
-
-  void setObjectDebugVisualization() const;
 };
 }  // namespace behavior_path_planner
 

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

@@ -25,6 +25,7 @@ namespace behavior_path_planner
 {
 struct LaneChangeParameters
 {
+  // trajectory generation
   double lane_change_prepare_duration{2.0};
   double lane_changing_safety_check_duration{4.0};
   double lane_changing_lateral_jerk{0.5};
@@ -34,18 +35,26 @@ struct LaneChangeParameters
   double prediction_time_resolution{0.5};
   double maximum_deceleration{1.0};
   int lane_change_sampling_num{10};
-  double abort_lane_change_velocity_thresh{0.5};
-  double abort_lane_change_angle_thresh{0.174533};
-  double abort_lane_change_distance_thresh{0.3};
-  double prepare_phase_ignore_target_speed_thresh{0.1};
-  bool enable_abort_lane_change{true};
+
+  // collision check
   bool enable_collision_check_at_prepare_phase{true};
+  double prepare_phase_ignore_target_speed_thresh{0.1};
   bool use_predicted_path_outside_lanelet{false};
   bool use_all_predicted_path{false};
-  bool publish_debug_marker{false};
+
+  // abort
+  bool enable_cancel_lane_change{true};
+  bool enable_abort_lane_change{false};
+
+  double abort_delta_time{3.0};
+  double abort_max_lateral_jerk{10.0};
+
   // drivable area expansion
   double drivable_area_right_bound_offset{0.0};
   double drivable_area_left_bound_offset{0.0};
+
+  // debug marker
+  bool publish_debug_marker{false};
 };
 
 struct LaneChangeStatus
@@ -59,6 +68,14 @@ struct LaneChangeStatus
   bool is_safe;
   double start_distance;
 };
+
+enum class LaneChangeStates {
+  Normal = 0,
+  Cancel,
+  Abort,
+  Stop,
+};
+
 }  // namespace behavior_path_planner
 
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_

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

@@ -29,12 +29,15 @@ using behavior_path_planner::TurnSignalInfo;
 struct LaneChangePath
 {
   PathWithLaneId path;
+  lanelet::ConstLanelets reference_lanelets;
+  lanelet::ConstLanelets target_lanelets;
   ShiftedPath shifted_path;
   ShiftLine shift_line;
   double acceleration{0.0};
   double preparation_length{0.0};
   double lane_change_length{0.0};
   TurnSignalInfo turn_signal_info;
+  PathWithLaneId prev_path;
 };
 using LaneChangePaths = std::vector<LaneChangePath>;
 

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

@@ -97,7 +97,7 @@ bool isLaneChangePathSafe(
   const size_t current_seg_idx, const Twist & current_twist,
   const BehaviorPathPlannerParameters & common_parameters,
   const behavior_path_planner::LaneChangeParameters & lane_change_parameters,
-  const double front_decel, const double rear_decel,
+  const double front_decel, const double rear_decel, Pose & ego_pose_before_collision,
   std::unordered_map<std::string, CollisionCheckDebug> & debug_data, const bool use_buffer = true,
   const double acceleration = 0.0);
 
@@ -133,12 +133,6 @@ PathWithLaneId getLaneChangePathLaneChangingSegment(
 bool isEgoWithinOriginalLane(
   const lanelet::ConstLanelets & current_lanes, const Pose & current_pose,
   const BehaviorPathPlannerParameters & common_param);
-bool isEgoDistanceNearToCenterline(
-  const lanelet::ConstLanelet & closest_lanelet, const Pose & current_pose,
-  const LaneChangeParameters & lane_change_param);
-bool isEgoHeadingAngleLessThanThreshold(
-  const lanelet::ConstLanelet & closest_lanelet, const Pose & current_pose,
-  const LaneChangeParameters & lane_change_param);
 
 TurnSignalInfo calc_turn_signal_info(
   const PathWithLaneId & prepare_path, const double prepare_velocity,
@@ -151,6 +145,18 @@ void get_turn_signal_info(
 std::vector<DrivableLanes> generateDrivableLanes(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & lane_change_lanes);
+
+std::optional<LaneChangePath> getAbortPaths(
+  const std::shared_ptr<const PlannerData> & planner_data, const LaneChangePath & selected_path,
+  const Pose & ego_lerp_pose_before_collision, const BehaviorPathPlannerParameters & common_param,
+  const LaneChangeParameters & lane_change_param);
+
+double getLateralShift(const LaneChangePath & path);
+
+bool hasEnoughDistanceToLaneChangeAfterAbort(
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
+  const Pose & curent_pose, const double abort_return_dist,
+  const BehaviorPathPlannerParameters & common_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

@@ -199,6 +199,9 @@ bool lerpByTimeStamp(const PredictedPath & path, const double t, Pose * lerped_p
 
 bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon);
 
+bool calcObjectPolygon(
+  const Shape & object_shape, const Pose & object_pose, Polygon2d * object_polygon);
+
 bool calcObjectPolygon(
   const Shape & object_shape, const Pose & object_pose, Polygon2d * object_polygon);
 
@@ -478,7 +481,7 @@ bool isSafeInLaneletCollisionCheck(
   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, const double front_decel,
-  const double rear_decel, CollisionCheckDebug & debug);
+  const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug);
 
 bool isSafeInFreeSpaceCollisionCheck(
   const Pose & ego_current_pose, const Twist & ego_current_twist,

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -381,6 +381,8 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   };
 
   LaneChangeParameters p{};
+
+  // trajectory generation
   p.lane_change_prepare_duration = dp("lane_change_prepare_duration", 2.0);
   p.lane_changing_safety_check_duration = dp("lane_changing_safety_check_duration", 4.0);
   p.lane_changing_lateral_jerk = dp("lane_changing_lateral_jerk", 0.5);
@@ -390,19 +392,27 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.prediction_time_resolution = dp("prediction_time_resolution", 0.5);
   p.maximum_deceleration = dp("maximum_deceleration", 1.0);
   p.lane_change_sampling_num = dp("lane_change_sampling_num", 10);
-  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.prepare_phase_ignore_target_speed_thresh = dp("prepare_phase_ignore_target_speed_thresh", 0.1);
-  p.enable_abort_lane_change = dp("enable_abort_lane_change", true);
+
+  // collision check
   p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true);
+  p.prepare_phase_ignore_target_speed_thresh = dp("prepare_phase_ignore_target_speed_thresh", 0.1);
   p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true);
   p.use_all_predicted_path = dp("use_all_predicted_path", true);
-  p.publish_debug_marker = dp("publish_debug_marker", false);
+
+  // abort
+  p.enable_cancel_lane_change = dp("enable_cancel_lane_change", true);
+  p.enable_abort_lane_change = dp("enable_abort_lane_change", false);
+
+  p.abort_delta_time = dp("abort_delta_time", 3.0);
+  p.abort_max_lateral_jerk = dp("abort_max_lateral_jerk", 10.0);
+
+  // drivable area expansion
   p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0);
   p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0);
 
+  // debug marker
+  p.publish_debug_marker = dp("publish_debug_marker", false);
+
   // validation of parameters
   if (p.lane_change_sampling_num < 1) {
     RCLCPP_FATAL_STREAM(
@@ -419,6 +429,13 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
     exit(EXIT_FAILURE);
   }
 
+  if (p.abort_delta_time < 1.0) {
+    RCLCPP_FATAL_STREAM(
+      get_logger(), "abort_delta_time: " << p.abort_delta_time << ", is too short.\n"
+                                         << "Terminating the program...");
+    exit(EXIT_FAILURE);
+  }
+
   return p;
 }