perf(behavior_velocity_planner): remove unnecessary debug data (autow…

…arefoundation#2462)

Signed-off-by: veqcc <ryuta.kambe@tier4.jp>

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;
   };
 

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;

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp

@@ -190,8 +190,6 @@ struct DebugData
   std::vector<geometry_msgs::msg::Point> parked_vehicle_point;
   std::vector<PossibleCollisionInfo> possible_collisions;
   std::vector<geometry_msgs::msg::Point> occlusion_points;
-  PathWithLaneId path_raw;
-  PathWithLaneId path_interpolated;
   void resetData()
   {
     close_partition.clear();

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

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.

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),

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);
 

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(

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;
 }