From efb919e7ae1bc3d58d80c31fe92102c71977f18e Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 7 Aug 2023 09:45:57 +0900 Subject: [PATCH] refactor(avoidance): remove unused marker utils Signed-off-by: satoshi-ota --- .../marker_utils/avoidance/debug.hpp | 3 - .../utils/avoidance/avoidance_module_data.hpp | 26 --------- .../src/marker_utils/avoidance/debug.cpp | 56 ------------------- .../avoidance/avoidance_module.cpp | 3 - 4 files changed, 88 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp index 9e9ad5361fed8..ac24591a0a283 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp +++ b/planning/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/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index a5995833879a9..ef956fb7950dd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -507,27 +507,6 @@ struct ShiftLineData std::vector> shift_line_history; }; -/* - * Data struct for longitudinal margin - */ -struct MarginData -{ - Pose pose{}; - - bool enough_lateral_margin{true}; - - double longitudinal_distance{std::numeric_limits::max()}; - - double longitudinal_margin{std::numeric_limits::lowest()}; - - double vehicle_width; - - double base_link2front; - - double base_link2rear; -}; -using MarginDataArray = std::vector; - /* * Debug information for marker array */ @@ -571,14 +550,9 @@ struct DebugData // shift path std::vector 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 index 061cb10883f6c..94073ad467326 100644 --- 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 index 2807bed8654b4..10a6307b263b4 100644 --- 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));