refactor(lane_change): add TransientData to store commonly used lane …

…change-related variables. (autowarefoundation#8954) * add transient data Signed-off-by: Zulfaqar Azmi <[email protected]> * reverted max lc dist in calcCurrentMinMax Signed-off-by: Zulfaqar Azmi <[email protected]> * rename Signed-off-by: Zulfaqar Azmi <[email protected]> * minor refactoring Signed-off-by: Zulfaqar Azmi <[email protected]> * update doxygen comments Signed-off-by: Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Zulfaqar Azmi <[email protected]>
StepTurtle · Sep 27, 2024 · 71cfaaf · 71cfaaf
1 parent 82b286c
commit 71cfaaf
Show file tree

Hide file tree

Showing 11 changed files with 342 additions and 222 deletions.
diff --git a/...ehavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/...ehavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp
@@ -124,7 +124,7 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
 
   const auto & nearest_object = data.target_objects.front();
   const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object);
-  const auto minimum_lane_change_length = calcMinimumLaneChangeLength();
+  const auto minimum_lane_change_length = calc_minimum_dist_buffer();
 
   lane_change_debug_.execution_area = create_execution_area(
     getCommonParam().vehicle_info, getEgoPose(),
@@ -315,16 +315,11 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_
   return avoidance_helper_->getMinAvoidanceDistance(shift_length);
 }
 
-double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
+double AvoidanceByLaneChange::calc_minimum_dist_buffer() const
 {
-  const auto current_lanes = get_current_lanes();
-  if (current_lanes.empty()) {
-    RCLCPP_DEBUG(logger_, "no empty lanes");
-    return std::numeric_limits<double>::infinity();
-  }
-
-  return utils::lane_change::calculation::calc_minimum_lane_change_length(
-    getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_);
+  const auto [_, dist_buffer] = utils::lane_change::calculation::calc_lc_length_and_dist_buffer(
+    common_data_ptr_, get_current_lanes());
+  return dist_buffer.min;
 }
 
 double AvoidanceByLaneChange::calcLateralOffset() const

diff --git a/...ehavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp b/...ehavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp
@@ -63,7 +63,7 @@ class AvoidanceByLaneChange : public NormalLaneChange
   void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const;
 
   double calcMinAvoidanceLength(const ObjectData & nearest_object) const;
-  double calcMinimumLaneChangeLength() const;
+  double calc_minimum_dist_buffer() const;
   double calcLateralOffset() const;
 };
 }  // namespace autoware::behavior_path_planner

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
@@ -54,6 +54,8 @@ class NormalLaneChange : public LaneChangeBase
 
   void update_lanes(const bool is_approved) final;
 
+  void update_transient_data() final;
+
   void update_filtered_objects() final;
 
   void updateLaneChangeStatus() override;

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
@@ -67,6 +67,8 @@ class LaneChangeBase
 
   virtual void update_lanes(const bool is_approved) = 0;
 
+  virtual void update_transient_data() = 0;
+
   virtual void update_filtered_objects() = 0;
 
   virtual void updateLaneChangeStatus() = 0;

diff --git a/...ane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/...ane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp
@@ -24,25 +24,9 @@ using autoware::route_handler::Direction;
 using autoware::route_handler::RouteHandler;
 using behavior_path_planner::lane_change::CommonDataPtr;
 using behavior_path_planner::lane_change::LCParamPtr;
+using behavior_path_planner::lane_change::MinMaxValue;
 using behavior_path_planner::lane_change::PhaseMetrics;
 
-/**
- * @brief Calculates the distance from the ego vehicle to the terminal point.
- *
- * This function computes the distance from the current position of the ego vehicle
- * to either the goal pose or the end of the current lane, depending on whether
- * the vehicle's current lane is within the goal section.
- *
- * @param common_data_ptr Shared pointer to a CommonData structure, which should include:
- *  - Non null `lanes_ptr` that points to the current lanes data.
- *  - Non null `self_odometry_ptr` that contains the current pose of the ego vehicle.
- *  - Non null `route_handler_ptr` that contains the goal pose of the route.
- *
- * @return The distance to the terminal point (either the goal pose or the end of the current lane)
- * in meters.
- */
-double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr);
-
 double calc_dist_from_pose_to_terminal_end(
   const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes,
   const Pose & src_pose);
@@ -119,13 +103,6 @@ double calc_ego_dist_to_lanes_start(
   const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes);
 
-double calc_minimum_lane_change_length(
-  const LaneChangeParameters & lane_change_parameters, const std::vector<double> & shift_intervals);
-
-double calc_minimum_lane_change_length(
-  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lane,
-  const LaneChangeParameters & lane_change_parameters, Direction direction);
-
 double calc_maximum_lane_change_length(
   const double current_velocity, const LaneChangeParameters & lane_change_parameters,
   const std::vector<double> & shift_intervals, const double max_acc);
@@ -134,6 +111,10 @@ double calc_maximum_lane_change_length(
   const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet,
   const double max_acc);
 
+double calc_lane_changing_acceleration(
+  const double initial_lane_changing_velocity, const double max_path_velocity,
+  const double lane_changing_time, const double prepare_longitudinal_acc);
+
 /**
  * @brief Calculates the distance required during a lane change operation.
  *
@@ -151,10 +132,6 @@ double calc_phase_length(
   const double velocity, const double maximum_velocity, const double acceleration,
   const double duration);
 
-double calc_lane_changing_acceleration(
-  const double initial_lane_changing_velocity, const double max_path_velocity,
-  const double lane_changing_time, const double prepare_longitudinal_acc);
-
 std::vector<PhaseMetrics> calc_prepare_phase_metrics(
   const CommonDataPtr & common_data_ptr, const std::vector<double> & prepare_durations,
   const std::vector<double> & lon_accel_values, const double current_velocity,
@@ -165,6 +142,36 @@ std::vector<PhaseMetrics> calc_shift_phase_metrics(
   const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity,
   const double max_velocity, const double lon_accel,
   const double max_length_threshold = std::numeric_limits<double>::max());
+
+/**
+ * @brief Calculates the minimum and maximum lane changing lengths, along with the corresponding
+ * distance buffers.
+ *
+ * This function computes the minimum and maximum lane change lengths based on shift intervals and
+ * vehicle parameters. It only accounts for the lane changing phase itself, excluding the prepare
+ * distance, which should be handled separately during path generation.
+ *
+ * Additionally, the function calculates the distance buffer to ensure that the ego vehicle has
+ * enough space to complete the lane change, including cases where multiple lane changes may be
+ * necessary.
+ *
+ * @param common_data_ptr Shared pointer to a CommonData structure, which includes:
+ *  - `lc_param_ptr`: Parameters related to lane changing, such as velocity, lateral acceleration,
+ * and jerk.
+ *  - `route_handler_ptr`: Pointer to the route handler for managing routes.
+ *  - `direction`: Lane change direction (e.g., left or right).
+ *  - `transient_data.acc.max`: Maximum acceleration of the vehicle.
+ *  - Other relevant data for the ego vehicle's dynamics and state.
+ * @param lanes The set of lanelets representing the lanes for the lane change.
+ *
+ * @return A pair of `MinMaxValue` structures where:
+ *  - The first element contains the minimum and maximum lane changing lengths in meters.
+ *  - The second element contains the minimum and maximum distance buffers in meters.
+ * If the lanes or necessary data are unavailable, returns `std::numeric_limits<double>::max()` for
+ * both values.
+ */
+std::pair<MinMaxValue, MinMaxValue> calc_lc_length_and_dist_buffer(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes);
 }  // namespace autoware::behavior_path_planner::utils::lane_change::calculation
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_
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
@@ -27,6 +27,7 @@
 #include <lanelet2_core/primitives/Lanelet.h>
 #include <lanelet2_core/primitives/Polygon.h>
 
+#include <limits>
 #include <memory>
 #include <utility>
 #include <vector>
@@ -218,6 +219,7 @@ struct PhaseMetrics
 struct Lanes
 {
   bool current_lane_in_goal_section{false};
+  bool target_lane_in_goal_section{false};
   lanelet::ConstLanelets current;
   lanelet::ConstLanelets target_neighbor;
   lanelet::ConstLanelets target;
@@ -313,6 +315,32 @@ struct LanesPolygon
   std::vector<lanelet::BasicPolygon2d> preceding_target;
 };
 
+struct MinMaxValue
+{
+  double min{std::numeric_limits<double>::infinity()};
+  double max{std::numeric_limits<double>::infinity()};
+};
+
+struct TransientData
+{
+  MinMaxValue acc;                   // acceleration profile for accelerating lane change path
+  MinMaxValue lane_changing_length;  // lane changing length for a single lane change
+  MinMaxValue
+    current_dist_buffer;  // distance buffer computed backward from current lanes' terminal end
+  MinMaxValue
+    next_dist_buffer;  // distance buffer computed backward  from target lanes' terminal end
+  double dist_to_terminal_end{
+    std::numeric_limits<double>::min()};  // distance from ego base link to the current lanes'
+                                          // terminal end
+  double dist_to_terminal_start{
+    std::numeric_limits<double>::min()};  // distance from ego base link to the current lanes'
+                                          // terminal start
+  double max_prepare_length{
+    std::numeric_limits<double>::max()};  // maximum prepare length, starting from ego's base link
+
+  bool is_ego_near_current_terminal_start{false};
+};
+
 using RouteHandlerPtr = std::shared_ptr<RouteHandler>;
 using BppParamPtr = std::shared_ptr<BehaviorPathPlannerParameters>;
 using LCParamPtr = std::shared_ptr<Parameters>;
@@ -327,12 +355,14 @@ struct CommonData
   LCParamPtr lc_param_ptr;
   LanesPtr lanes_ptr;
   LanesPolygonPtr lanes_polygon_ptr;
+  TransientData transient_data;
+  PathWithLaneId current_lanes_path;
   ModuleType lc_type;
   Direction direction;
 
-  [[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; }
+  [[nodiscard]] const Pose & get_ego_pose() const { return self_odometry_ptr->pose.pose; }
 
-  [[nodiscard]] Twist get_ego_twist() const { return self_odometry_ptr->twist.twist; }
+  [[nodiscard]] const Twist & get_ego_twist() const { return self_odometry_ptr->twist.twist; }
 
   [[nodiscard]] double get_ego_speed(bool use_norm = false) const
   {
@@ -344,6 +374,18 @@ struct CommonData
     const auto y = get_ego_twist().linear.y;
     return std::hypot(x, y);
   }
+
+  [[nodiscard]] bool is_data_available() const
+  {
+    return route_handler_ptr && self_odometry_ptr && bpp_param_ptr && lc_param_ptr && lanes_ptr &&
+           lanes_polygon_ptr;
+  }
+
+  [[nodiscard]] bool is_lanes_available() const
+  {
+    return lanes_ptr && !lanes_ptr->current.empty() && !lanes_ptr->target.empty() &&
+           !lanes_ptr->target_neighbor.empty();
+  }
 };
 using CommonDataPtr = std::shared_ptr<CommonData>;
 }  // namespace autoware::behavior_path_planner::lane_change

diff --git a/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
@@ -122,9 +122,7 @@ 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);
+  const CommonDataPtr & common_data_ptr, const double abort_return_dist);
 
 CandidateOutput assignToCandidate(
   const LaneChangePath & lane_change_path, const Point & ego_position);

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,6 +76,7 @@ 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_->updateSpecialData();