From a4287165be87fa7727f79c01dfb0bea6af54c333 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Wed, 7 Dec 2022 12:21:49 +0900 Subject: [PATCH] perf(behavior_velocity_planner): remove unnecessary debug data (#2462) Signed-off-by: veqcc --- .../include/scene_module/blind_spot/scene.hpp | 2 -- .../scene_module/intersection/scene_intersection.hpp | 1 - .../occlusion_spot/occlusion_spot_utils.hpp | 2 -- .../src/scene_module/blind_spot/debug.cpp | 10 ---------- .../src/scene_module/blind_spot/scene.cpp | 2 -- .../src/scene_module/intersection/debug.cpp | 5 ----- .../scene_module/intersection/scene_intersection.cpp | 1 - .../src/scene_module/occlusion_spot/debug.cpp | 12 ------------ .../occlusion_spot/scene_occlusion_spot.cpp | 4 ---- 9 files changed, 39 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp index 0453d03cd0b4e..e3cacf3a362ba 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp @@ -48,13 +48,11 @@ class BlindSpotModule : public SceneModuleInterface struct DebugData { - autoware_auto_planning_msgs::msg::PathWithLaneId path_raw; geometry_msgs::msg::Pose virtual_wall_pose; geometry_msgs::msg::Pose stop_point_pose; geometry_msgs::msg::Pose judge_point_pose; geometry_msgs::msg::Polygon conflict_area_for_blind_spot; geometry_msgs::msg::Polygon detection_area_for_blind_spot; - autoware_auto_planning_msgs::msg::PathWithLaneId spline_path; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; }; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index ba095a88b1fad..8decae844adb7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -46,7 +46,6 @@ class IntersectionModule : public SceneModuleInterface struct DebugData { bool stop_required; - autoware_auto_planning_msgs::msg::PathWithLaneId path_raw; geometry_msgs::msg::Pose slow_wall_pose; geometry_msgs::msg::Pose stop_wall_pose; diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 371ec93aa8253..bfe03cced0a77 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -190,8 +190,6 @@ struct DebugData std::vector parked_vehicle_point; std::vector possible_collisions; std::vector occlusion_points; - PathWithLaneId path_raw; - PathWithLaneId path_interpolated; void resetData() { close_partition.clear(); diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp index 7163a49aae6ea..fbd525294a50e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp @@ -92,11 +92,6 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() const auto state = state_machine_.getState(); const auto now = this->clock_->now(); - appendMarkerArray( - debug::createPathMarkerArray( - debug_data_.path_raw, "path_raw", lane_id_, now, 0.6, 0.3, 0.3, 0.0, 1.0, 1.0), - &debug_marker_array, now); - appendMarkerArray( createPoseMarkerArray( debug_data_.stop_point_pose, state, "stop_point_pose", module_id_, 1.0, 0.0, 0.0), @@ -124,11 +119,6 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), &debug_marker_array, now); - appendMarkerArray( - debug::createPathMarkerArray( - debug_data_.spline_path, "spline", lane_id_, now, 0.3, 0.1, 0.1, 0.5, 0.5, 0.5), - &debug_marker_array, now); - return debug_marker_array; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index 4647cb9d3f883..b831ec6ea2e84 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -83,7 +83,6 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto *stop_reason = planning_utils::initializeStopReason(StopReason::BLIND_SPOT); const auto input_path = *path; - debug_data_.path_raw = input_path; StateMachine::State current_state = state_machine_.getState(); RCLCPP_DEBUG( @@ -235,7 +234,6 @@ bool BlindSpotModule::generateStopLine( if (!splineInterpolate(*path, interval, path_ip, logger_)) { return false; } - debug_data_.spline_path = path_ip; /* generate stop point */ int stop_idx_ip = 0; // stop point index for interpolated path. diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index c7bbb10395af8..8697be34d0222 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -112,11 +112,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( const auto state = state_machine_.getState(); const auto now = this->clock_->now(); - appendMarkerArray( - debug::createPathMarkerArray( - debug_data_.path_raw, "path_raw", lane_id_, now, 0.6, 0.3, 0.3, 0.0, 1.0, 1.0), - &debug_marker_array, now); - appendMarkerArray( createLaneletPolygonsMarkerArray( debug_data_.detection_area, "detection_area", lane_id_, 0.0, 1.0, 0.0), diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index d6881a8928614..25bc702d9d6df 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -77,7 +77,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const StateMachine::State current_state = state_machine_.getState(); debug_data_ = DebugData(); - debug_data_.path_raw = *path; *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index a0d70279679b4..76e71b67a2d03 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -204,18 +204,6 @@ MarkerArray OcclusionSpotModule::createDebugMarkerArray() makePolygonMarker(debug_data_.close_partition, "close_partition", module_id_, debug_data_.z), &debug_marker_array, now); } - if (!debug_data_.path_interpolated.points.empty()) { - const int64_t virtual_lane_id = 0; - appendMarkerArray( - debug::createPathMarkerArray( - debug_data_.path_raw, "path_raw", virtual_lane_id, now, 0.6, 0.3, 0.3, 0.0, 1.0, 1.0), - &debug_marker_array, now); - appendMarkerArray( - debug::createPathMarkerArray( - debug_data_.path_interpolated, "path_interpolated", virtual_lane_id, now, 0.6, 0.3, 0.3, - 0.0, 1.0, 1.0), - &debug_marker_array, now); - } if (!debug_data_.occlusion_points.empty()) { appendMarkerArray( debug::createPointsMarkerArray( diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 510ea7fe88010..3ff3694762ae2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -185,10 +185,6 @@ bool OcclusionSpotModule::modifyPathVelocity( // these debug topics needs computation resource debug_data_.z = path->points.front().point.pose.position.z; debug_data_.possible_collisions = possible_collisions; - if (param_.is_show_occlusion) { - debug_data_.path_interpolated = path_interpolated; - debug_data_.path_raw.points = clipped_path.points; - } DEBUG_PRINT(show_time, "total [ms]: ", stop_watch_.toc("total_processing_time", true)); return true; }