diff --git a/src/detector/CollisionDetector.cpp b/src/detector/CollisionDetector.cpp index def0358..a1b1350 100644 --- a/src/detector/CollisionDetector.cpp +++ b/src/detector/CollisionDetector.cpp @@ -3,9 +3,18 @@ #include "config/SystemConfig.h" #include "utils/Logger.h" #include +#include #include #include +namespace { +// 冲突释放参数: +// 1) 当前距离达到预警阈值的倍数且不再预测碰撞 -> 立即释放 +// 2) 连续一段时间无风险 -> 强制释放(防止历史记录粘连) +constexpr double kResolveDistanceMultiplier = 1.5; +constexpr int64_t kResolveNoRiskTimeoutMs = 5000; +} + CollisionDetector::CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles) : airportBounds_(bounds), vehicleTree_(bounds.getAirportBounds(), 8), // 使用机场总边界初始化四叉树 @@ -61,6 +70,8 @@ void CollisionDetector::updateTraffic(const std::vector& aircraft, std::vector CollisionDetector::detectCollisions() { std::vector risks; + const auto now_ms = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); // 获取配置参数 const auto& predictionConfig = @@ -136,6 +147,7 @@ std::vector CollisionDetector::detectCollisions() { record.maxLevel = std::max(record.maxLevel, level); record.collisionPoint = collisionResult.collisionPoint; // 更新冲突点 + record.detectedTime = now_ms; Logger::debug("更新冲突记录: obj1=", aircraft.id, ", obj2=", vehicle.id, ", maxLevel=", static_cast(record.maxLevel), @@ -145,9 +157,7 @@ std::vector CollisionDetector::detectCollisions() { // 创建新的冲突记录 record = { collisionResult .collisionPoint, // 使用当前检测到的冲突点 - std::chrono::system_clock::now() - .time_since_epoch() - .count(), + now_ms, level, false }; Logger::debug("创建新的冲突记录: obj1=", aircraft.id, ", obj2=", vehicle.id, @@ -178,12 +188,26 @@ std::vector CollisionDetector::detectCollisions() { ", timeToCollision=", collisionResult.timeToCollision, "s", ", level=", static_cast(level)); } else if (hasUnresolvedConflict) { - // 检查是否满足解除条件 - if (checkCollisionResolved(aircraft, vehicle, it->second, - safeDistance)) { + // 专业化释放策略: + // A. 距离已明显拉开且当前不再预测碰撞 -> 立即释放 + // B. 连续无风险超过阈值 -> 强制释放,防止历史 maxLevel 粘连 + // C. 其余场景沿用原有几何释放条件 + const bool farEnoughAndNoRisk = + !collisionResult.willCollide && + collisionResult.distance >= safeDistance * kResolveDistanceMultiplier; + const bool noRiskTimeout = + !collisionResult.willCollide && + (now_ms - it->second.detectedTime) >= kResolveNoRiskTimeoutMs; + const bool geometricResolved = + checkCollisionResolved(aircraft, vehicle, it->second, safeDistance); + + if (farEnoughAndNoRisk || noRiskTimeout || geometricResolved) { it->second.resolved = true; Logger::debug("冲突已解除: obj1=", aircraft.id, - ", obj2=", vehicle.id); + ", obj2=", vehicle.id, + ", reason=", + farEnoughAndNoRisk ? "far_enough_no_risk" : + (noRiskTimeout ? "no_risk_timeout" : "geometric_condition")); } else { // 虽然当前没有检测到风险,但由于未满足解除条件,继续保持原有风险等级 currentCollisions.insert(collisionKey); @@ -509,4 +533,4 @@ CollisionResult CollisionDetector::predictCircleBasedCollision( result.object2State = CollisionObjectState(p2_hit, speed2, heading2); return result; -} \ No newline at end of file +}