From 7c9b5cc06b6732e789c78c620a4973176030d2f3 Mon Sep 17 00:00:00 2001 From: Tian jianyong <11429339@qq.com> Date: Wed, 22 Jan 2025 15:01:20 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B0=86=E5=AE=89=E5=85=A8=E5=8C=BA=E4=B8=8E?= =?UTF-8?q?=E6=99=AE=E9=80=9A=E7=A2=B0=E6=92=9E=E6=A3=80=E6=B5=8B=E5=90=88?= =?UTF-8?q?=E5=B9=B6=E6=A3=80=E6=B5=8B=EF=BC=8C=E5=B0=86=E5=9D=90=E6=A0=87?= =?UTF-8?q?=E7=B3=BB=E6=94=B9=E4=B8=BA=E6=9C=BA=E5=9C=BA=E5=9D=90=E6=A0=87?= =?UTF-8?q?=E7=B3=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 1 + config/airport_bounds.json | 37 +- config/system_config.json | 4 +- docs/design.md | 103 ++++ src/collector/DataCollector.cpp | 161 +++--- src/collector/DataCollector.h | 40 +- src/config/AirportBounds.cpp | 60 ++- src/config/AirportBounds.h | 14 + src/core/System.cpp | 655 +++++++++---------------- src/core/System.h | 18 +- src/detector/CollisionDetector.cpp | 96 ++-- src/network/HTTPDataSource.h | 1 - src/spatial/CoordinateConverter.cpp | 3 +- src/spatial/CoordinateConverter.h | 13 +- src/spatial/QuadTree.h | 4 - src/spatial/QuadTree.inl | 104 ---- src/types/BasicTypes.cpp | 78 ++- tests/AirportBoundsTest.cpp | 246 ++++++++++ tests/CMakeLists.txt | 1 + tests/DataCollectorTest.cpp | 469 +++++++++++++++--- tests/data/invalid_airport_bounds.json | 5 + tools/mock_server.py | 89 ++-- 22 files changed, 1357 insertions(+), 845 deletions(-) delete mode 100644 src/spatial/QuadTree.inl create mode 100644 tests/AirportBoundsTest.cpp create mode 100644 tests/data/invalid_airport_bounds.json diff --git a/CMakeLists.txt b/CMakeLists.txt index bec94bc..32b80b2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,7 @@ endif() # 设置 CMake 策略 cmake_policy(SET CMP0074 NEW) # 使用 _ROOT 变量 +cmake_policy(SET CMP0167 NEW) # 使用新的 Boost 查找机制 # 包含 FetchContent 模块 include(FetchContent) diff --git a/config/airport_bounds.json b/config/airport_bounds.json index 8a6aadf..3d1d153 100644 --- a/config/airport_bounds.json +++ b/config/airport_bounds.json @@ -1,17 +1,22 @@ { "airport": { + "rotation_angle": 68.53, + "reference_point": { + "x": 0.0, + "y": 0.0 + }, "bounds": { - "x": 0, - "y": 0, - "width": 5000, - "height": 4000 + "x": -100, + "y": -200, + "width": 800, + "height": 400 } }, "areas": { "runway": { "bounds": { - "x": 0, - "y": 0, + "x": -101, + "y": -201, "width": 1, "height": 1 }, @@ -36,8 +41,8 @@ }, "taxiway": { "bounds": { - "x": 0, - "y": 0, + "x": -101, + "y": -201, "width": 1, "height": 1 }, @@ -62,8 +67,8 @@ }, "gate": { "bounds": { - "x": 0, - "y": 0, + "x": -101, + "y": -201, "width": 1, "height": 1 }, @@ -88,8 +93,8 @@ }, "service": { "bounds": { - "x": 0, - "y": 0, + "x": -101, + "y": -201, "width": 1, "height": 1 }, @@ -114,10 +119,10 @@ }, "test_zone": { "bounds": { - "x": 0, - "y": 0, - "width": 5000, - "height": 4000 + "x": -100, + "y": -200, + "width": 800, + "height": 400 }, "config": { "collision_radius": { diff --git a/config/system_config.json b/config/system_config.json index e2ec469..367e35f 100644 --- a/config/system_config.json +++ b/config/system_config.json @@ -4,8 +4,8 @@ "iata": "TAO", "icao": "ZSQD", "reference_point": { - "latitude": 36.34807893, - "longitude": 120.08201044 + "latitude": 36.35448347, + "longitude": 120.08502054 }, "coordinate_points": [ {"point": "T1", "longitude": 120.0868853, "latitude": 36.35496367}, diff --git a/docs/design.md b/docs/design.md index 7064ef6..26b8ef8 100644 --- a/docs/design.md +++ b/docs/design.md @@ -965,3 +965,106 @@ void sendCommand(const ControlCommand& command, const std::string& ip, int port) - 添加部署回滚机制 - 添加部署验证步骤 - 完善部署文档 + +## 机场边界设计 + +### 问题描述 + +实际机场通常是倾斜的（如东南-西北走向），需要一个合适的方案来判断车辆和飞机是否在机场各个区域内。 + +### 设计方案 + +#### 坐标系统设计 + +1. 世界坐标系 + - 使用标准的笛卡尔坐标系 + - X 轴指向东，Y 轴指向北 + +2. 机场局部坐标系 + - 原点：可配置的机场参考点 + - 方向：通过旋转角度将世界坐标系对齐到机场主轴 + - 旋转规则： + - 以正北方向为基准 + - 正值表示逆时针旋转 + - 负值表示顺时针旋转 + - 例：东南-西北走向的机场（以青岛国际机场为例），从东南指向西北为 -21.47 度 + +#### 核心组件 + +1. `AirportBounds` 类 + - 负责管理机场边界和区域定义 + - 提供坐标转换功能 + - 判断位置所属区域 + +2. 配置文件 (`airport_bounds.json`) + + ```json + { + "airport": { + "rotation_angle": -45.0, // 机场旋转角度（度） + "reference_point": { // 机场参考点 + "x": 0.0, + "y": 0.0 + }, + "bounds": { // 机场总边界 + "x": -2000, + "y": -1000, + "width": 4000, + "height": 2000 + } + }, + "areas": { // 各功能区定义 + // ... 具体区域配置 + } + } + ``` + +#### 坐标转换流程 + +1. 位置判断流程 + - 接收世界坐标系中的点坐标 + - 将点坐标转换到机场局部坐标系 + - 在局部坐标系中进行边界检查 + +2. 坐标转换算法 + + ```cpp + Vector2D toAirportCoordinate(const Vector2D& point) { + // 1. 平移到参考点为原点 + double dx = point.x - referencePoint_.x; + double dy = point.y - referencePoint_.y; + + // 2. 应用旋转变换 + double cos_angle = std::cos(rotationAngle_); + double sin_angle = std::sin(rotationAngle_); + + return Vector2D{ + dx * cos_angle + dy * sin_angle, + -dx * sin_angle + dy * cos_angle + }; + } + ``` + +### 优势 + +1. 实现简单，易于理解和维护 +2. 计算开销小 +3. 不需要修改现有的碰撞检测逻辑 +4. 可以方便地调整机场方向 +5. 支持不同区域的差异化配置 + +### 使用说明 + +1. 确定机场实际旋转角度 +2. 选择合适的参考点（通常是机场中心或跑道中心） +3. 在机场局部坐标系中定义各个区域的边界 +4. 配置各区域的安全参数（如碰撞半径、警告区域等） + +### 注意事项 + +1. 角度配置采用度数（而非弧度） +2. 顺时针旋转使用负角度 +3. 确保参考点选择合理，便于区域划分 +4. 区域边界在局部坐标系中定义 + +如果需要计算角度（比如判断车辆朝向），则使用向量点积方法 diff --git a/src/collector/DataCollector.cpp b/src/collector/DataCollector.cpp index 569d14c..11b0a91 100644 --- a/src/collector/DataCollector.cpp +++ b/src/collector/DataCollector.cpp @@ -6,11 +6,12 @@ #include #include -DataCollector::DataCollector() +DataCollector::DataCollector(const AirportBounds& bounds) : last_position_fetch_(std::chrono::steady_clock::now()) , last_unmanned_fetch_(std::chrono::steady_clock::now()) , last_traffic_light_fetch_(std::chrono::steady_clock::now()) - , last_warning_time_(std::chrono::steady_clock::now()) { + , last_warning_time_(std::chrono::steady_clock::now()) + , airportBounds_(bounds) { } DataCollector::~DataCollector() { @@ -250,11 +251,36 @@ void DataCollector::sendTimeoutWarning(const std::string& source, int64_t elapse system_->broadcastTimeoutWarning(msg); last_warning_time_ = now; - - Logger::debug("Sent timeout warning: source=", msg.source, - " elapsed=", msg.elapsed_ms, "ms", - " is_read_timeout=", msg.is_read_timeout ? "true" : "false", - " timeout=", timeout_ms); + } +} + +void DataCollector::filterPositionData(std::vector& aircraft, std::vector& vehicles) { + size_t originalAircraftCount = aircraft.size(); + size_t originalVehicleCount = vehicles.size(); + + // 过滤航空器数据 + auto aircraftIt = std::remove_if(aircraft.begin(), aircraft.end(), + [this](const Aircraft& a) { + return !airportBounds_.isPointInBounds(a.position); + }); + aircraft.erase(aircraftIt, aircraft.end()); + + // 过滤车辆数据 + auto vehicleIt = std::remove_if(vehicles.begin(), vehicles.end(), + [this](const Vehicle& v) { + return !airportBounds_.isPointInBounds(v.position); + }); + vehicles.erase(vehicleIt, vehicles.end()); + + // 记录过滤结果 + size_t filteredAircraftCount = originalAircraftCount - aircraft.size(); + size_t filteredVehicleCount = originalVehicleCount - vehicles.size(); + + if (filteredAircraftCount > 0 || filteredVehicleCount > 0) { + Logger::info( + "数据过滤结果: 过滤掉 ", filteredAircraftCount, " 架航空器, ", + filteredVehicleCount, " 辆车辆" + ); } } @@ -272,6 +298,53 @@ bool DataCollector::fetchPositionData() { if (success) { std::lock_guard lock(cacheMutex_); + + // 更新运动信息 + for (auto& a : newAircraft) { + auto it = lastAircraftPositions_.find(a.id); + if (it != lastAircraftPositions_.end()) { + // 复制历史记录 + a.copyHistoryFrom(it->second); + } + // 更新运动信息 + a.updateMotion(a.geo, a.timestamp); + lastAircraftPositions_[a.id] = a; + // 更新时间戳 + lastAircraftTimestamp_ = a.timestamp; + } + + // 更新运动信息 + for (auto& v : newVehicles) { + auto it = lastVehiclePositions_.find(v.id); + if (it != lastVehiclePositions_.end()) { + // 复制历史记录 + v.copyHistoryFrom(it->second); + } + // 更新运动信息 + v.updateMotion(v.geo, v.timestamp); + lastVehiclePositions_[v.id] = v; + // 更新时间戳 + lastVehicleTimestamp_ = v.timestamp; + } + + // 转换坐标系 + for (auto& a : newAircraft) { + // 1. 先转换为世界坐标 + a.position = CoordinateConverter::instance().toLocalXY(a.geo.latitude, a.geo.longitude); + // 2. 再转换为机场坐标 + a.position = airportBounds_.toAirportCoordinate(a.position); + } + + for (auto& v : newVehicles) { + // 1. 先转换为世界坐标 + v.position = CoordinateConverter::instance().toLocalXY(v.geo.latitude, v.geo.longitude); + // 2. 再转换为机场坐标 + v.position = airportBounds_.toAirportCoordinate(v.position); + } + + // 过滤数据 + filterPositionData(newAircraft, newVehicles); + aircraftCache_ = std::move(newAircraft); vehicleCache_ = std::move(newVehicles); resetTimeout("position"); @@ -286,23 +359,30 @@ bool DataCollector::fetchUnmannedVehicleData() { return false; } - bool success = false; + bool any_success = false; + bool any_failure = false; auto& vehicles = ControllableVehicles::getInstance().getVehicles(); + for (const auto& vehicle : vehicles) { if (vehicle.type == "UNMANNED") { std::string status; if (dataSource_->fetchUnmannedVehicleStatus(vehicle.vehicleNo, status)) { - success = true; + any_success = true; } else { + any_failure = true; Logger::error("获取无人车状态失败, vehicleNo: " + vehicle.vehicleNo); } } } - if (success) { + if (any_success) { resetTimeout("unmanned"); + // 如果有部分成功，就返回 true + return true; } - return success; + + // 只有全部失败才返回 false + return !any_failure; } bool DataCollector::fetchTrafficLightData() { @@ -318,6 +398,10 @@ bool DataCollector::fetchTrafficLightData() { if (success) { std::lock_guard lock(cacheMutex_); trafficLightCache_ = std::move(newSignals); + // 更新时间戳 + if (!trafficLightCache_.empty()) { + lastTrafficLightTimestamp_ = trafficLightCache_[0].timestamp; + } resetTimeout("traffic_light"); } @@ -346,61 +430,6 @@ std::vector DataCollector::getVehicleData() { return vehicleCache_; } -void DataCollector::refresh() { - try { - // 从数据源获取最新数据 - std::vector newAircraft; - std::vector newVehicles; - std::vector newSignals; - - // 获取红绿灯信号 - if (dataSource_->fetchTrafficLightSignals(newSignals)) { - std::lock_guard lock(cacheMutex_); - trafficLightCache_ = std::move(newSignals); - } - - if (dataSource_->fetchPositionAircraftData(newAircraft)) { - std::lock_guard lock(cacheMutex_); - // 更新运动信息 - for (auto& a : newAircraft) { - auto it = lastAircraftPositions_.find(a.id); - if (it != lastAircraftPositions_.end()) { - // 复制历史记录 - a.copyHistoryFrom(it->second); - // 更新运动信息 - a.updateMotion(a.geo, a.timestamp); - } else { - a.updateMotion(a.geo, a.timestamp); - } - lastAircraftPositions_[a.id] = a; - } - aircraftCache_ = std::move(newAircraft); - } - - if (dataSource_->fetchPositionVehicleData(newVehicles)) { - std::lock_guard lock(cacheMutex_); - // 更新运动信息 - for (auto& v : newVehicles) { - auto it = lastVehiclePositions_.find(v.id); - if (it != lastVehiclePositions_.end()) { - // 复制历史记录 - v.copyHistoryFrom(it->second); - // 更新运动信息 - v.updateMotion(v.geo, v.timestamp); - } else { - v.updateMotion(v.geo, v.timestamp); - } - lastVehiclePositions_[v.id] = v; - } - vehicleCache_ = std::move(newVehicles); - } - } - catch (const std::exception& e) { - Logger::error("刷新数据失败: ", e.what()); - throw; - } -} - bool DataCollector::fetchUnmannedVehicleStatus(const std::string& vehicle_id, std::string& status) { if (!dataSource_) { Logger::error("No data source available"); diff --git a/src/collector/DataCollector.h b/src/collector/DataCollector.h index 0650ddf..70e505b 100644 --- a/src/collector/DataCollector.h +++ b/src/collector/DataCollector.h @@ -13,35 +13,43 @@ #include "config/WarnConfig.h" #include "types/TrafficLightTypes.h" #include "vehicle/ControllableVehicles.h" +#include "config/AirportBounds.h" // 前向声明 class System; class DataCollector { public: - DataCollector(); + DataCollector(const AirportBounds& bounds); ~DataCollector(); - bool initialize(const DataSourceConfig& config, const WarnConfig& warnConfig); + bool initialize(const DataSourceConfig& dataSourceConfig, const WarnConfig& warnConfig); void start(); void stop(); - void refresh(); - void setSystem(std::shared_ptr system) { system_ = system; } + // 获取最新数据的接口 + std::tuple, std::vector, std::vector> + getLatestData() const { + std::lock_guard lock(cacheMutex_); + return std::make_tuple(aircraftCache_, vehicleCache_, trafficLightCache_); + } - // 获取数据的接口 + // 获取数据的最后更新时间 + std::tuple getLastUpdateTimestamps() const { + std::lock_guard lock(cacheMutex_); + return {lastAircraftTimestamp_, lastVehicleTimestamp_, lastTrafficLightTimestamp_}; + } + + // 获取单个数据的接口 std::vector getAircraftData(); std::vector getVehicleData(); std::vector getTrafficLightSignals(); bool fetchTrafficLightSignals(std::vector& signals); - - // 无人车状态获取接口 bool fetchUnmannedVehicleStatus(const std::string& vehicle_id, std::string& status); + bool fetchPositionData(); // 获取位置数据 - // 添加设置数据源的方法（用于测试） - void setDataSource(std::shared_ptr source) { - dataSource_ = source; - } + void setSystem(std::shared_ptr system) { system_ = system; } + void setDataSource(std::shared_ptr source) { dataSource_ = source; } // 获取数据的引用访问接口 const std::vector& getAircraftRef() const { @@ -59,6 +67,7 @@ private: std::shared_ptr system_; DataSourceConfig dataSourceConfig_; WarnConfig warnConfig_; + const AirportBounds& airportBounds_; // 三个独立的采集线程 std::thread positionThread_; // 位置数据采集线程 @@ -80,6 +89,7 @@ private: // 数据时间戳 std::atomic lastAircraftTimestamp_{0}; std::atomic lastVehicleTimestamp_{0}; + std::atomic lastTrafficLightTimestamp_{0}; // 超时检测相关 std::chrono::steady_clock::time_point last_position_fetch_; // 位置数据最后获取时间 @@ -89,17 +99,19 @@ private: std::chrono::steady_clock::time_point last_log_time_; void checkTimeout(); - void resetTimeout(const std::string& source); // 修改resetTimeout方法签名 - void sendTimeoutWarning(const std::string& source, int64_t elapsed_ms); // 修改告警方法签名 + void resetTimeout(const std::string& source); + void sendTimeoutWarning(const std::string& source, int64_t elapsed_ms); // 三个独立的采集循环 void positionLoop(); // 位置数据采集循环 void unmannedLoop(); // 无人车状态采集循环 void trafficLightLoop(); // 红绿灯数据采集循环 - bool fetchPositionData(); // 获取位置数据 bool fetchUnmannedVehicleData(); // 获取无人车状态数据 bool fetchTrafficLightData(); // 获取红绿灯数据 + + // 数据过滤方法 + void filterPositionData(std::vector& aircraft, std::vector& vehicles); }; #endif // AIRPORT_COLLECTOR_DATA_COLLECTOR_H \ No newline at end of file diff --git a/src/config/AirportBounds.cpp b/src/config/AirportBounds.cpp index 78cdd8f..0e9e625 100644 --- a/src/config/AirportBounds.cpp +++ b/src/config/AirportBounds.cpp @@ -3,6 +3,7 @@ #include #include #include "utils/Logger.h" +#include using json = nlohmann::json; @@ -35,13 +36,25 @@ void AirportBounds::loadConfig(const std::string& configFile) { throw std::runtime_error("配置文件缺少 airport.bounds 字段"); } + // 加载机场旋转角度（度）和参考点 + auto& airport = j["airport"]; + if (airport.contains("rotation_angle")) { + // 将角度转换为弧度 + rotationAngle_ = airport["rotation_angle"].get() * M_PI / 180.0; + } + + if (airport.contains("reference_point")) { + referencePoint_.x = airport["reference_point"]["x"].get(); + referencePoint_.y = airport["reference_point"]["y"].get(); + } + // 加载机场边界 - auto& airport = j["airport"]["bounds"]; + auto& airportBounds = j["airport"]["bounds"]; airportBounds_ = { - airport["x"].get(), - airport["y"].get(), - airport["width"].get(), - airport["height"].get() + airportBounds["x"].get(), + airportBounds["y"].get(), + airportBounds["width"].get(), + airportBounds["height"].get() }; // 检查 areas 字段 @@ -115,13 +128,33 @@ void AirportBounds::loadConfig(const std::string& configFile) { } } +Vector2D AirportBounds::toAirportCoordinate(const Vector2D& point) const { + // 1. 计算相对于参考点的偏移 + double dx = point.x - referencePoint_.x; + double dy = point.y - referencePoint_.y; + + // 2. 应用旋转变换 + double cos_angle = std::cos(rotationAngle_); + double sin_angle = std::sin(rotationAngle_); + + // 3. 返回机场坐标系中的坐标（逆时针旋转） + Vector2D result{ + dx * cos_angle - dy * sin_angle, + dx * sin_angle + dy * cos_angle + }; + + return result; +} + AreaType AirportBounds::getAreaType(const Vector2D& position) const { + // 按优先级检查位置所在区域 for (const auto& [type, bounds] : areaBounds_) { if (bounds.contains(position)) { return type; } } + // 默认返回测试区 return AreaType::TEST_ZONE; } @@ -132,4 +165,21 @@ const AreaConfig& AirportBounds::getAreaConfig(AreaType type) const { throw std::runtime_error("Invalid area type"); } return it->second; +} + +bool AirportBounds::isPointInBounds(const Vector2D& position) const { + // 在机场坐标系中判断是否在边界内 + bool result = airportBounds_.contains(position); + return result; +} + +bool AirportBounds::isPointInArea(const Vector2D& position, AreaType areaType) const { + // 获取区域定义 + auto it = areaBounds_.find(areaType); + if (it == areaBounds_.end()) { + return false; + } + + // 在机场坐标系中判断是否在区域内 + return it->second.contains(position); } \ No newline at end of file diff --git a/src/config/AirportBounds.h b/src/config/AirportBounds.h index afdf6dc..55d212a 100644 --- a/src/config/AirportBounds.h +++ b/src/config/AirportBounds.h @@ -32,6 +32,17 @@ public: return it->second; } + // 判断点是否在机场边界内 + virtual bool isPointInBounds(const Vector2D& worldPoint) const; + + // 判断点是否在指定区域内（跑道、滑行道等） + virtual bool isPointInArea(const Vector2D& worldPoint, AreaType areaType) const; + + // 将点从世界坐标系转换到机场坐标系 + // 输入：世界坐标系中的点 (x, y) + // 输出：机场坐标系中的点，原点为机场参考点，方向为机场旋转后的方向 + Vector2D toAirportCoordinate(const Vector2D& point) const; + protected: Bounds airportBounds_; // 整个机场边界 std::unordered_map areaBounds_; // 各区域边界 @@ -39,6 +50,9 @@ protected: // 从配置文件加载数据 virtual void loadConfig(const std::string& configFile); + + double rotationAngle_ = 0.0; // 机场与正北方向的夹角（弧度） + Vector2D referencePoint_; // 机场参考点（旋转中心） }; #endif // AIRPORT_BOUNDS_H \ No newline at end of file diff --git a/src/core/System.cpp b/src/core/System.cpp index 2efc3f1..1e483b0 100644 --- a/src/core/System.cpp +++ b/src/core/System.cpp @@ -37,6 +37,12 @@ bool System::initialize() { // 配置已在 main 中加载，这里直接使用 const auto& system_config = SystemConfig::instance(); + // 初始化全局坐标转换器 + CoordinateConverter::instance().setReferencePoint( + system_config.airport.reference_point.latitude, + system_config.airport.reference_point.longitude + ); + // 加载路口配置 intersection_config_ = IntersectionConfig::load("config/intersections.json"); Logger::info("Loaded {} intersections", intersection_config_.getIntersections().size()); @@ -55,20 +61,18 @@ bool System::initialize() { // 初始化红绿灯检测器 trafficLightDetector_ = std::make_unique(intersection_config_, controllableVehicles_, *this); + + // 初始化安全区 + initializeSafetyZones(); - // 创建数据采集器 - dataCollector_ = std::make_unique(); - - // 数据采集器初始化并启动 + // 加载告警配置 WarnConfig warnConfig{ system_config.warning.warning_interval_ms, system_config.warning.log_interval_ms }; - - // 初始化安全区 - initializeSafetyZones(); - - // 初始化数据采集器 + + // 创建数据采集器并初始化 + dataCollector_ = std::make_unique(*airportBounds_); return dataCollector_->initialize(system_config.data_source, warnConfig); } catch (const std::exception& e) { @@ -84,17 +88,10 @@ void System::initializeSafetyZones() { // 获取所有路口配置 const auto& intersections = intersection_config_.getIntersections(); - // 创建坐标转换器 - CoordinateConverter converter; - converter.setReferencePoint( - SystemConfig::instance().airport.reference_point.latitude, - SystemConfig::instance().airport.reference_point.longitude - ); - // 为每个路口创建安全区 for (const auto& intersection : intersections) { // 获取路口中心点的地理坐标 - Vector2D center = converter.toLocalXY( + Vector2D center = CoordinateConverter::instance().toLocalXY( intersection.position.latitude, intersection.position.longitude ); @@ -156,241 +153,108 @@ void System::stop() { } void System::processLoop() { - auto last_aircraft_update = std::chrono::steady_clock::now(); - auto last_vehicle_update = std::chrono::steady_clock::now(); - auto last_collision_update = std::chrono::steady_clock::now(); - auto last_traffic_light_update = std::chrono::steady_clock::now(); - auto last_safety_zone_update = std::chrono::steady_clock::now(); - - // 添加三种数据的最后采集时间 - auto last_position_collection = std::chrono::steady_clock::now(); - auto last_vehicle_collection = std::chrono::steady_clock::now(); - auto last_traffic_light_collection = std::chrono::steady_clock::now(); - - int64_t last_aircraft_timestamp = 0; - int64_t last_vehicle_timestamp = 0; - int64_t last_collision_timestamp = 0; - int64_t last_traffic_light_timestamp = 0; - int64_t last_safety_zone_timestamp = 0; - - Logger::debug("数据处理循环启动"); - while (running_) { try { auto now = std::chrono::steady_clock::now(); - const auto& system_config = SystemConfig::instance(); - Logger::debug("开始获取数据..."); + // 获取最新数据和时间戳 + std::tie(latest_aircraft_, latest_vehicles_, latest_traffic_lights_) = dataCollector_->getLatestData(); + auto [aircraft_ts, vehicle_ts, traffic_light_ts] = dataCollector_->getLastUpdateTimestamps(); - // 计算各数据源是否需要采集 - bool need_position_collection = std::chrono::duration_cast( - now - last_position_collection).count() >= system_config.data_source.position.refresh_interval_ms; - - bool need_vehicle_collection = std::chrono::duration_cast( - now - last_vehicle_collection).count() >= system_config.data_source.vehicle.refresh_interval_ms; - - bool need_traffic_light_collection = std::chrono::duration_cast( - now - last_traffic_light_collection).count() >= system_config.data_source.traffic_light.refresh_interval_ms; + // 检查数据更新 + bool has_new_data = false; - // 根据需要刷新相应的数据 - if (need_position_collection || need_vehicle_collection || need_traffic_light_collection) { - dataCollector_->refresh(); // 刷新数据 - - if (need_position_collection) { - last_position_collection = now; - } - if (need_vehicle_collection) { - last_vehicle_collection = now; - } - if (need_traffic_light_collection) { - last_traffic_light_collection = now; - } + if (aircraft_ts > last_aircraft_timestamp) { + has_new_data = true; + last_aircraft_timestamp = aircraft_ts; + Logger::debug("航空器数据已更新，新时间戳: ", aircraft_ts); } - // 获取最新数据 - auto aircraft = dataCollector_->getAircraftData(); - auto vehicles = dataCollector_->getVehicleData(); - auto traffic_lights = dataCollector_->getTrafficLightSignals(); - - // 合并航空器和车辆数据用于安全区检查 - std::vector objects; - for (auto& ac : aircraft) { - objects.push_back(&ac); + if (vehicle_ts > last_vehicle_timestamp) { + has_new_data = true; + last_vehicle_timestamp = vehicle_ts; + Logger::debug("车辆数据已更新，新时间戳: ", vehicle_ts); } - for (auto& veh : vehicles) { - objects.push_back(&veh); - const auto* config = controllableVehicles_.findVehicle(veh.vehicleNo); - if (config) { - veh.type = config->type == "UNMANNED" ? MovingObjectType::UNMANNED : MovingObjectType::SPECIAL; - veh.isControllable = config->type == "UNMANNED"; + + if (traffic_light_ts > last_traffic_light_timestamp) { + has_new_data = true; + last_traffic_light_timestamp = traffic_light_ts; + Logger::debug("红绿灯数据已更新，新时间戳: ", traffic_light_ts); + } + + if (has_new_data) { + // 使用成员变量的引用构建 objects 向量 + std::vector objects; + for (auto& ac : latest_aircraft_) { + objects.push_back(&ac); + } + for (auto& veh : latest_vehicles_) { + const auto* config = controllableVehicles_.findVehicle(veh.vehicleNo); + if (config) { + veh.type = config->type == "UNMANNED" ? MovingObjectType::UNMANNED : MovingObjectType::SPECIAL; + veh.isControllable = config->type == "UNMANNED"; + } + objects.push_back(&veh); } - } - - // 检查安全区更新 - auto safety_zone_elapsed = std::chrono::duration_cast( - now - last_safety_zone_update).count(); - Logger::debug("安全区检测时间间隔: elapsed=", safety_zone_elapsed, "ms, interval=", system_config.collision_detection.update_interval_ms, "ms"); - if (safety_zone_elapsed >= system_config.collision_detection.update_interval_ms) { - // 只有在数据更新时才进行检测 - Logger::debug("数据时间戳: aircraft=", last_aircraft_timestamp, ", vehicle=", last_vehicle_timestamp, ", last_safety=", last_safety_zone_timestamp); - if (last_aircraft_timestamp > last_safety_zone_timestamp || - last_vehicle_timestamp > last_safety_zone_timestamp) { - // 更新安全区状态 + // 创建当前周期的风险管理器 + std::unordered_map vehicleMaxRiskLevels; // 统一记录所有车辆的最高风险等级 + std::vector detectedRisks; // 统一收集所有检测到的风险 + + // 检查安全区更新 + auto safety_zone_elapsed = std::chrono::duration_cast( + now - last_safety_zone_update).count(); + + if (safety_zone_elapsed >= SystemConfig::instance().collision_detection.update_interval_ms) { updateSafetyZoneStates(objects); Logger::debug("开始检查安全区冲突..."); - checkUnmannedVehicleSafetyZones(vehicles, objects); - last_safety_zone_timestamp = std::max(last_aircraft_timestamp, last_vehicle_timestamp); + + // 记录安全区风险检测前的风险数量 + size_t beforeSafetyZone = detectedRisks.size(); + checkUnmannedVehicleSafetyZones(latest_vehicles_, objects, vehicleMaxRiskLevels, detectedRisks); + Logger::debug("安全区检测新增风险数量: ", detectedRisks.size() - beforeSafetyZone); + + last_safety_zone_update = now; + } + + // 检查和处理常规碰撞风险 + collisionDetector_->updateTraffic(latest_aircraft_, latest_vehicles_); + auto collisionRisks = collisionDetector_->detectCollisions(); + + Logger::debug("碰撞检测器检测到风险数量: ", collisionRisks.size()); + for (const auto& risk : collisionRisks) { + Logger::debug("碰撞风险: id1=", risk.id1, + ", id2=", risk.id2, + ", level=", static_cast(risk.level), + ", distance=", risk.distance); + } + + // 合并风险 + size_t beforeMerge = detectedRisks.size(); + detectedRisks.insert(detectedRisks.end(), collisionRisks.begin(), collisionRisks.end()); + Logger::debug("合并后新增风险数量: ", detectedRisks.size() - beforeMerge); + + // 统一处理所有风险 + processCollisions(detectedRisks, vehicleMaxRiskLevels); + + // 广播位置更新 + for (const auto& ac : latest_aircraft_) { + broadcastPositionUpdate(ac); + } + for (const auto& veh : latest_vehicles_) { + broadcastPositionUpdate(veh); + } + + // 处理红绿灯信号 + for (const auto& signal : latest_traffic_lights_) { + broadcastTrafficLightStatus(signal); + trafficLightDetector_->processSignal(signal, latest_vehicles_); } - last_safety_zone_update = now; } - // 检查无人车与安全区的冲突 - //checkUnmannedVehicleSafetyZones(vehicles, objects); - - // 检查航空器更新 - auto aircraft_elapsed = std::chrono::duration_cast( - now - last_aircraft_update).count(); - if (aircraft_elapsed >= system_config.websocket.position_update.aircraft_interval_ms) { - bool has_new_aircraft = false; - int64_t max_timestamp = last_aircraft_timestamp; - - Logger::debug("开始检查航空器数据更新，当前时间戳: ", last_aircraft_timestamp); - for (const auto& ac : aircraft) { - Logger::debug("航空器 ", ac.flightNo, " 时间戳: current=", ac.timestamp, " last=", last_aircraft_timestamp); - if (ac.timestamp > last_aircraft_timestamp) { // 改为大于比较 - has_new_aircraft = true; - if (ac.timestamp > max_timestamp) { - max_timestamp = ac.timestamp; - } - Logger::debug("发现新数据: ", ac.flightNo, " (新时间戳: ", ac.timestamp, ")"); - } - } - - if (has_new_aircraft) { - Logger::debug("广播 ", aircraft.size(), " 架航空器位置, 时间戳更新: ", last_aircraft_timestamp, " -> ", max_timestamp); - for (const auto& ac : aircraft) { - broadcastPositionUpdate(ac); - } - last_aircraft_timestamp = max_timestamp; - } else { - Logger::debug("没有新的航空器数据，当前时间戳: ", last_aircraft_timestamp); - } - last_aircraft_update = now; - } - - // 检查车辆更新 - auto vehicle_elapsed = std::chrono::duration_cast( - now - last_vehicle_update).count(); - if (vehicle_elapsed >= system_config.websocket.position_update.vehicle_interval_ms) { - bool has_new_vehicles = false; - int64_t max_timestamp = last_vehicle_timestamp; - - Logger::debug("车辆位置更新检查开始 >>>>>>>>>>>>>>>>>"); - Logger::debug("当前系统时间: ", std::chrono::duration_cast( - now.time_since_epoch()).count()); - Logger::debug("上次更新时间: ", std::chrono::duration_cast( - last_vehicle_update.time_since_epoch()).count()); - Logger::debug("距离上次更新的时间间隔: ", vehicle_elapsed, "ms"); - Logger::debug("配置的更新间隔: ", system_config.websocket.position_update.vehicle_interval_ms, "ms"); - Logger::debug("上次时间戳: ", last_vehicle_timestamp); - Logger::debug("收辆数量: ", vehicles.size()); - - // 如果是第一次更新（last_vehicle_timestamp == 0），则广播所有车辆位置 - if (last_vehicle_timestamp == 0 && !vehicles.empty()) { - has_new_vehicles = true; - for (const auto& veh : vehicles) { - if (veh.timestamp > max_timestamp) { - max_timestamp = veh.timestamp; - } - Logger::debug("首次更新，广播车辆: ", veh.vehicleNo); - } - } else { - // 正常的更新检查 - for (const auto& veh : vehicles) { - if (veh.timestamp > last_vehicle_timestamp) { - has_new_vehicles = true; - if (veh.timestamp > max_timestamp) { - max_timestamp = veh.timestamp; - } - Logger::debug(" - 发现新数据!"); - } else { - Logger::debug(" - 数据未更新"); - } - } - } - - if (has_new_vehicles) { - Logger::debug("发现新数据，准备广播..."); - Logger::debug("更新间戳: ", last_vehicle_timestamp, " -> ", max_timestamp); - for (const auto& veh : vehicles) { - broadcastPositionUpdate(veh); - } - last_vehicle_timestamp = max_timestamp; - Logger::debug("广播完成"); - } else { - Logger::debug("没有新数据需要广播"); - } - - last_vehicle_update = now; - Logger::debug("车辆位置更新检查结束 <<<<<<<<<<<<<<<"); - } - - // 处理红绿灯信号 - auto traffic_light_elapsed = std::chrono::duration_cast( - now - last_traffic_light_update).count(); - if (traffic_light_elapsed >= system_config.websocket.position_update.traffic_light_interval_ms) { - bool has_new_signals = false; - int64_t max_timestamp = last_traffic_light_timestamp; - - Logger::debug("开始检查红绿灯数据更新，当前时间戳: ", last_traffic_light_timestamp); - for (const auto& signal : traffic_lights) { - if (signal.timestamp > last_traffic_light_timestamp) { - has_new_signals = true; - if (signal.timestamp > max_timestamp) { - max_timestamp = signal.timestamp; - } - Logger::debug("红绿灯 ", signal.trafficLightId, " 时间戳: current=", signal.timestamp, " last=", last_traffic_light_timestamp); - Logger::debug("发现新数据: ", signal.trafficLightId, " (新时间戳: ", signal.timestamp, ")"); - - // 广播红绿灯状态更新 - broadcastTrafficLightStatus(signal); - - // 处理红绿灯信号并检查是否需要停车 - trafficLightDetector_->processSignal(signal, vehicles); - } - } - - if (has_new_signals) { - Logger::debug("处理了 ", traffic_lights.size(), " 个红绿灯信号, 时间戳更新: ", - last_traffic_light_timestamp, " -> ", max_timestamp); - last_traffic_light_timestamp = max_timestamp; - } - last_traffic_light_update = now; - } - - // 计算下一次数据采集的等待时间 - auto next_position_collection = last_position_collection + - std::chrono::milliseconds(system_config.data_source.position.refresh_interval_ms); - - auto next_vehicle_collection = last_vehicle_collection + - std::chrono::milliseconds(system_config.data_source.vehicle.refresh_interval_ms); - - auto next_traffic_light_collection = last_traffic_light_collection + - std::chrono::milliseconds(system_config.data_source.traffic_light.refresh_interval_ms); - - // 取最小等待时间 - auto next_collection = std::min({next_position_collection, - next_vehicle_collection, - next_traffic_light_collection}); - - auto wait_time = std::chrono::duration_cast( - next_collection - now).count(); - - if (wait_time > 0) { - Logger::debug("等待 ", wait_time, "ms 进行下一次采集"); - std::this_thread::sleep_for(std::chrono::milliseconds(wait_time)); - } + // 等待下一次处理 + std::this_thread::sleep_for(std::chrono::milliseconds( + SystemConfig::instance().collision_detection.update_interval_ms)); } catch (const std::exception& e) { Logger::error("处理循环发生错误: ", e.what()); @@ -398,105 +262,150 @@ void System::processLoop() { } } -void System::processCollisions(const std::vector& risks) { +void System::checkUnmannedVehicleSafetyZones( + const std::vector& vehicles, + const std::vector& objects, + std::unordered_map& vehicleMaxRiskLevels, + std::vector& detectedRisks) { + + // 遍历所有无人车 + for (const auto& vehicle : vehicles) { + // 遍历所有路口安全区 + for (const auto& [intersectionId, zone] : safetyZones_) { + if (zone->getState() == SafetyZoneState::INACTIVE) { + continue; + } + + // 计算无人车到安全区中心的距离 + double dx = vehicle.position.x - zone->getCenter().x; + double dy = vehicle.position.y - zone->getCenter().y; + double distance = std::sqrt(dx * dx + dy * dy); + double zoneRadius = zone->getCurrentRadius(); + + RiskLevel riskLevel = RiskLevel::NONE; + if (distance <= zoneRadius) { + riskLevel = RiskLevel::CRITICAL; + } else if (distance <= 2 * zoneRadius) { + riskLevel = RiskLevel::WARNING; + } + + if (riskLevel != RiskLevel::NONE) { + // 查找触发安全区的目标物体 + for (const auto* obj : objects) { + // 添加判断：跳过与自己的碰撞检测 + if (obj->id == vehicle.id) { + continue; + } + + if ((obj->isAircraft() || obj->isSpecialVehicle()) && + zone->isObjectInZone(*obj)) { + // 创建风险对象 + CollisionRisk risk; + risk.id1 = vehicle.id; + risk.id2 = obj->id; + risk.level = riskLevel; + risk.distance = distance; + risk.minDistance = distance; + risk.relativeSpeed = std::sqrt( + std::pow(obj->speed * std::cos(obj->heading) - + vehicle.speed * std::cos(vehicle.heading), 2) + + std::pow(obj->speed * std::sin(obj->heading) - + vehicle.speed * std::sin(vehicle.heading), 2) + ); + risk.relativeMotion = { + obj->position.x - vehicle.position.x, + obj->position.y - vehicle.position.y + }; + + detectedRisks.push_back(risk); + + // 更新风险级别 + auto& currentRisk = vehicleMaxRiskLevels[vehicle.id]; + currentRisk = std::max(currentRisk, riskLevel); + + Logger::debug("安全区风险: id1=", risk.id1, + ", id2=", risk.id2, + ", level=", static_cast(risk.level), + ", distance=", risk.distance); + + break; + } + } + } + } + } +} + +void System::processCollisions( + const std::vector& detectedRisks, + std::unordered_map& vehicleMaxRiskLevels) { + // 记录当前有风险的可控车辆 std::unordered_set currentVehiclesWithRisk; - const auto& config = SystemConfig::instance().collision_detection; - Logger::debug("开始处理碰撞风险，当前风险数量: ", risks.size()); + // 修改日志输出，显示更多信息 + Logger::debug("开始处理碰撞风险，风险数量: ", detectedRisks.size()); + for (const auto& risk : detectedRisks) { + Logger::debug("待处理风险: id1=", risk.id1, + ", id2=", risk.id2, + ", level=", static_cast(risk.level), + ", distance=", risk.distance, + ", minDistance=", risk.minDistance); + } // 处理当前的碰撞风险 - for (const auto& risk : risks) { + for (const auto& risk : detectedRisks) { // 处理 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 RiskLevel::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::duration_cast( - 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; - cmd.relativeMotionX = risk.relativeMotion.x; - cmd.relativeMotionY = risk.relativeMotion.y; - cmd.minDistance = risk.minDistance; - } - - broadcastVehicleCommand(cmd); - controllableVehicles_.sendCommand(vehicleId, cmd); - Logger::warning("发送告警指令到车辆: ", vehicleId, - " 当前距离: ", risk.distance, "m", - " 预测最小距离: ", risk.minDistance, "m", - " 相对速度: ", risk.relativeSpeed, "m/s", - " 相对运动: (", risk.relativeMotion.x, ",", risk.relativeMotion.y, ")"); - break; - } - case RiskLevel::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::duration_cast( - 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; - cmd.relativeMotionX = risk.relativeMotion.x; - cmd.relativeMotionY = risk.relativeMotion.y; - cmd.minDistance = risk.minDistance; - } - - broadcastVehicleCommand(cmd); - controllableVehicles_.sendCommand(vehicleId, cmd); - Logger::info("发送预警指令到车辆: ", vehicleId, - " 当前距离: ", risk.distance, "m", - " 预测最小距离: ", risk.minDistance, "m", - " 相对速度: ", risk.relativeSpeed, "m/s", - " 相对运动: (", risk.relativeMotion.x, ",", risk.relativeMotion.y, ")"); - break; - } - default: - break; + // 发送指令 + VehicleCommand cmd; + cmd.vehicleId = vehicleId; + cmd.type = risk.level == RiskLevel::CRITICAL ? CommandType::ALERT : CommandType::WARNING; + cmd.reason = otherId.substr(0, 2) == "AC" ? + CommandReason::AIRCRAFT_CROSSING : + CommandReason::SPECIAL_VEHICLE; + cmd.timestamp = std::chrono::duration_cast( + 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; + cmd.relativeMotionX = risk.relativeMotion.x; + cmd.relativeMotionY = risk.relativeMotion.y; + cmd.minDistance = risk.minDistance; } + + Logger::debug("准备发送指令: 车辆=", vehicleId, + ", 类型=", cmd.type == CommandType::ALERT ? "ALERT" : "WARNING", + ", 原因=", cmd.reason == CommandReason::AIRCRAFT_CROSSING ? "AIRCRAFT_CROSSING" : "SPECIAL_VEHICLE", + ", 距离=", risk.distance, + ", 风险等级=", static_cast(risk.level)); + + broadcastVehicleCommand(cmd); + controllableVehicles_.sendCommand(vehicleId, cmd); + + // 更新风险记录 + currentVehiclesWithRisk.insert(vehicleId); + vehicleMaxRiskLevels[vehicleId] = risk.level; + + Logger::info("碰撞风险: 车辆=", vehicleId, + ", 目标=", otherId, + ", 距离=", risk.distance, "m", + ", 最小距离=", risk.minDistance, "m", + ", 相对速度=", risk.relativeSpeed, "m/s"); }; // 为每个可控车辆处理风险 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); } @@ -504,45 +413,26 @@ void System::processCollisions(const std::vector& risks) { 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::duration_cast( - std::chrono::system_clock::now().time_since_epoch()).count(); - - broadcastVehicleCommand(cmd); - controllableVehicles_.sendCommand(vehicleId, cmd); - Logger::info("发送恢复指令到车辆: ", vehicleId); - } else { - Logger::debug("车辆 ", vehicleId, " 仍然有风险，不发送恢复指令"); + auto riskIt = vehicleMaxRiskLevels.find(vehicleId); + if (riskIt == vehicleMaxRiskLevels.end() || riskIt->second == RiskLevel::NONE) { + VehicleCommand cmd; + cmd.vehicleId = vehicleId; + cmd.type = CommandType::RESUME; + cmd.reason = CommandReason::RESUME_TRAFFIC; + cmd.timestamp = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + + broadcastVehicleCommand(cmd); + controllableVehicles_.sendCommand(vehicleId, cmd); + Logger::info("发送恢复指令到车辆: ", 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); } @@ -779,67 +669,6 @@ void System::checkSafetyZoneIntrusion(const MovingObject& obj) { } } -void System::checkUnmannedVehicleSafetyZones(const std::vector& vehicles, - const std::vector& objects) { - // 遍历所有无人车 - for (const auto& vehicle : vehicles) { - bool hasRisk = false; - std::string riskIntersectionId; - - // 遍历所有路口安全区 - for (const auto& [intersectionId, zone] : safetyZones_) { - Logger::debug("检查路口 ", intersectionId, " 安全区..."); - // 如果安全区未激活，跳过 - if (zone->getState() == SafetyZoneState::INACTIVE) { - Logger::debug("路口 ", intersectionId, " 安全区未激活，跳过"); - continue; - } - - // 计算无人车到安全区中心的距离 - double dx = vehicle.position.x - zone->getCenter().x; - double dy = vehicle.position.y - zone->getCenter().y; - double distance = std::sqrt(dx * dx + dy * dy); - double zoneRadius = zone->getCurrentRadius(); - - // 检查是否在预警或告警范围内 - if (distance <= zoneRadius) { - // 告警范围 - Logger::warning("无人车 ", vehicle.id, " 进入路口 ", intersectionId, " 安全区告警范围"); - hasRisk = handleSafetyZoneRisk(vehicle, zone.get(), objects, distance, - intersectionId, CommandType::ALERT, "告警"); - riskIntersectionId = intersectionId; - } else if (distance <= 2 * zoneRadius) { - // 预警范围 - Logger::warning("无人车 ", vehicle.id, " 进入路口 ", intersectionId, " 安全区预警范围"); - hasRisk = handleSafetyZoneRisk(vehicle, zone.get(), objects, distance, - intersectionId, CommandType::WARNING, "预警"); - riskIntersectionId = intersectionId; - } - } - - // 检查是否需要发送恢复指令 - auto it = lastVehiclesWithSafetyZoneRisk_.find(vehicle.id); - if (!hasRisk && it != lastVehiclesWithSafetyZoneRisk_.end()) { - // 之前有风险，现在没有风险，发送恢复指令 - Logger::info("无人车 ", vehicle.id, " 离开路口 ", it->second, " 安全区"); - VehicleCommand cmd; - cmd.vehicleId = vehicle.id; - cmd.type = CommandType::RESUME; - cmd.reason = CommandReason::RESUME_TRAFFIC; - cmd.timestamp = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()).count(); - - broadcastVehicleCommand(cmd); - controllableVehicles_.sendCommand(vehicle.id, cmd); - - lastVehiclesWithSafetyZoneRisk_.erase(it); - } else if (hasRisk) { - // 更新最后一次风险记录 - lastVehiclesWithSafetyZoneRisk_[vehicle.id] = riskIntersectionId; - } - } -} - bool System::handleSafetyZoneRisk(const Vehicle& vehicle, const SafetyZone* zone, const std::vector& objects, @@ -889,7 +718,7 @@ bool System::handleSafetyZoneRisk(const Vehicle& vehicle, cmd.relativeMotionY = rel_dy; cmd.minDistance = distance; - Logger::debug("目标物体信息: id=", target->id, + Logger::debug("安全区冲突目标信息: id=", target->id, ", 相对距离=(", rel_dx, ",", rel_dy, ")", ", 相对速度=", cmd.relativeSpeed, ", 最小距离=", distance); diff --git a/src/core/System.h b/src/core/System.h index 2f2da10..501b685 100644 --- a/src/core/System.h +++ b/src/core/System.h @@ -44,7 +44,8 @@ public: private: void processLoop(); - void processCollisions(const std::vector& collisions); + void processCollisions(const std::vector& detectedRisks, + std::unordered_map& vehicleMaxRiskLevels); // 初始化安全区 void initializeSafetyZones(); @@ -57,7 +58,9 @@ private: // 检查无人车与安全区的冲突 void checkUnmannedVehicleSafetyZones(const std::vector& vehicles, - const std::vector& objects); + const std::vector& objects, + std::unordered_map& vehicleMaxRiskLevels, + std::vector& detectedRisks); // 处理安全区风险 bool handleSafetyZoneRisk(const Vehicle& vehicle, @@ -95,6 +98,17 @@ private: // 记录上一次有风险的车辆列表 std::unordered_set lastVehiclesWithRisk_; + // 时间戳 + int64_t last_aircraft_timestamp{0}; + int64_t last_vehicle_timestamp{0}; + int64_t last_traffic_light_timestamp{0}; + std::chrono::steady_clock::time_point last_safety_zone_update; + // 辅助函数 const MovingObject* findVehicle(const std::string& vehicleId) const; + + // 存储最新数据的成员变量 + std::vector latest_aircraft_; + std::vector latest_vehicles_; + std::vector latest_traffic_lights_; }; \ No newline at end of file diff --git a/src/detector/CollisionDetector.cpp b/src/detector/CollisionDetector.cpp index 0f4a9f2..d06cf41 100644 --- a/src/detector/CollisionDetector.cpp +++ b/src/detector/CollisionDetector.cpp @@ -16,11 +16,8 @@ CollisionDetector::CollisionDetector(const AirportBounds& bounds, const Controll void CollisionDetector::updateTraffic(const std::vector& aircraft, const std::vector& vehicles) { - // 记录更新前的状态 - Logger::debug( - "更新交通数据: 航空器数量=", aircraft.size(), - ", 车辆数量=", vehicles.size() - ); + Logger::debug("更新交通数据: 航空器数量=", aircraft.size(), + ", 车辆数量=", vehicles.size()); // 更新航空器数据 aircraftData_ = aircraft; @@ -30,53 +27,55 @@ void CollisionDetector::updateTraffic(const std::vector& aircraft, size_t validVehicles = 0; for (const auto& vehicle : vehicles) { - // 验证车辆位置是否在机场边界内 - const auto& bounds = airportBounds_.getAirportBounds(); - if (vehicle.position.x >= bounds.x && vehicle.position.x <= (bounds.x + bounds.width) && - vehicle.position.y >= bounds.y && vehicle.position.y <= (bounds.y + bounds.height)) { - - // 根据配置设置车辆类型 - Vehicle updatedVehicle = vehicle; - const auto* config = controllableVehicles_->findVehicle(vehicle.vehicleNo); - if (config) { - if (config->type == "UNMANNED") { - updatedVehicle.type = MovingObjectType::UNMANNED; - updatedVehicle.isControllable = true; - } else if (config->type == "SPECIAL") { - updatedVehicle.type = MovingObjectType::SPECIAL; - updatedVehicle.isControllable = false; - } - } else { - // 未配置的车辆默认为特勤车 + // 首先检查车辆是否在机场边界内 + if (!airportBounds_.isPointInBounds(vehicle.position)) { + Logger::error("车辆在机场边界外: id=", vehicle.id, + ", position=(", vehicle.position.x, + ",", vehicle.position.y, ")"); + continue; // 跳过边界外的车辆 + } + + // 根据配置设置车辆类型 + Vehicle updatedVehicle = vehicle; + const auto* config = controllableVehicles_->findVehicle(vehicle.vehicleNo); + if (config) { + if (config->type == "UNMANNED") { + updatedVehicle.type = MovingObjectType::UNMANNED; + updatedVehicle.isControllable = true; + } else if (config->type == "SPECIAL") { updatedVehicle.type = MovingObjectType::SPECIAL; updatedVehicle.isControllable = false; } - + } else { + updatedVehicle.type = MovingObjectType::SPECIAL; + updatedVehicle.isControllable = false; + } + + // 插入四叉树 + try { + Logger::debug("尝试插入车辆: id=", updatedVehicle.id, + ", position=(", updatedVehicle.position.x, ",", updatedVehicle.position.y, ")", + ", type=", updatedVehicle.isControllable ? "UNMANNED" : "SPECIAL"); vehicleTree_.insert(updatedVehicle); ++validVehicles; - } else { - Logger::warning( - "车辆位置超出机场边界: id=", vehicle.vehicleNo, - ", 位置=(", vehicle.position.x, - ",", vehicle.position.y, ")" - ); + Logger::debug("成功插入车辆: id=", updatedVehicle.id); + } catch (const std::exception& e) { + Logger::error("无法插入车辆到四叉树: id=", updatedVehicle.id, ", error=", e.what()); } } // 记录更新后的状态 - auto allVehicles = vehicleTree_.queryRange(vehicleTree_.getBounds()); - Logger::debug( - "交通数据更新完成: 有效车辆数量=", validVehicles, - ", 四叉树中的车辆数量=", allVehicles.size() - ); - - // 验证数据一致性 - if (validVehicles != allVehicles.size()) { - Logger::error( - "车辆数据不一致: 有效车辆数=", validVehicles, - ", 四叉树中的车辆数量=", allVehicles.size() - ); + auto bounds = vehicleTree_.getBounds(); + Logger::debug("四叉树边界: min=(", bounds.x, ",", bounds.y, + "), max=(", bounds.x + bounds.width, ",", bounds.y + bounds.height, ")"); + auto allVehicles = vehicleTree_.queryRange(bounds); + for (const auto& vehicle : allVehicles) { + Logger::debug("查询到车辆: id=", vehicle.id, + ", position=(", vehicle.position.x, ",", vehicle.position.y, ")", + ", type=", vehicle.isControllable ? "UNMANNED" : "SPECIAL"); } + Logger::debug("交通数据更新完成: 有效车辆数量=", validVehicles, + ", 四叉树中的车辆数量=", allVehicles.size()); } std::vector CollisionDetector::detectCollisions() { @@ -312,9 +311,11 @@ std::pair CollisionDetector::evaluateRisk( } } - // 记录调试信息 + // 修改日志，添加物体类型信息 Logger::debug( - "风险评估: 当前距离=", collisionResult.distance, + "风险评估: 物体1类型=", static_cast(type1), + ", 物体2类型=", static_cast(type2), + ", 当前距离=", collisionResult.distance, "m, 预测最小距离=", collisionResult.minDistance, "m, 预警阈值=", thresholds.warning, "m, 警报阈值=", thresholds.critical, @@ -332,6 +333,12 @@ CollisionResult CollisionDetector::checkCollision( const MovingObject& obj2, double timeWindow) const { + // 添加物体信息日志 + Logger::debug( + "检查碰撞: obj1[", obj1.id, "]=(", obj1.position.x, ",", obj1.position.y, + "), obj2[", obj2.id, "]=(", obj2.position.x, ",", obj2.position.y, ")" + ); + // 获取区域配置 const auto& areaConfig = getCollisionParams(obj1.position); @@ -368,6 +375,7 @@ CollisionResult CollisionDetector::checkCollision( CollisionResult result; result.willCollide = false; result.minDistance = current_distance; + result.distance = current_distance; return result; } diff --git a/src/network/HTTPDataSource.h b/src/network/HTTPDataSource.h index e81f0e3..25b42d3 100644 --- a/src/network/HTTPDataSource.h +++ b/src/network/HTTPDataSource.h @@ -41,7 +41,6 @@ public: bool fetchTrafficLightSignals(std::vector& signals) override; private: - CoordinateConverter coordinateConverter_; std::mutex mutex_; CURL* curl_; diff --git a/src/spatial/CoordinateConverter.cpp b/src/spatial/CoordinateConverter.cpp index dcbe02e..7081d49 100644 --- a/src/spatial/CoordinateConverter.cpp +++ b/src/spatial/CoordinateConverter.cpp @@ -1,5 +1,6 @@ #include "spatial/CoordinateConverter.h" #include "config/SystemConfig.h" +#include "utils/Logger.h" #include // 地球半径（米） @@ -16,7 +17,7 @@ void CoordinateConverter::setReferencePoint(double lat, double lon) noexcept { cos_ref_lat_ = std::cos(ref_lat_); // 预计算参考点的余弦值 } -Vector2D CoordinateConverter::toLocalXY(double lat, double lon) const noexcept { +Vector2D CoordinateConverter::toLocalXY(double lat, double lon) const noexcept { // 将经纬度转换为弧度 double lat_rad = lat * M_PI / 180.0; double lon_rad = lon * M_PI / 180.0; diff --git a/src/spatial/CoordinateConverter.h b/src/spatial/CoordinateConverter.h index 80cd164..cc78141 100644 --- a/src/spatial/CoordinateConverter.h +++ b/src/spatial/CoordinateConverter.h @@ -7,7 +7,15 @@ class CoordinateConverter { public: - CoordinateConverter() noexcept; + // 删除拷贝构造和赋值操作 + CoordinateConverter(const CoordinateConverter&) = delete; + CoordinateConverter& operator=(const CoordinateConverter&) = delete; + + // 获取全局单例 + static CoordinateConverter& instance() { + static CoordinateConverter instance; + return instance; + } // 设置参考点（经纬度） void setReferencePoint(double lat, double lon) noexcept; @@ -19,6 +27,9 @@ public: GeoPosition toLatLon(const Vector2D& pos) const noexcept; private: + // 私有构造函数 + CoordinateConverter() noexcept; + static constexpr double EARTH_RADIUS = 6378137.0; // WGS84椭球体赤道半径（米） double ref_lat_{0.0}; // 参考点纬度（弧度） double ref_lon_{0.0}; // 参考点经度（弧度） diff --git a/src/spatial/QuadTree.h b/src/spatial/QuadTree.h index e233c2d..808bb0f 100644 --- a/src/spatial/QuadTree.h +++ b/src/spatial/QuadTree.h @@ -45,10 +45,6 @@ public: const Bounds& getBounds() const { return bounds_; } void insert(const T& item) { - if (!bounds_.contains(getPosition(item))) { - return; - } - if (items_.size() < capacity_ && !divided_) { items_.push_back(item); return; diff --git a/src/spatial/QuadTree.inl b/src/spatial/QuadTree.inl deleted file mode 100644 index 7c02dde..0000000 --- a/src/spatial/QuadTree.inl +++ /dev/null @@ -1,104 +0,0 @@ -template -QuadTree::QuadTree(const Bounds& bounds, int capacity) - : bounds_(bounds), capacity_(capacity) {} - -template -void QuadTree::insert(const T& item) { - if (!bounds_.contains(getPosition(item))) { - return; - } - - if (items_.size() < capacity_ && !divided_) { - items_.push_back(item); - return; - } - - if (!divided_) { - subdivide(); - } - - northwest_->insert(item); - northeast_->insert(item); - southwest_->insert(item); - southeast_->insert(item); -} - -template -std::vector QuadTree::queryRange(const Bounds& range) const { - std::vector found; - if (!bounds_.intersects(range)) { - return found; - } - - for (const auto& item : items_) { - if (range.contains(getPosition(item))) { - found.push_back(item); - } - } - - if (divided_) { - auto nw = northwest_->queryRange(range); - auto ne = northeast_->queryRange(range); - auto sw = southwest_->queryRange(range); - auto se = southeast_->queryRange(range); - - found.insert(found.end(), nw.begin(), nw.end()); - found.insert(found.end(), ne.begin(), ne.end()); - found.insert(found.end(), sw.begin(), sw.end()); - found.insert(found.end(), se.begin(), se.end()); - } - - return found; -} - -template -std::vector QuadTree::queryNearby(const Vector2D& point, double radius) const { - Bounds range{ - point.x - radius, - point.y - radius, - radius * 2, - radius * 2 - }; - return queryRange(range); -} - -template -void QuadTree::clear() { - items_.clear(); - divided_ = false; - northwest_.reset(); - northeast_.reset(); - southwest_.reset(); - southeast_.reset(); -} - -template -void QuadTree::subdivide() { - double x = bounds_.x; - double y = bounds_.y; - double w = bounds_.width / 2; - double h = bounds_.height / 2; - - Bounds nw{x, y, w, h}; - northwest_ = std::make_unique(nw, capacity_); - - Bounds ne{x + w, y, w, h}; - northeast_ = std::make_unique(ne, capacity_); - - Bounds sw{x, y + h, w, h}; - southwest_ = std::make_unique(sw, capacity_); - - Bounds se{x + w, y + h, w, h}; - southeast_ = std::make_unique(se, capacity_); - - divided_ = true; -} - -template -Vector2D QuadTree::getPosition(const T& item) { - if constexpr (std::is_same_v) { - return item.position; - } else { - return item.getPosition(); - } -} \ No newline at end of file diff --git a/src/types/BasicTypes.cpp b/src/types/BasicTypes.cpp index c4822b1..79c7aec 100644 --- a/src/types/BasicTypes.cpp +++ b/src/types/BasicTypes.cpp @@ -84,57 +84,53 @@ bool Vehicle::isValidSpeed(double speed) const { } void MovingObject::updateMotion(const GeoPosition& newPos, uint64_t newTime) { - // 检查时间戳 - if (!positionHistory.empty() && newTime <= positionHistory.back().timestamp) { - return; // 忽略重复或过时的数据 - } - - // 更新位置历史 - positionHistory.emplace_back(newPos, newTime); - if (positionHistory.size() > MAX_HISTORY) { - positionHistory.pop_front(); - } - - // 计算速度和航向 - if (positionHistory.size() >= 2) { - // 使用最近的两个点来计算速度和航向 - const auto& curr = positionHistory.back(); - const auto& prev = positionHistory[positionHistory.size() - 2]; + // 检查是否是新数据 + bool isNewData = positionHistory.empty() || newTime > positionHistory.back().timestamp; + if (isNewData) { + // 更新位置历史 + positionHistory.emplace_back(newPos, newTime); + if (positionHistory.size() > MAX_HISTORY) { + positionHistory.pop_front(); + } - // 计算距离和时间差 - double distance = calculateDistance(prev.geo, curr.geo); // 单位：米 - double timeDiff = static_cast(curr.timestamp - prev.timestamp) / 1000.0; // 转换为秒 - - // 只有当位置变化足够大且时间差足够长时才更新速度和航向 - static const double MIN_DISTANCE = 0.1; // 最小位置变化阈值（米） - static const double MIN_TIME = 0.05; // 最小时间差阈值（秒） - - if (distance > MIN_DISTANCE && timeDiff > MIN_TIME) { - // 计算新的速度 - double newSpeed = distance / timeDiff; // 米/秒 + // 计算速度和航向 + if (positionHistory.size() >= 2) { + // 使用最近的两个点来计算速度和航向 + const auto& curr = positionHistory.back(); + const auto& prev = positionHistory[positionHistory.size() - 2]; - if (isValidSpeed(newSpeed)) { - // 使用指数移动平均来平滑速度，增大平滑因子以加快更新 - const double alpha = 0.8; // 增大平滑因子 - if (speed == 0) { - speed = newSpeed; // 第一次计算 - } else { - speed = speed * (1 - alpha) + newSpeed * alpha; // 平滑更新 + // 计算距离和时间差 + double distance = calculateDistance(prev.geo, curr.geo); // 单位：米 + double timeDiff = static_cast(curr.timestamp - prev.timestamp) / 1000.0; // 转换为秒 + + // 只有当位置变化足够大且时间差足够长时才更新速度和航向 + static const double MIN_DISTANCE = 0.1; // 最小位置变化阈值（米） + static const double MIN_TIME = 0.05; // 最小时间差阈值（秒） + + if (distance > MIN_DISTANCE && timeDiff > MIN_TIME) { + // 计算新的速度 + double newSpeed = distance / timeDiff; // 米/秒 + + if (isValidSpeed(newSpeed)) { + // 使用指数移动平均来平滑速度，增大平滑因子以加快更新 + const double alpha = 0.8; // 增大平滑因子 + if (speed == 0) { + speed = newSpeed; // 第一次计算 + } else { + speed = speed * (1 - alpha) + newSpeed * alpha; // 平滑更新 + } } + + // 计算并直接更新航向，不需要平滑处理 + heading = calculateHeading(prev.geo, curr.geo); } - - // 计算并直接更新航向，不需要平滑处理 - heading = calculateHeading(prev.geo, curr.geo); } } // 更新当前位置 geo = newPos; timestamp = newTime; - - // 使用坐标转换器更新平面坐标 - static CoordinateConverter converter; - position = converter.toLocalXY(geo.latitude, geo.longitude); + position = CoordinateConverter::instance().toLocalXY(geo.latitude, geo.longitude); } void MovingObject::copyHistoryFrom(const MovingObject& other) { diff --git a/tests/AirportBoundsTest.cpp b/tests/AirportBoundsTest.cpp new file mode 100644 index 0000000..c8a09dc --- /dev/null +++ b/tests/AirportBoundsTest.cpp @@ -0,0 +1,246 @@ +#include +#include +#include "config/AirportBounds.h" +#include "utils/Logger.h" +#include +#include +#include + +// 测试用的 AirportBounds 类，暴露 protected 成员以便测试 +class TestAirportBounds : public AirportBounds { +public: + using AirportBounds::AirportBounds; + using AirportBounds::toAirportCoordinate; + using AirportBounds::airportBounds_; + using AirportBounds::areaBounds_; + using AirportBounds::referencePoint_; + using AirportBounds::rotationAngle_; + + // 设置测试数据 + void setTestData(const Vector2D& refPoint, double angle, const Bounds& bounds) { + referencePoint_ = refPoint; + rotationAngle_ = angle * M_PI / 180.0; // 转换为弧度 + airportBounds_ = bounds; + } +}; + +class AirportBoundsTest : public ::testing::Test { +protected: + void SetUp() override { + // 设置日志级别 + Logger::setLogLevel(LogLevel::DEBUG); + + // 创建测试实例 + bounds_ = std::make_unique(); + + // 设置基本测试数据 + // 参考点设为原点 (0, 0) + // 旋转角度设为 90 度，便于验证 + bounds_->setTestData( + Vector2D{0, 0}, // 参考点 + 90.0, // 旋转角度（度） + Bounds(-2000, -2000, 4000, 4000) // 4km x 4km 的区域 + ); + } + + // 辅助函数：检查两个点是否足够接近（考虑浮点误差） + bool pointsAreClose(const Vector2D& p1, const Vector2D& p2, double tolerance = 0.1) { + return std::abs(p1.x - p2.x) < tolerance && std::abs(p1.y - p2.y) < tolerance; + } + + std::unique_ptr bounds_; +}; + +// 测试坐标转换 - 参考点 +TEST_F(AirportBoundsTest, ReferencePointTransformation) { + // 参考点应该转换为原点 (0,0) + Vector2D result = bounds_->toAirportCoordinate(Vector2D{0, 0}); + EXPECT_TRUE(pointsAreClose(result, Vector2D{0, 0})) + << "参考点转换结果: (" << result.x << ", " << result.y << ")"; +} + +// 测试坐标转换 - 基本位移（90度逆时针旋转） +TEST_F(AirportBoundsTest, BasicTranslation) { + // 向右移动 100 米，逆时针旋转 90 度后，应该在 y 轴正方向 + Vector2D result = bounds_->toAirportCoordinate(Vector2D{100, 0}); + EXPECT_NEAR(result.x, 0, 0.001); + EXPECT_NEAR(result.y, 100, 0.001) + << "向右100米的点逆时针旋转90度后应该在 (0, 100)，实际在: (" + << result.x << ", " << result.y << ")"; + + // 向上移动 100 米，逆时针旋转 90 度后，应该在 x 轴负方向 + result = bounds_->toAirportCoordinate(Vector2D{0, 100}); + EXPECT_NEAR(result.x, -100, 0.001); + EXPECT_NEAR(result.y, 0, 0.001) + << "向上100米的点逆时针旋转90度后应该在 (-100, 0)，实际在: (" + << result.x << ", " << result.y << ")"; +} + +// 测试坐标转换 - 复合变换（90度逆时针旋转） +TEST_F(AirportBoundsTest, CompositeTransformation) { + // 向右上方移动 (100, 100)，逆时针旋转 90 度 + Vector2D result = bounds_->toAirportCoordinate(Vector2D{100, 100}); + EXPECT_NEAR(result.x, -100, 0.001); + EXPECT_NEAR(result.y, 100, 0.001) + << "点(100,100)逆时针旋转90度后应该在 (-100, 100)，实际在: (" + << result.x << ", " << result.y << ")"; +} + +// 测试边界检查 - 边界内的点 +TEST_F(AirportBoundsTest, PointInBounds) { + // 测试参考点（一定在边界内） + EXPECT_TRUE(bounds_->isPointInBounds(Vector2D{0, 0})) + << "参考点应该在边界内"; + + // 测试边界内的点 + EXPECT_TRUE(bounds_->isPointInBounds(Vector2D{1000, 1000})) + << "距离参考点1km的点应该在边界内"; +} + +// 测试边界检查 - 边界外的点 +TEST_F(AirportBoundsTest, PointOutOfBounds) { + // 测试边界外的点 + EXPECT_FALSE(bounds_->isPointInBounds(Vector2D{3000, 3000})) + << "距离参考点3km的点应该在边界外"; + + // 测试另一个边界外的点 + EXPECT_FALSE(bounds_->isPointInBounds(Vector2D{-2500, -2500})) + << "距离参考点2.5km的点应该在边界外"; +} + +// 测试不同旋转角度（45度逆时针旋转） +TEST_F(AirportBoundsTest, DifferentRotations) { + // 重新设置为45度逆时针旋转 + bounds_->setTestData( + Vector2D{0, 0}, // 参考点 + 45.0, // 旋转角度（度） + Bounds(-2000, -2000, 4000, 4000) + ); + + // 向右移动 100 米，逆时针旋转 45 度 + Vector2D result = bounds_->toAirportCoordinate(Vector2D{100, 0}); + double sqrt2_2 = std::sqrt(2.0) / 2.0; // cos(45°) = sin(45°) = √2/2 + EXPECT_NEAR(result.x, 100 * sqrt2_2, 0.001); + EXPECT_NEAR(result.y, 100 * sqrt2_2, 0.001) + << "向右100米的点逆时针旋转45度后应该在 (70.71, 70.71)，实际在: (" + << result.x << ", " << result.y << ")"; + + // 向上移动 100 米，逆时针旋转 45 度 + result = bounds_->toAirportCoordinate(Vector2D{0, 100}); + EXPECT_NEAR(result.x, -100 * sqrt2_2, 0.001); + EXPECT_NEAR(result.y, 100 * sqrt2_2, 0.001) + << "向上100米的点逆时针旋转45度后应该在 (-70.71, 70.71)，实际在: (" + << result.x << ", " << result.y << ")"; +} + +// 测试旋转变换（30度逆时针旋转） +TEST_F(AirportBoundsTest, RotationTransformation) { + // 重新设置为30度逆时针旋转 + bounds_->setTestData( + Vector2D{0, 0}, // 参考点 + 30.0, // 旋转角度（度） + Bounds(-2000, -2000, 4000, 4000) + ); + + // 在世界坐标系中向右移动1000米，逆时针旋转 30 度 + Vector2D result = bounds_->toAirportCoordinate(Vector2D{1000, 0}); + double cos30 = std::cos(M_PI/6); // cos(30°) ≈ 0.866025 + double sin30 = std::sin(M_PI/6); // sin(30°) ≈ 0.5 + + // 在机场坐标系中的期望结果： + // x = 1000 * cos(30°) ≈ 866.025 + // y = 1000 * sin(30°) ≈ 500 + EXPECT_NEAR(result.x, 1000 * cos30, 0.1); + EXPECT_NEAR(result.y, 1000 * sin30, 0.1) + << "向右1000米的点逆时针旋转30度后应该在 (866.025, 500)，实际在: (" + << result.x << ", " << result.y << ")"; + + // 在世界坐标系中向上移动1000米，逆时针旋转 30 度 + result = bounds_->toAirportCoordinate(Vector2D{0, 1000}); + + // 在机场坐标系中的期望结果： + // x = -1000 * sin(30°) ≈ -500 + // y = 1000 * cos(30°) ≈ 866.025 + EXPECT_NEAR(result.x, -1000 * sin30, 0.1); + EXPECT_NEAR(result.y, 1000 * cos30, 0.1) + << "向上1000米的点逆时针旋转30度后应该在 (-500, 866.025)，实际在: (" + << result.x << ", " << result.y << ")"; +} + +// 测试区域检查 +TEST_F(AirportBoundsTest, AreaCheck) { + // 重新设置为0度旋转，便于测试 + bounds_->setTestData( + Vector2D{0, 0}, // 参考点 + 0.0, // 旋转角度（度） + Bounds(-2000, -2000, 4000, 4000) + ); + + // 设置测试区域 + bounds_->areaBounds_[AreaType::RUNWAY] = Bounds(-1000, -100, 2000, 200); + bounds_->areaBounds_[AreaType::TAXIWAY] = Bounds(-800, -300, 1600, 600); + + // 测试跑道上的点 + Vector2D runwayPoint{500, 0}; // 跑道中心偏右500米 + EXPECT_TRUE(bounds_->isPointInArea(runwayPoint, AreaType::RUNWAY)) + << "跑道上的点应该在跑道区域内"; + + // 测试滑行道上的点 + Vector2D taxiwayPoint{0, 200}; // 滑行道中心向上200米 + EXPECT_TRUE(bounds_->isPointInArea(taxiwayPoint, AreaType::TAXIWAY)) + << "滑行道上的点应该在滑行道区域内"; + + // 测试区域外的点 + Vector2D outPoint{3000, 3000}; // 远离机场的点 + EXPECT_FALSE(bounds_->isPointInArea(outPoint, AreaType::RUNWAY)) + << "远离机场的点不应该在任何区域内"; + EXPECT_FALSE(bounds_->isPointInArea(outPoint, AreaType::TAXIWAY)) + << "远离机场的点不应该在任何区域内"; +} + +// 测试配置文件加载 +TEST_F(AirportBoundsTest, ConfigLoading) { + // 使用项目中的配置文件 + AirportBounds bounds("config/airport_bounds.json"); + + // 验证配置是否正确加载 + const auto& airportBounds = bounds.getAirportBounds(); + EXPECT_TRUE(airportBounds.width > 0 && airportBounds.height > 0) + << "机场边界的宽度和高度应该为正值"; + + // 验证区域配置 + const auto& runwayBounds = bounds.getAreaBounds(AreaType::RUNWAY); + EXPECT_TRUE(runwayBounds.width > 0 && runwayBounds.height > 0) + << "跑道区域的宽度和高度应该为正值"; + EXPECT_TRUE(runwayBounds.width < airportBounds.width && runwayBounds.height < airportBounds.height) + << "跑道区域应该小于机场边界"; +} + +// 测试配置文件错误处理 +TEST_F(AirportBoundsTest, ConfigErrorHandling) { + // 测试文件不存在 + EXPECT_THROW(AirportBounds("nonexistent.json"), std::runtime_error); + + // 测试配置文件格式错误 + EXPECT_THROW(AirportBounds("tests/data/invalid_airport_bounds.json"), std::runtime_error); +} + +// 测试边界条件 +TEST_F(AirportBoundsTest, EdgeCases) { + // 重新设置为0度旋转，便于测试 + bounds_->setTestData( + Vector2D{0, 0}, // 参考点 + 0.0, // 旋转角度（度） + Bounds(-2000, -2000, 4000, 4000) + ); + + // 测试正好在边界上的点 + Vector2D edge{2000, 2000}; // 右上角边界点 + EXPECT_TRUE(bounds_->isPointInBounds(edge)) + << "边界上的点应该被认为在边界内"; + + // 测试边界外的点 + Vector2D outside{2001, 2001}; // 刚好超出边界 + EXPECT_FALSE(bounds_->isPointInBounds(outside)) + << "边界外的点应该被认为在边界外"; +} \ No newline at end of file diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 7afe78f..a32fb0d 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,5 +1,6 @@ # 设置测试源文件 set(TEST_SOURCES + AirportBoundsTest.cpp BasicCollisionTest.cpp CollisionDetectorTest.cpp TrafficLightDetectorTest.cpp diff --git a/tests/DataCollectorTest.cpp b/tests/DataCollectorTest.cpp index 31f37c6..7529aea 100644 --- a/tests/DataCollectorTest.cpp +++ b/tests/DataCollectorTest.cpp @@ -1,6 +1,8 @@ #include #include #include "collector/DataCollector.h" +#include "config/AirportBounds.h" +#include "spatial/CoordinateConverter.h" #include "utils/Logger.h" #include "network/MessageTypes.h" #include "core/System.h" @@ -18,41 +20,98 @@ public: MOCK_METHOD2(fetchUnmannedVehicleStatus, bool(const std::string&, std::string&)); }; +// 创建测试专用的 AirportBounds 类 +class TestAirportBounds : public AirportBounds { +public: + using AirportBounds::AirportBounds; // 继承构造函数 + + void setBounds(const Bounds& bounds) { + this->airportBounds_ = bounds; + } + + void setReferencePoint(const Vector2D& point) { + this->referencePoint_ = point; + } + + void setRotationAngle(double angle) { + this->rotationAngle_ = angle * M_PI / 180.0; // 转换为弧度 + } + +protected: + using AirportBounds::airportBounds_; + using AirportBounds::referencePoint_; + using AirportBounds::rotationAngle_; +}; + class DataCollectorTest : public ::testing::Test { protected: void SetUp() override { - collector = std::make_unique(); + // 设置日志级别 + Logger::setLogLevel(LogLevel::DEBUG); + + // 初始化系统配置 + SystemConfig::instance().load("config/system_config.json"); + + // 创建测试实例 + bounds_ = std::make_unique(); + + // 设置机场边界参数 + bounds_->setReferencePoint(Vector2D{0, 0}); // 世界坐标系中的参考点 + bounds_->setRotationAngle(68.53); // 与 airport_bounds.json 保持一致 + bounds_->setBounds(Bounds(-100, -200, 800, 400)); // 与 airport_bounds.json 保持一致 + + // 初始化坐标转换器 + const auto& refPoint = SystemConfig::instance().airport.reference_point; + CoordinateConverter::instance().setReferencePoint(refPoint.latitude, refPoint.longitude); + + collector = std::make_unique(*bounds_); mockSource = std::make_shared<::testing::NiceMock>(); collector->setDataSource(mockSource); + + // 初始化配置 + dataSourceConfig_.position.refresh_interval_ms = 100; + dataSourceConfig_.position.timeout_ms = 5000; // 添加连接超时配置 + dataSourceConfig_.position.read_timeout_ms = 2000; // 添加读取超时配置 + + dataSourceConfig_.vehicle.refresh_interval_ms = 100; + dataSourceConfig_.vehicle.timeout_ms = 5000; + dataSourceConfig_.vehicle.read_timeout_ms = 2000; + + dataSourceConfig_.traffic_light.refresh_interval_ms = 100; + dataSourceConfig_.traffic_light.timeout_ms = 5000; + dataSourceConfig_.traffic_light.read_timeout_ms = 2000; + + WarnConfig warnConfig; + warnConfig.warning_interval_ms = 500; // 设置警告间隔 + EXPECT_TRUE(collector->initialize(dataSourceConfig_, warnConfig)); } void TearDown() override { collector.reset(); } - // 创建测试数据 + // 创建测试用的航空器 Aircraft createTestAircraft(const std::string& id, double lat, double lon) { - Aircraft a; - a.id = id; - a.flightNo = id; - a.trackNumber = "TN" + id; - a.geo.latitude = lat; - a.geo.longitude = lon; - a.altitude = 5.0; - a.timestamp = std::chrono::duration_cast( + Aircraft aircraft; + aircraft.id = id; + aircraft.flightNo = id; + aircraft.geo.latitude = lat; + aircraft.geo.longitude = lon; + aircraft.timestamp = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()).count(); - return a; + return aircraft; } + // 创建测试用的车辆 Vehicle createTestVehicle(const std::string& id, double lat, double lon) { - Vehicle v; - v.id = id; - v.vehicleNo = id; - v.geo.latitude = lat; - v.geo.longitude = lon; - v.timestamp = std::chrono::duration_cast( + Vehicle vehicle; + vehicle.id = id; + vehicle.vehicleNo = id; + vehicle.geo.latitude = lat; + vehicle.geo.longitude = lon; + vehicle.timestamp = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()).count(); - return v; + return vehicle; } // 创建测试用的红绿灯信号 @@ -65,11 +124,38 @@ protected: return signal; } + // 创建边界内的航空器 + Aircraft createOutOfBoundsAircraft(const std::string& id) { + // 计算偏移量，考虑机场旋转角度 + double angle = 68.53 * M_PI / 180.0; // 转换为弧度 + double offset = 0.027; // 约 3km + double lat_offset = offset * std::cos(angle); + double lon_offset = offset * std::sin(angle); + + const auto& refPoint = SystemConfig::instance().airport.reference_point; + return createTestAircraft(id, refPoint.latitude + lat_offset, refPoint.longitude + lon_offset); + } + + // 创建边界内的车辆 + Vehicle createOutOfBoundsVehicle(const std::string& id) { + // 计算偏移量，考虑机场旋转角度 + double angle = 68.53 * M_PI / 180.0; // 转换为弧度 + double offset = 0.0225; // 约 2.5km + double lat_offset = -offset * std::cos(angle); // 负值表示向相反方向偏移 + double lon_offset = -offset * std::sin(angle); + + const auto& refPoint = SystemConfig::instance().airport.reference_point; + return createTestVehicle(id, refPoint.latitude + lat_offset, refPoint.longitude + lon_offset); + } + std::unique_ptr collector; std::shared_ptr mockSource; + std::unique_ptr bounds_; + std::unique_ptr converter_; + DataSourceConfig dataSourceConfig_; }; -// 测试初始化 +// 修改初始化测试用例 TEST_F(DataCollectorTest, Initialization) { DataSourceConfig dataSourceConfig; @@ -79,7 +165,7 @@ TEST_F(DataCollectorTest, Initialization) { dataSourceConfig.position.aircraft_path = "/api/getCurrentFlightPositions"; dataSourceConfig.position.refresh_interval_ms = 1000; dataSourceConfig.position.timeout_ms = 5000; - dataSourceConfig.position.read_timeout_ms = 1000; + dataSourceConfig.position.read_timeout_ms = 2000; WarnConfig warnConfig; warnConfig.warning_interval_ms = 1000; @@ -88,58 +174,126 @@ TEST_F(DataCollectorTest, Initialization) { EXPECT_TRUE(collector->initialize(dataSourceConfig, warnConfig)); } -// 测试刷新方法 -TEST_F(DataCollectorTest, RefreshTest) { +// 测试数据采集 +TEST_F(DataCollectorTest, DataCollectionTest) { + // 设置数据源配置 + DataSourceConfig config; + + // 设置位置数据配置 + config.position.host = "localhost"; + config.position.port = 8080; + config.position.refresh_interval_ms = 100; + config.position.timeout_ms = 1000; + config.position.read_timeout_ms = 500; + + // 设置无人车数据配置 + config.vehicle.host = "localhost"; + config.vehicle.port = 8081; + config.vehicle.refresh_interval_ms = 100; + config.vehicle.timeout_ms = 1000; + config.vehicle.read_timeout_ms = 500; + + // 设置红绿灯数据配置 + config.traffic_light.host = "localhost"; + config.traffic_light.port = 8082; + config.traffic_light.refresh_interval_ms = 100; + config.traffic_light.timeout_ms = 1000; + config.traffic_light.read_timeout_ms = 500; + + WarnConfig warnConfig; + warnConfig.warning_interval_ms = 200; + + collector->initialize(config, warnConfig); + std::vector testAircraft = { - createTestAircraft("TEST1", 36.36, 120.08), - createTestAircraft("TEST2", 36.37, 120.09) + createTestAircraft("TEST1", 36.354483470, 120.08502054), // 参考点，转换后是 (0, 0) + createTestAircraft("TEST2", 36.354483470 + 0.0002 * std::cos(68.53 * M_PI / 180.0), + 120.08502054 + 0.0002 * std::sin(68.53 * M_PI / 180.0)) // 沿机场方向偏移约 20m }; std::vector testVehicles = { - createTestVehicle("VEH1", 36.36, 120.08), - createTestVehicle("VEH2", 36.37, 120.09) + createTestVehicle("VEH1", 36.354483470, 120.08502054), // 参考点，转换后是 (0, 0) + createTestVehicle("VEH2", 36.354483470 - 0.0002 * std::cos(68.53 * M_PI / 180.0), + 120.08502054 - 0.0002 * std::sin(68.53 * M_PI / 180.0)) // 沿机场方向反向偏移约 20m + }; + + std::vector testSignals = { + createTestTrafficLight("TL001", 0), + createTestTrafficLight("TL002", 1) }; // 设置 Mock 数据返回 EXPECT_CALL(*mockSource, fetchPositionAircraftData(::testing::_)) - .WillOnce(::testing::DoAll( + .WillRepeatedly(::testing::DoAll( ::testing::SetArgReferee<0>(testAircraft), ::testing::Return(true) )); EXPECT_CALL(*mockSource, fetchPositionVehicleData(::testing::_)) - .WillOnce(::testing::DoAll( + .WillRepeatedly(::testing::DoAll( ::testing::SetArgReferee<0>(testVehicles), ::testing::Return(true) )); + + EXPECT_CALL(*mockSource, fetchTrafficLightSignals(::testing::_)) + .WillRepeatedly(::testing::DoAll( + ::testing::SetArgReferee<0>(testSignals), + ::testing::Return(true) + )); + + std::string testStatus = "NORMAL"; + EXPECT_CALL(*mockSource, fetchUnmannedVehicleStatus(::testing::_, ::testing::_)) + .WillRepeatedly(::testing::DoAll( + ::testing::SetArgReferee<1>(testStatus), + ::testing::Return(true) + )); - // 执行刷新 - collector->refresh(); + // 启动数据采集 + collector->start(); + + // 等待数据更新（至少 1000ms，确保数据被采集） + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + // 获取最新数据 + auto [aircraft, vehicles, traffic_lights] = collector->getLatestData(); + auto [aircraft_ts, vehicle_ts, traffic_light_ts] = collector->getLastUpdateTimestamps(); // 验证数据 - auto aircraft = collector->getAircraftData(); EXPECT_EQ(aircraft.size(), 2); if (!aircraft.empty()) { EXPECT_EQ(aircraft[0].flightNo, "TEST1"); EXPECT_EQ(aircraft[1].flightNo, "TEST2"); } - auto vehicles = collector->getVehicleData(); EXPECT_EQ(vehicles.size(), 2); if (!vehicles.empty()) { EXPECT_EQ(vehicles[0].vehicleNo, "VEH1"); EXPECT_EQ(vehicles[1].vehicleNo, "VEH2"); } + + EXPECT_EQ(traffic_lights.size(), 2); + if (!traffic_lights.empty()) { + EXPECT_EQ(traffic_lights[0].trafficLightId, "TL001"); + EXPECT_EQ(traffic_lights[1].trafficLightId, "TL002"); + } + + // 验证时间戳 + EXPECT_GT(aircraft_ts, 0); + EXPECT_GT(vehicle_ts, 0); + EXPECT_GT(traffic_light_ts, 0); + + // 停止数据采集 + collector->stop(); } // 测试数据采集循环 TEST_F(DataCollectorTest, DataCollectionLoop) { std::vector testAircraft = { - createTestAircraft("TEST1", 36.36, 120.08) + createTestAircraft("TEST1", 36.354483470, 120.08502054) // 参考点 }; std::vector testVehicles = { - createTestVehicle("VEH1", 36.36, 120.08) + createTestVehicle("VEH1", 36.354483470, 120.08502054) // 参考点 }; // 设置 Mock 数据返回 @@ -176,19 +330,26 @@ TEST_F(DataCollectorTest, DataCollectionLoop) { TEST_F(DataCollectorTest, ErrorHandling) { // 设置 Mock 返回错误 EXPECT_CALL(*mockSource, fetchPositionAircraftData(::testing::_)) - .WillOnce(::testing::Return(false)); + .WillRepeatedly(::testing::Return(false)); EXPECT_CALL(*mockSource, fetchPositionVehicleData(::testing::_)) - .WillOnce(::testing::Return(false)); + .WillRepeatedly(::testing::Return(false)); - // 执行刷新 - collector->refresh(); + // 启动数据采集 + collector->start(); + + // 等待数据更新（至少三个采集周期） + std::this_thread::sleep_for(std::chrono::milliseconds( + dataSourceConfig_.position.refresh_interval_ms * 3)); + + // 获取最新数据 + auto [aircraft, vehicles, traffic_lights] = collector->getLatestData(); // 验证数据为空 - auto aircraft = collector->getAircraftData(); EXPECT_TRUE(aircraft.empty()); - - auto vehicles = collector->getVehicleData(); EXPECT_TRUE(vehicles.empty()); + + // 停止数据采集 + collector->stop(); } // 测试红绿灯信号获取 @@ -201,13 +362,17 @@ TEST_F(DataCollectorTest, TrafficLightSignalsTest) { // 设置 Mock 数据返回 EXPECT_CALL(*mockSource, fetchTrafficLightSignals(::testing::_)) - .WillOnce(::testing::DoAll( + .WillRepeatedly(::testing::DoAll( ::testing::SetArgReferee<0>(testSignals), ::testing::Return(true) )); - // 执行刷新 - collector->refresh(); + // 启动数据采集 + collector->start(); + + // 等待数据更新（至少三个采集周期） + std::this_thread::sleep_for(std::chrono::milliseconds( + dataSourceConfig_.position.refresh_interval_ms * 3)); // 验证数据 auto signals = collector->getTrafficLightSignals(); @@ -218,20 +383,30 @@ TEST_F(DataCollectorTest, TrafficLightSignalsTest) { EXPECT_EQ(signals[1].trafficLightId, "TL002"); EXPECT_EQ(signals[1].status, SignalStatus::GREEN); // GREEN = 1 } + + // 停止数据采集 + collector->stop(); } // 测试红绿灯信号错误处理 TEST_F(DataCollectorTest, TrafficLightSignalsErrorTest) { // 设置 Mock 返回错误 EXPECT_CALL(*mockSource, fetchTrafficLightSignals(::testing::_)) - .WillOnce(::testing::Return(false)); + .WillRepeatedly(::testing::Return(false)); - // 执行刷新 - collector->refresh(); + // 启动数据采集 + collector->start(); + + // 等待数据更新（至少三个采集周期） + std::this_thread::sleep_for(std::chrono::milliseconds( + dataSourceConfig_.position.refresh_interval_ms * 3)); // 验证数据为空 auto signals = collector->getTrafficLightSignals(); EXPECT_TRUE(signals.empty()); + + // 停止数据采集 + collector->stop(); } // 测试连接健康检查和超时告警 @@ -283,37 +458,97 @@ TEST_F(DataCollectorTest, ConnectionRecovery) { // 设置位置数据配置 config.position.host = "localhost"; config.position.port = 8080; - config.position.refresh_interval_ms = 10; + config.position.refresh_interval_ms = 100; config.position.timeout_ms = 1000; config.position.read_timeout_ms = 500; + + // 设置无人车数据配置 + config.vehicle.host = "localhost"; + config.vehicle.port = 8081; + config.vehicle.refresh_interval_ms = 100; + config.vehicle.timeout_ms = 1000; + config.vehicle.read_timeout_ms = 500; + + // 设置红绿灯数据配置 + config.traffic_light.host = "localhost"; + config.traffic_light.port = 8082; + config.traffic_light.refresh_interval_ms = 100; + config.traffic_light.timeout_ms = 1000; + config.traffic_light.read_timeout_ms = 500; WarnConfig warnConfig; - warnConfig.warning_interval_ms = 20; + warnConfig.warning_interval_ms = 200; collector->initialize(config, warnConfig); std::vector testAircraft = { - createTestAircraft("TEST1", 36.36, 120.08) + createTestAircraft("TEST1", 36.354483470, 120.08502054) // 参考点 }; - // 移除 InSequence，直接设置期望 + // 设置 Mock 行为: 前两次失败,然后成功 EXPECT_CALL(*mockSource, fetchPositionAircraftData(::testing::_)) .WillOnce(::testing::Return(false)) - .WillOnce(::testing::DoAll( + .WillOnce(::testing::Return(false)) + .WillRepeatedly(::testing::DoAll( ::testing::SetArgReferee<0>(testAircraft), ::testing::Return(true) )); + + // 添加车辆数据的 mock 行为 + std::vector testVehicles; + EXPECT_CALL(*mockSource, fetchPositionVehicleData(::testing::_)) + .WillOnce(::testing::Return(false)) + .WillOnce(::testing::Return(false)) + .WillRepeatedly(::testing::DoAll( + ::testing::SetArgReferee<0>(testVehicles), + ::testing::Return(true) + )); + + // 设置无人车数据 Mock 行为 + std::string testStatus = "NORMAL"; + EXPECT_CALL(*mockSource, fetchUnmannedVehicleStatus(::testing::_, ::testing::_)) + .WillOnce(::testing::Return(false)) + .WillOnce(::testing::Return(false)) + .WillRepeatedly(::testing::DoAll( + ::testing::SetArgReferee<1>(testStatus), + ::testing::Return(true) + )); + + // 设置红绿灯数据 Mock 行为 + std::vector testSignals = { + createTestTrafficLight("TL001", 0) + }; + EXPECT_CALL(*mockSource, fetchTrafficLightSignals(::testing::_)) + .WillOnce(::testing::Return(false)) + .WillOnce(::testing::Return(false)) + .WillRepeatedly(::testing::DoAll( + ::testing::SetArgReferee<0>(testSignals), + ::testing::Return(true) + )); - // 执行两次刷新 - collector->refresh(); // 第一次失败 - collector->refresh(); // 第二次成功 + // 启动数据采集 + collector->start(); + + // 等待足够长的时间以确保连接恢复 + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + // 获取最新数据 + auto [aircraft, vehicles, traffic_lights] = collector->getLatestData(); // 验证数据 - auto aircraft = collector->getAircraftData(); EXPECT_EQ(aircraft.size(), 1); if (!aircraft.empty()) { EXPECT_EQ(aircraft[0].flightNo, "TEST1"); } + + // 验证红绿灯数据 + EXPECT_EQ(traffic_lights.size(), 1); + if (!traffic_lights.empty()) { + EXPECT_EQ(traffic_lights[0].trafficLightId, "TL001"); + } + + // 停止数据采集 + collector->stop(); } // 测试长连接下的数据获取 @@ -330,11 +565,15 @@ TEST_F(DataCollectorTest, LongConnectionDataFetch) { collector->initialize(dataSourceConfig, WarnConfig{}); std::vector testAircraft = { - createTestAircraft("TEST1", 36.36, 120.08) + createTestAircraft("TEST1", 36.354483470, 120.08502054), // 参考点，转换后是 (0, 0) + createTestAircraft("TEST2", 36.354483470 + 0.0002 * std::cos(68.53 * M_PI / 180.0), + 120.08502054 + 0.0002 * std::sin(68.53 * M_PI / 180.0)) // 沿机场方向偏移约 20m }; std::vector testVehicles = { - createTestVehicle("VEH1", 36.36, 120.08) + createTestVehicle("VEH1", 36.354483470, 120.08502054), // 参考点，转换后是 (0, 0) + createTestVehicle("VEH2", 36.354483470 - 0.0002 * std::cos(68.53 * M_PI / 180.0), + 120.08502054 - 0.0002 * std::sin(68.53 * M_PI / 180.0)) // 沿机场方向反向偏移约 20m }; std::vector testSignals = { @@ -523,12 +762,26 @@ TEST_F(DataCollectorTest, ReadTimeoutMechanism) { // 设置位置数据配置 config.position.host = "localhost"; config.position.port = 8080; - config.position.timeout_ms = 100; // 连接超时 100ms - config.position.read_timeout_ms = 50; // 读取超时 50ms - config.position.refresh_interval_ms = 10; + config.position.timeout_ms = 1000; // 连接超时 1000ms + config.position.read_timeout_ms = 500; // 读取超时 500ms + config.position.refresh_interval_ms = 100; + + // 设置无人车数据配置 + config.vehicle.host = "localhost"; + config.vehicle.port = 8081; + config.vehicle.timeout_ms = 1000; + config.vehicle.read_timeout_ms = 500; + config.vehicle.refresh_interval_ms = 100; + + // 设置红绿灯数据配置 + config.traffic_light.host = "localhost"; + config.traffic_light.port = 8082; + config.traffic_light.timeout_ms = 1000; + config.traffic_light.read_timeout_ms = 500; + config.traffic_light.refresh_interval_ms = 100; WarnConfig warnConfig; - warnConfig.warning_interval_ms = 20; // 警告间隔 20ms + warnConfig.warning_interval_ms = 200; // 警告间隔 200ms // 创建一个计数器来记录收到的警告数量 int warningCount = 0; @@ -552,7 +805,7 @@ TEST_F(DataCollectorTest, ReadTimeoutMechanism) { ::testing::StrEq("traffic_light") )); EXPECT_GT(warning.elapsed_ms, 0); - EXPECT_TRUE(warning.is_read_timeout); // 标记为读取超时 + EXPECT_TRUE(warning.is_read_timeout); })); // 设置 mock system 到 collector @@ -562,31 +815,91 @@ TEST_F(DataCollectorTest, ReadTimeoutMechanism) { // 启动采集器 collector->start(); - // 模拟所有数据源读取超时 + // 模拟所有数据源获取失败 EXPECT_CALL(*mockSource, fetchPositionAircraftData(::testing::_)) - .WillRepeatedly(::testing::Invoke([](std::vector& aircraft) { - std::this_thread::sleep_for(std::chrono::milliseconds(60)); // 睡眠超过读取超时时间但小于连接超时 - return false; - })); - + .WillRepeatedly(::testing::Return(false)); + EXPECT_CALL(*mockSource, fetchPositionVehicleData(::testing::_)) - .WillRepeatedly(::testing::Invoke([](std::vector& vehicles) { - std::this_thread::sleep_for(std::chrono::milliseconds(60)); - return false; - })); + .WillRepeatedly(::testing::Return(false)); EXPECT_CALL(*mockSource, fetchTrafficLightSignals(::testing::_)) - .WillRepeatedly(::testing::Invoke([](std::vector& signals) { - std::this_thread::sleep_for(std::chrono::milliseconds(60)); - return false; - })); + .WillRepeatedly(::testing::Return(false)); + + EXPECT_CALL(*mockSource, fetchUnmannedVehicleStatus(::testing::_, ::testing::_)) + .WillRepeatedly(::testing::Return(false)); // 等待足够长的时间以确保警告被发送 - std::this_thread::sleep_for(std::chrono::milliseconds(300)); + std::this_thread::sleep_for(std::chrono::milliseconds(1500)); // 停止采集器 collector->stop(); // 验证是否收到了警告 EXPECT_GT(warningCount, 0); +} + +// 添加新的测试用例：测试边界外数据过滤 +TEST_F(DataCollectorTest, OutOfBoundsDataFiltering) { + DataSourceConfig dataSourceConfig; + WarnConfig warnConfig; + EXPECT_TRUE(collector->initialize(dataSourceConfig, warnConfig)); + + // 创建测试数据：混合边界内外的数据 + const auto& refPoint = SystemConfig::instance().airport.reference_point; + + // 创建边界内的航空器 + std::vector testAircraft = { + createTestAircraft("IN_BOUNDS_1", refPoint.latitude, refPoint.longitude), // 参考点 + createTestAircraft("IN_BOUNDS_2", + refPoint.latitude + 0.001 * std::cos(68.53 * M_PI / 180.0), // 沿机场方向偏移约 100m + refPoint.longitude + 0.001 * std::sin(68.53 * M_PI / 180.0)), + createOutOfBoundsAircraft("OUT_BOUNDS_1"), // 边界外偏移约 3km + createOutOfBoundsAircraft("OUT_BOUNDS_2") // 边界外偏移约 3km + }; + + // 创建边界内的车辆 + std::vector testVehicles = { + createTestVehicle("VEH_IN_1", refPoint.latitude, refPoint.longitude), // 参考点 + createTestVehicle("VEH_IN_2", + refPoint.latitude + 0.001 * std::cos(68.53 * M_PI / 180.0), // 沿机场方向偏移约 100m + refPoint.longitude + 0.001 * std::sin(68.53 * M_PI / 180.0)), + createOutOfBoundsVehicle("VEH_OUT_1"), // 边界外偏移约 2.5km + createOutOfBoundsVehicle("VEH_OUT_2") // 边界外偏移约 2.5km + }; + + // 设置 Mock 数据返回 + EXPECT_CALL(*mockSource, fetchPositionAircraftData(::testing::_)) + .WillOnce(::testing::DoAll( + ::testing::SetArgReferee<0>(testAircraft), + ::testing::Return(true) + )); + + EXPECT_CALL(*mockSource, fetchPositionVehicleData(::testing::_)) + .WillOnce(::testing::DoAll( + ::testing::SetArgReferee<0>(testVehicles), + ::testing::Return(true) + )); + + // 执行刷新 + collector->fetchPositionData(); + + // 验证过滤结果 + const auto& filteredAircraft = collector->getAircraftData(); + const auto& filteredVehicles = collector->getVehicleData(); + + // 检查过滤后的数量 + EXPECT_EQ(filteredAircraft.size(), 2) << "应该只保留2架在边界内的航空器"; + EXPECT_EQ(filteredVehicles.size(), 2) << "应该只保留2辆在边界内的车辆"; + + // 检查保留的是正确的航空器 + if (filteredAircraft.size() >= 2) { + EXPECT_EQ(filteredAircraft[0].flightNo, "IN_BOUNDS_1"); + EXPECT_EQ(filteredAircraft[1].flightNo, "IN_BOUNDS_2"); + } + + // 检查保留的是正确的车辆 + if (filteredVehicles.size() >= 2) { + EXPECT_EQ(filteredVehicles[0].vehicleNo, "VEH_IN_1"); + EXPECT_EQ(filteredVehicles[1].vehicleNo, "VEH_IN_2"); + } } \ No newline at end of file diff --git a/tests/data/invalid_airport_bounds.json b/tests/data/invalid_airport_bounds.json new file mode 100644 index 0000000..bb7f5b4 --- /dev/null +++ b/tests/data/invalid_airport_bounds.json @@ -0,0 +1,5 @@ +{ + "airport": { + "rotation_angle": 68.53 + } +} \ No newline at end of file diff --git a/tools/mock_server.py b/tools/mock_server.py index f637d5a..9edc29d 100644 --- a/tools/mock_server.py +++ b/tools/mock_server.py @@ -26,10 +26,8 @@ EARTH_RADIUS = 6378137.0 COMMAND_PRIORITIES = { "ALERT": 5, - "RED": 4, - "YELLOW": 3, "WARNING": 3, - "GREEN": 2, + "PARKING": 2, "RESUME": 1 } @@ -174,85 +172,65 @@ class VehicleState: self.traffic_light_state = None # 当前红绿灯状态 self.target_lat = None # 目标纬度 self.target_lon = None # 目标经度 + self.is_traffic_light_stop = False # 是否因红灯停车 def can_be_overridden_by(self, command_type): """判断当前指令是否可以被新指令覆盖""" - priority_map = { - "ALERT": 5, # 告警指令，最高优先级 - "RED": 4, # 红灯指令，次高优先级 - "YELLOW": 3, # 黄灯指令，表示红绿灯故障 - "WARNING": 3, # 预警指令，中等优先级 - "GREEN": 2, # 绿灯状态，低优先级 - "RESUME": 1 # 恢复指令，最低优先级 - } - new_priority = priority_map.get(command_type, 0) - current_priority = priority_map.get(self.current_command, 0) + # 红绿灯信号不参与指令优先级判断 + if command_type in ["RED", "GREEN", "YELLOW"]: + return True + + new_priority = COMMAND_PRIORITIES.get(command_type, 0) + current_priority = COMMAND_PRIORITIES.get(self.current_command, 0) + + # 相同类型的指令可以覆盖（因为需要持续发送） + if command_type == self.current_command: + return True # ALERT 指令可以被 RESUME 解除 if self.current_command == "ALERT": return command_type == "RESUME" - # RED 指令可以被 GREEN 或相同及更高优先级指令覆盖 - if self.current_command == "RED": - return command_type == "GREEN" or new_priority >= current_priority - - # YELLOW 指令（红绿灯故障）可以被任何新指令覆盖 - if self.current_command == "YELLOW": - return True - # WARNING 指令可以被 RESUME 或相同及更高优先级指令覆盖 if self.current_command == "WARNING": return command_type == "RESUME" or new_priority >= current_priority - # GREEN 不阻止任何指令 - if self.current_command == "GREEN": - return True - # 其他情况下，相同或更高优先级可以覆盖 return new_priority >= current_priority def update_command(self, command_type, target_lat=None, target_lon=None): """更新指令状态""" - priority_map = { - "ALERT": 5, # 告警指令，最高优先级 - "RED": 4, # 红灯指令，次高优先级 - "YELLOW": 3, # 黄灯指令，表示红绿灯故障 - "WARNING": 3, # 预警指令，中等优先级 - "GREEN": 2, # 绿灯状态，低优先级 - "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", "YELLOW"]: + old_state = self.traffic_light_state self.traffic_light_state = command_type - self.current_command = command_type - self.command_priority = priority_map[command_type] + # 更新红灯停车状态 if command_type == "RED": - self.is_running = False - print(f"车辆 {self.vehicle_no} 收到红灯指令，停止运行") - elif command_type == "YELLOW": - # 黄灯表示红绿灯故障，清除当前红绿灯状态，允许车辆继续行驶 - self.is_running = True - print(f"车辆 {self.vehicle_no} 收到黄灯指令（红绿灯故障），继续行驶") - else: # GREEN - # 如果没有其他阻塞性指令，允许移动 - if self.current_command not in ["ALERT", "WARNING"]: - self.is_running = True - print(f"车辆 {self.vehicle_no} 收到绿灯指令，恢复运行") + self.is_traffic_light_stop = True + elif command_type in ["GREEN", "YELLOW"]: + self.is_traffic_light_stop = False + + print(f"车辆 {self.vehicle_no} 收到红绿灯信号: {command_type} (原状态: {old_state})") else: # 其他指令正常更新 self.current_command = command_type - self.command_priority = priority_map.get(command_type, 0) + self.command_priority = COMMAND_PRIORITIES.get(command_type, 0) + + # RESUME 指令不清除红绿灯状态，红绿灯状态由信号灯独立控制 if command_type == "RESUME": - self.traffic_light_state = None # 清除红绿灯状态 + # 清除其他状态 + self.brake_mode = None + self.command_reason = None - print(f"更新车辆 {self.vehicle_no} 状态: command={self.current_command}, traffic_light={self.traffic_light_state}, priority={self.command_priority}") + # 更新运行状态 + self.is_running = self.can_move() + print(f"更新车辆 {self.vehicle_no} 状态: command={self.current_command}, traffic_light={self.traffic_light_state}, priority={self.command_priority}, is_running={self.is_running}") def can_move(self): """检查车辆是否可以移动""" @@ -263,9 +241,13 @@ class VehicleState: # 如果有预警指令，不能移动 if self.current_command == "WARNING": return False + + # 如果有安全停靠指令，不能移动 + if self.current_command == "PARKING": + return False - # 如果是红灯且当前指令是 RED，不能移动 - if self.traffic_light_state == "RED" and self.current_command == "RED": + # 如果是红灯停车状态，不能移动 + if self.is_traffic_light_stop: return False # 其他情况可以移动 @@ -279,6 +261,7 @@ class VehicleState: - 运行状态: {'运行中' if self.is_running else '已停止'} - 当前指令: {self.current_command} - 红绿灯状态: {self.traffic_light_state} +- 红灯停车: {'是' if self.is_traffic_light_stop else '否'} - 指令优先级: {self.command_priority} - 目标速度: {self.target_speed} - 制动模式: {self.brake_mode}