Skip to content

Commit 36befe0

Browse files
kyoichi-sugaharakarishma1911
authored andcommitted
refactor(start_planner): separate debug and info marker for rviz visualization (autowarefoundation#6376)
* separate debug and info marker Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * replace util function for footprint Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent dc140e8 commit 36befe0

File tree

1 file changed

+54
-41
lines changed

1 file changed

+54
-41
lines changed

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+54-41
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,24 +1486,16 @@ 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,
1482-
Marker::LINE_LIST, createMarkerScale(0.1, 0.1, 0.1), red_color);
1483-
const auto footprint = transformVector(
1484-
local_footprint, tier4_autoware_utils::pose2transform(*collision_check_end_pose));
1485-
const double ego_z = planner_data_->self_odometry->pose.pose.position.z;
1486-
for (size_t i = 0; i < footprint.size(); i++) {
1487-
const auto & current_point = footprint.at(i);
1488-
const auto & next_point = footprint.at((i + 1) % footprint.size());
1489-
marker.points.push_back(
1490-
tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), ego_z));
1491-
marker.points.push_back(
1492-
tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), ego_z));
1493-
}
1495+
Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1), red_color);
1496+
addFootprintMarker(marker, *collision_check_end_pose, vehicle_info_);
14941497
marker.lifetime = life_time;
1495-
debug_marker_.markers.push_back(marker);
1498+
info_marker_.markers.push_back(marker);
14961499
}
14971500
}
14981501
// start pose candidates
@@ -1519,8 +1522,8 @@ void StartPlannerModule::setDebugData()
15191522
start_pose_text_marker_array.markers.push_back(text_marker);
15201523
}
15211524

1522-
add(start_pose_footprint_marker_array);
1523-
add(start_pose_text_marker_array);
1525+
add(start_pose_footprint_marker_array, debug_marker_);
1526+
add(start_pose_text_marker_array, debug_marker_);
15241527
}
15251528

15261529
// visualize the footprint from pull_out_start pose to pull_out_end pose along the path
@@ -1552,26 +1555,32 @@ void StartPlannerModule::setDebugData()
15521555
pull_out_path_footprint_marker_array.markers.push_back(pull_out_path_footprint_marker);
15531556
}
15541557

1555-
add(pull_out_path_footprint_marker_array);
1558+
add(pull_out_path_footprint_marker_array, debug_marker_);
15561559
}
15571560

15581561
// safety check
15591562
if (parameters_->safety_check_params.enable_safety_check) {
15601563
if (debug_data_.ego_predicted_path.size() > 0) {
15611564
const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath(
15621565
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));
1566+
add(
1567+
createPredictedPathMarkerArray(
1568+
ego_predicted_path, vehicle_info_, "ego_predicted_path_start_planner", 0, 0.0, 0.5, 0.9),
1569+
debug_marker_);
15651570
}
15661571

15671572
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));
1573+
add(
1574+
createObjectsMarkerArray(
1575+
debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9),
1576+
info_marker_);
15701577
}
15711578

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"));
1579+
add(showSafetyCheckInfo(debug_data_.collision_check, "object_debug_info"), debug_marker_);
1580+
add(
1581+
showPredictedPath(debug_data_.collision_check, "predicted_path_for_safety_check"),
1582+
info_marker_);
1583+
add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation"), info_marker_);
15751584

15761585
// set objects of interest
15771586
for (const auto & [uuid, data] : debug_data_.collision_check) {
@@ -1599,17 +1608,21 @@ void StartPlannerModule::setDebugData()
15991608
std::to_string(status_.pull_out_path.partial_paths.size() - 1);
16001609
marker.lifetime = life_time;
16011610
planner_type_marker_array.markers.push_back(marker);
1602-
add(planner_type_marker_array);
1611+
add(planner_type_marker_array, info_marker_);
16031612
}
16041613

1605-
add(laneletsAsTriangleMarkerArray(
1606-
"departure_check_lanes_for_shift_pull_out_path", debug_data_.departure_check_lanes,
1607-
cyan_color));
1614+
add(
1615+
laneletsAsTriangleMarkerArray(
1616+
"departure_check_lanes_for_shift_pull_out_path", debug_data_.departure_check_lanes,
1617+
cyan_color),
1618+
debug_marker_);
16081619

16091620
const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
16101621
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));
1622+
add(
1623+
laneletsAsTriangleMarkerArray(
1624+
"pull_out_lanes_for_static_objects_collision_check", pull_out_lanes, pink_color),
1625+
debug_marker_);
16131626
}
16141627

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

0 commit comments

Comments
 (0)