refactor

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

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp

@@ -136,7 +136,15 @@ class NormalLaneChange : public LaneChangeBase
 
   bool get_path_using_frenet(
     const std::vector<LaneChangePhaseMetrics> & prepare_metrics,
-    const lane_change::TargetObjects & target_objects, LaneChangePaths & candidate_paths) const;
+    const lane_change::TargetObjects & target_objects,
+    const std::vector<std::vector<int64_t>> & sorted_lane_ids,
+    LaneChangePaths & candidate_paths) const;
+
+  bool get_path_using_path_shifter(
+    const std::vector<LaneChangePhaseMetrics> & prepare_metrics,
+    const lane_change::TargetObjects & target_objects,
+    const std::vector<std::vector<int64_t>> & sorted_lane_ids,
+    LaneChangePaths & candidate_paths) const;
 
   bool check_candidate_path_safety(
     const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const;

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp

@@ -105,7 +105,8 @@ std::vector<lane_change::TrajectoryGroup> generate_frenet_candidates(
   const std::vector<LaneChangePhaseMetrics> & metrics);
 
 std::optional<LaneChangePath> get_candidate_path(
-  const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr);
+  const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr,
+  const std::vector<std::vector<int64_t>> & sorted_lane_ids);
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

@@ -73,8 +73,9 @@ void set_prepare_velocity(
   PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity);
 
 std::vector<int64_t> replaceWithSortedIds(
-  const std::vector<int64_t> & original_lane_ids,
-  const std::vector<std::vector<int64_t>> & sorted_lane_ids);
+  const std::vector<int64_t> & current_lane_ids,
+  const std::vector<std::vector<int64_t>> & sorted_lane_ids, std::vector<int64_t> & prev_lane_ids,
+  std::vector<int64_t> & prev_sorted_lane_ids);
 
 std::vector<std::vector<int64_t>> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr);
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1092,20 +1092,81 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths)
   }
 
   const auto & current_lanes = get_current_lanes();
-  const auto & target_lanes = get_target_lanes();
 
-  const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_);
   const auto target_objects = get_target_objects(filtered_objects_, current_lanes);
 
   const auto prepare_phase_metrics = get_prepare_metrics();
 
+  const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_);
   if (
     common_data_ptr_->lc_param_ptr->frenet.enable &&
     common_data_ptr_->transient_data.is_ego_near_current_terminal_start) {
-    return get_path_using_frenet(prepare_phase_metrics, target_objects, candidate_paths);
+    return get_path_using_frenet(
+      prepare_phase_metrics, target_objects, sorted_lane_ids, candidate_paths);
+  }
+
+  return get_path_using_path_shifter(
+    prepare_phase_metrics, target_objects, sorted_lane_ids, candidate_paths);
+}
+
+bool NormalLaneChange::get_path_using_frenet(
+  const std::vector<LaneChangePhaseMetrics> & prepare_metrics,
+  const lane_change::TargetObjects & target_objects,
+  const std::vector<std::vector<int64_t>> & sorted_lane_ids,
+  LaneChangePaths & candidate_paths) const
+{
+  stop_watch_.tic("frenet_candidates");
+  constexpr auto found_safe_path = true;
+  const auto frenet_candidates = utils::lane_change::generate_frenet_candidates(
+    common_data_ptr_, prev_module_output_.path, prepare_metrics);
+  RCLCPP_DEBUG(
+    logger_, "Generated %lu candidate paths in %2.2f[us]", frenet_candidates.size(),
+    stop_watch_.toc("frenet_candidates"));
+
+  for (const auto & frenet_candidate : frenet_candidates) {
+    std::optional<LaneChangePath> candidate_path_opt;
+    try {
+      candidate_path_opt =
+        utils::lane_change::get_candidate_path(frenet_candidate, common_data_ptr_, sorted_lane_ids);
+    } catch (const std::exception & e) {
+      RCLCPP_DEBUG(logger_, "%s", e.what());
+    }
+
+    if (!candidate_path_opt) {
+      continue;
+    }
+    try {
+      if (check_candidate_path_safety(*candidate_path_opt, target_objects)) {
+        RCLCPP_DEBUG(
+          logger_, "Found safe path after %lu candidate(s). Total time: %2.2f[us]",
+          frenet_candidates.size(), stop_watch_.toc("frenet_candidates"));
+        candidate_paths.push_back(*candidate_path_opt);
+        return found_safe_path;
+      }
+    } catch (const std::exception & e) {
+      RCLCPP_DEBUG(logger_, "%s", e.what());
+    }
+
+    if (candidate_paths.empty()) {
+      candidate_paths.push_back(*candidate_path_opt);
+    }
   }
+
+  RCLCPP_DEBUG(
+    logger_, "No safe path after %lu candidate(s). Total time: %2.2f[us]", frenet_candidates.size(),
+    stop_watch_.toc("frenet_candidates"));
+  return !found_safe_path;
+}
+
+bool NormalLaneChange::get_path_using_path_shifter(
+  const std::vector<LaneChangePhaseMetrics> & prepare_metrics,
+  const lane_change::TargetObjects & target_objects,
+  const std::vector<std::vector<int64_t>> & sorted_lane_ids,
+  LaneChangePaths & candidate_paths) const
+{
+  const auto & target_lanes = get_target_lanes();
   candidate_paths.reserve(
-    prepare_phase_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num);
+    prepare_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num);
 
   const bool only_tl = getStopTime() >= lane_change_parameters_->th_stop_time;
   const auto dist_to_next_regulatory_element =
@@ -1124,7 +1185,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths)
       return lc_diff > lane_change_parameters_->trajectory.th_lane_changing_length_diff;
     };
 
-  for (const auto & prep_metric : prepare_phase_metrics) {
+  for (const auto & prep_metric : prepare_metrics) {
     const auto debug_print = [&](const std::string & s) {
       RCLCPP_DEBUG(
         logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s.c_str(),
@@ -1199,53 +1260,6 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths)
   return false;
 }
 
-bool NormalLaneChange::get_path_using_frenet(
-  const std::vector<LaneChangePhaseMetrics> & prepare_metrics,
-  const lane_change::TargetObjects & target_objects, LaneChangePaths & candidate_paths) const
-{
-  stop_watch_.tic("frenet_candidates");
-  constexpr auto found_safe_path = true;
-  const auto frenet_candidates = utils::lane_change::generate_frenet_candidates(
-    common_data_ptr_, prev_module_output_.path, prepare_metrics);
-  RCLCPP_DEBUG(
-    logger_, "Generated %lu candidate paths in %2.2f[us]", frenet_candidates.size(),
-    stop_watch_.toc("frenet_candidates"));
-
-  for (const auto & frenet_candidate : frenet_candidates) {
-    std::optional<LaneChangePath> candidate_path_opt;
-    try {
-      candidate_path_opt =
-        utils::lane_change::get_candidate_path(frenet_candidate, common_data_ptr_);
-    } catch (const std::exception & e) {
-      RCLCPP_DEBUG(logger_, "%s", e.what());
-    }
-
-    if (!candidate_path_opt) {
-      continue;
-    }
-    try {
-      if (check_candidate_path_safety(*candidate_path_opt, target_objects)) {
-        RCLCPP_DEBUG(
-          logger_, "Found safe path after %lu candidate(s). Total time: %2.2f[us]",
-          frenet_candidates.size(), stop_watch_.toc("frenet_candidates"));
-        candidate_paths.push_back(*candidate_path_opt);
-        return found_safe_path;
-      }
-    } catch (const std::exception & e) {
-      RCLCPP_DEBUG(logger_, "%s", e.what());
-    }
-
-    if (candidate_paths.empty()) {
-      candidate_paths.push_back(*candidate_path_opt);
-    }
-  }
-
-  RCLCPP_DEBUG(
-    logger_, "No safe path after %lu candidate(s). Total time: %2.2f[us]", frenet_candidates.size(),
-    stop_watch_.toc("frenet_candidates"));
-  return !found_safe_path;
-}
-
 bool NormalLaneChange::check_candidate_path_safety(
   const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const
 {

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp

@@ -366,10 +366,14 @@ LaneChangePath construct_candidate_path(
     throw std::logic_error("Lane change end idx not found on target path.");
   }
 
+  std::vector<int64_t> prev_ids;
+  std::vector<int64_t> prev_sorted_ids;
   for (size_t i = 0; i < shifted_path.path.points.size(); ++i) {
     auto & point = shifted_path.path.points.at(i);
     if (i < *lc_end_idx_opt) {
-      point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids);
+      const auto & current_ids = point.lane_ids;
+      point.lane_ids =
+        replaceWithSortedIds(current_ids, sorted_lane_ids, prev_ids, prev_sorted_ids);
       point.point.longitudinal_velocity_mps = std::min(
         point.point.longitudinal_velocity_mps, static_cast<float>(terminal_lane_changing_velocity));
       continue;
@@ -501,40 +505,48 @@ std::vector<lane_change::TrajectoryGroup> generate_frenet_candidates(
 }
 
 std::optional<LaneChangePath> get_candidate_path(
-  const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr)
+  const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr,
+  const std::vector<std::vector<int64_t>> & sorted_lane_ids)
 {
   if (trajectory_group.lane_changing.frenet_points.empty()) {
     return std::nullopt;
   }
 
   ShiftedPath shifted_path;
-  PathPointWithLaneId pp;
+  std::vector<int64_t> prev_ids;
+  std::vector<int64_t> prev_sorted_ids;
   const auto & lane_changing_candidate = trajectory_group.lane_changing;
   const auto & target_lane_ref_path = trajectory_group.target_lane_ref_path;
   const auto & prepare_segment = trajectory_group.prepare;
   const auto & prepare_metric = trajectory_group.prepare_metric;
   const auto & initial_state = trajectory_group.initial_state;
   const auto & target_ref_sums = trajectory_group.target_lane_ref_path_dist;
-  for (auto i = 0UL; i < lane_changing_candidate.poses.size(); ++i) {
-    const auto s = lane_changing_candidate.frenet_points[i].s;
-    pp.point.pose = lane_changing_candidate.poses[i];
-    pp.point.longitudinal_velocity_mps =
-      static_cast<float>(lane_changing_candidate.longitudinal_velocities[i]);
-    pp.point.lateral_velocity_mps =
-      static_cast<float>(lane_changing_candidate.lateral_velocities[i]);
-    pp.point.heading_rate_rps = static_cast<float>(
-      lane_changing_candidate
-        .curvatures[i]);  // TODO(Maxime): dirty way to attach the curvature at each point
-    // copy from original reference path
+  auto zipped_candidates = ranges::views::zip(
+    lane_changing_candidate.poses, lane_changing_candidate.frenet_points,
+    lane_changing_candidate.longitudinal_velocities, lane_changing_candidate.lateral_velocities,
+    lane_changing_candidate.curvatures);
+
+  for (const auto & [pose, frenet_point, longitudinal_velocity, lateral_velocity, curvature] :
+       zipped_candidates) {
+    // Find the reference index
+    const auto & s = frenet_point.s;
     auto ref_i_itr = std::find_if(
       target_ref_sums.begin(), target_ref_sums.end(),
       [s](const double ref_s) { return ref_s > s; });
     auto ref_i = std::distance(target_ref_sums.begin(), ref_i_itr);
-    pp.point.pose.position.z = target_lane_ref_path.points[ref_i].point.pose.position.z;
-    pp.lane_ids = target_lane_ref_path.points[ref_i].lane_ids;
 
-    shifted_path.shift_length.push_back(lane_changing_candidate.frenet_points[i].d);
-    shifted_path.path.points.push_back(pp);
+    PathPointWithLaneId point;
+    point.point.pose = pose;
+    point.point.longitudinal_velocity_mps = static_cast<float>(longitudinal_velocity);
+    point.point.lateral_velocity_mps = static_cast<float>(lateral_velocity);
+    point.point.heading_rate_rps = static_cast<float>(curvature);
+    point.point.pose.position.z = target_lane_ref_path.points[ref_i].point.pose.position.z;
+    const auto & current_ids = target_lane_ref_path.points[ref_i].lane_ids;
+    point.lane_ids = replaceWithSortedIds(current_ids, sorted_lane_ids, prev_ids, prev_sorted_ids);
+
+    // Add to shifted path
+    shifted_path.shift_length.push_back(frenet_point.d);
+    shifted_path.path.points.push_back(point);
   }
 
   if (shifted_path.path.points.empty()) {
@@ -581,16 +593,13 @@ std::optional<LaneChangePath> get_candidate_path(
   ShiftLine sl;
 
   sl.start = lane_changing_candidate.poses.front();
-  // prepare_segment.points.back() .point.pose;  // TODO(Maxime): should it be 1st point of
-  // lane_changing_candidate ?
   sl.end = lane_changing_candidate.poses.back();
   sl.start_shift_length = 0.0;
   sl.end_shift_length = initial_state.position.d;
   sl.start_idx = 0UL;
   sl.end_idx = shifted_path.shift_length.size() - 1;
 
   info.shift_line = sl;
-
   info.terminal_lane_changing_velocity = lane_changing_candidate.longitudinal_velocities.back();
   info.lateral_acceleration = lane_changing_candidate.lateral_accelerations.front();
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp

@@ -455,17 +455,25 @@ std::vector<std::vector<int64_t>> get_sorted_lane_ids(const CommonDataPtr & comm
 }
 
 std::vector<int64_t> replaceWithSortedIds(
-  const std::vector<int64_t> & original_lane_ids,
-  const std::vector<std::vector<int64_t>> & sorted_lane_ids)
+  const std::vector<int64_t> & current_lane_ids,
+  const std::vector<std::vector<int64_t>> & sorted_lane_ids, std::vector<int64_t> & prev_lane_ids,
+  std::vector<int64_t> & prev_sorted_lane_ids)
 {
-  for (const auto original_id : original_lane_ids) {
+  if (current_lane_ids == prev_lane_ids) {
+    return prev_sorted_lane_ids;
+  }
+
+  for (const auto original_id : current_lane_ids) {
     for (const auto & sorted_id : sorted_lane_ids) {
       if (std::find(sorted_id.cbegin(), sorted_id.cend(), original_id) != sorted_id.cend()) {
-        return sorted_id;
+        prev_lane_ids = current_lane_ids;
+        prev_sorted_lane_ids = sorted_id;
+        return prev_sorted_lane_ids;
       }
     }
   }
-  return original_lane_ids;
+
+  return current_lane_ids;
 }
 
 CandidateOutput assignToCandidate(