From dce6f88c77ed18fbfa269ffae9fa321c80c5c2ea Mon Sep 17 00:00:00 2001 From: Tian jianyong <11429339@qq.com> Date: Thu, 12 Dec 2024 21:10:02 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E7=A2=B0=E6=92=9E=E6=A3=80?= =?UTF-8?q?=E6=B5=8B=E9=80=BB=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 3 +- config/airport_bounds.json | 10 +- config/system_config.json | 24 +-- src/{spatial => config}/AirportBounds.cpp | 4 +- src/{spatial => config}/AirportBounds.h | 6 +- src/config/SystemConfig.cpp | 14 +- src/config/SystemConfig.h | 10 - src/core/System.cpp | 251 +++++++++++++++++++++- src/core/System.h | 7 +- src/detector/CollisionDetector.cpp | 119 +++++----- src/detector/CollisionDetector.h | 2 +- src/detector/SimpleCollisionDetector.cpp | 202 +++++++++++++++++ src/detector/SimpleCollisionDetector.h | 72 +++++++ src/network/HTTPDataSource.cpp | 14 +- src/types/BasicTypes.cpp | 55 ++--- tests/CollisionDetectorTest.cpp | 2 +- tools/mock_server.py | 103 +++++---- 17 files changed, 675 insertions(+), 223 deletions(-) rename src/{spatial => config}/AirportBounds.cpp (97%) rename src/{spatial => config}/AirportBounds.h (89%) create mode 100644 src/detector/SimpleCollisionDetector.cpp create mode 100644 src/detector/SimpleCollisionDetector.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 546c33c..ba73f21 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,14 +49,15 @@ set(LIB_SOURCES src/collector/DataCollector.cpp src/core/System.cpp src/detector/CollisionDetector.cpp + src/detector/SimpleCollisionDetector.cpp src/network/HTTPDataSource.cpp src/network/WebSocketServer.cpp src/network/HTTPClient.cpp - src/spatial/AirportBounds.cpp src/spatial/CoordinateConverter.cpp src/types/BasicTypes.cpp src/types/VehicleData.cpp src/vehicle/ControllableVehicles.cpp + src/config/AirportBounds.cpp src/config/SystemConfig.cpp src/config/IntersectionConfig.cpp src/detector/TrafficLightDetector.cpp diff --git a/config/airport_bounds.json b/config/airport_bounds.json index 16aecca..2d13917 100644 --- a/config/airport_bounds.json +++ b/config/airport_bounds.json @@ -17,7 +17,7 @@ }, "config": { "vehicle_collision_radius": 50.0, - "aircraft_ground_radius": 100.0, + "aircraft_collision_radius": 100.0, "height_threshold": 15.0, "warning_zone_radius": 100.0, "alert_zone_radius": 50.0 @@ -32,7 +32,7 @@ }, "config": { "vehicle_collision_radius": 30.0, - "aircraft_ground_radius": 50.0, + "aircraft_collision_radius": 50.0, "height_threshold": 10.0, "warning_zone_radius": 50.0, "alert_zone_radius": 25.0 @@ -47,7 +47,7 @@ }, "config": { "vehicle_collision_radius": 20.0, - "aircraft_ground_radius": 40.0, + "aircraft_collision_radius": 40.0, "height_threshold": 5.0, "warning_zone_radius": 40.0, "alert_zone_radius": 20.0 @@ -62,7 +62,7 @@ }, "config": { "vehicle_collision_radius": 15.0, - "aircraft_ground_radius": 30.0, + "aircraft_collision_radius": 30.0, "height_threshold": 5.0, "warning_zone_radius": 30.0, "alert_zone_radius": 15.0 @@ -77,7 +77,7 @@ }, "config": { "vehicle_collision_radius": 25.0, - "aircraft_ground_radius": 50.0, + "aircraft_collision_radius": 50.0, "height_threshold": 10.0, "warning_zone_radius": 100.0, "alert_zone_radius": 50.0 diff --git a/config/system_config.json b/config/system_config.json index 021d25a..4976dc7 100644 --- a/config/system_config.json +++ b/config/system_config.json @@ -35,27 +35,9 @@ "collision_detection": { "update_interval_ms": 200, "prediction": { - "time_window": 30.0, - "vehicle_size": 10.0, - "aircraft_size": 50.0 - }, - "thresholds": { - "runway": { - "aircraft_ground": 100.0, - "vehicle": 50.0 - }, - "taxiway": { - "aircraft_ground": 50.0, - "vehicle": 30.0 - }, - "apron": { - "aircraft_ground": 40.0, - "vehicle": 20.0 - }, - "service": { - "aircraft_ground": 30.0, - "vehicle": 15.0 - } + "time_window": 20.0, + "vehicle_size": 5.0, + "aircraft_size": 25.0 } }, "logging": { diff --git a/src/spatial/AirportBounds.cpp b/src/config/AirportBounds.cpp similarity index 97% rename from src/spatial/AirportBounds.cpp rename to src/config/AirportBounds.cpp index 348a756..4dba8e4 100644 --- a/src/spatial/AirportBounds.cpp +++ b/src/config/AirportBounds.cpp @@ -1,4 +1,4 @@ -#include "spatial/AirportBounds.h" +#include "AirportBounds.h" #include #include #include @@ -77,7 +77,7 @@ void AirportBounds::loadConfig(const std::string& configFile) { auto& config = value["config"]; areaConfigs_[type] = { config["vehicle_collision_radius"].get(), - config["aircraft_ground_radius"].get(), + config["aircraft_collision_radius"].get(), config["height_threshold"].get(), config["warning_zone_radius"].get(), config["alert_zone_radius"].get() diff --git a/src/spatial/AirportBounds.h b/src/config/AirportBounds.h similarity index 89% rename from src/spatial/AirportBounds.h rename to src/config/AirportBounds.h index f508122..5e40038 100644 --- a/src/spatial/AirportBounds.h +++ b/src/config/AirportBounds.h @@ -17,7 +17,7 @@ enum class AreaType { // 区域配置 struct AreaConfig { double vehicleCollisionRadius = 0.0; // 车辆间冲突检测半径 - double aircraftGroundRadius = 0.0; // 航空器与车辆冲突检测半径 + double aircraftCollisionRadius = 0.0; // 航空器与车辆冲突检测半径 double heightThreshold = 0.0; // 高度阈值 double warning_zone_radius = 0.0; // 预警区域半径 double alert_zone_radius = 0.0; // 警报区域半径 @@ -32,9 +32,9 @@ struct AreaConfig { CollisionThresholds getThresholds(bool isAircraft1, bool isAircraft2) const { // 如果没有配置预警和警报距离，使用碰撞半径作为默认值 double warningRadius = warning_zone_radius > 0 ? warning_zone_radius : - (isAircraft1 || isAircraft2 ? aircraftGroundRadius : vehicleCollisionRadius); + (isAircraft1 || isAircraft2 ? aircraftCollisionRadius : vehicleCollisionRadius); double alertRadius = alert_zone_radius > 0 ? alert_zone_radius : - (isAircraft1 || isAircraft2 ? aircraftGroundRadius * 0.5 : vehicleCollisionRadius * 0.5); + (isAircraft1 || isAircraft2 ? aircraftCollisionRadius * 0.5 : vehicleCollisionRadius * 0.5); return { warningRadius, // 预警距离 diff --git a/src/config/SystemConfig.cpp b/src/config/SystemConfig.cpp index 68846d3..340c71c 100644 --- a/src/config/SystemConfig.cpp +++ b/src/config/SystemConfig.cpp @@ -39,16 +39,10 @@ void SystemConfig::load(const std::string& filename) { // 加载碰撞检测配置 collision_detection.update_interval_ms = j["collision_detection"]["update_interval_ms"]; - // 加载阈值配置 - auto& thresholds = j["collision_detection"]["thresholds"]; - collision_detection.thresholds.runway.aircraft_ground = thresholds["runway"]["aircraft_ground"]; - collision_detection.thresholds.runway.vehicle = thresholds["runway"]["vehicle"]; - collision_detection.thresholds.taxiway.aircraft_ground = thresholds["taxiway"]["aircraft_ground"]; - collision_detection.thresholds.taxiway.vehicle = thresholds["taxiway"]["vehicle"]; - collision_detection.thresholds.apron.aircraft_ground = thresholds["apron"]["aircraft_ground"]; - collision_detection.thresholds.apron.vehicle = thresholds["apron"]["vehicle"]; - collision_detection.thresholds.service.aircraft_ground = thresholds["service"]["aircraft_ground"]; - collision_detection.thresholds.service.vehicle = thresholds["service"]["vehicle"]; + // 加载碰撞预测配置 + collision_detection.prediction.time_window = j["collision_detection"]["prediction"]["time_window"]; + collision_detection.prediction.vehicle_size = j["collision_detection"]["prediction"]["vehicle_size"]; + collision_detection.prediction.aircraft_size = j["collision_detection"]["prediction"]["aircraft_size"]; // 加载日志配置 logging.level = j["logging"]["level"]; diff --git a/src/config/SystemConfig.h b/src/config/SystemConfig.h index 28a39f4..1d3c520 100644 --- a/src/config/SystemConfig.h +++ b/src/config/SystemConfig.h @@ -59,16 +59,6 @@ public: double vehicle_size; double aircraft_size; } prediction; - struct Thresholds { - struct Area { - double aircraft_ground; - double vehicle; - }; - Area runway; - Area taxiway; - Area apron; - Area service; - } thresholds; } collision_detection; struct Logging { diff --git a/src/core/System.cpp b/src/core/System.cpp index 2770efb..525962b 100644 --- a/src/core/System.cpp +++ b/src/core/System.cpp @@ -74,6 +74,11 @@ bool System::initialize() { system_config.warning.log_interval_ms }; + // 初始化简单冲突检测器 + // simpleCollisionDetector_ = std::make_unique( + // intersection_config_, *controllableVehicles_); + // Logger::info("简单冲突检测器初始化完成"); + return dataCollector_->initialize(dataSourceConfig, warnConfig); } catch (const std::exception& e) { @@ -207,11 +212,11 @@ void System::processLoop() { } else { // 正常的更新检查 for (const auto& veh : vehicles) { - Logger::debug("处理车辆: ", veh.vehicleNo); - Logger::debug(" - 位置: (", veh.geo.longitude, ", ", veh.geo.latitude, ")"); - Logger::debug(" - 速度: ", veh.speed, "m/s"); - Logger::debug(" - 时间戳: ", veh.timestamp); - Logger::debug(" - 时间差: ", veh.timestamp - last_vehicle_timestamp); + // Logger::debug("处理车辆: ", veh.vehicleNo); + // Logger::debug(" - 位置: (", veh.geo.longitude, ", ", veh.geo.latitude, ")"); + // Logger::debug(" - 速度: ", veh.speed, "m/s"); + // Logger::debug(" - 时间戳: ", veh.timestamp); + // Logger::debug(" - 时间差: ", veh.timestamp - last_vehicle_timestamp); if (veh.timestamp > last_vehicle_timestamp) { has_new_vehicles = true; @@ -248,6 +253,7 @@ void System::processLoop() { // 只有当有新的数据时才更新冲突检测 if (last_aircraft_timestamp > last_collision_timestamp || last_vehicle_timestamp > last_collision_timestamp) { + // 更新冲突检测器 collisionDetector_->updateTraffic(aircraft, vehicles); auto collisions = collisionDetector_->detectCollisions(); @@ -261,8 +267,47 @@ void System::processLoop() { ", 区域类型=", static_cast(risk.zoneType)); } processCollisions(collisions); + + // 简单冲突检测 + // simpleCollisionDetector_->updateTraffic(aircraft, vehicles); + // auto simpleCollisions = simpleCollisionDetector_->detectCollisions(); + + // if(!simpleCollisions.empty()){ + // Logger::debug("检测到 ", simpleCollisions.size(), "个碰撞风险"); + // for (const auto& risk : simpleCollisions) { + // Logger::debug("碰撞风险详情: id1=", risk.id1, + // ", id2=", risk.id2, + // ", 距离=", risk.distance, "m, 风险等级=", static_cast(risk.level), + // ", 区域类型=", static_cast(risk.isIntersection)); + // } + // processCollisions(simpleCollisions); + } else if (!lastVehiclesWithRisk_.empty()) { + // 当前没有任何风险，但上次有风险车辆，需要处理恢复指令 + Logger::debug("当前无碰撞风险，检查是否需要发送恢复指令"); + for (const auto& vehicleId : lastVehiclesWithRisk_) { + Logger::debug("车辆 ", vehicleId, " 当前没有风险，准备发送恢复指令"); + VehicleCommand cmd; + cmd.vehicleId = vehicleId; + cmd.type = CommandType::RESUME; + cmd.reason = CommandReason::RESUME_TRAFFIC; + cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); + + broadcastVehicleCommand(cmd); + controllableVehicles_->sendCommand(vehicleId, cmd); + Logger::info("发送恢复指令到车辆: ", vehicleId); + } + // 清空风险车辆列表 + Logger::debug("清空风险车辆列表: 原数量=", lastVehiclesWithRisk_.size(), + ", 原列表={", [&]() { + std::string s; + for (const auto& id : lastVehiclesWithRisk_) { + s += id + ","; + } + return s; + }(), "}"); + lastVehiclesWithRisk_.clear(); } - + last_collision_timestamp = std::max(last_aircraft_timestamp, last_vehicle_timestamp); } last_collision_update = now; @@ -316,6 +361,141 @@ void System::processLoop() { } } +void System::processCollisions(const std::vector& risks) { + // 记录当前有风险的可控车辆 + std::unordered_set currentVehiclesWithRisk; + const auto& config = SystemConfig::instance().collision_detection; + + Logger::debug("开始处理碰撞风险，当前风险数量: ", risks.size()); + + // 处理当前的碰撞风险 + for (const auto& risk : risks) { + // 处理 id1 和 id2 中的可控车辆 + bool id1_controllable = controllableVehicles_->isControllable(risk.id1); + bool id2_controllable = controllableVehicles_->isControllable(risk.id2); + + // 如果两个都不是可控车辆，跳过 + if (!id1_controllable && !id2_controllable) { + continue; + } + + // 根据风险等级处理 + auto processVehicle = [&](const std::string& vehicleId, const std::string& otherId) { + switch (risk.level) { + case SimpleRiskLevel::CRITICAL: { + // 危险区域：立即发送告警指令 + VehicleCommand cmd; + cmd.vehicleId = vehicleId; + cmd.type = CommandType::ALERT; + cmd.reason = otherId.substr(0, 2) == "AC" ? + CommandReason::AIRCRAFT_CROSSING : + CommandReason::SPECIAL_VEHICLE; + cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); + + // 设置目标位置（冲突对象的位置） + const MovingObject* target = findVehicle(otherId); + if (target) { + cmd.latitude = target->geo.latitude; + cmd.longitude = target->geo.longitude; + cmd.relativeSpeed = risk.relativeSpeed; + } + + broadcastVehicleCommand(cmd); + controllableVehicles_->sendCommand(vehicleId, cmd); + Logger::warning("发送告警指令到车辆: ", vehicleId, + " 当前距离: ", risk.distance, "m", + " 相对速度: ", risk.relativeSpeed, "m/s"); + break; + } + case SimpleRiskLevel::WARNING: { + // 预警区域：发送预警指令 + VehicleCommand cmd; + cmd.vehicleId = vehicleId; + cmd.type = CommandType::WARNING; + cmd.reason = otherId.substr(0, 2) == "AC" ? + CommandReason::AIRCRAFT_CROSSING : + CommandReason::SPECIAL_VEHICLE; + cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); + + // 设置目标位置 + const MovingObject* target = findVehicle(otherId); + if (target) { + cmd.latitude = target->geo.latitude; + cmd.longitude = target->geo.longitude; + cmd.relativeSpeed = risk.relativeSpeed; + } + + broadcastVehicleCommand(cmd); + controllableVehicles_->sendCommand(vehicleId, cmd); + Logger::info("发送预警指令到车辆: ", vehicleId, + " 当前距离: ", risk.distance, "m", + " 相对速度: ", risk.relativeSpeed, "m/s"); + break; + } + default: + break; + } + }; + + // 为每个可控车辆处理风险 + if (id1_controllable) { + currentVehiclesWithRisk.insert(risk.id1); + Logger::debug("添加当前有风险的可控车辆: ", risk.id1, + ", 当前风险车辆数量: ", currentVehiclesWithRisk.size()); + processVehicle(risk.id1, risk.id2); + } + if (id2_controllable) { + currentVehiclesWithRisk.insert(risk.id2); + Logger::debug("添加当前有风险的可控车辆: ", risk.id2, + ", 当前风险车辆数量: ", currentVehiclesWithRisk.size()); + processVehicle(risk.id2, risk.id1); + } + + // 广播碰撞预警消息 + broadcastCollisionWarning(risk); + } + + Logger::debug("当前有风险的可控车辆数量: ", currentVehiclesWithRisk.size()); + Logger::debug("上一次有风险的可控车辆数量: ", lastVehiclesWithRisk_.size()); + + // 对之前有风险但现在没有风险的车辆发送恢复指令 + for (const auto& vehicleId : lastVehiclesWithRisk_) { + Logger::debug("检查车辆是否需要恢复: ", vehicleId); + if (currentVehiclesWithRisk.find(vehicleId) == currentVehiclesWithRisk.end()) { + Logger::debug("车辆 ", vehicleId, " 当前没有风险，准备发送恢复指令"); + VehicleCommand cmd; + cmd.vehicleId = vehicleId; + cmd.type = CommandType::RESUME; + cmd.reason = CommandReason::RESUME_TRAFFIC; + cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); + + broadcastVehicleCommand(cmd); + controllableVehicles_->sendCommand(vehicleId, cmd); + Logger::info("发送恢复指令到车辆: ", vehicleId); + } else { + Logger::debug("车辆 ", vehicleId, " 仍然有风险，不发送恢复指令"); + } + } + + // 更新上一次有风险的车辆列表 + Logger::debug("更新风险车辆列表: 原数量=", lastVehiclesWithRisk_.size(), + ", 新数量=", currentVehiclesWithRisk.size(), + ", 原列表={", [&]() { + std::string s; + for (const auto& id : lastVehiclesWithRisk_) { + s += id + ","; + } + return s; + }(), "}, 新列表={", [&]() { + std::string s; + for (const auto& id : currentVehiclesWithRisk) { + s += id + ","; + } + return s; + }(), "}"); + lastVehiclesWithRisk_ = std::move(currentVehiclesWithRisk); +} + void System::processCollisions(const std::vector& risks) { // 记录当前有风险的可控车辆 std::unordered_set currentVehiclesWithRisk; @@ -403,12 +583,14 @@ void System::processCollisions(const std::vector& risks) { // 为每个可控车辆处理风险 if (id1_controllable) { currentVehiclesWithRisk.insert(risk.id1); - Logger::debug("添加当前有风险的可控车辆: ", risk.id1); + Logger::debug("添加当前有风险的可控车辆: ", risk.id1, + ", 当前风险车辆数量: ", currentVehiclesWithRisk.size()); processVehicle(risk.id1, risk.id2); } if (id2_controllable) { currentVehiclesWithRisk.insert(risk.id2); - Logger::debug("添加当前有风险的可控车辆: ", risk.id2); + Logger::debug("添加当前有风险的可控车辆: ", risk.id2, + ", 当前风险车辆数量: ", currentVehiclesWithRisk.size()); processVehicle(risk.id2, risk.id1); } @@ -438,8 +620,22 @@ void System::processCollisions(const std::vector& risks) { } } - // 更新上一次有风险的可控车辆列表 - Logger::debug("更新风险车辆列表: ", lastVehiclesWithRisk_.size(), " -> ", currentVehiclesWithRisk.size()); + // 更新上一次有风险的车辆列表 + Logger::debug("更新风险车辆列表: 原数量=", lastVehiclesWithRisk_.size(), + ", 新数量=", currentVehiclesWithRisk.size(), + ", 原列表={", [&]() { + std::string s; + for (const auto& id : lastVehiclesWithRisk_) { + s += id + ","; + } + return s; + }(), "}, 新列表={", [&]() { + std::string s; + for (const auto& id : currentVehiclesWithRisk) { + s += id + ","; + } + return s; + }(), "}"); lastVehiclesWithRisk_ = std::move(currentVehiclesWithRisk); } @@ -520,6 +716,17 @@ std::string getRiskLevelString(RiskLevel level) { } } +std::string getRiskLevelString(SimpleRiskLevel level) { + switch (level) { + case SimpleRiskLevel::CRITICAL: + return "CRITICAL"; + case SimpleRiskLevel::WARNING: + return "WARNING"; + default: + return "NONE"; + } +} + void System::broadcastCollisionWarning(const CollisionRisk& risk) { if (!ws_server_) { return; @@ -544,6 +751,30 @@ void System::broadcastCollisionWarning(const CollisionRisk& risk) { " level=", getRiskLevelString(risk.level)); } +void System::broadcastCollisionWarning(const SimpleCollisionRisk& risk) { + if (!ws_server_) { + return; + } + + // 构造冲突预警消息 + nlohmann::json j = { + {"type", "collision_warning"}, + {"id1", risk.id1}, + {"id2", risk.id2}, + {"distance", risk.distance}, + {"relativeSpeed", risk.relativeSpeed}, + {"warningLevel", getRiskLevelString(risk.level)}, + {"timestamp", std::chrono::system_clock::now().time_since_epoch().count()} + }; + + // 广播冲突预警消息 + ws_server_->broadcast(j.dump()); + Logger::debug("广播冲突预警: id1=", risk.id1, " id2=", risk.id2, + " distance=", risk.distance, "m", + " relativeSpeed=", risk.relativeSpeed, "m/s", + " level=", getRiskLevelString(risk.level)); +} + void System::broadcastTimeoutWarning(const network::TimeoutWarningMessage& warning) { if (ws_server_) { ws_server_->broadcast(warning.toJson().dump()); diff --git a/src/core/System.h b/src/core/System.h index ee4a438..8b768b0 100644 --- a/src/core/System.h +++ b/src/core/System.h @@ -8,12 +8,13 @@ #include "types/BasicTypes.h" #include "detector/CollisionDetector.h" #include "detector/TrafficLightDetector.h" -#include "spatial/AirportBounds.h" +#include "config/AirportBounds.h" #include "vehicle/ControllableVehicles.h" #include "network/WebSocketServer.h" #include "network/MessageTypes.h" #include "config/SystemConfig.h" #include "config/IntersectionConfig.h" +#include "detector/SimpleCollisionDetector.h" // 前向声明 class DataCollector; @@ -34,6 +35,7 @@ public: virtual void broadcastTimeoutWarning(const network::TimeoutWarningMessage& warning); void broadcastPositionUpdate(const MovingObject& obj); void broadcastCollisionWarning(const CollisionRisk& risk); + void broadcastCollisionWarning(const SimpleCollisionRisk& risk); void broadcastVehicleCommand(const VehicleCommand& cmd); void broadcastTrafficLightStatus(const TrafficLightSignal& signal); @@ -43,6 +45,7 @@ public: private: void processLoop(); void processCollisions(const std::vector& collisions); + void processCollisions(const std::vector& risks); std::atomic running_{false}; std::thread processThread_; @@ -70,4 +73,6 @@ private: // 辅助函数 const MovingObject* findVehicle(const std::string& vehicleId) const; + + std::unique_ptr simpleCollisionDetector_; }; \ No newline at end of file diff --git a/src/detector/CollisionDetector.cpp b/src/detector/CollisionDetector.cpp index 66c00df..b6687b1 100644 --- a/src/detector/CollisionDetector.cpp +++ b/src/detector/CollisionDetector.cpp @@ -11,11 +11,6 @@ CollisionDetector::CollisionDetector(const AirportBounds& bounds, const Controll // 记录初始化信息 const auto& airportBounds = bounds.getAirportBounds(); - Logger::debug( - "碰撞检测器初始化: 机场边界=(", - airportBounds.x, ",", airportBounds.y, ") - (", - airportBounds.x + airportBounds.width, ",", airportBounds.y + airportBounds.height, ")" - ); } void CollisionDetector::updateTraffic(const std::vector& aircraft, @@ -85,6 +80,8 @@ 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; @@ -93,8 +90,8 @@ std::vector CollisionDetector::detectCollisions() { // 计算预测阈值 double maxSpeed = std::max(aircraft.speed, vehicle.speed); double predictionDistance = maxSpeed * predictionConfig.time_window; - double baseThreshold = getCollisionParams(aircraft.position).aircraftGroundRadius; - double predictionThreshold = std::max(predictionDistance, baseThreshold * 2.0); + double baseThreshold = getCollisionParams(aircraft.position).alert_zone_radius; + double predictionThreshold = std::max(predictionDistance, baseThreshold * 4.0); // 如果在预测距离内，进行轨迹预测 if (currentDistance <= predictionThreshold) { @@ -152,7 +149,7 @@ std::vector CollisionDetector::detectCollisions() { } } - // 检测可控车辆之间的碰撞 + // 检测可控车辆与所有车辆的碰撞 for (size_t i = 0; i < controlVehicles.size(); ++i) { const auto& vehicle1 = controlVehicles[i]; @@ -174,8 +171,8 @@ std::vector CollisionDetector::detectCollisions() { // 计算预测阈值 double maxSpeed = std::max(vehicle1.speed, vehicle2.speed); double predictionDistance = maxSpeed * predictionConfig.time_window; - double baseThreshold = getCollisionParams(vehicle1.position).vehicleCollisionRadius; - double predictionThreshold = std::max(predictionDistance, baseThreshold * 2.0); + double baseThreshold = getCollisionParams(vehicle1.position).alert_zone_radius; + double predictionThreshold = std::max(predictionDistance, baseThreshold * 4.0); // 如果在预测距离内，进行轨迹预测 if (currentDistance <= predictionThreshold) { @@ -246,54 +243,6 @@ std::vector CollisionDetector::detectCollisions() { return risks; } -RiskLevel CollisionDetector::calculateRiskLevel(double distance, const Vector2D& position, - bool isAircraft1, bool isAircraft2) const { - const auto& areaConfig = getCollisionParams(position); - - // 获取合适的阈值配置 - auto thresholds = areaConfig.getThresholds(isAircraft1, isAircraft2); - double warningDistance = thresholds.warning; - double alertDistance = thresholds.critical; - - // 记录调试信息 - Logger::debug( - "风险等级计算: 距离=", distance, - "m, 预警阈值=", warningDistance, - "m, 警报阈值=", alertDistance, - "m, 物体类型=", - isAircraft1 ? (isAircraft2 ? "航空器-航空器" : "航空器-车辆") : "车辆-车辆" - ); - - // 根据距离判断风险等级 - if (distance <= alertDistance) { - return RiskLevel::CRITICAL; - } else if (distance <= warningDistance) { - return RiskLevel::WARNING; - } - return RiskLevel::NONE; -} - -WarningZoneType CollisionDetector::determineWarningZone(double distance, double threshold) const { - // 获取警报和预警距离 - double alertDistance = threshold * 0.5; // 警报距离为阈值的50% - double warningDistance = threshold; // 预警距离为阈值的100% - - // 记录调试信息 - Logger::debug( - "预警区域判断: 当前距离=", distance, - "m, 警报距离=", alertDistance, - "m, 预警距离=", warningDistance, "m" - ); - - // 根据距离判断区域类型 - if (distance <= alertDistance) { - return WarningZoneType::DANGER; - } else if (distance <= warningDistance) { - return WarningZoneType::WARNING; - } - return WarningZoneType::NONE; -} - // 添加一个新的辅助函数来统一处理风险等级和预警区域的判断 std::pair CollisionDetector::evaluateRisk( double currentDistance, @@ -309,13 +258,13 @@ std::pair CollisionDetector::evaluateRisk( RiskLevel level = RiskLevel::NONE; WarningZoneType zoneType = WarningZoneType::NONE; - // 如果预测到碰撞或距离小于警报距离，设置为危险等级 - if (willCollide || currentDistance <= thresholds.critical) { + // 如果预测到碰撞并且当前距离小于警报距离，设置为危险等级 + if (willCollide && currentDistance <= thresholds.critical) { level = RiskLevel::CRITICAL; zoneType = WarningZoneType::DANGER; } - // 如果距离在预警范围内，设置为预警等级 - else if (currentDistance <= thresholds.warning) { + // 如果预测到碰撞并且当前距离在预警范围内，设置为预警等级 + else if (willCollide && currentDistance <= thresholds.warning) { level = RiskLevel::WARNING; zoneType = WarningZoneType::WARNING; } @@ -352,7 +301,7 @@ bool CollisionDetector::checkCollisionRisk( double predictionDistance = maxSpeed * predictionConfig.time_window; // 根据物体类型选择基准阈值 double baseThreshold = (isAircraft1 || isAircraft2) ? - areaConfig.aircraftGroundRadius : + areaConfig.aircraftCollisionRadius : areaConfig.vehicleCollisionRadius; double predictionThreshold = std::max(predictionDistance, baseThreshold * 2.0); @@ -389,12 +338,26 @@ bool CollisionDetector::checkCollisionRisk( // 计算相对速度 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) || prediction.willCollide || currentDistance <= alertDistance; + // 如果相对运动小于等于0，且相对速度大于阈值，或者预测会碰撞，或者已经进入警戒区域，则有碰撞风险 + hasCollisionRisk = (relativeMotion <= 0 && relativeSpeed > 1.0) || + prediction.willCollide || + currentDistance <= alertDistance; + + Logger::debug( + "碰撞风险分析: id1=", id1, + ", id2=", id2, + ", 当前距离=", currentDistance, + "m, 相对速度=", relativeSpeed, + "m/s, 相对运动=", relativeMotion, + ", 预测碰撞=", prediction.willCollide ? "是" : "否", + ", 警戒距离=", alertDistance, + "m, 有风险=", hasCollisionRisk ? "是" : "否" + ); } if (hasCollisionRisk) { @@ -451,7 +414,7 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision( result.minDistance = std::numeric_limits::infinity(); // 计算安全距离 - double safeDistance = size1 + size2; + double safeDistance = (size1 + size2) * 1.5; // 计算速度分量 MovementVector mv1(speed1, heading1); @@ -480,7 +443,7 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision( ", 正在远离=", result.isMovingAway ? "是" : "否" ); - // 如果物体静止相对于此，直接返回当前状态 + // 如果物体相对静止，直接返回当前状态 if (relativeSpeed < 1e-6) { result.minDistance = currentDistance; result.willCollide = (currentDistance <= safeDistance); @@ -494,6 +457,25 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision( 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); @@ -568,6 +550,9 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision( ", 正在远离=", result.isMovingAway ? "是" : "否" ); } + else { + Logger::debug("没有预测到碰撞: 时间=", t, "s, 距离=", dist_t, "m, 位置=(", x2_t, ",", y2_t, ")", ", 正在远离=", result.isMovingAway ? "是" : "否"); + } } } } diff --git a/src/detector/CollisionDetector.h b/src/detector/CollisionDetector.h index f6ce5db..b83c051 100644 --- a/src/detector/CollisionDetector.h +++ b/src/detector/CollisionDetector.h @@ -3,7 +3,7 @@ #include "types/BasicTypes.h" #include "spatial/QuadTree.h" -#include "spatial/AirportBounds.h" +#include "config/AirportBounds.h" #include "vehicle/ControllableVehicles.h" #include "config/SystemConfig.h" #include diff --git a/src/detector/SimpleCollisionDetector.cpp b/src/detector/SimpleCollisionDetector.cpp new file mode 100644 index 0000000..1c373a3 --- /dev/null +++ b/src/detector/SimpleCollisionDetector.cpp @@ -0,0 +1,202 @@ +#include "detector/SimpleCollisionDetector.h" +#include "utils/Logger.h" +#include + +SimpleCollisionDetector::SimpleCollisionDetector( + const IntersectionConfig& intersectionConfig, + const ControllableVehicles& controllableVehicles) + : intersection_config_(intersectionConfig) + , controllable_vehicles_(controllableVehicles) { + Logger::info("简单冲突检测器初始化完成"); +} + +void SimpleCollisionDetector::updateTraffic( + const std::vector& aircraft, + const std::vector& vehicles) { + aircraft_data_ = aircraft; + vehicle_data_ = vehicles; + Logger::debug("更新交通数据: 航空器=", aircraft.size(), " 车辆=", vehicles.size()); +} + +std::vector SimpleCollisionDetector::detectCollisions() { + std::vector risks; + + // 检查可控车辆与飞机的冲突 + for (const auto& aircraft : aircraft_data_) { + for (const auto& vehicle : vehicle_data_) { + if (controllable_vehicles_.isControllable(vehicle.vehicleNo)) { + auto risk = checkAircraftVehicleCollision(aircraft, vehicle); + if (risk.level != SimpleRiskLevel::NONE) { + risks.push_back(risk); + } + } + } + } + + // 检查可控车辆之间的冲突 + for (size_t i = 0; i < vehicle_data_.size(); ++i) { + if (!controllable_vehicles_.isControllable(vehicle_data_[i].vehicleNo)) { + continue; + } + + for (size_t j = i + 1; j < vehicle_data_.size(); ++j) { + auto risk = checkVehicleCollision(vehicle_data_[i], vehicle_data_[j]); + if (risk.level != SimpleRiskLevel::NONE) { + risks.push_back(risk); + } + } + } + + return risks; +} + +SimpleCollisionRisk SimpleCollisionDetector::checkAircraftVehicleCollision( + const Aircraft& aircraft, const Vehicle& vehicle) { + + SimpleCollisionRisk risk; + risk.id1 = aircraft.flightNo; + risk.id2 = vehicle.vehicleNo; + risk.level = SimpleRiskLevel::NONE; + + // 计算当前距离 + risk.distance = calculateDistance(aircraft.position, vehicle.position); + + // 检查是否在路口附近 + if (!isNearIntersection(vehicle.position, risk.intersectionId)) { + return risk; // 不在路口，无风险 + } + + risk.isIntersection = true; + + // 计算到路口的距离 + double aircraftDist = calculateDistanceToIntersection(aircraft.position, risk.intersectionId); + double vehicleDist = calculateDistanceToIntersection(vehicle.position, risk.intersectionId); + + // 计算相对速度（取较大值） + risk.relativeSpeed = std::max(aircraft.speed, vehicle.speed); + if (risk.relativeSpeed < MIN_SPEED) { + risk.relativeSpeed = MIN_SPEED; + } + + // 计算预计碰撞时间（取较大值） + risk.timeToCollision = std::max(aircraftDist, vehicleDist) / risk.relativeSpeed; + + // 安全距离 = 路口宽度 + 飞机安全缓冲 + double safeDistance = INTERSECTION_WIDTH + AIRCRAFT_BUFFER; + + // 判断风险等级 + if (risk.distance < safeDistance || risk.timeToCollision < MIN_TIME_WINDOW) { + risk.level = SimpleRiskLevel::CRITICAL; + } else if (risk.distance < safeDistance * 1.5 || risk.timeToCollision < WARNING_TIME_WINDOW) { + risk.level = SimpleRiskLevel::WARNING; + } + + return risk; +} + +SimpleCollisionRisk SimpleCollisionDetector::checkVehicleCollision( + const Vehicle& v1, const Vehicle& v2) { + + SimpleCollisionRisk risk; + risk.id1 = v1.vehicleNo; + risk.id2 = v2.vehicleNo; + risk.level = SimpleRiskLevel::NONE; + + // 计算当前距离 + risk.distance = calculateDistance(v1.position, v2.position); + + // 检查是否在路口附近 + if (isNearIntersection(v1.position, risk.intersectionId) || + isNearIntersection(v2.position, risk.intersectionId)) { + // 路口场景 + risk.isIntersection = true; + + double v1Dist = calculateDistanceToIntersection(v1.position, risk.intersectionId); + double v2Dist = calculateDistanceToIntersection(v2.position, risk.intersectionId); + + risk.relativeSpeed = std::max(v1.speed, v2.speed); + if (risk.relativeSpeed < MIN_SPEED) { + risk.relativeSpeed = MIN_SPEED; + } + + risk.timeToCollision = std::max(v1Dist, v2Dist) / risk.relativeSpeed; + + // 路口安全距离 + double safeDistance = INTERSECTION_WIDTH + VEHICLE_BUFFER; + + if (risk.distance < safeDistance || risk.timeToCollision < MIN_TIME_WINDOW) { + risk.level = SimpleRiskLevel::CRITICAL; + } else if (risk.distance < safeDistance * 1.5 || risk.timeToCollision < WARNING_TIME_WINDOW) { + risk.level = SimpleRiskLevel::WARNING; + } + } else { + // 非路口场景（对向或同向） + risk.isIntersection = false; + + // 计算相对速度 + risk.relativeSpeed = std::abs(v1.speed - v2.speed); // 同向 + if (std::abs(v1.heading - v2.heading) > 150) { // 对向 + risk.relativeSpeed = v1.speed + v2.speed; + } + + if (risk.relativeSpeed < MIN_SPEED) { + risk.relativeSpeed = MIN_SPEED; + } + + risk.timeToCollision = risk.distance / risk.relativeSpeed; + + // 非路口安全距离 + double safeDistance = VEHICLE_BUFFER * 2; + + if (risk.distance < safeDistance || risk.timeToCollision < MIN_TIME_WINDOW) { + risk.level = SimpleRiskLevel::CRITICAL; + } else if (risk.distance < safeDistance * 1.5 || risk.timeToCollision < WARNING_TIME_WINDOW) { + risk.level = SimpleRiskLevel::WARNING; + } + } + + return risk; +} + +double SimpleCollisionDetector::calculateDistance(const Vector2D& pos1, const Vector2D& pos2) { + double dx = pos1.x - pos2.x; + double dy = pos1.y - pos2.y; + return std::sqrt(dx*dx + dy*dy); +} + +double SimpleCollisionDetector::calculateDistanceToIntersection( + const Vector2D& position, const std::string& intersectionId) { + + const auto* intersection = intersection_config_.findById(intersectionId); + if (!intersection) { + return std::numeric_limits::max(); + } + + // 将 IntersectionPosition 转换为 Vector2D + Vector2D intersectionPos; + intersectionPos.x = intersection->position.longitude; + intersectionPos.y = intersection->position.latitude; + + return calculateDistance(position, intersectionPos); +} + +bool SimpleCollisionDetector::isNearIntersection( + const Vector2D& position, std::string& intersectionId) { + + const double CHECK_DISTANCE = INTERSECTION_WIDTH * 5; // 检查范围是路口宽度的2倍 + + for (const auto& intersection : intersection_config_.getIntersections()) { + // 将 IntersectionPosition 转换为 Vector2D + Vector2D intersectionPos; + intersectionPos.x = intersection.position.longitude; + intersectionPos.y = intersection.position.latitude; + + double distance = calculateDistance(position, intersectionPos); + if (distance < CHECK_DISTANCE) { + intersectionId = intersection.id; + return true; + } + } + + return false; +} \ No newline at end of file diff --git a/src/detector/SimpleCollisionDetector.h b/src/detector/SimpleCollisionDetector.h new file mode 100644 index 0000000..48f8a2f --- /dev/null +++ b/src/detector/SimpleCollisionDetector.h @@ -0,0 +1,72 @@ +#ifndef AIRPORT_DETECTOR_SIMPLE_COLLISION_DETECTOR_H +#define AIRPORT_DETECTOR_SIMPLE_COLLISION_DETECTOR_H + +#include "types/BasicTypes.h" +#include "config/IntersectionConfig.h" +#include "vehicle/ControllableVehicles.h" +#include + +// 简单冲突检测器的风险等级 +enum class SimpleRiskLevel { + NONE = 0, // 无风险 + WARNING = 1, // 预警 + CRITICAL = 2 // 告警 +}; + +// 简单冲突检测器的风险信息 +struct SimpleCollisionRisk { + std::string id1, id2; // 冲突物体的ID + SimpleRiskLevel level; // 风险等级 + double distance; // 当前距离 + double timeToCollision; // 预计碰撞时间 + double relativeSpeed; // 相对速度 + bool isIntersection; // 是否在路口 + std::string intersectionId; // 路口ID（如果在路口） +}; + +class SimpleCollisionDetector { +public: + SimpleCollisionDetector(const IntersectionConfig& intersectionConfig, + const ControllableVehicles& controllableVehicles); + + // 更新交通数据 + void updateTraffic(const std::vector& aircraft, + const std::vector& vehicles); + + // 检测所有可能的冲突 + std::vector detectCollisions(); + +private: + // 检查车辆与飞机的冲突 + SimpleCollisionRisk checkAircraftVehicleCollision( + const Aircraft& aircraft, const Vehicle& vehicle); + + // 检查车辆与车辆的冲突 + SimpleCollisionRisk checkVehicleCollision( + const Vehicle& v1, const Vehicle& v2); + + // 计算到路口的距离 + double calculateDistanceToIntersection( + const Vector2D& position, const std::string& intersectionId); + + // 计算两点间距离 + double calculateDistance(const Vector2D& pos1, const Vector2D& pos2); + + // 判断是否在路口附近 + bool isNearIntersection(const Vector2D& position, std::string& intersectionId); + + // 配置参数 + static constexpr double INTERSECTION_WIDTH = 30.0; // 路口宽度 + static constexpr double AIRCRAFT_BUFFER = 20.0; // 飞机安全缓冲 + static constexpr double VEHICLE_BUFFER = 10.0; // 车辆安全缓冲 + static constexpr double MIN_TIME_WINDOW = 3.0; // 最小时间窗口（秒） + static constexpr double WARNING_TIME_WINDOW = 5.0; // 预警时间窗口（秒） + static constexpr double MIN_SPEED = 0.5; // 最小速度（米/秒） + + const IntersectionConfig& intersection_config_; + const ControllableVehicles& controllable_vehicles_; + std::vector aircraft_data_; + std::vector vehicle_data_; +}; + +#endif // AIRPORT_DETECTOR_SIMPLE_COLLISION_DETECTOR_H \ No newline at end of file diff --git a/src/network/HTTPDataSource.cpp b/src/network/HTTPDataSource.cpp index 362b336..78e3147 100644 --- a/src/network/HTTPDataSource.cpp +++ b/src/network/HTTPDataSource.cpp @@ -286,17 +286,12 @@ bool HTTPDataSource::parseVehicleResponse(const std::string& response, std::vect } veh.timestamp = static_cast(time); - // 解析可选字段 - if (item.contains("direction")) { - veh.heading = item["direction"].get(); - } - // 更新位置信息 veh.position = coordinateConverter_.toLocalXY(veh.geo.latitude, veh.geo.longitude); veh.updateMotion(veh.geo, veh.timestamp); // 更新速度和航向 - Logger::debug("Parsed vehicle: id=", veh.id, " vehicleNo=", veh.vehicleNo, - " timestamp=", veh.timestamp, " pos=(", veh.geo.longitude, ",", veh.geo.latitude, ")"); + // Logger::debug("Parsed vehicle: id=", veh.id, " vehicleNo=", veh.vehicleNo, + // " timestamp=", veh.timestamp, " pos=(", veh.geo.longitude, ",", veh.geo.latitude, ")"); vehicles.push_back(veh); } @@ -348,11 +343,6 @@ bool HTTPDataSource::parseAircraftResponse(const std::string& response, std::vec } ac.timestamp = static_cast(time); - // 解析可选字段 - if (item.contains("direction")) { - ac.heading = item["direction"].get(); - } - // 更新位置信息 ac.position = coordinateConverter_.toLocalXY(ac.geo.latitude, ac.geo.longitude); ac.updateMotion(ac.geo, ac.timestamp); // 更新速度和航向 diff --git a/src/types/BasicTypes.cpp b/src/types/BasicTypes.cpp index faf76aa..3f2a456 100644 --- a/src/types/BasicTypes.cpp +++ b/src/types/BasicTypes.cpp @@ -38,18 +38,21 @@ double MovingObject::calculateDistance(const GeoPosition& pos1, const GeoPositio } double MovingObject::calculateHeading(const GeoPosition& from, const GeoPosition& to) { - const double METERS_PER_DEGREE = 111000.0; - double lat_rad = from.latitude * M_PI / 180.0; + // 计算经纬度差值 double dlon = to.longitude - from.longitude; double dlat = to.latitude - from.latitude; - double dx = dlon * METERS_PER_DEGREE * std::cos(lat_rad); - double dy = dlat * METERS_PER_DEGREE; + // 将经度差转换为实际距离（考虑纬度影响） + double lat_rad = from.latitude * M_PI / 180.0; + double dx = dlon * std::cos(lat_rad); // 东西方向的距离（经度） + double dy = dlat; // 南北方向的距离（纬度） + // 计算航向角（正北为0度，顺时针增加） double angle = std::atan2(dx, dy) * 180.0 / M_PI; if (angle < 0) { angle += 360.0; } + return angle; } @@ -82,8 +85,6 @@ bool Vehicle::isValidSpeed(double speed) const { void MovingObject::updateMotion(const GeoPosition& newPos, uint64_t newTime) { // 检查时间戳 if (!positionHistory.empty() && newTime <= positionHistory.back().timestamp) { - Logger::debug("[Motion] Ignore outdated data: current=", newTime, - " last=", positionHistory.back().timestamp); return; // 忽略重复或过时的数据 } @@ -97,16 +98,17 @@ void MovingObject::updateMotion(const GeoPosition& newPos, uint64_t newTime) { if (positionHistory.size() >= 2) { // 使用最近的两个点来计算速度和航向 const auto& curr = positionHistory.back(); - const auto& prev = positionHistory[positionHistory.size() - 2]; // 使用倒数第二个点 + const auto& prev = positionHistory[positionHistory.size() - 2]; // 计算距离和时间差 double distance = calculateDistance(prev.geo, curr.geo); // 单位：米 double timeDiff = static_cast(curr.timestamp - prev.timestamp) / 1000.0; // 转换为秒 - Logger::debug("[Motion] Position update: ", - "\n Current: lat=", curr.geo.latitude, ", lon=", curr.geo.longitude, - "\n Previous: lat=", prev.geo.latitude, ", lon=", prev.geo.longitude, - "\n Distance=", distance, "m, TimeDiff=", timeDiff, "s"); + // 只有当位置变化足够大且时间差足够长时才更新速度和航向 + // Logger::debug("[Motion] Position update: ", + // "\n Current: lat=", curr.geo.latitude, ", lon=", curr.geo.longitude, + // "\n Previous: lat=", prev.geo.latitude, ", lon=", prev.geo.longitude, + // "\n Distance=", distance, "m, TimeDiff=", timeDiff, "s"); // 只有当位置变化足够大且时间差足够长时才更新速度和航向 static const double MIN_DISTANCE = 0.1; // 最小位置变化阈值（米） @@ -124,36 +126,13 @@ void MovingObject::updateMotion(const GeoPosition& newPos, uint64_t newTime) { } else { speed = speed * (1 - alpha) + newSpeed * alpha; // 平滑更新 } - - Logger::debug("[Motion] Speed updated: distance=", distance, "m, timeDiff=", timeDiff, - "s, newSpeed=", newSpeed, "m/s, smoothedSpeed=", speed, "m/s"); - } else { - Logger::debug("[Motion] Invalid speed: ", newSpeed, "m/s (exceeds limit)"); } - // 计算新的航向 - double newHeading = calculateHeading(prev.geo, curr.geo); - - // 使用指数移动平均来平滑航向，增大平滑因子以加快更新 - const double beta = 0.8; // 增大航向平滑因子 - if (heading == 0) { - heading = newHeading; // 第一次计算 - } else { - // 处理航向角的循环性（0-360度） - double diff = newHeading - heading; - if (diff > 180) { - diff -= 360; - } else if (diff < -180) { - diff += 360; - } - heading = fmod(heading + diff * beta + 360, 360); - } - - Logger::debug("[Motion] Heading updated: newHeading=", newHeading, - "deg, smoothedHeading=", heading, "deg"); + // 计算并直接更新航向，不需要平滑处理 + heading = calculateHeading(prev.geo, curr.geo); } else { - Logger::debug("[Motion] No update: distance=", distance, "m < ", MIN_DISTANCE, - "m or timeDiff=", timeDiff, "s < ", MIN_TIME, "s"); + // Logger::debug("[Motion] No update: distance=", distance, "m < ", MIN_DISTANCE, + // "m or timeDiff=", timeDiff, "s < ", MIN_TIME, "s"); } } diff --git a/tests/CollisionDetectorTest.cpp b/tests/CollisionDetectorTest.cpp index cf2b72f..712ee01 100644 --- a/tests/CollisionDetectorTest.cpp +++ b/tests/CollisionDetectorTest.cpp @@ -1,6 +1,6 @@ #include "detector/CollisionDetector.h" #include "vehicle/ControllableVehicles.h" -#include "spatial/AirportBounds.h" +#include "config/AirportBounds.h" #include #include #include "utils/Logger.h" diff --git a/tools/mock_server.py b/tools/mock_server.py index f6ed015..2dcfa84 100644 --- a/tools/mock_server.py +++ b/tools/mock_server.py @@ -62,7 +62,7 @@ NORMAL_BRAKE_DECELERATION = 0.2 # 正常制动减速度 (每次更新减速 20% vehicle_data = [ { "vehicleNo": "QN001", # 无人车1（西路口） - "latitude": WEST_INTERSECTION["latitude"] + (DIST_50M / 111319.9), # 西路口北50米 + "latitude": WEST_INTERSECTION["latitude"] + (DIST_100M / 111319.9), # 西路口北50米 "longitude": WEST_INTERSECTION["longitude"], "time": int(time.time() * 1000), "direction": -1, # -1表示向南 @@ -101,11 +101,11 @@ class VehicleState: self.command_reason = None # 指令原因 self.last_command_time = time.time() # 最后一次指令时间 self.traffic_light_state = None # 当前红绿灯状态 + self.target_lat = None # 目标纬度 + self.target_lon = None # 目标经度 def can_be_overridden_by(self, command_type): - """ - 判断当前指令是否可以被新指令覆盖 - """ + """判断当前指令是否可以被新指令覆盖""" priority_map = { "ALERT": 5, # 告警指令，最高优先级 "RED": 4, # 红灯指令，次高优先级 @@ -116,7 +116,7 @@ class VehicleState: new_priority = priority_map.get(command_type, 0) current_priority = priority_map.get(self.current_command, 0) - # ALERT 指令只能被 RESUME 解除 + # ALERT 指令可以被 RESUME 解除 if self.current_command == "ALERT": return command_type == "RESUME" @@ -135,7 +135,7 @@ class VehicleState: # 其他情况按优先级判断 return new_priority >= current_priority - def update_command(self, command_type): + def update_command(self, command_type, target_lat=None, target_lon=None): """更新指令状态""" priority_map = { "ALERT": 5, # 告警指令，最高优先级 @@ -145,6 +145,11 @@ class VehicleState: "RESUME": 1 # 恢复指令，最低优先级 } + # 更新目标位置 + if target_lat is not None and target_lon is not None: + self.target_lat = target_lat + self.target_lon = target_lon + # 如果是红绿灯状态，只更新状态不改变当前指令 if command_type in ["RED", "GREEN"]: self.traffic_light_state = command_type @@ -167,12 +172,14 @@ class VehicleState: def can_move(self): """检查车辆是否可以移动""" - # 如果有阻塞性指令，不能移动 - if self.current_command in ["ALERT", "WARNING", "RED"]: - return False # 如果是红灯，不能移动 if self.traffic_light_state == "RED": return False + + # 如果有告警或预警指令，不能移动 + if self.current_command in ["ALERT", "WARNING"]: + return False + # 其他情况可以移动 return True @@ -188,6 +195,7 @@ class VehicleState: - 目标速度: {self.target_speed} - 制动模式: {self.brake_mode} - 指令原因: {self.command_reason} +- 目标位置: ({self.target_lat}, {self.target_lon}) """) # 添加车辆状态管理 @@ -216,6 +224,8 @@ def handle_vehicle_command(): command_type = data.get("type", "").upper() reason = data.get("reason", "").upper() signal_state = data.get("signalState", "").upper() + target_lat = data.get("latitude", None) + target_lon = data.get("longitude", None) print(f"收到车辆控制指令: vehicle_id={vehicle_id}, type={command_type}, reason={reason}, signal_state={signal_state}") print(f"完整请求数据: {data}") @@ -260,27 +270,33 @@ def handle_vehicle_command(): f"new_command={command_type}, can_override={can_override}") if not can_override: - print(f"指令优先级过低，但仍继续处理: vehicle={vehicle_id}, current_priority={vehicle_state.command_priority}, " + print(f"指令优先级过低: vehicle={vehicle_id}, current_priority={vehicle_state.command_priority}, " f"command={command_type}") + return jsonify({ + "status": "error", + "message": "Command priority too low" + }), 400 # 处理不同类型的指令 - if command_type == "ALERT": - print(f"执行告警指令: vehicle_id={vehicle_id}") - vehicle_state.is_running = False - vehicle_state.target_speed = 0 - vehicle_state.brake_mode = "emergency" # 告警紧急制动 - # 立即更新车辆速度 + if command_type in ["ALERT", "WARNING"]: + # 查找当前车辆 + current_vehicle = None for v in vehicle_data: if v["vehicleNo"] == vehicle_id: - v["speed"] = 0 - print(f"车辆 {vehicle_id} 速度已设置为0") + current_vehicle = v break - - elif command_type == "WARNING": - print(f"执行预警指令: vehicle_id={vehicle_id}") + + # 执行告警指令，直接停车 + print(f"执行{'紧急' if command_type == 'ALERT' else '正常'}制动: vehicle={vehicle_id}") + vehicle_state.current_command = command_type + vehicle_state.command_priority = COMMAND_PRIORITIES.get(command_type, 0) vehicle_state.is_running = False vehicle_state.target_speed = 0 - vehicle_state.brake_mode = "normal" # 预警正常制动 + vehicle_state.brake_mode = "emergency" if command_type == "ALERT" else "normal" + vehicle_state.target_lat = target_lat + vehicle_state.target_lon = target_lon + # 立即更新车辆速度 + current_vehicle["speed"] = 0 elif command_type in ["RED", "GREEN"]: print(f"执行红绿灯指令: vehicle_id={vehicle_id}, state={command_type}") @@ -289,21 +305,26 @@ def handle_vehicle_command(): elif command_type == "RESUME": print(f"执行恢复指令: vehicle_id={vehicle_id}") - # RESUME 指令只能解除 ALERT 和 WARNING - if vehicle_state.current_command not in ["ALERT", "WARNING", None]: - print(f"车辆 {vehicle_id} 当前指令({vehicle_state.current_command})不能被 RESUME 解除") - return jsonify({ - "status": "error", - "message": "Current command cannot be resumed" - }), 400 + # 检查当前车辆 + current_vehicle = None + for v in vehicle_data: + if v["vehicleNo"] == vehicle_id: + current_vehicle = v + break - # 恢复正常行驶 - vehicle_state.is_running = True - vehicle_state.target_speed = DEFAULT_VEHICLE_SPEED - vehicle_state.brake_mode = None + # 清除限制性指令 + if vehicle_state.current_command in ["ALERT", "WARNING"]: + print(f"清除限制性指令: vehicle={vehicle_id}") + vehicle_state.current_command = None + vehicle_state.command_priority = 0 + vehicle_state.is_running = True + vehicle_state.target_speed = DEFAULT_VEHICLE_SPEED + vehicle_state.brake_mode = None + vehicle_state.target_lat = None + vehicle_state.target_lon = None # 更新车辆状态 - vehicle_state.update_command(command_type) + vehicle_state.update_command(command_type, target_lat, target_lon) vehicle_state.command_reason = reason vehicle_state.last_command_time = time.time() @@ -447,7 +468,7 @@ def update_vehicle_position(vehicle, elapsed_time): vehicle["phase"] = 1 vehicle["direction"] = 1 # 向东 print(f"QN001 到达西路口，切换到东西移动，方向=东") - elif vehicle["direction"] == 1 and new_lat >= WEST_INTERSECTION["latitude"] + (DIST_100M / 111319.9): + elif vehicle["direction"] == 1 and new_lat >= WEST_INTERSECTION["latitude"] + (DIST_50M / 111319.9): # 返回起点 vehicle["phase"] = 0 vehicle["direction"] = -1 # 向南 @@ -472,7 +493,7 @@ def update_vehicle_position(vehicle, elapsed_time): else: # QN002 # 无人车2：西向往返（西路口） new_lon = vehicle["longitude"] + (dlon * vehicle["direction"]) - if new_lon >= EAST_INTERSECTION["longitude"] + (DIST_100M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): + if new_lon >= EAST_INTERSECTION["longitude"] + (DIST_150M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): vehicle["direction"] = -1 # 向西 print(f"QN002 到达东端，切换方向=西") elif new_lon <= EAST_INTERSECTION["longitude"] - (DIST_150M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): @@ -489,7 +510,7 @@ def update_vehicle_position(vehicle, elapsed_time): vehicle["phase"] = 1 # 切换到南北移动 vehicle["direction"] = -1 # 向南 print(f"TQ001 到达西路口，切换到南北移动，方向=南") - elif vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_100M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): + elif vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_50M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): vehicle["direction"] = -1 # 向西 print(f"TQ001 到达东端，切换方向=西") vehicle["longitude"] = new_lon @@ -501,9 +522,9 @@ def update_vehicle_position(vehicle, elapsed_time): print(f"TQ001 到达南端，切换方向=北") elif new_lat >= WEST_INTERSECTION["latitude"]: # 返回路口 vehicle["phase"] = 0 # 切换回东西移动 - vehicle["direction"] = 1 # 向北 + vehicle["direction"] = 1 # 向东 vehicle["longitude"] = WEST_INTERSECTION["longitude"] # 重置到起始位置 - print(f"TQ001 返回西路口，切换到东西移动，方向=北") + print(f"TQ001 返回西路口，切换到东西移动，方向=东") vehicle["latitude"] = new_lat print(f"TQ001 南北移动: lat={new_lat}, direction={vehicle['direction']}") @@ -520,9 +541,9 @@ def update_aircraft_position(aircraft, elapsed_time): dlat, dlon = meters_to_degrees(distance, aircraft["latitude"]) new_lat = aircraft["latitude"] + (dlat * aircraft["direction"]) - if new_lat >= EAST_INTERSECTION["latitude"] + (DIST_200M / 111319.9): + if new_lat >= EAST_INTERSECTION["latitude"] + (DIST_150M / 111319.9): aircraft["direction"] = -1 # 向南 - elif new_lat <= EAST_INTERSECTION["latitude"] - (DIST_200M / 111319.9): + elif new_lat <= EAST_INTERSECTION["latitude"] - (DIST_150M / 111319.9): aircraft["direction"] = 1 # 向北 aircraft["latitude"] = new_lat