Skip to content

Commit

Permalink
add debug for state
Browse files Browse the repository at this point in the history 
Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 committed Jan 6, 2025
1 parent eab7afe commit 36099e5
Show file tree
Hide file tree
Showing 5 changed files with 90 additions and 29 deletions.
15 changes: 15 additions & 0 deletions ...th_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -52,6 +66,7 @@ struct Debug
lanelet::ConstLanelets target_lanes;
lanelet::ConstLanelets target_backward_lanes;
std::vector<MetricsDebug> lane_change_metrics;
std::vector<FrenetStateDebug> frenet_states;
double collision_check_object_debug_lifetime{0.0};
double distance_to_end_of_current_lane{std::numeric_limits<double>::max()};
double distance_to_lane_change_finished{std::numeric_limits<double>::max()};
Expand Down
7 changes: 5 additions & 2 deletions ...ath_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double> 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)
{
}
};
Expand Down
7 changes: 7 additions & 0 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<LaneChangePath> candidate_path_opt;
try {
candidate_path_opt =
Expand All @@ -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(
Expand Down
82 changes: 59 additions & 23 deletions ...ing/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
8 changes: 4 additions & 4 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -521,7 +521,6 @@ std::vector<lane_change::TrajectoryGroup> 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 {
Expand All @@ -541,11 +540,11 @@ std::vector<lane_change::TrajectoryGroup> 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;
}
Expand Down Expand Up @@ -580,6 +579,7 @@ std::vector<lane_change::TrajectoryGroup> 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(
Expand All @@ -599,7 +599,7 @@ std::vector<lane_change::TrajectoryGroup> 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);
}
}

Expand Down

0 comments on commit 36099e5

Please sign in to comment.