refactor(lane_change): add helper class to deal with checks

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -270,13 +270,7 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_
 double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
 {
   const auto current_lanes = getCurrentLanes();
-  if (current_lanes.empty()) {
-    RCLCPP_DEBUG(logger_, "no empty lanes");
-    return std::numeric_limits<double>::infinity();
-  }
-
-  return utils::lane_change::calcMinimumLaneChangeLength(
-    getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_);
+  return lc_helper_->calc_min_lane_change_length(current_lanes, direction_);
 }
 
 double AvoidanceByLaneChange::calcLateralOffset() const

planning/behavior_path_lane_change_module/CMakeLists.txt

@@ -11,6 +11,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/scene.cpp
   src/utils/markers.cpp
   src/utils/utils.cpp
+  src/utils/helper.cpp
 )
 
 if(BUILD_TESTING)

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -16,6 +16,7 @@
 
 #include "behavior_path_lane_change_module/utils/base_class.hpp"
 #include "behavior_path_lane_change_module/utils/data_structs.hpp"
+#include "behavior_path_lane_change_module/utils/helper.hpp"
 
 #include <memory>
 #include <utility>
@@ -81,10 +82,6 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isRequiredStop(const bool is_object_coming_from_rear) 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;
@@ -113,9 +110,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> calcPrepareDuration() const;
 
   ExtendedPredictedObjects getTargetObjects(
     const LaneChangeLanesFilteredObjects & predicted_objects,

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp

@@ -16,6 +16,7 @@
 
 #include "behavior_path_lane_change_module/utils/data_structs.hpp"
 #include "behavior_path_lane_change_module/utils/debug_structs.hpp"
+#include "behavior_path_lane_change_module/utils/helper.hpp"
 #include "behavior_path_lane_change_module/utils/path.hpp"
 #include "behavior_path_lane_change_module/utils/utils.hpp"
 #include "behavior_path_planner_common/interface/scene_module_interface.hpp"
@@ -41,6 +42,7 @@ using data::lane_change::PathSafetyStatus;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
+using helper::lane_change::Helper;
 using route_handler::Direction;
 using tier4_autoware_utils::StopWatch;
 
@@ -52,6 +54,7 @@ class LaneChangeBase
     Direction direction)
   : lane_change_parameters_{std::move(parameters)}, direction_{direction}, type_{type}
   {
+    lc_helper_ = std::make_shared<Helper>(lane_change_parameters_);
   }
 
   LaneChangeBase(const LaneChangeBase &) = delete;
@@ -99,10 +102,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;
@@ -151,7 +150,11 @@ class LaneChangeBase
 
   bool isValidPath() const { return status_.is_valid_path; }
 
-  void setData(const std::shared_ptr<const PlannerData> & data) { planner_data_ = data; }
+  void setData(const std::shared_ptr<const PlannerData> & data)
+  {
+    planner_data_ = data;
+    lc_helper_->set_data(planner_data_);
+  }
 
   void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; }
 
@@ -219,7 +222,7 @@ class LaneChangeBase
   std::shared_ptr<const PlannerData> planner_data_{};
   BehaviorModuleOutput prev_module_output_{};
   std::optional<Pose> lane_change_stop_pose_{std::nullopt};
-
+  std::shared_ptr<Helper> lc_helper_{};
   PathWithLaneId prev_approved_path_{};
 
   int unsafe_hysteresis_count_{0};

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/helper.hpp

@@ -0,0 +1,71 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__HELPER_HPP_
+#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__HELPER_HPP_
+
+#include "behavior_path_lane_change_module/utils/data_structs.hpp"
+#include "behavior_path_lane_change_module/utils/debug_structs.hpp"
+#include "behavior_path_planner_common/data_manager.hpp"
+
+#include <memory>
+#include <vector>
+
+namespace behavior_path_planner::helper::lane_change
+{
+using nav_msgs::msg::Odometry;
+using route_handler::Direction;
+class Helper
+{
+public:
+  Helper() = default;
+  explicit Helper(const std::shared_ptr<LaneChangeParameters> & lc_params);
+  Helper(const Helper &) = delete;
+  Helper(Helper &&) = delete;
+  Helper & operator=(const Helper &) = delete;
+  Helper & operator=(Helper &&) = delete;
+
+  ~Helper() = default;
+
+  void set_data(const std::shared_ptr<const PlannerData> &);
+  void set_lanes(
+    const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes);
+
+  double calc_min_lane_change_length(const std::vector<double> & shift_intervals) const;
+
+  double calc_min_lane_change_length(
+    const lanelet::ConstLanelets & lanes, Direction direction = Direction::NONE) const;
+
+  [[nodiscard]] bool is_near_end_of_current_lanes() const;
+
+  [[nodiscard]] bool has_enough_length_to_retry(
+    const double abort_return_dist, Direction direction) const;
+
+private:
+  std::shared_ptr<RouteHandler> route_handler_;
+  Odometry::ConstSharedPtr self_odometry_;
+  BehaviorPathPlannerParameters common_params_;
+  std::shared_ptr<LaneChangeParameters> lc_params_;
+
+  [[nodiscard]] Pose get_ego_pose() const;
+  double get_signed_distance_from_current_pose(const lanelet::ConstLanelets & lanes) const;
+
+  lanelet::ConstLanelets current_lanes_;
+  lanelet::ConstLanelets target_lanes_;
+  rclcpp::Logger logger_{rclcpp::get_logger("lane_change.helper")};
+
+  mutable data::lane_change::Debug debug_;
+};
+}  // namespace behavior_path_planner::helper::lane_change
+
+#endif  // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__HELPER_HPP_

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

@@ -58,13 +58,6 @@ using tier4_autoware_utils::Polygon2d;
 double calcLaneChangeResampleInterval(
   const double lane_changing_length, const double lane_changing_velocity);
 
-double calcMinimumLaneChangeLength(
-  const LaneChangeParameters & lane_change_parameters, const std::vector<double> & shift_intervals);
-
-double calcMinimumLaneChangeLength(
-  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lane,
-  const LaneChangeParameters & lane_change_parameters, Direction direction);
-
 double calcMaximumLaneChangeLength(
   const double current_velocity, const LaneChangeParameters & lane_change_parameters,
   const std::vector<double> & shift_intervals, const double max_acc);
@@ -142,11 +135,6 @@ std::vector<DrivableLanes> generateDrivableLanes(
 
 double getLateralShift(const LaneChangePath & path);
 
-bool hasEnoughLengthToLaneChangeAfterAbort(
-  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const Pose & curent_pose, const double abort_return_dist,
-  const LaneChangeParameters & lane_change_parameters, const Direction direction);
-
 double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0);
 
 double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer);