From dcd29d30894af1f92559d727995bc086b553e260 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 12 Feb 2024 02:07:38 +0900 Subject: [PATCH] refactor(start_planner): separate debug and info marker for rviz visualization (#6376) * separate debug and info marker Signed-off-by: kyoichi-sugahara * replace util function for footprint Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../src/start_planner_module.cpp | 95 +++++++++++-------- 1 file changed, 54 insertions(+), 41 deletions(-) diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 85732fea66dd0..16677ee4fddea 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -139,6 +139,7 @@ void StartPlannerModule::initVariables() resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); + info_marker_.markers.clear(); initializeSafetyCheckParameters(); initializeCollisionCheckDebugMap(debug_data_.collision_check); updateDepartureCheckLanes(); @@ -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 collision_check_distances = { {PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end}, {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}}; @@ -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 @@ -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 @@ -1552,7 +1555,7 @@ 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 @@ -1560,18 +1563,24 @@ void StartPlannerModule::setDebugData() 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) { @@ -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