refactor(avoidance): remove unused marker utils

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Aug 17, 2023 · efb919e · efb919e
1 parent 9b0ce57
commit efb919e
Show file tree

Hide file tree

Showing 4 changed files with 0 additions and 88 deletions.
diff --git a/...ning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp b/...ning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp
@@ -49,9 +49,6 @@ using visualization_msgs::msg::MarkerArray;
 MarkerArray createEgoStatusMarkerArray(
   const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns);
 
-MarkerArray createSafetyCheckMarkerArray(
-  const AvoidanceState & state, const Pose & pose, const DebugData & data);
-
 MarkerArray createAvoidLineMarkerArray(
   const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g,
   const float & b, const double & w);

diff --git a/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
@@ -507,27 +507,6 @@ struct ShiftLineData
   std::vector<std::vector<double>> shift_line_history;
 };
 
-/*
- * Data struct for longitudinal margin
- */
-struct MarginData
-{
-  Pose pose{};
-
-  bool enough_lateral_margin{true};
-
-  double longitudinal_distance{std::numeric_limits<double>::max()};
-
-  double longitudinal_margin{std::numeric_limits<double>::lowest()};
-
-  double vehicle_width;
-
-  double base_link2front;
-
-  double base_link2rear;
-};
-using MarginDataArray = std::vector<MarginData>;
-
 /*
  * Debug information for marker array
  */
@@ -571,14 +550,9 @@ struct DebugData
   // shift path
   std::vector<double> proposed_spline_shift;
 
-  bool exist_adjacent_objects{false};
-
   // future pose
   PathWithLaneId path_with_planned_velocity;
 
-  // margin
-  MarginDataArray margin_data_array;
-
   // avoidance require objects
   ObjectDataArray unavoidable_objects;
 

diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp
@@ -222,62 +222,6 @@ MarkerArray createEgoStatusMarkerArray(
   return msg;
 }
 
-MarkerArray createSafetyCheckMarkerArray(
-  const AvoidanceState & state, const Pose & pose, const DebugData & data)
-{
-  const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();
-  MarkerArray msg;
-
-  if (data.exist_adjacent_objects) {
-    auto marker = createDefaultMarker(
-      "map", current_time, "safety_alert", 0L, Marker::CYLINDER, createMarkerScale(0.2, 0.2, 2.0),
-      createMarkerColor(1.0, 1.0, 0.0, 0.8));
-
-    marker.color = state == AvoidanceState::YIELD ? createMarkerColor(1.0, 0.0, 0.0, 0.8)
-                                                  : createMarkerColor(1.0, 1.0, 0.0, 0.8);
-
-    marker.pose = calcOffsetPose(pose, 0.0, 1.5, 1.0);
-    msg.markers.push_back(marker);
-
-    marker.pose = calcOffsetPose(pose, 0.0, -1.5, 1.0);
-    marker.id++;
-    msg.markers.push_back(marker);
-  }
-
-  if (state == AvoidanceState::YIELD) {
-    return msg;
-  }
-
-  {
-    auto marker = createDefaultMarker(
-      "map", current_time, "safety_longitudinal_margin", 0L, Marker::CUBE,
-      createMarkerScale(3.0, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 0.1));
-
-    for (const auto & m : data.margin_data_array) {
-      if (m.enough_lateral_margin) {
-        continue;
-      }
-
-      constexpr double max_x = 10.0;
-
-      const auto offset = 0.5 * (m.base_link2front + m.base_link2rear) - m.base_link2rear;
-      const auto diff = m.longitudinal_distance - m.longitudinal_margin;
-      const auto scale_x = std::min(max_x, 2.0 * (m.base_link2front + m.base_link2rear + diff));
-
-      const auto ratio = std::clamp(diff / max_x, 0.0, 1.0);
-
-      marker.pose = calcOffsetPose(m.pose, offset, 0.0, 0.0);
-      marker.pose.position.z += 1.0;
-      marker.scale = createMarkerScale(scale_x, 2.0 * m.vehicle_width, 2.0);
-      marker.color = createMarkerColor(1.0 - ratio, ratio, 0.0, 0.1);
-      marker.id++;
-      msg.markers.push_back(marker);
-    }
-  }
-
-  return msg;
-}
-
 MarkerArray createAvoidLineMarkerArray(
   const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g,
   const float & b, const double & w)

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -2660,7 +2660,6 @@ void AvoidanceModule::updateDebugMarker(
   using marker_utils::avoidance_marker::createOtherObjectsMarkerArray;
   using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray;
   using marker_utils::avoidance_marker::createPredictedVehiclePositions;
-  using marker_utils::avoidance_marker::createSafetyCheckMarkerArray;
   using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray;
   using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray;
   using tier4_autoware_utils::appendMarkerArray;
@@ -2696,8 +2695,6 @@ void AvoidanceModule::updateDebugMarker(
     helper_.getPreviousLinearShiftPath().path, "prev_linear_shift", 0, 0.5, 0.4, 0.6));
   add(createPoseMarkerArray(data.reference_pose, "reference_pose", 0, 0.9, 0.3, 0.3));
 
-  add(createSafetyCheckMarkerArray(data.state, getEgoPose(), debug));
-
   add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0));
   add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1));