From 881afe1891071d91db213f89eb36f49f4b59ea32 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 27 Dec 2024 11:02:36 +0900 Subject: [PATCH] refactor Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 10 +- .../utils/path.hpp | 3 +- .../utils/utils.hpp | 5 +- .../src/scene.cpp | 118 ++++++++++-------- .../src/utils/path.cpp | 51 ++++---- .../src/utils/utils.cpp | 18 ++- 6 files changed, 123 insertions(+), 82 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index c0e66d44ea3e0..065205872e48f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/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 & prepare_metrics, - const lane_change::TargetObjects & target_objects, LaneChangePaths & candidate_paths) const; + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const; + + bool get_path_using_path_shifter( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const; bool check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp index d34905b73f5c8..d8eebda57bbbb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp +++ b/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 generate_frenet_candidates( const std::vector & metrics); std::optional get_candidate_path( - const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr); + const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr, + const std::vector> & sorted_lane_ids); } // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 1691120623d7a..48658cf2d731d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/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 replaceWithSortedIds( - const std::vector & original_lane_ids, - const std::vector> & sorted_lane_ids); + const std::vector & current_lane_ids, + const std::vector> & sorted_lane_ids, std::vector & prev_lane_ids, + std::vector & prev_sorted_lane_ids); std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 5604db0ffe7f7..c99c37f45915f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/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 & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & 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 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 & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & 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 & 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 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 { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp index 5dc893c1edcad..aad351f3486fd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp +++ b/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 prev_ids; + std::vector 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(terminal_lane_changing_velocity)); continue; @@ -501,40 +505,48 @@ std::vector generate_frenet_candidates( } std::optional get_candidate_path( - const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr) + const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr, + const std::vector> & sorted_lane_ids) { if (trajectory_group.lane_changing.frenet_points.empty()) { return std::nullopt; } ShiftedPath shifted_path; - PathPointWithLaneId pp; + std::vector prev_ids; + std::vector 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(lane_changing_candidate.longitudinal_velocities[i]); - pp.point.lateral_velocity_mps = - static_cast(lane_changing_candidate.lateral_velocities[i]); - pp.point.heading_rate_rps = static_cast( - 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(longitudinal_velocity); + point.point.lateral_velocity_mps = static_cast(lateral_velocity); + point.point.heading_rate_rps = static_cast(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,8 +593,6 @@ std::optional 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; @@ -590,7 +600,6 @@ std::optional get_candidate_path( 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(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index c21374fd90a5b..705ccc41ad2b4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -455,17 +455,25 @@ std::vector> get_sorted_lane_ids(const CommonDataPtr & comm } std::vector replaceWithSortedIds( - const std::vector & original_lane_ids, - const std::vector> & sorted_lane_ids) + const std::vector & current_lane_ids, + const std::vector> & sorted_lane_ids, std::vector & prev_lane_ids, + std::vector & 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(