Skip to content

Commit

Permalink
refactor(start_planner): separate debug and info marker for rviz visu…
Browse files Browse the repository at this point in the history 
…alization (#6376)

* separate debug and info marker

Signed-off-by: kyoichi-sugahara <[email protected]>

* replace util function for footprint

Signed-off-by: kyoichi-sugahara <[email protected]>

---------

Signed-off-by: kyoichi-sugahara <[email protected]>
  • Loading branch information
@kyoichi-sugahara
kyoichi-sugahara authored Feb 11, 2024
1 parent 1121f3b commit dcd29d3
Showing 1 changed file with 54 additions and 41 deletions.
95 changes: 54 additions & 41 deletions planning/behavior_path_start_planner_module/src/start_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,6 +139,7 @@ void StartPlannerModule::initVariables()
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 @@ void StartPlannerModule::setDebugData()
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) {
for (auto & marker : added.markers) {
marker.lifetime = life_time;
}
tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
tier4_autoware_utils::appendMarkerArray(added, &target_marker_array);
};

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),
info_marker_);
add(
createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3),
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),
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 @@ void StartPlannerModule::setDebugData()
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),
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_);
marker.lifetime = life_time;
debug_marker_.markers.push_back(marker);
info_marker_.markers.push_back(marker);
}
}
// start pose candidates
Expand All @@ -1519,8 +1522,8 @@ void StartPlannerModule::setDebugData()
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_);
}

// visualize the footprint from pull_out_start pose to pull_out_end pose along the path
Expand Down Expand Up @@ -1552,26 +1555,32 @@ void StartPlannerModule::setDebugData()
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_);
}

// 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),
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),
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"),
info_marker_);
add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation"), info_marker_);

// set objects of interest
for (const auto & [uuid, data] : debug_data_.collision_check) {
Expand Down Expand Up @@ -1599,17 +1608,21 @@ void StartPlannerModule::setDebugData()
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_);
}

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,
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(
"pull_out_lanes_for_static_objects_collision_check", pull_out_lanes, pink_color),
debug_marker_);
}

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

0 comments on commit dcd29d3

Please sign in to comment.