diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp index 90b13f86976b2..9777b839df4f8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp @@ -39,6 +39,20 @@ struct MetricsDebug double max_lane_changing_length; }; +struct FrenetStateDebug +{ + LaneChangePhaseMetrics prep_metric; + frenet_planner::SamplingParameter sampling_parameter; + double max_lane_changing_length; + + FrenetStateDebug( + LaneChangePhaseMetrics prep_metric, frenet_planner::SamplingParameter sampling_param, + const double max_len) + : prep_metric(prep_metric), sampling_parameter(sampling_param), max_lane_changing_length(max_len) + { + } +}; + struct Debug { std::string module_type; @@ -52,6 +66,7 @@ struct Debug lanelet::ConstLanelets target_lanes; lanelet::ConstLanelets target_backward_lanes; std::vector lane_change_metrics; + std::vector frenet_states; double collision_check_object_debug_lifetime{0.0}; double distance_to_end_of_current_lane{std::numeric_limits::max()}; double distance_to_lane_change_finished{std::numeric_limits::max()}; 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 9b1244f38d5e1..0fa2c6cc8dfbc 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 @@ -40,18 +40,21 @@ struct TrajectoryGroup LaneChangePhaseMetrics prepare_metric; frenet_planner::Trajectory lane_changing; frenet_planner::FrenetState initial_state; + double max_lane_changing_length{0.0}; 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) + frenet_planner::Trajectory lane_changing, frenet_planner::FrenetState initial_state, + const double max_lane_changing_length) : 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) + initial_state(initial_state), + max_lane_changing_length(max_lane_changing_length) { } }; 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 c798036f0b96b..d1cb6a4825d3e 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 @@ -1173,7 +1173,13 @@ bool NormalLaneChange::get_path_using_frenet( stop_watch_.toc("frenet_candidates")); candidate_paths.reserve(frenet_candidates.size()); + lane_change_debug_.frenet_states.clear(); + lane_change_debug_.frenet_states.reserve(frenet_candidates.size()); for (const auto & frenet_candidate : frenet_candidates) { + lane_change_debug_.frenet_states.emplace_back( + frenet_candidate.prepare_metric, frenet_candidate.lane_changing.sampling_parameter, + frenet_candidate.max_lane_changing_length); + std::optional candidate_path_opt; try { candidate_path_opt = @@ -1185,6 +1191,7 @@ bool NormalLaneChange::get_path_using_frenet( if (!candidate_path_opt) { continue; } + try { if (check_candidate_path_safety(*candidate_path_opt, target_objects)) { RCLCPP_DEBUG( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index 355ab8ab43501..06c169248f786 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -193,39 +193,75 @@ MarkerArray ShowLaneChangeMetricsInfo( const Debug & debug_data, const geometry_msgs::msg::Pose & pose) { MarkerArray marker_array; - if (debug_data.lane_change_metrics.empty()) { - return marker_array; - } auto text_marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "sampling_metrics", 0, Marker::TEXT_VIEW_FACING, createMarkerScale(0.6, 0.6, 0.6), colors::yellow()); text_marker.pose = autoware::universe_utils::calcOffsetPose(pose, 10.0, 15.0, 0.0); - text_marker.text = fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lat_accel[m/s2]") + - fmt::format("{:^18}|", "lon_accel[m/s2]") + - fmt::format("{:^17}|", "velocity[m/s]") + - fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + - fmt::format("{:^20}\n", "max_length_th[m]"); - for (const auto & metrics : debug_data.lane_change_metrics) { - text_marker.text += fmt::format("{:-<170}\n", ""); - const auto & p_m = metrics.prep_metric; - text_marker.text += - fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^10.3f}", p_m.lat_accel) + - fmt::format("{:^21.3f}", p_m.actual_lon_accel) + fmt::format("{:^12.3f}", p_m.velocity) + - fmt::format("{:^15.3f}", p_m.duration) + fmt::format("{:^15.3f}", p_m.length) + - fmt::format("{:^17.3f}\n", metrics.max_prepare_length); - text_marker.text += fmt::format("{:<20}\n", "lc_metrics:"); - for (const auto lc_m : metrics.lc_metrics) { + if (!debug_data.lane_change_metrics.empty()) { + text_marker.text = fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lat_accel[m/s2]") + + fmt::format("{:^18}|", "lon_accel[m/s2]") + + fmt::format("{:^17}|", "velocity[m/s]") + + fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + + fmt::format("{:^20}\n", "max_length_th[m]"); + for (const auto & metrics : debug_data.lane_change_metrics) { + text_marker.text += fmt::format("{:-<170}\n", ""); + const auto & p_m = metrics.prep_metric; + text_marker.text += + fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^10.3f}", p_m.lat_accel) + + fmt::format("{:^21.3f}", p_m.actual_lon_accel) + fmt::format("{:^12.3f}", p_m.velocity) + + fmt::format("{:^15.3f}", p_m.duration) + fmt::format("{:^15.3f}", p_m.length) + + fmt::format("{:^17.3f}\n", metrics.max_prepare_length); + text_marker.text += fmt::format("{:<20}\n", "lc_metrics:"); + for (const auto lc_m : metrics.lc_metrics) { + text_marker.text += fmt::format("{:<15}", "") + fmt::format("{:^10.3f}", lc_m.lat_accel) + + fmt::format("{:^21.3f}", lc_m.actual_lon_accel) + + fmt::format("{:^12.3f}", lc_m.velocity) + + fmt::format("{:^15.3f}", lc_m.duration) + + fmt::format("{:^15.3f}", lc_m.length) + + fmt::format("{:^17.3f}\n", metrics.max_lane_changing_length); + } + } + marker_array.markers.push_back(text_marker); + } + + if (!debug_data.frenet_states.empty()) { + text_marker.text = fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lon_accel[m/s2]") + + fmt::format("{:^17}|", "lon_vel[m/s]") + + fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + + fmt::format("{:^17}|", "lat_accel[m/s2]") + + fmt::format("{:^15}|", "lat_vel[m/s2]") + fmt::format("{:^15}|", "s[m]") + + fmt::format("{:^15}|", "d[m]") + fmt::format("{:^20}\n", "max_length_th[m]"); + for (const auto & metrics : debug_data.frenet_states) { + text_marker.text += fmt::format("{:-<250}\n", ""); + const auto & p_m = metrics.prep_metric; + const auto max_len = metrics.max_lane_changing_length; + text_marker.text += + fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^13.3f}", p_m.actual_lon_accel) + + fmt::format("{:^15.3f}", p_m.velocity) + fmt::format("{:^15.3f}", p_m.duration) + + fmt::format("{:^12.3f}", p_m.length) + + fmt::format("{:^13}", "") + // Empty string for lat_accel + fmt::format("{:^13}", "") + // Empty string for lat_vel + fmt::format("{:^15}", "") + // Empty string for s + fmt::format("{:^15}", "") + // Empty string for d // Empty string for d + fmt::format("{:^20.3f}\n", max_len); // Empty string for max_length_t + const auto & lc_m = metrics.sampling_parameter.target_state; // Assuming lc_metric exists + const auto duration = metrics.sampling_parameter.target_duration; text_marker.text += - fmt::format("{:<15}", "") + fmt::format("{:^10.3f}", lc_m.lat_accel) + - fmt::format("{:^21.3f}", lc_m.actual_lon_accel) + fmt::format("{:^12.3f}", lc_m.velocity) + - fmt::format("{:^15.3f}", lc_m.duration) + fmt::format("{:^15.3f}", lc_m.length) + - fmt::format("{:^17.3f}\n", metrics.max_lane_changing_length); + fmt::format("{:<17}", "frenet_state:") + + fmt::format("{:^15.3f}", lc_m.longitudinal_acceleration) + + fmt::format("{:^13.3f}", lc_m.longitudinal_velocity) + fmt::format("{:^17.3f}", duration) + + fmt::format("{:^10.3f}", lc_m.position.s) + + fmt::format("{:^19.3f}", lc_m.lateral_acceleration) + + fmt::format("{:^10.3f}", lc_m.lateral_velocity) + + fmt::format("{:^18.3f}", lc_m.position.s) + fmt::format("{:^15.3f}", lc_m.position.d) + + fmt::format("{:^16.3f}\n", max_len); // Empty string for max_length_t } + + marker_array.markers.push_back(text_marker); } - marker_array.markers.push_back(text_marker); return marker_array; } 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 af2ec62400215..2e70c0e629669 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 @@ -521,7 +521,6 @@ std::vector generate_frenet_candidates( const auto current_lane_boundary = get_linestring_bound(current_lanes, direction); const auto is_shift_to_left = direction == Direction::LEFT; - trajectory_groups.reserve(metrics.size()); for (const auto & metric : metrics) { PathWithLaneId prepare_segment; try { @@ -541,11 +540,11 @@ std::vector generate_frenet_candidates( 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 max_lane_changing_length = std::min(dist_to_end_from_lc_start, max_lc_len); constexpr auto resample_interval = 0.5; 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), - resample_interval); + common_data_ptr, lc_start_pose, max_lane_changing_length, resample_interval); if (target_lane_reference_path.points.empty()) { continue; } @@ -580,6 +579,7 @@ std::vector generate_frenet_candidates( auto frenet_trajectories = frenet_planner::generateTrajectories( reference_spline, initial_state, *sampling_parameters_opt); + trajectory_groups.reserve(trajectory_groups.size() + frenet_trajectories.size()); for (const auto & traj : frenet_trajectories) { if (!trajectory_groups.empty()) { const auto diff = std::abs( @@ -599,7 +599,7 @@ std::vector generate_frenet_candidates( trajectory_groups.emplace_back( prepare_segment, target_lane_reference_path, target_ref_path_sums, metric, traj, - initial_state); + initial_state, max_lane_changing_length); } }