Skip to content

Commit c004f94

Browse files
separate debug and info marker
Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent 1121f3b commit c004f94

File tree

1 file changed

+52
-29
lines changed

1 file changed

+52
-29
lines changed

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+52-29
Original file line numberDiff line numberDiff line change
@@ -139,6 +139,7 @@ void StartPlannerModule::initVariables()
139139
resetPathCandidate();
140140
resetPathReference();
141141
debug_marker_.markers.clear();
142+
info_marker_.markers.clear();
142143
initializeSafetyCheckParameters();
143144
initializeCollisionCheckDebugMap(debug_data_.collision_check);
144145
updateDepartureCheckLanes();
@@ -1447,25 +1448,35 @@ void StartPlannerModule::setDebugData()
14471448
const auto white_color = createMarkerColor(1.0, 1.0, 1.0, 0.99);
14481449

14491450
const auto life_time = rclcpp::Duration::from_seconds(1.5);
1450-
auto add = [&](MarkerArray added) {
1451+
auto add = [&](MarkerArray added, MarkerArray & target_marker_array) {
14511452
for (auto & marker : added.markers) {
14521453
marker.lifetime = life_time;
14531454
}
1454-
tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
1455+
tier4_autoware_utils::appendMarkerArray(added, &target_marker_array);
14551456
};
14561457

14571458
debug_marker_.markers.clear();
1458-
add(createPoseMarkerArray(status_.pull_out_start_pose, "back_end_pose", 0, 0.9, 0.3, 0.3));
1459-
add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3));
1460-
add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3));
1461-
add(createFootprintMarkerArray(
1462-
debug_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3));
1463-
add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9));
1464-
add(createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0));
1459+
info_marker_.markers.clear();
1460+
add(
1461+
createPoseMarkerArray(status_.pull_out_start_pose, "back_end_pose", 0, 0.9, 0.3, 0.3),
1462+
info_marker_);
1463+
add(
1464+
createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3),
1465+
info_marker_);
1466+
add(
1467+
createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3),
1468+
info_marker_);
1469+
add(
1470+
createFootprintMarkerArray(
1471+
debug_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3),
1472+
debug_marker_);
1473+
add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9), debug_marker_);
1474+
add(
1475+
createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0),
1476+
debug_marker_);
14651477

14661478
// visualize collision_check_end_pose and footprint
14671479
{
1468-
const auto local_footprint = vehicle_info_.createFootprint();
14691480
std::map<PlannerType, double> collision_check_distances = {
14701481
{PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end},
14711482
{PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}};
@@ -1475,8 +1486,10 @@ void StartPlannerModule::setDebugData()
14751486
getFullPath().points, status_.pull_out_path.end_pose.position,
14761487
collision_check_distance_from_end);
14771488
if (collision_check_end_pose) {
1478-
add(createPoseMarkerArray(
1479-
*collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0));
1489+
add(
1490+
createPoseMarkerArray(
1491+
*collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0),
1492+
info_marker_);
14801493
auto marker = createDefaultMarker(
14811494
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), "static_collision_check_end_polygon", 0,
14821495
Marker::LINE_LIST, createMarkerScale(0.1, 0.1, 0.1), red_color);
@@ -1492,7 +1505,7 @@ void StartPlannerModule::setDebugData()
14921505
tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), ego_z));
14931506
}
14941507
marker.lifetime = life_time;
1495-
debug_marker_.markers.push_back(marker);
1508+
info_marker_.markers.push_back(marker);
14961509
}
14971510
}
14981511
// start pose candidates
@@ -1519,8 +1532,8 @@ void StartPlannerModule::setDebugData()
15191532
start_pose_text_marker_array.markers.push_back(text_marker);
15201533
}
15211534

1522-
add(start_pose_footprint_marker_array);
1523-
add(start_pose_text_marker_array);
1535+
add(start_pose_footprint_marker_array, debug_marker_);
1536+
add(start_pose_text_marker_array, debug_marker_);
15241537
}
15251538

15261539
// visualize the footprint from pull_out_start pose to pull_out_end pose along the path
@@ -1552,26 +1565,32 @@ void StartPlannerModule::setDebugData()
15521565
pull_out_path_footprint_marker_array.markers.push_back(pull_out_path_footprint_marker);
15531566
}
15541567

1555-
add(pull_out_path_footprint_marker_array);
1568+
add(pull_out_path_footprint_marker_array, debug_marker_);
15561569
}
15571570

15581571
// safety check
15591572
if (parameters_->safety_check_params.enable_safety_check) {
15601573
if (debug_data_.ego_predicted_path.size() > 0) {
15611574
const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath(
15621575
debug_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution);
1563-
add(createPredictedPathMarkerArray(
1564-
ego_predicted_path, vehicle_info_, "ego_predicted_path_start_planner", 0, 0.0, 0.5, 0.9));
1576+
add(
1577+
createPredictedPathMarkerArray(
1578+
ego_predicted_path, vehicle_info_, "ego_predicted_path_start_planner", 0, 0.0, 0.5, 0.9),
1579+
debug_marker_);
15651580
}
15661581

15671582
if (debug_data_.filtered_objects.objects.size() > 0) {
1568-
add(createObjectsMarkerArray(
1569-
debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9));
1583+
add(
1584+
createObjectsMarkerArray(
1585+
debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9),
1586+
info_marker_);
15701587
}
15711588

1572-
add(showSafetyCheckInfo(debug_data_.collision_check, "object_debug_info"));
1573-
add(showPredictedPath(debug_data_.collision_check, "ego_predicted_path"));
1574-
add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation"));
1589+
add(showSafetyCheckInfo(debug_data_.collision_check, "object_debug_info"), debug_marker_);
1590+
add(
1591+
showPredictedPath(debug_data_.collision_check, "predicted_path_for_safety_check"),
1592+
info_marker_);
1593+
add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation"), info_marker_);
15751594

15761595
// set objects of interest
15771596
for (const auto & [uuid, data] : debug_data_.collision_check) {
@@ -1599,17 +1618,21 @@ void StartPlannerModule::setDebugData()
15991618
std::to_string(status_.pull_out_path.partial_paths.size() - 1);
16001619
marker.lifetime = life_time;
16011620
planner_type_marker_array.markers.push_back(marker);
1602-
add(planner_type_marker_array);
1621+
add(planner_type_marker_array, info_marker_);
16031622
}
16041623

1605-
add(laneletsAsTriangleMarkerArray(
1606-
"departure_check_lanes_for_shift_pull_out_path", debug_data_.departure_check_lanes,
1607-
cyan_color));
1624+
add(
1625+
laneletsAsTriangleMarkerArray(
1626+
"departure_check_lanes_for_shift_pull_out_path", debug_data_.departure_check_lanes,
1627+
cyan_color),
1628+
debug_marker_);
16081629

16091630
const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
16101631
planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
1611-
add(laneletsAsTriangleMarkerArray(
1612-
"pull_out_lanes_for_static_objects_collision_check", pull_out_lanes, pink_color));
1632+
add(
1633+
laneletsAsTriangleMarkerArray(
1634+
"pull_out_lanes_for_static_objects_collision_check", pull_out_lanes, pink_color),
1635+
debug_marker_);
16131636
}
16141637

16151638
void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const

0 commit comments

Comments
 (0)