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 ac24591a0a283..d308f55799ce0 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 @@ -59,8 +59,6 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); -MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); - MarkerArray makeOverhangToRoadShoulderMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns); 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 0278b73b0acb4..3264ec1e9ddfd 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 @@ -556,9 +556,6 @@ struct DebugData // shift path std::vector proposed_spline_shift; - // future pose - PathWithLaneId path_with_planned_velocity; - // avoidance require objects ObjectDataArray unavoidable_objects; @@ -568,6 +565,10 @@ struct DebugData // tmp for plot PathWithLaneId center_line; + // collision check debug map + utils::path_safety_checker::CollisionCheckDebugMap collision_check; + + // debug msg array AvoidanceDebugMsgArray avoidance_debug_msg_array; }; 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 94073ad467326..bea1447b0d95a 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -416,12 +416,6 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const return msg; } -MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) -{ - return createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), createMarkerColor(0.0, 0.0, 1.0, 0.8)); -} - MarkerArray makeOverhangToRoadShoulderMarkerArray( const ObjectDataArray & objects, std::string && ns) { 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 543f02abe730f..98b727e9dbc6c 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 @@ -1852,25 +1852,35 @@ bool AvoidanceModule::isSafePath( avoid_data_, planner_data_, parameters_, is_right_shift.value()); for (const auto & object : safety_check_target_objects) { + auto current_debug_data = marker_utils::createObjectDebug(object); + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); + const auto is_object_incoming = + std::abs(calcYawDeviation(getEgoPose(), object.initial_pose.pose)) > M_PI_2; + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); - const auto & ego_predicted_path = - is_object_front ? ego_predicted_path_for_front_object : ego_predicted_path_for_rear_object; + const auto & ego_predicted_path = is_object_front && !is_object_incoming + ? ego_predicted_path_for_front_object + : ego_predicted_path_for_rear_object; for (const auto & obj_path : obj_predicted_paths) { - CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, - hysteresis_factor, collision)) { + hysteresis_factor, current_debug_data.second)) { + marker_utils::updateCollisionCheckDebugMap( + debug.collision_check, current_debug_data, false); + safe_count_ = 0; return false; + } else { + marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true); } } } @@ -2682,12 +2692,14 @@ void AvoidanceModule::updateDebugMarker( using marker_utils::createShiftGradMarkerArray; using marker_utils::createShiftLengthMarkerArray; using marker_utils::createShiftLineMarkerArray; + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using marker_utils::avoidance_marker::createAvoidLineMarkerArray; using marker_utils::avoidance_marker::createEgoStatusMarkerArray; using marker_utils::avoidance_marker::createOtherObjectsMarkerArray; using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray; using marker_utils::avoidance_marker::createPredictedVehiclePositions; - using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray; using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray; using tier4_autoware_utils::appendMarkerArray; @@ -2711,9 +2723,11 @@ void AvoidanceModule::updateDebugMarker( add(createShiftLineMarkerArray(sl_arr, shifter.getBaseOffset(), ns, r, g, b, w)); }; + const auto addObjects = [&](const ObjectDataArray & objects, const auto & ns) { + add(createOtherObjectsMarkerArray(objects, ns)); + }; + add(createEgoStatusMarkerArray(data, getEgoPose(), "ego_status")); - add(createPredictedVehiclePositions( - debug.path_with_planned_velocity, "predicted_vehicle_positions")); const auto & path = data.reference_path; add(createPathMarkerArray(debug.center_line, "centerline", 0, 0.0, 0.5, 0.9)); @@ -2725,31 +2739,37 @@ void AvoidanceModule::updateDebugMarker( 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)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE)); - add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE)); - add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT)); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("MovingObject"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("CrosswalkUser"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("OutOfTargetArea"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("LessThanExecutionThreshold"))); - add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); - add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects")); + // ignore objects + { + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL); + addObjects(data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE); + addObjects(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT); + addObjects(data.other_objects, std::string("MovingObject")); + addObjects(data.other_objects, std::string("CrosswalkUser")); + addObjects(data.other_objects, std::string("OutOfTargetArea")); + addObjects(data.other_objects, std::string("NotNeedAvoidance")); + addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); + } // parent object info - addAvoidLine(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0); - addAvoidLine(debug.current_raw_shift, "p_current_raw_shift", 0.5, 0.2, 0.2); - addAvoidLine(debug.extra_return_shift, "p_extra_return_shift", 0.0, 0.5, 0.8); + { + addAvoidLine(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0); + addAvoidLine(debug.current_raw_shift, "p_current_raw_shift", 0.5, 0.2, 0.2); + addAvoidLine(debug.extra_return_shift, "p_extra_return_shift", 0.0, 0.5, 0.8); + } + + // safety check + { + add(showSafetyCheckInfo(debug.collision_check, "object_debug_info")); + add(showPredictedPath(debug.collision_check, "ego_predicted_path")); + add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation")); + } // shift length {