refactor(lane_change): update lanes and its polygons only when it's u…

…pdated (autowarefoundation#7989) * refactor(lane_change): compute lanes and polygon only when updated Signed-off-by: Zulfaqar Azmi <[email protected]> * Revert accidental changesd This reverts commit cbfd9ae. Signed-off-by: Zulfaqar Azmi <[email protected]> * fix spell check Signed-off-by: Zulfaqar Azmi <[email protected]> * Make a common getter for current lanes Signed-off-by: Zulfaqar Azmi <[email protected]> * add target lanes getter Signed-off-by: Zulfaqar Azmi <[email protected]> * some minor function refactoring Signed-off-by: Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Zulfaqar Azmi <[email protected]>
tier4 · Aug 1, 2024 · 161060d · 161060d
1 parent a20a3f0
commit 161060d
Show file tree

Hide file tree

Showing 9 changed files with 246 additions and 172 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
@@ -135,7 +135,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
   const auto resample_interval = avoidance_parameters_->resample_interval_for_planning;
   data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval);
 
-  data.current_lanelets = getCurrentLanes();
+  data.current_lanelets = get_current_lanes();
 
   fillAvoidanceTargetObjects(data, debug);
 
@@ -275,7 +275,7 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_
 
 double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
 {
-  const auto current_lanes = getCurrentLanes();
+  const auto current_lanes = get_current_lanes();
   if (current_lanes.empty()) {
     RCLCPP_DEBUG(logger_, "no empty lanes");
     return std::numeric_limits<double>::infinity();

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
@@ -51,6 +51,8 @@ class NormalLaneChange : public LaneChangeBase
   NormalLaneChange & operator=(NormalLaneChange &&) = delete;
   ~NormalLaneChange() override = default;
 
+  void update_lanes(const bool is_approved) final;
+
   void updateLaneChangeStatus() override;
 
   std::pair<bool, bool> getSafePath(LaneChangePath & safe_path) const override;
@@ -105,8 +107,6 @@ class NormalLaneChange : public LaneChangeBase
   TurnSignalInfo get_current_turn_signal_info() override;
 
 protected:
-  lanelet::ConstLanelets getCurrentLanes() const override;
-
   lanelet::ConstLanelets getLaneChangeLanes(
     const lanelet::ConstLanelets & current_lanes, Direction direction) const override;
 
@@ -124,9 +124,7 @@ class NormalLaneChange : public LaneChangeBase
     const LaneChangeLanesFilteredObjects & predicted_objects,
     const lanelet::ConstLanelets & current_lanes) const;
 
-  LaneChangeLanesFilteredObjects filterObjects(
-    const lanelet::ConstLanelets & current_lanes,
-    const lanelet::ConstLanelets & target_lanes) const;
+  LaneChangeLanesFilteredObjects filterObjects() const;
 
   void filterOncomingObjects(PredictedObjects & objects) const;
 
@@ -203,6 +201,11 @@ class NormalLaneChange : public LaneChangeBase
 
   double getStopTime() const { return stop_time_; }
 
+  const lanelet::ConstLanelets & get_target_lanes() const
+  {
+    return common_data_ptr_->lanes_ptr->target;
+  }
+
   double stop_time_{0.0};
 };
 }  // namespace autoware::behavior_path_planner

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
@@ -65,6 +65,8 @@ class LaneChangeBase
   LaneChangeBase & operator=(LaneChangeBase &&) = delete;
   virtual ~LaneChangeBase() = default;
 
+  virtual void update_lanes(const bool is_approved) = 0;
+
   virtual void updateLaneChangeStatus() = 0;
 
   virtual std::pair<bool, bool> getSafePath(LaneChangePath & safe_path) const = 0;
@@ -139,6 +141,11 @@ class LaneChangeBase
 
   const Twist & getEgoTwist() const { return planner_data_->self_odometry->twist.twist; }
 
+  const lanelet::ConstLanelets & get_current_lanes() const
+  {
+    return common_data_ptr_->lanes_ptr->current;
+  }
+
   const BehaviorPathPlannerParameters & getCommonParam() const { return planner_data_->parameters; }
 
   LaneChangeParameters getLaneChangeParam() const { return *lane_change_parameters_; }
@@ -163,9 +170,19 @@ class LaneChangeBase
       common_data_ptr_->bpp_param_ptr =
         std::make_shared<BehaviorPathPlannerParameters>(data->parameters);
     }
+
+    if (!common_data_ptr_->lanes_ptr) {
+      common_data_ptr_->lanes_ptr = std::make_shared<lane_change::Lanes>();
+    }
+
+    if (!common_data_ptr_->lanes_polygon_ptr) {
+      common_data_ptr_->lanes_polygon_ptr = std::make_shared<lane_change::LanesPolygon>();
+    }
+
     common_data_ptr_->self_odometry_ptr = data->self_odometry;
     common_data_ptr_->route_handler_ptr = data->route_handler;
     common_data_ptr_->lc_param_ptr = lane_change_parameters_;
+    common_data_ptr_->lc_type = type_;
     common_data_ptr_->direction = direction_;
   }
 
@@ -211,8 +228,6 @@ class LaneChangeBase
   virtual TurnSignalInfo get_current_turn_signal_info() = 0;
 
 protected:
-  virtual lanelet::ConstLanelets getCurrentLanes() const = 0;
-
   virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0;
 
   virtual PathWithLaneId getPrepareSegment(

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
@@ -183,16 +183,20 @@ struct PhaseInfo
   }
 };
 
+struct Lanes
+{
+  lanelet::ConstLanelets current;
+  lanelet::ConstLanelets target;
+  std::vector<lanelet::ConstLanelets> preceding_target;
+};
+
 struct Info
 {
   PhaseInfo longitudinal_acceleration{0.0, 0.0};
   PhaseInfo velocity{0.0, 0.0};
   PhaseInfo duration{0.0, 0.0};
   PhaseInfo length{0.0, 0.0};
 
-  lanelet::ConstLanelets current_lanes{};
-  lanelet::ConstLanelets target_lanes{};
-
   Pose lane_changing_start{};
   Pose lane_changing_end{};
 
@@ -225,23 +229,26 @@ struct LanesPolygon
 {
   std::optional<lanelet::BasicPolygon2d> current;
   std::optional<lanelet::BasicPolygon2d> target;
-  std::vector<lanelet::BasicPolygon2d> target_backward;
+  std::optional<lanelet::BasicPolygon2d> expanded_target;
+  lanelet::BasicPolygon2d target_neighbor;
+  std::vector<lanelet::BasicPolygon2d> preceding_target;
 };
 
-struct Lanes
-{
-  lanelet::ConstLanelets current;
-  lanelet::ConstLanelets target;
-  std::vector<lanelet::ConstLanelets> preceding_target;
-};
+using RouteHandlerPtr = std::shared_ptr<RouteHandler>;
+using BppParamPtr = std::shared_ptr<BehaviorPathPlannerParameters>;
+using LCParamPtr = std::shared_ptr<Parameters>;
+using LanesPtr = std::shared_ptr<Lanes>;
+using LanesPolygonPtr = std::shared_ptr<LanesPolygon>;
 
 struct CommonData
 {
-  std::shared_ptr<RouteHandler> route_handler_ptr;
+  RouteHandlerPtr route_handler_ptr;
   Odometry::ConstSharedPtr self_odometry_ptr;
-  std::shared_ptr<BehaviorPathPlannerParameters> bpp_param_ptr;
-  std::shared_ptr<Parameters> lc_param_ptr;
-  Lanes lanes;
+  BppParamPtr bpp_param_ptr;
+  LCParamPtr lc_param_ptr;
+  LanesPtr lanes_ptr;
+  LanesPolygonPtr lanes_polygon_ptr;
+  ModuleType lc_type;
   Direction direction;
 
   [[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; }
@@ -259,12 +266,7 @@ struct CommonData
     return std::hypot(x, y);
   }
 };
-
-using RouteHandlerPtr = std::shared_ptr<RouteHandler>;
-using BppParamPtr = std::shared_ptr<BehaviorPathPlannerParameters>;
-using LCParamPtr = std::shared_ptr<Parameters>;
 using CommonDataPtr = std::shared_ptr<CommonData>;
-using LanesPtr = std::shared_ptr<Lanes>;
 }  // namespace autoware::behavior_path_planner::lane_change
 
 namespace autoware::behavior_path_planner

diff --git a/..._path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/..._path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp
@@ -23,31 +23,31 @@
 
 #include <vector>
 
-namespace autoware::behavior_path_planner
+namespace autoware::behavior_path_planner::lane_change
 {
 using autoware::behavior_path_planner::TurnSignalInfo;
 using tier4_planning_msgs::msg::PathWithLaneId;
-struct LaneChangePath
+struct Path
 {
   PathWithLaneId path{};
   ShiftedPath shifted_path{};
-  LaneChangeInfo info;
-  bool is_safe{false};
+  Info info{};
 };
-using LaneChangePaths = std::vector<LaneChangePath>;
 
-struct LaneChangeStatus
+struct Status
 {
-  PathWithLaneId lane_follow_path{};
-  LaneChangePath lane_change_path{};
-  lanelet::ConstLanelets current_lanes{};
-  lanelet::ConstLanelets target_lanes{};
-  std::vector<lanelet::Id> lane_follow_lane_ids{};
-  std::vector<lanelet::Id> lane_change_lane_ids{};
+  Path lane_change_path{};
   bool is_safe{false};
   bool is_valid_path{false};
   double start_distance{0.0};
 };
 
+}  // namespace autoware::behavior_path_planner::lane_change
+
+namespace autoware::behavior_path_planner
+{
+using LaneChangePath = lane_change::Path;
+using LaneChangePaths = std::vector<lane_change::Path>;
+using LaneChangeStatus = lane_change::Status;
 }  // namespace autoware::behavior_path_planner
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_
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
@@ -48,6 +48,7 @@ using autoware::universe_utils::Polygon2d;
 using autoware_perception_msgs::msg::PredictedObject;
 using autoware_perception_msgs::msg::PredictedObjects;
 using autoware_perception_msgs::msg::PredictedPath;
+using behavior_path_planner::lane_change::CommonDataPtr;
 using behavior_path_planner::lane_change::LanesPolygon;
 using behavior_path_planner::lane_change::PathSafetyStatus;
 using geometry_msgs::msg::Point;
@@ -114,8 +115,9 @@ bool isPathInLanelets(
   const lanelet::ConstLanelets & target_lanes);
 
 std::optional<LaneChangePath> constructCandidatePath(
-  const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment,
-  const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path,
+  const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info,
+  const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment,
+  const PathWithLaneId & target_lane_reference_path,
   const std::vector<std::vector<int64_t>> & sorted_lane_ids);
 
 ShiftLine getLaneChangingShiftLine(
@@ -177,10 +179,9 @@ bool isParkedObject(
   const double ratio_threshold);
 
 bool passParkedObject(
-  const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
+  const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
   const std::vector<ExtendedPredictedObject> & objects, const double minimum_lane_change_length,
-  const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters,
-  CollisionCheckDebugMap & object_debug);
+  const bool is_goal_in_route, CollisionCheckDebugMap & object_debug);
 
 std::optional<size_t> getLeadingStaticObjectIdx(
   const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
@@ -195,7 +196,8 @@ ExtendedPredictedObject transform(
   const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase);
 
 bool isCollidedPolygonsInLanelet(
-  const std::vector<Polygon2d> & collided_polygons, const lanelet::ConstLanelets & lanes);
+  const std::vector<Polygon2d> & collided_polygons,
+  const std::optional<lanelet::BasicPolygon2d> & lanes_polygon);
 
 /**
  * @brief Generates expanded lanelets based on the given direction and offsets.
@@ -295,9 +297,11 @@ double calcPhaseLength(
   const double velocity, const double maximum_velocity, const double acceleration,
   const double time);
 
-LanesPolygon createLanesPolygon(
-  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
-  const std::vector<lanelet::ConstLanelets> & target_backward_lanes);
+LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr);
+
+bool is_same_lane_with_prev_iteration(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes,
+  const lanelet::ConstLanelets & target_lanes);
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 
 namespace autoware::behavior_path_planner::utils::lane_change::debug

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
@@ -75,6 +75,7 @@ void LaneChangeInterface::updateData()
 {
   universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper());
   module_type_->setPreviousModuleOutput(getPreviousModuleOutput());
+  module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING);
   module_type_->updateSpecialData();
 
   if (isWaitingApproval() || module_type_->isAbortState()) {
@@ -136,7 +137,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
   *prev_approved_path_ = getPreviousModuleOutput().path;
 
   BehaviorModuleOutput out = getPreviousModuleOutput();
-  module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path);
+  module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path);
   out.turn_signal_info = module_type_->get_current_turn_signal_info();
 
   const auto & lane_change_debug = module_type_->getDebugData();