feat(behavior_path_planner): apply abort lane change

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
autowarefoundation · Jan 6, 2023 · a11ecf7 · a11ecf7
1 parent a937300
commit a11ecf7
Show file tree

Hide file tree

Showing 6 changed files with 192 additions and 113 deletions.
diff --git a/...de/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp b/...de/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp
@@ -59,8 +59,6 @@ class ExternalRequestLaneChangeModule : public SceneModuleInterface
     const std::string & name, rclcpp::Node & node, std::shared_ptr<LaneChangeParameters> parameters,
     const Direction & direction);
 
-  BehaviorModuleOutput run() override;
-
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
   BT::NodeStatus updateState() override;
@@ -79,20 +77,29 @@ class ExternalRequestLaneChangeModule : 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_;
 
   Direction direction_;
 
   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;
 
+  void resetParameters();
+  void resetPathIfAbort();
+
   void waitApproval(const double start_distance, const double finish_distance)
   {
     updateRTCStatus(start_distance, finish_distance);
     is_waiting_approval_ = true;
   }
 
+  lanelet::ConstLanelets get_original_lanes() const;
   PathWithLaneId getReferencePath() const;
   lanelet::ConstLanelets getLaneChangeLanes(
     const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const;
@@ -106,13 +113,15 @@ class ExternalRequestLaneChangeModule : public SceneModuleInterface
   void updateSteeringFactorPtr(const BehaviorModuleOutput & output);
   void updateSteeringFactorPtr(
     const CandidateOutput & output, const LaneChangePath & selected_path) const;
+
   bool isSafe() const;
   bool isValidPath(const PathWithLaneId & path) const;
+  bool isApprovedPathSafe(Pose & ego_pose_before_collision) const;
   bool isNearEndOfLane() const;
   bool isCurrentSpeedLow() const;
-  bool isAbortConditionSatisfied() const;
+  bool isAbortConditionSatisfied();
   bool hasFinishedLaneChange() const;
-  void resetParameters();
+  bool isAbortState() const;
 
   // getter
   Pose getEgoPose() const;

diff --git a/...ing/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/...ing/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp
@@ -113,8 +113,7 @@ PathWithLaneId getReferencePathFromTargetLane(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const Pose & lane_changing_start_pose, const double prepare_distance,
   const double lane_changing_distance, const double forward_path_length,
-  const double lane_changing_speed, const int num_to_preferred_lane,
-  const double minimum_lane_change_length);
+  const double lane_changing_speed, const double lane_change_buffer);
 
 PathWithLaneId getReferencePathFromTargetLane(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,