From 3c3b4190b873ddd23b87ebcd8b92d6af2a63bf2b Mon Sep 17 00:00:00 2001 From: Tian jianyong <11429339@qq.com> Date: Sat, 14 Dec 2024 11:18:14 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E5=96=84=E4=BA=86=E7=A2=B0=E6=92=9E?= =?UTF-8?q?=E6=A3=80=E6=B5=8B=E7=AE=97=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/detector/CollisionDetector.cpp | 494 +++++------------------------ src/detector/CollisionDetector.h | 56 ---- src/detector/RectangleCollision.h | 132 ++++---- tests/CollisionDetectorTest.cpp | 85 ++--- tools/mock_server.py | 2 +- 5 files changed, 186 insertions(+), 583 deletions(-) diff --git a/src/detector/CollisionDetector.cpp b/src/detector/CollisionDetector.cpp index b9c4beb..82d1011 100644 --- a/src/detector/CollisionDetector.cpp +++ b/src/detector/CollisionDetector.cpp @@ -81,29 +81,27 @@ std::vector CollisionDetector::detectCollisions() { // 检测可控车辆与航空器的碰撞 for (const auto& aircraft : aircraftData_) { for (const auto& vehicle : controlVehicles) { - - Logger::debug("检查车辆与航空器碰撞: ", vehicle.vehicleNo, " 与 ", aircraft.flightNo); - // 计算当前距离 - double dx = aircraft.position.x - vehicle.position.x; - double dy = aircraft.position.y - vehicle.position.y; - double currentDistance = std::sqrt(dx*dx + dy*dy); + // 检测碰撞 + bool hasCollision = checkRectangleBasedAircraftVehicleCollision( + aircraft, vehicle, predictionConfig.time_window); - // 计算预测阈值 - double maxSpeed = std::max(aircraft.speed, vehicle.speed); - double predictionDistance = maxSpeed * predictionConfig.time_window; - double baseThreshold = getCollisionParams(aircraft.position).alert_zone_radius; - double predictionThreshold = std::max(predictionDistance, baseThreshold * 4.0); - - // 如果在预测距离内，进行轨迹预测 - if (currentDistance <= predictionThreshold) { - // 获取预测结果 - auto prediction = predictTrajectoryCollision( - aircraft.position, aircraft.speed, aircraft.heading, - vehicle.position, vehicle.speed, vehicle.heading, - predictionConfig.aircraft_size, - predictionConfig.vehicle_size, - predictionConfig.time_window - ); + if (hasCollision) { + // 计算当前距离 + double dx = aircraft.position.x - vehicle.position.x; + double dy = aircraft.position.y - vehicle.position.y; + double currentDistance = std::sqrt(dx*dx + dy*dy); + + // 计算相对运动 + MovementVector av(aircraft.speed, aircraft.heading); + MovementVector vv(vehicle.speed, vehicle.heading); + double vx = av.vx - vv.vx; + double vy = av.vy - vv.vy; + double relativeSpeed = std::sqrt(vx*vx + vy*vy); + + // 计算相对运动方向（点积），如果大于0表示正在远离 + double relativeDirection = (dx*vx + dy*vy) / (currentDistance * relativeSpeed); + // 如果正在远离，相对速度为正；如果正在接近，相对速度为负 + relativeSpeed = relativeSpeed * (relativeDirection > 0 ? 1 : -1); // 评估风险等级 auto [level, zoneType] = evaluateRisk( @@ -111,30 +109,17 @@ std::vector CollisionDetector::detectCollisions() { aircraft.position, true, // aircraft false, // vehicle - prediction.willCollide + true // hasCollision ); - // 如果存在风险，添加到结果中 if (level != RiskLevel::NONE) { - // 计算相对运动 - MovementVector av(aircraft.speed, aircraft.heading); - MovementVector vv(vehicle.speed, vehicle.heading); - double vx = av.vx - vv.vx; - double vy = av.vy - vv.vy; - double relativeSpeed = std::sqrt(vx*vx + vy*vy); - - // 计算相对运动方向（点积），如果大于0表示正在远离 - double relativeDirection = (dx*vx + dy*vy) / (currentDistance * relativeSpeed); - // 如果正在远离，相对速度为正；如果正在接近，相对速度为负 - relativeSpeed = relativeSpeed * (relativeDirection > 0 ? 1 : -1); - risks.push_back({ aircraft.flightNo, vehicle.vehicleNo, level, currentDistance, - prediction.minDistance, - relativeSpeed, // 带方向的相对速度 + currentDistance, // 最小距离就是当前距离 + relativeSpeed, {vx, vy}, // 相对运动向量 zoneType }); @@ -158,33 +143,30 @@ std::vector CollisionDetector::detectCollisions() { for (const auto& vehicle2 : allVehicles) { // 跳过自身 if (vehicle1.vehicleNo == vehicle2.vehicleNo) { - Logger::debug("跳过自身车辆: ", vehicle1.vehicleNo); continue; } - Logger::debug("检查车辆碰撞: ", vehicle1.vehicleNo, " 与 ", vehicle2.vehicleNo); + // 检测碰撞 + bool hasCollision = checkRectangleBasedVehicleCollision( + vehicle1, vehicle2, predictionConfig.time_window); - // 计算当前距离 - double dx = vehicle1.position.x - vehicle2.position.x; - double dy = vehicle1.position.y - vehicle2.position.y; - double currentDistance = std::sqrt(dx*dx + dy*dy); - - // 计算预测阈值 - double maxSpeed = std::max(vehicle1.speed, vehicle2.speed); - double predictionDistance = maxSpeed * predictionConfig.time_window; - double baseThreshold = getCollisionParams(vehicle1.position).alert_zone_radius; - double predictionThreshold = std::max(predictionDistance, baseThreshold * 4.0); - - // 如果在预测距离内，进行轨迹预测 - if (currentDistance <= predictionThreshold) { - // 获取预测结果 - auto prediction = predictTrajectoryCollision( - vehicle1.position, vehicle1.speed, vehicle1.heading, - vehicle2.position, vehicle2.speed, vehicle2.heading, - predictionConfig.vehicle_size, - predictionConfig.vehicle_size, - predictionConfig.time_window - ); + if (hasCollision) { + // 计算当前距离 + double dx = vehicle1.position.x - vehicle2.position.x; + double dy = vehicle1.position.y - vehicle2.position.y; + double currentDistance = std::sqrt(dx*dx + dy*dy); + + // 计算相对运动 + MovementVector v1v(vehicle1.speed, vehicle1.heading); + MovementVector v2v(vehicle2.speed, vehicle2.heading); + double vx = v1v.vx - v2v.vx; + double vy = v1v.vy - v2v.vy; + double relativeSpeed = std::sqrt(vx*vx + vy*vy); + + // 计算相对运动方向（点积），如果大于0表示正在远离 + double relativeDirection = (dx*vx + dy*vy) / (currentDistance * relativeSpeed); + // 如果正在远离，相对速度为正；如果正在接近，相对速度为负 + relativeSpeed = relativeSpeed * (relativeDirection > 0 ? 1 : -1); // 评估风险等级 auto [level, zoneType] = evaluateRisk( @@ -192,55 +174,32 @@ std::vector CollisionDetector::detectCollisions() { vehicle1.position, false, // vehicle false, // vehicle - prediction.willCollide + true // hasCollision ); - // 如果存在风险，添加到结果中 if (level != RiskLevel::NONE) { - // 计算相对运动 - MovementVector v1v(vehicle1.speed, vehicle1.heading); - MovementVector v2v(vehicle2.speed, vehicle2.heading); - double vx = v1v.vx - v2v.vx; - double vy = v1v.vy - v2v.vy; - double relativeSpeed = std::sqrt(vx*vx + vy*vy); - risks.push_back({ vehicle1.vehicleNo, vehicle2.vehicleNo, level, currentDistance, - prediction.minDistance, + currentDistance, // 最小距离就是当前距离 relativeSpeed, {vx, vy}, // 相对运动向量 zoneType }); Logger::debug("检测到车辆间碰撞风险: vehicle1=", vehicle1.vehicleNo, - "vehicle2=", vehicle2.vehicleNo, - "distance=", currentDistance, "m", - "level=", static_cast(level), - "zone=", static_cast(zoneType), - "相对速度=", relativeSpeed, "m/s", - "相对运动=(", vx, ",", vy, ")", - "正在远离=", relativeSpeed > 0 ? "是" : "否"); + ", vehicle2=", vehicle2.vehicleNo, + ", distance=", currentDistance, "m, level=", static_cast(level), + ", zone=", static_cast(zoneType), + ", relativeSpeed=", relativeSpeed, "m/s, relativeMotion=(", vx, ",", vy, ")", + ", isMovingAway=", relativeSpeed > 0 ? "是" : "否"); } } } } - // 记录检测结果 - if (!risks.empty()) { - Logger::debug("碰撞检测完成: 发现 ", risks.size(), "个风险"); - for (const auto& risk : risks) { - Logger::debug("风险详情: id1=", risk.id1, - ", id2=", risk.id2, - ", distance=", risk.distance, "m, level=", static_cast(risk.level), - ", zone=", static_cast(risk.zoneType), - ", 相对速度=", risk.relativeSpeed, - "m/s, 相对运动=(", risk.relativeMotion.x, ",", risk.relativeMotion.y, ")"); - } - } - return risks; } @@ -283,343 +242,50 @@ std::pair CollisionDetector::evaluateRisk( return {level, zoneType}; } -bool CollisionDetector::checkCollisionRisk( - const Vector2D& pos1, double speed1, double heading1, const std::string& id1, - const Vector2D& pos2, double speed2, double heading2, const std::string& id2, - bool isAircraft1, bool isAircraft2) const { - - // 获取区域配置 - const auto& areaConfig = getCollisionParams(pos1); - const auto& predictionConfig = SystemConfig::instance().collision_detection.prediction; - - // 计算当前距离 - double dx = pos1.x - pos2.x; - double dy = pos1.y - pos2.y; - double currentDistance = std::sqrt(dx*dx + dy*dy); - - // 计算预测阈值 - double maxSpeed = std::max(speed1, speed2); - double predictionDistance = maxSpeed * predictionConfig.time_window; - // 根据物体类型选择基准阈值 - double baseThreshold = (isAircraft1 || isAircraft2) ? - areaConfig.aircraftCollisionRadius : - areaConfig.vehicleCollisionRadius; - double predictionThreshold = std::max(predictionDistance, baseThreshold * 2.0); - - // 如果当前距离超过预测阈值，直接返回false - if (currentDistance > predictionThreshold) { - return false; - } - - // 直接告警条件：距离小于阈值的一半 - if (currentDistance < baseThreshold * 0.5) { - return true; - } - - // 获取预测结果 - auto prediction = predictTrajectoryCollision( - pos1, speed1, heading1, - pos2, speed2, heading2, - isAircraft1 ? predictionConfig.aircraft_size : predictionConfig.vehicle_size, - isAircraft2 ? predictionConfig.aircraft_size : predictionConfig.vehicle_size, - predictionConfig.time_window - ); - - // 获取阈值配置 - auto thresholds = areaConfig.getThresholds(isAircraft1, isAircraft2); - double alertDistance = thresholds.critical; - - // 如果距离在阈值范围内，进行相对运动分析 - bool hasCollisionRisk = false; - if (currentDistance < baseThreshold) { - // 使用 MovementVector 计算速度分量 - MovementVector mv1(speed1, heading1); - MovementVector mv2(speed2, heading2); - - // 计算相对速度 - double vx = mv1.vx - mv2.vx; - double vy = mv1.vy - mv2.vy; - double relativeSpeed = std::sqrt(vx*vx + vy*vy); - - // 计算相对运动（点积），判断是否在接近 - double relativeMotion = dx*vx + dy*vy; - - // 如果相对运动小于等于0，且相对速度大于阈值，或者预测会碰撞，或者已经进入警戒区域，则有碰撞风险 - hasCollisionRisk = (relativeMotion <= 0 && relativeSpeed > 1.0) || - prediction.willCollide || - currentDistance <= alertDistance; - - Logger::debug( - "碰撞风险分析: id1=", id1, - ", id2=", id2, - ", 当前距离=", currentDistance, - "m, 相对速度=", relativeSpeed, - "m/s, 相对运动=", relativeMotion, - ", 正在远离=", prediction.isMovingAway ? "是" : "否" - ); - } - - if (hasCollisionRisk) { - // 使用 MovementVector 计算相对运动，用于日志记录 - MovementVector mv1(speed1, heading1); - MovementVector mv2(speed2, heading2); - double vx = mv1.vx - mv2.vx; - double vy = mv1.vy - mv2.vy; - double relativeSpeed = std::sqrt(vx*vx + vy*vy); - - std::string type1 = isAircraft1 ? "航空器" : "车辆"; - std::string type2 = isAircraft2 ? "航空器" : "车辆"; - - Logger::debug( - "检测到碰撞风险:", - "type1=", type1, - "id1=", id1, - "type2=", type2, - "id2=", id2, - "distance=", currentDistance, "m,", - "relativeSpeed=", relativeSpeed, "m/s,", - "prediction=", prediction.willCollide ? "是" : "否" - ); - } - - return hasCollisionRisk; -} - -bool CollisionDetector::checkAircraftVehicleCollision(const Aircraft& aircraft, - const Vehicle& vehicle) const { - return checkCollisionRisk( - aircraft.position, aircraft.speed, aircraft.heading, aircraft.flightNo, - vehicle.position, vehicle.speed, vehicle.heading, vehicle.vehicleNo, - true, false - ); -} - -bool CollisionDetector::checkVehicleCollision(const Vehicle& v1, const Vehicle& v2) const { - return checkCollisionRisk( - v1.position, v1.speed, v1.heading, v1.vehicleNo, - v2.position, v2.speed, v2.heading, v2.vehicleNo, - false, false - ); -} - -CollisionPrediction CollisionDetector::predictTrajectoryCollision( - const Vector2D& pos1, double speed1, double heading1, - const Vector2D& pos2, double speed2, double heading2, - double size1, double size2, double timeWindow) const { - - CollisionPrediction result; - result.willCollide = false; - result.timeToCollision = std::numeric_limits::infinity(); - result.minDistance = std::numeric_limits::infinity(); - - // 计算安全距离等于两个物体尺寸之和，加上 20m 的缓冲区 - double safeDistance = size1 + size2 + 20.0; - - // 计算速度分量 - MovementVector mv1(speed1, heading1); - MovementVector mv2(speed2, heading2); - - // 计算相对速度 - double dvx = mv1.vx - mv2.vx; - double dvy = mv1.vy - mv2.vy; - double relativeSpeed = std::sqrt(dvx*dvx + dvy*dvy); - - // 计算相对位置 - double dx = pos1.x - pos2.x; - double dy = pos1.y - pos2.y; - double currentDistance = std::sqrt(dx*dx + dy*dy); - - // 计算相对运动方向（点积） - double relativeMotion = dx*dvx + dy*dvy; - result.isMovingAway = (relativeMotion > 0); // 添加到结果中 - - // 记录初始状态 - Logger::debug( - "轨迹预测初始状态: 当前距离=", currentDistance, - "m, 相对速度=", relativeSpeed, - "m/s, 安全距离=", safeDistance, - "m, 相对运动=", relativeMotion, - ", 正在远离=", result.isMovingAway ? "是" : "否" - ); - - // 如果物体相对静止，直接返回当前状态 - if (relativeSpeed < 1e-6) { - result.minDistance = currentDistance; - result.willCollide = (currentDistance <= safeDistance); - if (result.willCollide) { - result.timeToCollision = 0.0; - // 根据物体尺寸计算碰撞点 - double ratio = size1 / (size1 + size2); - result.collisionPoint.x = pos2.x + dx * ratio; - result.collisionPoint.y = pos2.y + dy * ratio; - } - return result; - } - - // 如果当前距离小于安全距离，且相对运动为负（正在接近），直接判定为碰撞 - if (currentDistance <= safeDistance && relativeMotion < 0) { - result.willCollide = true; - result.timeToCollision = 0.0; - result.minDistance = currentDistance; - result.collisionPoint = { - pos2.x + dx * (size1 / (size1 + size2)), - pos2.y + dy * (size1 / (size1 + size2)) - }; - - Logger::debug( - "预测到碰撞: 当前距离=", currentDistance, - "m < 安全距离=", safeDistance, - "m, 且正在接近, 相对运动=", relativeMotion - ); - - return result; - } - - // 计算相对运动方程的系数 - double a = dvx * dvx + dvy * dvy; - double b = 2.0 * (dx * dvx + dy * dvy); - double c = dx * dx + dy * dy - safeDistance * safeDistance; - - // 计算判别式 - double discriminant = b * b - 4.0 * a * c; - - // 如果判别式小于0，不会发生碰撞 - if (discriminant < 0) { - // 计算时间窗口内的最小距离 - double t_min = -b / (2.0 * a); - t_min = std::clamp(t_min, 0.0, timeWindow); - - // 计算最小距离时刻的位置 - double x1_min = pos1.x + mv1.vx * t_min; - double y1_min = pos1.y + mv1.vy * t_min; - double x2_min = pos2.x + mv2.vx * t_min; - double y2_min = pos2.y + mv2.vy * t_min; - - result.minDistance = std::sqrt( - std::pow(x1_min - x2_min, 2) + - std::pow(y1_min - y2_min, 2) - ); - - Logger::debug( - "无碰撞可能: 最小距离=", result.minDistance, - "m, 发生时间=", t_min, "s" - ); - - return result; - } - - // 计算可能的碰撞时间点 - double t1 = (-b - std::sqrt(discriminant)) / (2.0 * a); - double t2 = (-b + std::sqrt(discriminant)) / (2.0 * a); - - // 检查两个时间点 - for (double t : {t1, t2}) { - if (t >= 0 && t <= timeWindow) { - // 计算t时刻的位置 - double x1_t = pos1.x + mv1.vx * t; - double y1_t = pos1.y + mv1.vy * t; - double x2_t = pos2.x + mv2.vx * t; - double y2_t = pos2.y + mv2.vy * t; - - // 计算t时刻的距离 - double dist_t = std::sqrt( - std::pow(x1_t - x2_t, 2) + - std::pow(y1_t - y2_t, 2) - ); - - // 更新最小距离 - if (dist_t < result.minDistance) { - result.minDistance = dist_t; - - // 如果距离小于等于安全距离，记录碰撞信息 - if (dist_t <= safeDistance && !result.willCollide) { - result.willCollide = true; - result.timeToCollision = t; - - // 根据物体尺寸计算碰撞点 - double ratio = size1 / (size1 + size2); - result.collisionPoint.x = x2_t + (x1_t - x2_t) * ratio; - result.collisionPoint.y = y2_t + (y1_t - y2_t) * ratio; - - Logger::debug( - "预测到碰撞: 时间=", t, - "s, 距离=", dist_t, - "m, 位置=(", result.collisionPoint.x, - ",", result.collisionPoint.y, ")", - ", 正在远离=", result.isMovingAway ? "是" : "否" - ); - } - else { - Logger::debug("没有预测到碰撞: 时间=", t, "s, 距离=", dist_t, "m, 位置=(", x2_t, ",", y2_t, ")", ", 正在远离=", result.isMovingAway ? "是" : "否"); - } - } - } - } - - return result; -} - -// 基于矩形的航空器和车辆碰撞检测 bool CollisionDetector::checkRectangleBasedAircraftVehicleCollision( - const Aircraft& aircraft, + const Aircraft& aircraft, const Vehicle& vehicle, double timeWindow) const { - // 获取系统配置 - const auto& config = SystemConfig::instance(); + // 创建矩形 + collision::Rectangle rect1{ + aircraft.position, + SystemConfig::instance().collision_detection.prediction.aircraft_size, + {aircraft.speed * std::sin(aircraft.heading * M_PI / 180.0), + aircraft.speed * std::cos(aircraft.heading * M_PI / 180.0)} + }; - // 计算速度分量 - MovementVector mv1(aircraft.speed, aircraft.heading); - MovementVector mv2(vehicle.speed, vehicle.heading); - - // 创建航空器和车辆的正方形表示 - collision::Rectangle aircraftRect( - aircraft.position, // 中心点 - config.collision_detection.prediction.aircraft_size * 2.0, // 边长 - Vector2D{mv1.vx, mv1.vy}, // 速度向量 - aircraft.heading // 朝向 - ); - - collision::Rectangle vehicleRect( - vehicle.position, // 中心点 - config.collision_detection.prediction.vehicle_size * 2.0, // 边长 - Vector2D{mv2.vx, mv2.vy}, // 速度向量 - vehicle.heading // 朝向 - ); + collision::Rectangle rect2{ + vehicle.position, + SystemConfig::instance().collision_detection.prediction.vehicle_size, + {vehicle.speed * std::sin(vehicle.heading * M_PI / 180.0), + vehicle.speed * std::cos(vehicle.heading * M_PI / 180.0)} + }; // 检测碰撞 - return aircraftRect.checkCollision(vehicleRect, timeWindow); + return collision::predictRectangleBasedCollision(rect1, rect2, timeWindow); } -// 基于矩形的车辆间碰撞检测 bool CollisionDetector::checkRectangleBasedVehicleCollision( - const Vehicle& v1, + const Vehicle& v1, const Vehicle& v2, double timeWindow) const { - // 获取系统配置 - const auto& config = SystemConfig::instance(); - double vehicleSize = config.collision_detection.prediction.vehicle_size; + // 创建矩形 + collision::Rectangle rect1{ + v1.position, + SystemConfig::instance().collision_detection.prediction.vehicle_size, + {v1.speed * std::sin(v1.heading * M_PI / 180.0), + v1.speed * std::cos(v1.heading * M_PI / 180.0)} + }; - // 计算速度分量 - MovementVector mv1(v1.speed, v1.heading); - MovementVector mv2(v2.speed, v2.heading); - - // 创建两个车辆的正方形表示 - collision::Rectangle rect1( - v1.position, // 中心点 - vehicleSize * 2.0, // 边长 - Vector2D{mv1.vx, mv1.vy}, // 速度向量 - v1.heading // 朝向 - ); - - collision::Rectangle rect2( - v2.position, // 中心点 - vehicleSize * 2.0, // 边长 - Vector2D{mv2.vx, mv2.vy}, // 速度向量 - v2.heading // 朝向 - ); + collision::Rectangle rect2{ + v2.position, + SystemConfig::instance().collision_detection.prediction.vehicle_size, + {v2.speed * std::sin(v2.heading * M_PI / 180.0), + v2.speed * std::cos(v2.heading * M_PI / 180.0)} + }; // 检测碰撞 - return rect1.checkCollision(rect2, timeWindow); + return collision::predictRectangleBasedCollision(rect1, rect2, timeWindow); } \ No newline at end of file diff --git a/src/detector/CollisionDetector.h b/src/detector/CollisionDetector.h index b7596cb..8f2caf1 100644 --- a/src/detector/CollisionDetector.h +++ b/src/detector/CollisionDetector.h @@ -35,15 +35,6 @@ struct CollisionRisk { WarningZoneType zoneType; // 预警区域类型 }; -// 轨迹预测碰撞结果 -struct CollisionPrediction { - bool willCollide; // 是否可能发生碰撞 - double timeToCollision; // 到碰撞点的时间(秒) - Vector2D collisionPoint; // 可能的碰撞点 - double minDistance; // 最小距离 - bool isMovingAway; // 是否正在远离 -}; - class CollisionDetector { public: CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles); @@ -69,15 +60,6 @@ public: double timeWindow = 30.0 ) const; - // 基于轨迹预测的碰撞检测 - CollisionPrediction predictTrajectoryCollision( - const Vector2D& pos1, double speed1, double heading1, - const Vector2D& pos2, double speed2, double heading2, - double size1, - double size2, - double timeWindow = 30.0 - ) const; - private: const AirportBounds& airportBounds_; QuadTree vehicleTree_; @@ -90,24 +72,6 @@ private: return airportBounds_.getAreaConfig(areaType); } - // 通用碰撞风险检测函数 - bool checkCollisionRisk( - const Vector2D& pos1, double speed1, double heading1, const std::string& id1, - const Vector2D& pos2, double speed2, double heading2, const std::string& id2, - bool isAircraft1, bool isAircraft2) const; - - // 检查航空器和车辆是否可能碰撞 - bool checkAircraftVehicleCollision(const Aircraft& aircraft, const Vehicle& vehicle) const; - // 检查两辆车是否可能碰撞 - bool checkVehicleCollision(const Vehicle& v1, const Vehicle& v2) const; - - // 计算风险等级 - RiskLevel calculateRiskLevel(double distance, const Vector2D& position, - bool isAircraft1, bool isAircraft2) const; - - // 确定预警区域类型 - WarningZoneType determineWarningZone(double distance, double threshold) const; - // 统一的风险评估函数 std::pair evaluateRisk( double currentDistance, @@ -133,26 +97,6 @@ private: "°, vx=", vx, "m/s, vy=", vy, "m/s"); } }; - - // 创建正方形 - collision::Rectangle createRectangle( - const Vector2D& position, - double size, - double heading, - double speed - ) const { - // 计算速度分量 - MovementVector mv(speed, heading); - Logger::debug("速度分量计算: speed=" + std::to_string(speed) + "m/s, heading=" + - std::to_string(heading) + "°, vx=" + std::to_string(mv.vx) + - "m/s, vy=" + std::to_string(mv.vy) + "m/s"); - - return collision::Rectangle{ - position, // 中心点 - size, // 正方形边长 - {mv.vx, mv.vy} // 速度向量 - }; - } }; #endif // AIRPORT_DETECTOR_COLLISION_DETECTOR_H \ No newline at end of file diff --git a/src/detector/RectangleCollision.h b/src/detector/RectangleCollision.h index c473d8c..5fed6c0 100644 --- a/src/detector/RectangleCollision.h +++ b/src/detector/RectangleCollision.h @@ -26,12 +26,6 @@ struct Rectangle { // 构造函数 Rectangle(const Vector2D& c, double s, const Vector2D& v, double h = 0.0) : center(c), size(s), velocity(v), heading(h) { -#ifdef TESTING - std::cout << "正方形构造: 中心点=(" << center.x << "," << - center.y << "), 边长=" << size << - ", 速度=(" << velocity.x << "," << - velocity.y << ")" << std::endl; -#endif } // 默认构造函数 @@ -54,87 +48,73 @@ struct Rectangle { future.center.y += velocity.y * t; return future; } +}; - // 检测与另一个正方形是否碰撞 - bool checkCollision(const Rectangle& other, double timeWindow = 1.0) const { - // 1. 首先检查当前是否碰撞 - double x1_min, x1_max, y1_min, y1_max; - double x2_min, x2_max, y2_min, y2_max; +// 预测两个矩形在给定时间窗口内是否会发生碰撞 +inline bool predictRectangleBasedCollision(const Rectangle& rect1, const Rectangle& rect2, double timeWindow) { + // 1. 首先检查当前是否碰撞 + double x1_min, x1_max, y1_min, y1_max; + double x2_min, x2_max, y2_min, y2_max; + + rect1.getAABB(x1_min, x1_max, y1_min, y1_max); + rect2.getAABB(x2_min, x2_max, y2_min, y2_max); + + // 检查当前是否重叠 + if (x1_max >= x2_min && x2_max >= x1_min && + y1_max >= y2_min && y2_max >= y1_min) { + return true; + } + + // 2. 检查未来一段时间内是否会碰撞 + // 计算相对速度 + double rel_vx = rect1.velocity.x - rect2.velocity.x; + double rel_vy = rect1.velocity.y - rect2.velocity.y; + double rel_speed = std::sqrt(rel_vx*rel_vx + rel_vy*rel_vy); + + // 如果相对速度接近0，使用当前距离判断 + if (rel_speed < 0.1) { + double dx = rect1.center.x - rect2.center.x; + double dy = rect1.center.y - rect2.center.y; + double distance = std::sqrt(dx*dx + dy*dy); + double safe_distance = rect1.size/2.0 + rect2.size/2.0; // 两个正方形从中心到边缘的距离之和 - this->getAABB(x1_min, x1_max, y1_min, y1_max); - other.getAABB(x2_min, x2_max, y2_min, y2_max); + if (distance <= safe_distance) { + return true; + } + return false; + } + + // 3. 在时间窗口内采样检查 + const int STEPS = 60; + double dt = timeWindow / STEPS; + + for (int i = 1; i <= STEPS; ++i) { + double t = i * dt; + Rectangle future1 = rect1.predictPosition(t); + Rectangle future2 = rect2.predictPosition(t); - // 检查当前是否重叠 + future1.getAABB(x1_min, x1_max, y1_min, y1_max); + future2.getAABB(x2_min, x2_max, y2_min, y2_max); + + // 检查是否重叠 if (x1_max >= x2_min && x2_max >= x1_min && y1_max >= y2_min && y2_max >= y1_min) { -#ifdef TESTING - std::cout << "检测到当���碰撞" << std::endl; -#endif return true; } - // 2. 检查未来一段时间内是否会碰撞 - // 计算相对速度 - double rel_vx = velocity.x - other.velocity.x; - double rel_vy = velocity.y - other.velocity.y; - double rel_speed = std::sqrt(rel_vx*rel_vx + rel_vy*rel_vy); + // 计算中心点距离 + double dx = (future1.center.x - future2.center.x); + double dy = (future1.center.y - future2.center.y); + double distance = std::sqrt(dx*dx + dy*dy); + double safe_distance = rect1.size/2.0 + rect2.size/2.0; // 两个正方形从中心到边缘的距离之和 - // 如果相对速度接近0，使用当前距离判断 - if (rel_speed < 0.1) { - double dx = center.x - other.center.x; - double dy = center.y - other.center.y; - double distance = std::sqrt(dx*dx + dy*dy); - double safe_distance = size/2.0 + other.size/2.0; // 两个正方形从中心到边缘的距离之和 - - if (distance <= safe_distance) { -#ifdef TESTING - std::cout << "静止状态检测到碰撞，距离=" << distance << "米" << std::endl; -#endif - return true; - } - return false; + if (distance <= safe_distance) { + return true; } - - // 3. 在时间窗口内采样检查 - const int STEPS = 20; - double dt = timeWindow / STEPS; - - for (int i = 1; i <= STEPS; ++i) { - double t = i * dt; - Rectangle future1 = predictPosition(t); - Rectangle future2 = other.predictPosition(t); - - future1.getAABB(x1_min, x1_max, y1_min, y1_max); - future2.getAABB(x2_min, x2_max, y2_min, y2_max); - - // 检查是否重叠 - if (x1_max >= x2_min && x2_max >= x1_min && - y1_max >= y2_min && y2_max >= y1_min) { -#ifdef TESTING - std::cout << "预测到碰撞，时间=" << t << "秒" << std::endl; -#endif - return true; - } - - // 计算中心点距离 - double dx = (future1.center.x - future2.center.x); - double dy = (future1.center.y - future2.center.y); - double distance = std::sqrt(dx*dx + dy*dy); - double safe_distance = size/2.0 + other.size/2.0; // 两个正方形从中心到边缘的距离之和 - - if (distance <= safe_distance) { -#ifdef TESTING - std::cout << "预测到危险接近，时间=" << t - << "秒，距离=" << distance - << "米，安全距离=" << safe_distance << "米" << std::endl; -#endif - return true; - } - } - - return false; } -}; + + return false; +} } // namespace collision diff --git a/tests/CollisionDetectorTest.cpp b/tests/CollisionDetectorTest.cpp index ccd2e2a..ee84b08 100644 --- a/tests/CollisionDetectorTest.cpp +++ b/tests/CollisionDetectorTest.cpp @@ -103,7 +103,7 @@ TEST_F(CollisionDetectorTest, DetectControllableVehicleAircraftCollision) { Vehicle vehicle; vehicle.vehicleNo = "VEH001"; - vehicle.position = {120, 100}; // 离航空器20米 + vehicle.position = {120, 100}; // ���离航空器20米 vehicle.speed = 5; vehicle.heading = 270; Logger::info("Created vehicle: vehicleNo=", vehicle.vehicleNo, @@ -136,7 +136,7 @@ TEST_F(CollisionDetectorTest, DetectControllableVehicleOtherVehicleCollision) { EXPECT_CALL(*mockControllableVehicles_, isControllable("VEH002")) .WillRepeatedly(testing::Return(false)); - // 设置测试数据 + // 设置测试数��� Vehicle controlVehicle; controlVehicle.vehicleNo = "VEH001"; controlVehicle.position = {100, 100}; @@ -254,7 +254,7 @@ TEST_F(CollisionDetectorTest, PerformanceTest) { EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_)) .WillRepeatedly(testing::Return(false)); - // 设置3辆可控车辆 + // 设3辆可控车辆 std::vector controlVehicleNos = { "VEH001", // 滑行道上的可控车辆 "VEH002", // 停机位的可控车辆 @@ -291,7 +291,7 @@ TEST_F(CollisionDetectorTest, PerformanceTest) { aircraft.push_back(a); } - // 停机位区域：100架航空器，180辆车辆 + // 停机位区域：100架航空器，180辆���辆 for (int i = 0; i < 100; ++i) { Aircraft a; a.flightNo = "GT" + std::to_string(i + 1); @@ -391,7 +391,8 @@ TEST_F(CollisionDetectorTest, PerformanceTest) { // 修改矩形碰撞检测测试 TEST_F(CollisionDetectorTest, RectangleCollisionParallelMotion) { // 设置系统配置 - SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0; + SystemConfig::instance().collision_detection.prediction.vehicle_size = 10.0; // 改为10米全边长 + SystemConfig::instance().collision_detection.prediction.aircraft_size = 20.0; // 改为20米全边长 // 创建两个平行运动的车辆 Vehicle v1; @@ -407,15 +408,16 @@ TEST_F(CollisionDetectorTest, RectangleCollisionParallelMotion) { v2.heading = 90.0; // 向东行驶 // 检测碰撞 - bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); + bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0); - // 由于两车间距小于安全距离，应该检测到碰撞 - EXPECT_TRUE(hasCollision) << "平行运动的车辆应该检测到碰撞"; + // 由���两车间距小于安全距离（10米），应该在30秒时间窗口内检测到碰撞 + EXPECT_TRUE(hasCollision) << "平行运动的车辆应该在30秒时间窗口内检测到碰撞"; } TEST_F(CollisionDetectorTest, RectangleCollisionCrossingPaths) { // 设置系统配置 SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0; + SystemConfig::instance().collision_detection.prediction.aircraft_size = 10.0; // 创建两个交叉路径的车辆 Vehicle v1; @@ -431,16 +433,16 @@ TEST_F(CollisionDetectorTest, RectangleCollisionCrossingPaths) { v2.heading = 180.0; // 向南行驶 // 检测碰撞 - bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); + bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0); - // 两车将在(100, 100)处相遇，应该检测到碰撞 - EXPECT_TRUE(hasCollision) << "交叉路径的车辆应该检测到碰撞"; + // 两车将在(100, 100)处相遇，应该在30秒时间窗口内检测到碰撞 + EXPECT_TRUE(hasCollision) << "交叉路径的车辆应该在30秒时间窗口内检测到碰撞"; } TEST_F(CollisionDetectorTest, RectangleCollisionAircraftVehicle) { // 设置系统配置 - SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0; - SystemConfig::instance().collision_detection.prediction.aircraft_size = 10.0; + SystemConfig::instance().collision_detection.prediction.vehicle_size = 10.0; // 改为10米全边长 + SystemConfig::instance().collision_detection.prediction.aircraft_size = 20.0; // 改为20米全边长 // 创建一个航空器和一个车辆 Aircraft aircraft; @@ -455,6 +457,7 @@ TEST_F(CollisionDetectorTest, RectangleCollisionAircraftVehicle) { vehicle.speed = 8.0; vehicle.heading = 270.0; // 向西行驶（与航空器相向而行） + // 输出测试配置 std::cout << "\n=== 矩形碰撞检测测试（航空器-车辆） ===" << std::endl; std::cout << "配置信息：" << std::endl; std::cout << "- 航空器尺寸: " << SystemConfig::instance().collision_detection.prediction.aircraft_size << "米" << std::endl; @@ -487,23 +490,31 @@ TEST_F(CollisionDetectorTest, RectangleCollisionAircraftVehicle) { std::cout << "预计碰撞时间: " << timeToCollision << "秒" << std::endl; // 检测碰撞 - bool hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle); + bool hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle, 30.0); std::cout << "\n碰撞检测结果: " << (hasCollision ? "是" : "否") << std::endl; - // 由于距离较近且相向而行，应该检测到碰撞 - EXPECT_TRUE(hasCollision) << "相向而行且距离较近的航空器和车辆应该检测到碰撞"; + // 由于距离较近且相向而行，应该在30秒时间窗口内检测到碰撞 + EXPECT_TRUE(hasCollision) << "相向而行且距离较近的航空器和车辆应该在30秒时间窗口内检测到碰撞"; // 测试边界情况：增加距离 - vehicle.position.x = 150.0; // 增加到50米距离 - hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle); - EXPECT_FALSE(hasCollision) << "距离较远时不应该检测到碰撞"; + vehicle.position = {900.0, 100.0}; // 增加到800米距离 + dx = vehicle.position.x - aircraft.position.x; + dy = vehicle.position.y - aircraft.position.y; + distance = std::sqrt(dx*dx + dy*dy); + std::cout << "\n增加距离后：" << std::endl; + std::cout << "- 车辆新位置: (" << vehicle.position.x << ", " << vehicle.position.y << ")" << std::endl; + std::cout << "- 实际距离: " << distance << "米" << std::endl; + + hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle, 30.0); + std::cout << "- 碰撞检测结果: " << (hasCollision ? "是" : "否") << std::endl; + EXPECT_FALSE(hasCollision) << "距离较远时不应该在30秒时间窗口内检测到碰撞"; // 测试边界情况：静止状态 - vehicle.position.x = 110.0; // 距离缩短到10米 + vehicle.position = {110.0, 100.0}; // 距离缩短到10米 vehicle.speed = 0.0; aircraft.speed = 0.0; - hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle); - EXPECT_TRUE(hasCollision) << "静止状态下距离较近时应该检测到碰撞"; + hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle, 30.0); + EXPECT_TRUE(hasCollision) << "静止状态下距离较近时应该在30秒时间窗口内检测到碰撞"; // 测试边界情况：垂直路径 vehicle.position = {100.0, 120.0}; // 在航空器正北方 @@ -511,13 +522,14 @@ TEST_F(CollisionDetectorTest, RectangleCollisionAircraftVehicle) { vehicle.heading = 180.0; // 向南行驶 aircraft.speed = 10.0; aircraft.heading = 90.0; // 向东行驶 - hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle); - EXPECT_TRUE(hasCollision) << "垂直路径且距离较近时应该检测到碰撞"; + hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle, 30.0); + EXPECT_TRUE(hasCollision) << "垂直路径且距离较近时应该在30秒时间窗口内检测到碰撞"; } TEST_F(CollisionDetectorTest, RectangleCollisionWithRotation) { // 设置系统配置 SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0; + SystemConfig::instance().collision_detection.prediction.aircraft_size = 10.0; // 创建两个有一定角度的车辆 Vehicle v1; @@ -533,22 +545,23 @@ TEST_F(CollisionDetectorTest, RectangleCollisionWithRotation) { v2.heading = 315.0; // 向西北行驶 // 检测碰撞 - bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); + bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0); - // 由于两车轨迹交叉且距离较近，应该检测到碰撞 - EXPECT_TRUE(hasCollision) << "不同角度的车辆相交时应该检测到碰撞"; + // 由于两车轨迹交叉且距离较近，应该在30秒时间窗口内检测到碰撞 + EXPECT_TRUE(hasCollision) << "不同角度的车辆相交时应该在30秒时间窗口内检测到碰撞"; // 将v2移动到更远的位置 v2.position = {150.0, 150.0}; - hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); + hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0); // 距离较远时不应该检测到碰撞 - EXPECT_FALSE(hasCollision) << "距离较远时不应该检测到碰撞"; + EXPECT_FALSE(hasCollision) << "距离较远时不应该在30秒时间窗口内检测到碰撞"; } TEST_F(CollisionDetectorTest, RectangleCollisionEdgeCases) { // 设置系统配置 SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0; + SystemConfig::instance().collision_detection.prediction.aircraft_size = 10.0; Vehicle v1; v1.vehicleNo = "VEH001"; @@ -560,21 +573,21 @@ TEST_F(CollisionDetectorTest, RectangleCollisionEdgeCases) { v2.vehicleNo = "VEH002"; // 测试1：两个静止车辆，刚好接触 - v2.position = {110.0, 100.0}; // 距离为10米（两车长度和） + v2.position = {105.0, 100.0}; // 距离为5米（两车长度和的一半） v2.speed = 0.0; v2.heading = 90.0; - bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); - EXPECT_TRUE(hasCollision) << "两个静止且刚好接触的车辆应该检测到碰撞"; + bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0); + EXPECT_TRUE(hasCollision) << "两个静止且刚好接触的车辆应该在30秒时间窗口内检测到碰撞"; // 测试2：两个静止车辆，距离略大于安全距离 v2.position = {111.0, 100.0}; // 距离为11米 - hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); - EXPECT_FALSE(hasCollision) << "两个静止且距离大于安全距离的车辆不应该检测到碰撞"; + hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0); + EXPECT_FALSE(hasCollision) << "两个静止且距离大于安全距离的车辆不应该在30秒时间窗口内检测到碰撞"; // 测试3：极低速度 v2.position = {120.0, 100.0}; v2.speed = 0.1; // 极低速度 - hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); - EXPECT_FALSE(hasCollision) << "距离足够且速度极低的车辆不应该检测到碰撞"; + hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2, 30.0); + EXPECT_FALSE(hasCollision) << "距离足够且速度极低的车辆不应该在30秒时间窗口内检测到碰撞"; } \ No newline at end of file diff --git a/tools/mock_server.py b/tools/mock_server.py index e3dfee4..b14d1e9 100644 --- a/tools/mock_server.py +++ b/tools/mock_server.py @@ -667,7 +667,7 @@ def switch_traffic_light_state(): lat_diff = abs(aircraft["latitude"] - EAST_INTERSECTION["latitude"]) * 111319.9 old_state = traffic_light_data[1]["state"] - traffic_light_data[1]["state"] = 1 if lat_diff > (AIRCRAFT_SIZE_M + DIST_50M) else 0 + traffic_light_data[1]["state"] = 1 if lat_diff > DIST_50M else 0 if old_state != traffic_light_data[1]["state"]: print(f"东路口红绿灯状态切换为: {'绿灯' if traffic_light_data[1]['state'] == 1 else '红灯'}")