From 4cae773e53806051bfbaf25a4ec37a590503f902 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 24 Dec 2024 18:49:29 +0900 Subject: [PATCH] frenet planner Signed-off-by: Zulfaqar Azmi --- .../structs/data.hpp | 1 + .../structs/path.hpp | 34 +- .../utils/path.hpp | 12 +- .../utils/utils.hpp | 13 + .../package.xml | 1 + .../src/scene.cpp | 191 ++++++----- .../src/utils/path.cpp | 306 +++++++++++++++++- .../src/utils/utils.cpp | 145 ++++++++- .../src/frenet_planner/frenet_planner.cpp | 9 + 9 files changed, 630 insertions(+), 82 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp index 5f36be806bba4..0ec2557292fee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp @@ -250,6 +250,7 @@ struct CommonData LanesPolygonPtr lanes_polygon_ptr; TransientData transient_data; PathWithLaneId current_lanes_path; + PathWithLaneId target_lanes_path; ModuleType lc_type; Direction direction; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp index 0fa0a9d977a26..9b1244f38d5e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp @@ -19,19 +19,50 @@ #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include + #include +#include #include namespace autoware::behavior_path_planner::lane_change { +enum class PathType { ConstantJerk = 0, FrenetPlanner }; + using autoware::behavior_path_planner::TurnSignalInfo; using tier4_planning_msgs::msg::PathWithLaneId; +struct TrajectoryGroup +{ + PathWithLaneId prepare; + PathWithLaneId target_lane_ref_path; + std::vector target_lane_ref_path_dist; + LaneChangePhaseMetrics prepare_metric; + frenet_planner::Trajectory lane_changing; + frenet_planner::FrenetState initial_state; + + TrajectoryGroup() = default; + TrajectoryGroup( + PathWithLaneId prepare, PathWithLaneId target_lane_ref_path, + std::vector target_lane_ref_path_dist, LaneChangePhaseMetrics prepare_metric, + frenet_planner::Trajectory lane_changing, frenet_planner::FrenetState initial_state) + : prepare(std::move(prepare)), + target_lane_ref_path(std::move(target_lane_ref_path)), + target_lane_ref_path_dist(std::move(target_lane_ref_path_dist)), + prepare_metric(prepare_metric), + lane_changing(std::move(lane_changing)), + initial_state(initial_state) + { + } +}; + struct Path { + Info info; PathWithLaneId path; ShiftedPath shifted_path; - Info info; + TrajectoryGroup frenet_path; + PathType type = PathType::ConstantJerk; }; struct Status @@ -39,7 +70,6 @@ struct Status 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 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 dcc327b4793e1..935b253e762f5 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 @@ -12,9 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ - #include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include "autoware/behavior_path_lane_change_module/structs/path.hpp" @@ -26,6 +23,7 @@ namespace autoware::behavior_path_planner::utils::lane_change { using behavior_path_planner::LaneChangePath; using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::TrajectoryGroup; /** * @brief Generates a prepare segment for a lane change maneuver. @@ -98,5 +96,11 @@ LaneChangePath construct_candidate_path( const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids); + +/// @brief generate lane change candidate paths using the Frenet planner +std::vector generate_frenet_candidates( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const std::vector & metrics); + +std::optional get_candidate_path(const TrajectoryGroup & trajectory_group); } // 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 69362994dbbac..1691120623d7a 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 @@ -24,6 +24,8 @@ #include #include +#include +#include #include #include @@ -56,6 +58,7 @@ using behavior_path_planner::lane_change::LCParamPtr; using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; using behavior_path_planner::lane_change::TargetLaneLeadingObjects; +using behavior_path_planner::lane_change::TrajectoryGroup; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -83,6 +86,10 @@ bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin = 0.1); +void filter_out_of_bound_trajectories( + const CommonDataPtr & common_data_ptr, + std::vector & trajectory_groups); + std::vector generateDrivableLanes( const std::vector & original_drivable_lanes, const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes); @@ -432,5 +439,11 @@ std::vector> convert_to_predicted_paths( * @return true if the pose is within the target or target neighbor polygons, false otherwise. */ bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose); + +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const lane_change::TrajectoryGroup & frenet_candidate, + [[maybe_unused]] const size_t deceleration_sampling_num); + +double calc_limit(const CommonDataPtr & common_data_ptr, const Pose & lc_end_pose); } // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index dd4dbe63e41d4..6de5be913f374 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -23,6 +23,7 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_frenet_planner autoware_motion_utils autoware_rtc_interface autoware_universe_utils 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 f8a200a9685a7..98ea60f8f70d6 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 @@ -14,6 +14,13 @@ #include "autoware/behavior_path_lane_change_module/scene.hpp" +#include + +#include +#include +// for the geometry types +#include +// for the svg mapper #include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" #include "autoware/behavior_path_lane_change_module/utils/path.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" @@ -28,7 +35,10 @@ #include #include #include +#include #include +#include +#include #include #include #include @@ -38,8 +48,10 @@ #include #include +#include #include +#include #include #include #include @@ -103,6 +115,9 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->current_lanes_path = route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + common_data_ptr_->target_lanes_path = + route_handler_ptr->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::get_target_neighbor_lanes( *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); @@ -228,8 +243,6 @@ void NormalLaneChange::updateLaneChangeStatus() // Update status status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - - status_.start_distance = common_data_ptr_->transient_data.target_lanes_ego_arc.length; status_.lane_change_path.path.header = getRouteHeader(); } @@ -1091,99 +1104,133 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto prepare_phase_metrics = get_prepare_metrics(); - candidate_paths.reserve( - prepare_phase_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 = - utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); - - auto check_length_diff = - [&](const double prep_length, const double lc_length, const bool check_lc) { - if (candidate_paths.empty()) return true; - - const auto prep_diff = std::abs(candidate_paths.back().info.length.prepare - prep_length); - if (prep_diff > lane_change_parameters_->trajectory.th_prepare_length_diff) return true; - - if (!check_lc) return false; - - const auto lc_diff = std::abs(candidate_paths.back().info.length.lane_changing - lc_length); - return lc_diff > lane_change_parameters_->trajectory.th_lane_changing_length_diff; - }; - - for (const auto & prep_metric : prepare_phase_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(), - prep_metric.duration, prep_metric.actual_lon_accel, prep_metric.length); - }; - - if (!check_length_diff(prep_metric.length, 0.0, false)) { - RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold."); - continue; - } + if (common_data_ptr_->transient_data.is_ego_near_current_terminal_start) { + universe_utils::StopWatch sw; + const auto frenet_candidates = utils::lane_change::generate_frenet_candidates( + common_data_ptr_, prev_module_output_.path, prepare_phase_metrics); + std::printf("Generated %lu candidate paths in %2.2fus\n", frenet_candidates.size(), sw.toc()); + for (const auto & frenet_candidate : frenet_candidates) { + std::optional candidate_path_opt; + try { + candidate_path_opt = utils::lane_change::get_candidate_path(frenet_candidate); + } catch (const std::exception & e) { + RCLCPP_DEBUG(logger_, "%s", e.what()); + } - PathWithLaneId prepare_segment; - try { - if (!utils::lane_change::get_prepare_segment( - common_data_ptr_, prev_module_output_.path, prep_metric, prepare_segment)) { - debug_print("Reject: failed to get valid prepare segment!"); + if (!candidate_path_opt) { continue; } - } catch (const std::exception & e) { - debug_print(e.what()); - break; + if (candidate_paths.empty()) { + candidate_paths.push_back(*candidate_path_opt); + } + try { + if (check_candidate_path_safety(*candidate_path_opt, target_objects)) { + std::printf( + "(FOUND) Search time: %2.2fus (%lu candidates)\n", sw.toc(), candidate_paths.size()); + candidate_paths.push_back(*candidate_path_opt); + return true; + } + } catch (const std::exception & e) { + std::printf( + "(%s) Search time: %2.2fus (%lu candidates)\n", e.what(), sw.toc(), + candidate_paths.size()); + // return false; + } } + std::printf( + "(NO SAFE PATH) Search time: %2.2fus (%lu candidates)\n", sw.toc(), candidate_paths.size()); + } else { + candidate_paths.reserve( + prepare_phase_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); - debug_print("Prepare path satisfy constraints"); + const bool only_tl = getStopTime() >= lane_change_parameters_->th_stop_time; + const auto dist_to_next_regulatory_element = + utils::lane_change::get_distance_to_next_regulatory_element( + common_data_ptr_, only_tl, only_tl); - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + auto check_length_diff = + [&](const double prep_length, const double lc_length, const bool check_lc) { + if (candidate_paths.empty()) return true; - const auto shift_length = - lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + const auto prep_diff = std::abs(candidate_paths.back().info.length.prepare - prep_length); + if (prep_diff > lane_change_parameters_->trajectory.th_prepare_length_diff) return true; - const auto lane_changing_metrics = get_lane_changing_metrics( - prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element); + if (!check_lc) return false; - // set_prepare_velocity must only be called after computing lane change metrics, as lane change - // metrics rely on the prepare segment's original velocity as max_path_velocity. - utils::lane_change::set_prepare_velocity( - prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity); + const auto lc_diff = std::abs(candidate_paths.back().info.length.lane_changing - lc_length); + return lc_diff > lane_change_parameters_->trajectory.th_lane_changing_length_diff; + }; - for (const auto & lc_metric : lane_changing_metrics) { - const auto debug_print_lat = [&](const std::string & s) { + for (const auto & prep_metric : prepare_phase_metrics) { + const auto debug_print = [&](const std::string & s) { RCLCPP_DEBUG( - logger_, "%s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s.c_str(), - lc_metric.duration, lc_metric.actual_lon_accel, lc_metric.lat_accel, lc_metric.length); + logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s.c_str(), + prep_metric.duration, prep_metric.actual_lon_accel, prep_metric.length); }; - if (!check_length_diff(prep_metric.length, lc_metric.length, true)) { - RCLCPP_DEBUG(logger_, "Skip: Change in lane changing length is less than threshold."); + if (!check_length_diff(prep_metric.length, 0.0, false)) { + RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold."); continue; } - LaneChangePath candidate_path; + PathWithLaneId prepare_segment; try { - candidate_path = utils::lane_change::get_candidate_path( - common_data_ptr_, prep_metric, lc_metric, prepare_segment, sorted_lane_ids, shift_length); + if (!utils::lane_change::get_prepare_segment( + common_data_ptr_, prev_module_output_.path, prep_metric, prepare_segment)) { + debug_print("Reject: failed to get valid prepare segment!"); + continue; + } } catch (const std::exception & e) { - debug_print_lat(std::string("Reject: ") + e.what()); - continue; + debug_print(e.what()); + break; } - candidate_paths.push_back(candidate_path); + debug_print("Prepare path satisfy constraints"); - try { - if (check_candidate_path_safety(candidate_path, target_objects)) { - debug_print_lat("ACCEPT!!!: it is valid and safe!"); - return true; + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + + const auto lane_changing_metrics = get_lane_changing_metrics( + prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element); + + for (const auto & lc_metric : lane_changing_metrics) { + const auto debug_print_lat = [&](const std::string & s) { + RCLCPP_DEBUG( + logger_, "%s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s.c_str(), + lc_metric.duration, lc_metric.actual_lon_accel, lc_metric.lat_accel, lc_metric.length); + }; + + if (!check_length_diff(prep_metric.length, lc_metric.length, true)) { + RCLCPP_DEBUG(logger_, "Skip: Change in lane changing length is less than threshold."); + continue; } - } catch (const std::exception & e) { - debug_print_lat(std::string("Reject: ") + e.what()); - return false; - } - debug_print_lat("Reject: sampled path is not safe."); + LaneChangePath candidate_path; + try { + candidate_path = utils::lane_change::get_candidate_path( + common_data_ptr_, prep_metric, lc_metric, prepare_segment, sorted_lane_ids, + shift_length); + } catch (const std::exception & e) { + debug_print_lat(std::string("Reject: ") + e.what()); + continue; + } + + candidate_paths.push_back(candidate_path); + + try { + if (check_candidate_path_safety(candidate_path, target_objects)) { + debug_print_lat("ACCEPT!!!: it is valid and safe!"); + return true; + } + } catch (const std::exception & e) { + debug_print_lat(std::string("Reject: ") + e.what()); + return false; + } + + debug_print_lat("Reject: sampled path is not safe."); + } } } 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 d7303d7d1df2d..7b811bfa242c4 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 @@ -15,12 +15,14 @@ #include "autoware/behavior_path_lane_change_module/utils/path.hpp" #include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include #include +#include #include #include #include @@ -42,6 +44,10 @@ using autoware::behavior_path_planner::lane_change::LCParamPtr; using autoware::behavior_path_planner::LaneChangePhaseMetrics; using autoware::behavior_path_planner::ShiftLine; +using autoware::frenet_planner::FrenetState; +using autoware::frenet_planner::SamplingParameters; +using autoware::sampler_common::FrenetPoint; +using autoware::sampler_common::transform::Spline2D; using geometry_msgs::msg::Pose; double calc_resample_interval( @@ -53,7 +59,7 @@ double calc_resample_interval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } -PathWithLaneId get_reference_path_from_target_Lane( +PathWithLaneId get_reference_path_from_target_lane( const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, const double lane_changing_length, const double resample_interval) { @@ -146,11 +152,83 @@ std::optional exceed_yaw_threshold( } return std::nullopt; } + +Spline2D init_reference_spline(const std::vector & target_lanes_ref_path) +{ + std::vector xs; + std::vector ys; + xs.reserve(target_lanes_ref_path.size()); + ys.reserve(target_lanes_ref_path.size()); + for (const auto & p : target_lanes_ref_path) { + xs.push_back(p.point.pose.position.x); + ys.push_back(p.point.pose.position.y); + } + + return {xs, ys}; +} + +FrenetState init_frenet_state( + const FrenetPoint & start_position, const LaneChangePhaseMetrics & prepare_metrics) +{ + FrenetState initial_state; + initial_state.position = start_position; + initial_state.longitudinal_velocity = prepare_metrics.velocity; + initial_state.lateral_velocity = + 0.0; // TODO(Maxime): this can be sampled if we want but it would impact the LC duration + initial_state.longitudinal_acceleration = prepare_metrics.sampled_lon_accel; + initial_state.lateral_acceleration = prepare_metrics.lat_accel; + std::printf( + "\tInitial state [s=%2.2f, d=%2.2f, s'=%2.2f, d'=%2.2f, s''=%2.2f, d''=%2.2f], ", + initial_state.position.s, initial_state.position.d, initial_state.longitudinal_velocity, + initial_state.lateral_velocity, initial_state.longitudinal_acceleration, + initial_state.lateral_acceleration); + return initial_state; +} + +std::optional init_sampling_parameters( + const LCParamPtr & lc_param_ptr, const LaneChangePhaseMetrics & prepare_metrics, + const FrenetState & initial_state, const Spline2D & ref_spline, const Pose & lc_start_pose) +{ + const auto min_lc_vel = lc_param_ptr->trajectory.min_lane_changing_velocity; + const auto [min_lateral_acc, max_lateral_acc] = + lc_param_ptr->trajectory.lat_acc_map.find(std::max(min_lc_vel, prepare_metrics.velocity)); + const auto duration = autoware::motion_utils::calc_shift_time_from_jerk( + std::abs(initial_state.position.d), lc_param_ptr->trajectory.lateral_jerk, max_lateral_acc); + const auto final_velocity = + std::max(min_lc_vel, prepare_metrics.velocity + prepare_metrics.sampled_lon_accel * duration); + const auto lc_length = duration * (prepare_metrics.velocity + final_velocity) * 0.5; + const auto target_s = initial_state.position.s + lc_length; + const auto initial_yaw = tf2::getYaw(lc_start_pose.orientation); + const auto target_lat_vel = + (1 - ref_spline.curvature(target_s + 1e-3) * initial_state.position.d) * + std::tan(initial_yaw - ref_spline.yaw(target_s)); + + if (std::isnan(target_lat_vel)) { + std::cout << " Skipped (invalid target lateral velocity)\n"; + return std::nullopt; + } + + SamplingParameters sampling_parameters; + sampling_parameters.resolution = lc_param_ptr->safety.collision_check.prediction_time_resolution; + sampling_parameters.parameters.emplace_back(); + sampling_parameters.parameters.back().target_duration = duration; + sampling_parameters.parameters.back().target_state.position = {target_s, 0.0}; + // TODO(Maxime): not sure if we should use curvature at initial or target s + sampling_parameters.parameters.back().target_state.lateral_velocity = target_lat_vel; + sampling_parameters.parameters.back().target_state.lateral_acceleration = 0.0; + sampling_parameters.parameters.back().target_state.longitudinal_velocity = final_velocity; + sampling_parameters.parameters.back().target_state.longitudinal_acceleration = + prepare_metrics.sampled_lon_accel; + std::cout << " Target : " << sampling_parameters.parameters.back() << "\n"; + return sampling_parameters; +} + }; // namespace namespace autoware::behavior_path_planner::utils::lane_change { using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::PathType; bool get_prepare_segment( const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, @@ -205,7 +283,7 @@ LaneChangePath get_candidate_path( } const auto & lc_start_pose = prep_segment.points.back().point.pose; - const auto target_lane_reference_path = get_reference_path_from_target_Lane( + const auto target_lane_reference_path = get_reference_path_from_target_lane( common_data_ptr, lc_start_pose, lc_metric.length, resample_interval); if (target_lane_reference_path.points.empty()) { @@ -286,6 +364,230 @@ LaneChangePath construct_candidate_path( candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); candidate_path.shifted_path = shifted_path; candidate_path.info = lane_change_info; + candidate_path.type = PathType::ConstantJerk; + + return candidate_path; +} + +std::vector generate_frenet_candidates( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const std::vector & metrics) +{ + std::vector trajectory_groups; + universe_utils::StopWatch sw; + double prepare_segment_us = 0.0; + double ref_spline_us = 0.0; + double gen_us = 0.0; + double ref_path_us = 0.0; + + const auto & transient_data = common_data_ptr->transient_data; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + + trajectory_groups.reserve(metrics.size()); + for (const auto & metric : metrics) { + PathWithLaneId prepare_segment; + try { + sw.tic("prepare_segment"); + if (!utils::lane_change::get_prepare_segment( + common_data_ptr, prev_module_path, metric, prepare_segment)) { + RCLCPP_DEBUG(get_logger(), "Reject: failed to get valid prepare segment!"); + continue; + } + prepare_segment_us += sw.toc("prepare_segment"); + } catch (const std::exception & e) { + RCLCPP_WARN(get_logger(), "%s", e.what()); + break; + } + const auto lc_start_pose = prepare_segment.points.back().point.pose; + sw.tic("ref_path"); // TODO(Maxime): this is the most time consuming step. We can probably only + // do it once + + const auto dist_to_end_from_lc_start = + calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr, target_lanes, lc_start_pose) - + common_data_ptr->lc_param_ptr->lane_change_finish_judge_buffer; + const auto max_lc_len = transient_data.lane_changing_length.max; + + const auto target_lane_reference_path = get_reference_path_from_target_lane( + common_data_ptr, lc_start_pose, std::min(dist_to_end_from_lc_start, max_lc_len), 0.5); + if (target_lane_reference_path.points.empty()) { + continue; + } + + std::vector target_ref_path_sums{0.0}; + target_ref_path_sums.reserve(target_lane_reference_path.points.size() - 1); + double ref_s = 0.0; + for (auto it = target_lane_reference_path.points.begin(); + std::next(it) != target_lane_reference_path.points.end(); ++it) { + ref_s += universe_utils::calcDistance2d(*it, *std::next(it)); + target_ref_path_sums.push_back(ref_s); + } + + ref_path_us += sw.toc("ref_path"); + + sw.tic("ref_spline"); + const auto reference_spline = init_reference_spline(target_lane_reference_path.points); + ref_spline_us += sw.toc("ref_spline"); + + const auto initial_state = init_frenet_state( + reference_spline.frenet({lc_start_pose.position.x, lc_start_pose.position.y}), metric); + + const auto sampling_parameters_opt = init_sampling_parameters( + common_data_ptr->lc_param_ptr, metric, initial_state, reference_spline, lc_start_pose); + + if (!sampling_parameters_opt) { + continue; + } + + sw.tic("gen"); + auto frenet_trajectories = frenet_planner::generateTrajectories( + reference_spline, initial_state, *sampling_parameters_opt); + gen_us += sw.toc("gen"); + + ranges::for_each(frenet_trajectories, [&](const auto & frenet_trajectory) { + trajectory_groups.emplace_back( + prepare_segment, target_lane_reference_path, target_ref_path_sums, metric, + frenet_trajectory, initial_state); + }); + } + + const auto avg_curvature = [](const std::vector & curvatures) { + const auto filter_zeros = [](const auto & k) { return k != 0.0; }; + const auto sums_of_curvatures = [](float sum, const double k) { return sum + std::abs(k); }; + auto filtered_k = curvatures | ranges::views::filter(filter_zeros); + const auto sum_of_k = ranges::accumulate(filtered_k, 0.0, sums_of_curvatures); + const auto count_k = static_cast(ranges::distance(filtered_k)); + return sum_of_k / count_k; + }; + + const auto limit_vel = [&](TrajectoryGroup & group) { + const auto max_vel = + utils::lane_change::calc_limit(common_data_ptr, group.lane_changing.poses.back()); + for (auto & vel : group.lane_changing.longitudinal_velocities) { + vel = std::clamp(vel, 0.0, max_vel); + } + }; + + ranges::for_each(trajectory_groups, limit_vel); + + ranges::sort(trajectory_groups, [&](const auto & p1, const auto & p2) { + return avg_curvature(p1.lane_changing.curvatures) < avg_curvature(p2.lane_changing.curvatures); + }); + + utils::lane_change::filter_out_of_bound_trajectories(common_data_ptr, trajectory_groups); + + std::printf( + "\tGenerated %lu candidates | prepare_segment %2.2fus, reference spline %2.2fus, frenet " + "generation %2.2fus, reference path %2.2fus\n", + trajectory_groups.size(), prepare_segment_us, ref_spline_us, gen_us, ref_path_us); + return trajectory_groups; +} + +std::optional get_candidate_path(const TrajectoryGroup & trajectory_group) +{ + if (trajectory_group.lane_changing.frenet_points.empty()) { + return std::nullopt; + } + + ShiftedPath shifted_path; + PathPointWithLaneId pp; + 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 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); + } + + if (shifted_path.path.points.empty()) { + return std::nullopt; + } + + const auto nearest_segment_idx = autoware::motion_utils::findNearestSegmentIndex( + target_lane_ref_path.points, shifted_path.path.points.back().point.pose.position); + for (auto i = nearest_segment_idx + 2; i < target_lane_ref_path.points.size(); ++i) { + shifted_path.path.points.push_back(target_lane_ref_path.points[i]); + shifted_path.shift_length.push_back(0.0); + } + + const auto lane_change_end_idx = autoware::motion_utils::findNearestIndex( + shifted_path.path.points, shifted_path.path.points.back().point.pose); + for (size_t i = 0; i < *lane_change_end_idx; ++i) { + auto & point = shifted_path.path.points.at(i); + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + static_cast(shifted_path.path.points.back().point.longitudinal_velocity_mps)); + } + + constexpr auto yaw_diff_th = autoware::universe_utils::deg2rad(5.0); + if ( + const auto yaw_diff_opt = + exceed_yaw_threshold(prepare_segment, shifted_path.path, yaw_diff_th)) { + std::stringstream err_msg; + err_msg << "Excessive yaw difference " << yaw_diff_opt.value() << " which exceeds the " + << yaw_diff_th << " radian threshold."; + throw std::logic_error(err_msg.str()); + } + + LaneChangeInfo info; + info.longitudinal_acceleration = { + prepare_metric.actual_lon_accel, lane_changing_candidate.longitudinal_accelerations.front()}; + info.velocity = {prepare_metric.velocity, initial_state.longitudinal_velocity}; + info.duration = { + prepare_metric.duration, lane_changing_candidate.sampling_parameter.target_duration}; + info.length = {prepare_metric.length, lane_changing_candidate.lengths.back()}; + + std::printf( + "p: l=%2.2f, t=%2.2f, v=%2.2f, a=%2.2f | lc: l=%2.2f, t=%2.2f, v=%2.2f, a=%2.2f", + info.length.prepare, info.duration.prepare, info.velocity.prepare, + info.longitudinal_acceleration.prepare, info.length.lane_changing, info.duration.lane_changing, + info.velocity.lane_changing, info.longitudinal_acceleration.lane_changing); + + info.lane_changing_start = prepare_segment.points.back().point.pose; + info.lane_changing_end = lane_changing_candidate.poses.back(); + + 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(); + + LaneChangePath candidate_path; + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); + candidate_path.info = info; + candidate_path.shifted_path = shifted_path; + candidate_path.frenet_path = trajectory_group; + candidate_path.type = PathType::FrenetPlanner; return candidate_path; } 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 147ef3856ac4e..c21374fd90a5b 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 @@ -24,12 +24,18 @@ #include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +// for the geometry types +#include +#include +// for the svg mapper #include #include #include #include #include #include +#include +#include #include #include #include @@ -40,9 +46,10 @@ #include #include -#include - +#include #include +#include +#include #include #include @@ -64,9 +71,11 @@ namespace autoware::behavior_path_planner::utils::lane_change { using autoware::route_handler::RouteHandler; using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::lane_change::PathType; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; @@ -153,6 +162,69 @@ bool path_footprint_exceeds_target_lane_bound( return false; } +void filter_out_of_bound_trajectories( + const CommonDataPtr & common_data_ptr, + std::vector & trajectory_groups) +{ + const auto lane_boundary = std::invoke([&]() { + universe_utils::LineString2d line_string; + const auto & get_bound = [&](const lanelet::ConstLanelet & lane) { + const auto direction = common_data_ptr->direction; + return (direction == Direction::LEFT) ? lane.leftBound2d() : lane.rightBound2d(); + }; + + const auto & lanes = common_data_ptr->lanes_ptr->current; + + const auto reserve_size = ranges::accumulate( + lanes, 0UL, + [&](auto sum, const lanelet::ConstLanelet & lane) { return sum + get_bound(lane).size(); }); + line_string.reserve(reserve_size); + for (const auto & lane : lanes) { + const auto & bound = + (common_data_ptr->direction == Direction::LEFT) ? lane.leftBound2d() : lane.rightBound2d(); + ranges::for_each(get_bound(lane), [&line_string](const auto & point) { + boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); + }); + } + return line_string; + }); + + const auto shift_point = [&](const Point2d & p1, const Point2d & p2) { + const auto direction = common_data_ptr->direction; + const auto left_side = direction == Direction::LEFT; + const auto distance = (0.5 * common_data_ptr->bpp_param_ptr->vehicle_width + 0.1); + const auto offset = (left_side ? 1.0 : -1.0) * distance; // invert direction + // Calculate the perpendicular vector + double dx = p2.x() - p1.x(); + double dy = p2.y() - p1.y(); + double length = std::sqrt(dx * dx + dy * dy); + + // Normalize and find the perpendicular direction + double nx = -dy / length; + double ny = dx / length; + + return Point2d(p1.x() + nx * offset, p1.y() + ny * offset); + }; + + trajectory_groups |= ranges::actions::remove_if([&](const TrajectoryGroup & candidate) { + if (candidate.lane_changing.poses.size() <= 2) { + return true; // Remove candidates with insufficient poses + } + + universe_utils::LineString2d path_ls; + path_ls.reserve(candidate.lane_changing.poses.size()); + + const auto segments = candidate.lane_changing.poses | ranges::views::sliding(2); + ranges::for_each(segments | ranges::views::drop(1), [&](const auto & segment) { + const auto & p1 = segment[0].position; + const auto & p2 = segment[1].position; + boost::geometry::append(path_ls, shift_point({p2.x, p2.y}, {p1.x, p1.y})); + }); + + return boost::geometry::disjoint(path_ls, lane_boundary); // Remove if disjoint + }); +} + std::vector generateDrivableLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes) @@ -475,6 +547,7 @@ std::vector convert_to_predicted_path( 0.5 * lane_changing_acceleration * delta_t * delta_t + offset; const auto pose = autoware::motion_utils::calcInterpolatedPose( path.points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); } @@ -1129,6 +1202,10 @@ std::vector> convert_to_predicted_paths( const auto acc_resolution = (min_acc - lane_changing_acc) / static_cast(sampling_num); const auto ego_predicted_path = [&](size_t n) { + if (lane_change_path.type == PathType::FrenetPlanner) { + return convert_to_predicted_path( + common_data_ptr, lane_change_path.frenet_path, deceleration_sampling_num); + } auto acc = lane_changing_acc + static_cast(n) * acc_resolution; return utils::lane_change::convert_to_predicted_path(common_data_ptr, lane_change_path, acc); }; @@ -1137,6 +1214,48 @@ std::vector> convert_to_predicted_paths( ranges::to(); } +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const lane_change::TrajectoryGroup & frenet_candidate, + [[maybe_unused]] const size_t deceleration_sampling_num) +{ + const auto initial_velocity = common_data_ptr->get_ego_speed(); + const auto prepare_time = frenet_candidate.prepare_metric.duration; + const auto resolution = + common_data_ptr->lc_param_ptr->safety.collision_check.prediction_time_resolution; + const auto prepare_acc = frenet_candidate.prepare_metric.sampled_lon_accel; + std::vector predicted_path; + const auto & path = frenet_candidate.prepare.points; + const auto & vehicle_pose = common_data_ptr->get_ego_pose(); + const auto & bpp_param_ptr = common_data_ptr->bpp_param_ptr; + const auto nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path, vehicle_pose, bpp_param_ptr->ego_nearest_dist_threshold, + bpp_param_ptr->ego_nearest_yaw_threshold); + + const auto vehicle_pose_frenet = + convertToFrenetPoint(path, vehicle_pose.position, nearest_seg_idx); + + for (double t = 0.0; t < prepare_time; t += resolution) { + const auto velocity = + std::clamp(initial_velocity + prepare_acc * t, 0.0, frenet_candidate.prepare_metric.velocity); + const auto length = initial_velocity * t + 0.5 * prepare_acc * t * t; + const auto pose = + autoware::motion_utils::calcInterpolatedPose(path, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + const auto & poses = frenet_candidate.lane_changing.poses; + const auto & velocities = frenet_candidate.lane_changing.longitudinal_velocities; + const auto & times = frenet_candidate.lane_changing.times; + + for (const auto [t, pose, velocity] : + ranges::views::zip(times, poses, velocities) | ranges::views::drop(1)) { + predicted_path.emplace_back(prepare_time + t, pose, velocity); + } + + return predicted_path; +} + bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) { const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y); @@ -1148,4 +1267,26 @@ bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, co return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) || boost::geometry::covered_by(lc_start_point, target_lane_poly); } + +double calc_limit(const CommonDataPtr & common_data_ptr, const Pose & lc_end_pose) +{ + const auto dist_to_target_end = std::invoke([&]() { + if (common_data_ptr->lanes_ptr->target_lane_in_goal_section) { + return motion_utils::calcSignedArcLength( + common_data_ptr->target_lanes_path.points, lc_end_pose.position, + common_data_ptr->route_handler_ptr->getGoalPose().position); + } + return motion_utils::calcSignedArcLength( + common_data_ptr->target_lanes_path.points, lc_end_pose.position, + common_data_ptr->target_lanes_path.points.back().point.pose.position); + }); + + // v2 = u2 + 2ad + // u = sqrt(2ad) + return std::clamp( + std::sqrt( + std::abs(2.0 * common_data_ptr->bpp_param_ptr->min_acc * std::max(dist_to_target_end, 0.0))), + common_data_ptr->lc_param_ptr->trajectory.min_lane_changing_velocity, + common_data_ptr->bpp_param_ptr->max_vel); +} } // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index cab4acfbb8556..54d4bc2e90714 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -197,6 +197,15 @@ void calculateCartesian( trajectory.longitudinal_accelerations.push_back(0.0); trajectory.lateral_accelerations.push_back(0.0); } + for (auto i = 0UL; i < trajectory.points.size(); ++i) { + geometry_msgs::msg::Pose pose; + pose.position.x = trajectory.points[i].x(); + pose.position.y = trajectory.points[i].y(); + pose.position.z = 0.0; + pose.orientation = + autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, trajectory.yaws[i]); + trajectory.poses.push_back(pose); + } } }