Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(start_planner): separate debug and info marker for rviz visualization #6376

Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2022 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File

The lines of code increases from 1286 to 1299, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -139,6 +139,7 @@
resetPathCandidate();
resetPathReference();
debug_marker_.markers.clear();
info_marker_.markers.clear();
initializeSafetyCheckParameters();
initializeCollisionCheckDebugMap(debug_data_.collision_check);
updateDepartureCheckLanes();
Expand Down Expand Up @@ -1447,25 +1448,35 @@
const auto white_color = createMarkerColor(1.0, 1.0, 1.0, 0.99);

const auto life_time = rclcpp::Duration::from_seconds(1.5);
auto add = [&](MarkerArray added) {
auto add = [&](MarkerArray added, MarkerArray & target_marker_array) {

Check warning on line 1451 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1451

Added line #L1451 was not covered by tests
for (auto & marker : added.markers) {
marker.lifetime = life_time;
}
tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
tier4_autoware_utils::appendMarkerArray(added, &target_marker_array);

Check warning on line 1455 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1455

Added line #L1455 was not covered by tests
};

debug_marker_.markers.clear();
add(createPoseMarkerArray(status_.pull_out_start_pose, "back_end_pose", 0, 0.9, 0.3, 0.3));
add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3));
add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3));
add(createFootprintMarkerArray(
debug_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3));
add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9));
add(createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0));
info_marker_.markers.clear();
add(
createPoseMarkerArray(status_.pull_out_start_pose, "back_end_pose", 0, 0.9, 0.3, 0.3),
info_marker_);
add(
createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3),

Check warning on line 1464 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1459-L1464

Added lines #L1459 - L1464 were not covered by tests
info_marker_);
add(
createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3),

Check warning on line 1467 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1466-L1467

Added lines #L1466 - L1467 were not covered by tests
info_marker_);
add(
createFootprintMarkerArray(
debug_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3),
debug_marker_);
add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9), debug_marker_);
add(
createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0),

Check warning on line 1475 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1469-L1475

Added lines #L1469 - L1475 were not covered by tests
debug_marker_);

// visualize collision_check_end_pose and footprint
{
const auto local_footprint = vehicle_info_.createFootprint();
std::map<PlannerType, double> collision_check_distances = {
{PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end},
{PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}};
Expand All @@ -1475,24 +1486,16 @@
getFullPath().points, status_.pull_out_path.end_pose.position,
collision_check_distance_from_end);
if (collision_check_end_pose) {
add(createPoseMarkerArray(
*collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0));
add(
createPoseMarkerArray(
*collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0),

Check warning on line 1491 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1489-L1491

Added lines #L1489 - L1491 were not covered by tests
info_marker_);
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), "static_collision_check_end_polygon", 0,
Marker::LINE_LIST, createMarkerScale(0.1, 0.1, 0.1), red_color);
const auto footprint = transformVector(
local_footprint, tier4_autoware_utils::pose2transform(*collision_check_end_pose));
const double ego_z = planner_data_->self_odometry->pose.pose.position.z;
for (size_t i = 0; i < footprint.size(); i++) {
const auto & current_point = footprint.at(i);
const auto & next_point = footprint.at((i + 1) % footprint.size());
marker.points.push_back(
tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), ego_z));
marker.points.push_back(
tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), ego_z));
}
Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1), red_color);
addFootprintMarker(marker, *collision_check_end_pose, vehicle_info_);

Check warning on line 1496 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1495-L1496

Added lines #L1495 - L1496 were not covered by tests
marker.lifetime = life_time;
debug_marker_.markers.push_back(marker);
info_marker_.markers.push_back(marker);

Check warning on line 1498 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1498

Added line #L1498 was not covered by tests
}
}
// start pose candidates
Expand All @@ -1519,8 +1522,8 @@
start_pose_text_marker_array.markers.push_back(text_marker);
}

add(start_pose_footprint_marker_array);
add(start_pose_text_marker_array);
add(start_pose_footprint_marker_array, debug_marker_);
add(start_pose_text_marker_array, debug_marker_);

Check warning on line 1526 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1525-L1526

Added lines #L1525 - L1526 were not covered by tests
}

// visualize the footprint from pull_out_start pose to pull_out_end pose along the path
Expand Down Expand Up @@ -1552,26 +1555,32 @@
pull_out_path_footprint_marker_array.markers.push_back(pull_out_path_footprint_marker);
}

add(pull_out_path_footprint_marker_array);
add(pull_out_path_footprint_marker_array, debug_marker_);

Check warning on line 1558 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1558

Added line #L1558 was not covered by tests
}

// safety check
if (parameters_->safety_check_params.enable_safety_check) {
if (debug_data_.ego_predicted_path.size() > 0) {
const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath(
debug_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution);
add(createPredictedPathMarkerArray(
ego_predicted_path, vehicle_info_, "ego_predicted_path_start_planner", 0, 0.0, 0.5, 0.9));
add(
createPredictedPathMarkerArray(
ego_predicted_path, vehicle_info_, "ego_predicted_path_start_planner", 0, 0.0, 0.5, 0.9),

Check warning on line 1568 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1566-L1568

Added lines #L1566 - L1568 were not covered by tests
debug_marker_);
}

if (debug_data_.filtered_objects.objects.size() > 0) {
add(createObjectsMarkerArray(
debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9));
add(
createObjectsMarkerArray(
debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9),

Check warning on line 1575 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1573-L1575

Added lines #L1573 - L1575 were not covered by tests
info_marker_);
}

add(showSafetyCheckInfo(debug_data_.collision_check, "object_debug_info"));
add(showPredictedPath(debug_data_.collision_check, "ego_predicted_path"));
add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation"));
add(showSafetyCheckInfo(debug_data_.collision_check, "object_debug_info"), debug_marker_);
add(
showPredictedPath(debug_data_.collision_check, "predicted_path_for_safety_check"),

Check warning on line 1581 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1579-L1581

Added lines #L1579 - L1581 were not covered by tests
info_marker_);
add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation"), info_marker_);

Check warning on line 1583 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1583

Added line #L1583 was not covered by tests

// set objects of interest
for (const auto & [uuid, data] : debug_data_.collision_check) {
Expand Down Expand Up @@ -1599,17 +1608,21 @@
std::to_string(status_.pull_out_path.partial_paths.size() - 1);
marker.lifetime = life_time;
planner_type_marker_array.markers.push_back(marker);
add(planner_type_marker_array);
add(planner_type_marker_array, info_marker_);

Check warning on line 1611 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1611

Added line #L1611 was not covered by tests
}

add(laneletsAsTriangleMarkerArray(
"departure_check_lanes_for_shift_pull_out_path", debug_data_.departure_check_lanes,
cyan_color));
add(
laneletsAsTriangleMarkerArray(
"departure_check_lanes_for_shift_pull_out_path", debug_data_.departure_check_lanes,

Check warning on line 1616 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1614-L1616

Added lines #L1614 - L1616 were not covered by tests
cyan_color),
debug_marker_);

const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
add(laneletsAsTriangleMarkerArray(
"pull_out_lanes_for_static_objects_collision_check", pull_out_lanes, pink_color));
add(
laneletsAsTriangleMarkerArray(

Check warning on line 1623 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1622-L1623

Added lines #L1622 - L1623 were not covered by tests
"pull_out_lanes_for_static_objects_collision_check", pull_out_lanes, pink_color),
debug_marker_);

Check notice on line 1625 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method

StartPlannerModule::setDebugData decreases in cyclomatic complexity from 13 to 12, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check notice on line 1625 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Bumpy Road Ahead

StartPlannerModule::setDebugData is no longer above the threshold for logical blocks with deeply nested code. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
}

void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const
Expand Down
Loading