refactor(lane_change): replace any code that can use transient data (#…

…8999) * RT1-8004 replace hasEnoughLength Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * RT1-8004 Removed isNearEndOfCurrentLanes Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * RT1-8004 refactor sample longitudinal acc values Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * remove calc maximum lane change length Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Revert "remove calc maximum lane change length" This reverts commit e9cc386. * Revert "RT1-8004 refactor sample longitudinal acc values" This reverts commit 775bcdb. * replace generateCenterLinePath Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * RT1-8004 simplify stuck detection Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * swap call to update filtered_objects and update transient data Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * RT1-8004 fix conflict Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * RT1-8004 Rename isVehicleStuck to is_ego_stuck() Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * RT1-8004 change calcPrepareDuration to snake case Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
autowarefoundation · Oct 4, 2024 · 85f578c · 85f578c
1 parent 047758e
commit 85f578c
Show file tree

Hide file tree

Showing 6 changed files with 74 additions and 194 deletions.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md
@@ -195,7 +195,7 @@ if (max_lane_change_length >  ego's distance to the end of the current lanes.) t
   stop
 endif
 
-if (isVehicleStuck(current_lanes)) then (yes)
+if ego is stuck in the current lanes then (yes)
   :Return **sampled acceleration values**;
   stop
 else (no)

diff --git a/...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp
@@ -89,10 +89,6 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isRequiredStop(const bool is_trailing_object) override;
 
-  bool isNearEndOfCurrentLanes(
-    const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
-    const double threshold) const override;
-
   bool hasFinishedLaneChange() const override;
 
   bool isAbleToReturnCurrentLane() const override;
@@ -127,9 +123,7 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;
 
-  std::vector<double> calcPrepareDuration(
-    const lanelet::ConstLanelets & current_lanes,
-    const lanelet::ConstLanelets & target_lanes) const;
+  std::vector<double> calc_prepare_durations() const;
 
   lane_change::TargetObjects getTargetObjects(
     const FilteredByLanesExtendedObjects & predicted_objects,
@@ -150,10 +144,6 @@ class NormalLaneChange : public LaneChangeBase
     const double target_lane_length, const double lane_changing_length,
     const double lane_changing_velocity, const double buffer_for_next_lane_change) const;
 
-  bool hasEnoughLength(
-    const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,
-    const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const;
-
   std::vector<LaneChangePhaseMetrics> get_prepare_metrics() const;
   std::vector<LaneChangePhaseMetrics> get_lane_changing_metrics(
     const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics,
@@ -167,8 +157,7 @@ class NormalLaneChange : public LaneChangeBase
     const Pose & lc_start_pose, const double shift_length) const;
 
   bool check_candidate_path_safety(
-    const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects,
-    const bool is_stuck) const;
+    const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const;
 
   std::optional<LaneChangePath> calcTerminalLaneChangePath(
     const lanelet::ConstLanelets & current_lanes,
@@ -192,15 +181,9 @@ class NormalLaneChange : public LaneChangeBase
     const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
     const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) 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 isVehicleStuck(
-    const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const;
-
   double get_max_velocity_for_safety_check() const;
 
-  bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const;
+  bool is_ego_stuck() const;
 
   /**
    * @brief Checks if the given pose is a valid starting point for a lane change.

diff --git a/...lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/...lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp
@@ -112,10 +112,6 @@ class LaneChangeBase
   virtual PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis(
     PathSafetyStatus approve_path_safety_status) = 0;
 
-  virtual bool isNearEndOfCurrentLanes(
-    const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
-    const double threshold) const = 0;
-
   virtual bool isStoppedAtRedTrafficLight() const = 0;
 
   virtual bool calcAbortPath() = 0;

diff --git a/...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp
@@ -344,6 +344,7 @@ struct TransientData
   lanelet::ArcCoordinates target_lanes_ego_arc;   // arc coordinates of ego pose along target lanes
 
   bool is_ego_near_current_terminal_start{false};
+  bool is_ego_stuck{false};
 };
 
 using RouteHandlerPtr = std::shared_ptr<RouteHandler>;

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp
@@ -76,8 +76,8 @@ void LaneChangeInterface::updateData()
   universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper());
   module_type_->setPreviousModuleOutput(getPreviousModuleOutput());
   module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING);
-  module_type_->update_transient_data();
   module_type_->update_filtered_objects();
+  module_type_->update_transient_data();
   module_type_->updateSpecialData();
 
   if (isWaitingApproval() || module_type_->isAbortState()) {