refactor function names and create new functions

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
zulfaqar-azmi-t4 · May 24, 2024 · 86bfc17 · 86bfc17
1 parent 72b538f
commit 86bfc17
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 8 deletions.
diff --git a/...ehavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/helper.hpp b/...ehavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/helper.hpp
@@ -57,8 +57,10 @@ class Helper
   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;
+  Pose get_ego_pose() const;
+  double calc_signed_distance_from_current_pose(const lanelet::ConstLanelets & lanes) const;
+
+  [[nodiscard]] bool is_in_goal_route_section(const lanelet::ConstLanelets & lanes) const;
 
   lanelet::ConstLanelets current_lanes_;
   lanelet::ConstLanelets target_lanes_;

diff --git a/planning/behavior_path_lane_change_module/src/utils/helper.cpp b/planning/behavior_path_lane_change_module/src/utils/helper.cpp
@@ -93,7 +93,7 @@ bool Helper::is_near_end_of_current_lanes() const
 
   const auto min_lc_buffer = calc_min_lane_change_length(current_lanes_);
 
-  const auto remaining_distance = get_signed_distance_from_current_pose(current_lanes_);
+  const auto remaining_distance = calc_signed_distance_from_current_pose(current_lanes_);
 
   const auto threshold =
     common_params_.base_link2front + lc_params_->min_length_for_turn_signal_activation;
@@ -120,7 +120,7 @@ bool Helper::has_enough_length_to_retry(const double abort_return_dist, Directio
   const auto required_length =
     calc_min_lane_change_length(current_lanes_, direction) + abort_return_dist;
 
-  const auto remaining_dist = get_signed_distance_from_current_pose(current_lanes_);
+  const auto remaining_dist = calc_signed_distance_from_current_pose(current_lanes_);
 
   return required_length <= remaining_dist;
 }
@@ -130,18 +130,23 @@ Pose Helper::get_ego_pose() const
   return self_odometry_->pose.pose;
 }
 
-double Helper::get_signed_distance_from_current_pose(const lanelet::ConstLanelets & lanes) const
+double Helper::calc_signed_distance_from_current_pose(const lanelet::ConstLanelets & lanes) const
 {
   if (lanes.empty()) {
     return std::numeric_limits<double>::infinity();
   }
 
-  const auto is_in_goal_route_section = route_handler_->isInGoalRouteSection(lanes.back());
-
-  if (is_in_goal_route_section) {
+  if (is_in_goal_route_section(lanes)) {
     return utils::getSignedDistance(get_ego_pose(), route_handler_->getGoalPose(), current_lanes_);
   }
 
   return utils::getDistanceToEndOfLane(get_ego_pose(), current_lanes_);
 }
+
+bool Helper::is_in_goal_route_section(const lanelet::ConstLanelets & lanes) const
+{
+  return std::any_of(lanes.rbegin(), lanes.rend(), [&](const auto & lane) {
+    return route_handler_->isInGoalRouteSection(lane);
+  });
+}
 }  // namespace behavior_path_planner::helper::lane_change