From 9ec81a75245f3b1303321a847eed6a5fbcc7df15 Mon Sep 17 00:00:00 2001 From: Tian jianyong <11429339@qq.com> Date: Mon, 20 Jan 2025 09:26:44 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86=E5=AE=89=E5=85=A8?= =?UTF-8?q?=E5=81=9C=E9=9D=A0=E6=8C=87=E4=BB=A4=EF=BC=8C=E5=88=A0=E9=99=A4?= =?UTF-8?q?=E4=BA=86=E6=97=A0=E7=94=A8=E7=9A=84=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 22 +-- HTTPClient.cpp | 12 ++ docs/conflict_detection_algorithms.md | 197 ++++++++++++++++++++++++++ docs/requirements.md | 17 ++- src/collector/DataSourceConfig.cpp | 22 +-- src/config/SystemConfigJson.cpp | 1 - src/core/System.cpp | 2 + src/detector/CollisionDetector.cpp | 4 +- src/detector/CollisionDetector.h | 2 +- src/mock/MockDataService.h | 2 +- src/network/HTTPClient.cpp | 44 +++--- src/network/HTTPDataSource.cpp | 25 +++- src/spatial/QuadTree.cpp | 12 -- src/types/VehicleCommand.h | 6 +- src/utils/Logger.cpp | 28 ---- src/utils/Logger.h | 1 + src/vehicle/ControllableVehicles.cpp | 6 +- tests/CMakeLists.txt | 5 +- tests/HTTPDataSourceTest.cpp | 147 ++++++++++++------- tests/TrafficLightDetectorTest.cpp | 59 ++++---- tools/map_websocket.html | 78 ++++++++-- tools/mock_server.py | 166 ++++++++++++---------- tools/test_websocket.html | 12 +- 23 files changed, 586 insertions(+), 284 deletions(-) create mode 100644 HTTPClient.cpp create mode 100644 docs/conflict_detection_algorithms.md delete mode 100644 src/config/SystemConfigJson.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 31cdbdf..bec94bc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -129,26 +129,8 @@ if(BUILD_TESTS AND EXISTS "${CMAKE_SOURCE_DIR}/tests") FetchContent_MakeAvailable(googletest) endif() - # 测试源文件列表 - set(TEST_SOURCES - tests/BasicCollisionTest.cpp - tests/CollisionDetectorTest.cpp - tests/BasicTypesTest.cpp - tests/HTTPDataSourceTest.cpp - tests/DataCollectorTest.cpp - tests/SafetyZoneTest.cpp - ) - - # 创建测试可执行文件 - add_executable(unit_tests ${TEST_SOURCES}) - - # 统一 macOS 和 CentOS 的链接设置 - target_link_libraries(unit_tests - PRIVATE - ${PROJECT_NAME}_lib - GTest::gtest_main - GTest::gmock_main - ) + # 添加 tests 子目录 + add_subdirectory(tests) endif() # 打印配置信息 diff --git a/HTTPClient.cpp b/HTTPClient.cpp new file mode 100644 index 0000000..721017f --- /dev/null +++ b/HTTPClient.cpp @@ -0,0 +1,12 @@ +std::string getSignalStateString(SignalState state) { + switch (state) { + case SignalState::RED: + return "RED"; + case SignalState::YELLOW: + return "YELLOW"; + case SignalState::GREEN: + return "GREEN"; + default: + return "UNKNOWN"; + } +} \ No newline at end of file diff --git a/docs/conflict_detection_algorithms.md b/docs/conflict_detection_algorithms.md new file mode 100644 index 0000000..9b3d14e --- /dev/null +++ b/docs/conflict_detection_algorithms.md @@ -0,0 +1,197 @@ +# 机场地面冲突检测算法方案 + +## 1. 算法概述 + +机场地面冲突检测系统采用多层次检测策略，结合不同算法的优势，实现快速、准确、可靠的冲突检测。 + +## 2. 常用冲突检测算法 + +### 2.1 基于几何的检测算法 + +#### 2.1.1 CPA (Closest Point of Approach) 算法 + +- 原理： + - 计算两个移动物体之间的最近接近点 + - 预测未来一段时间内的最小距离 + - 判断是否小于安全阈值 + +- 优点： + - 计算简单快速 + - 适合实时系统 + - 易于实现 + +- 缺点： + - 不考虑路网约束 + - 预测精度有限 + +#### 2.1.2 安全区域重叠检测 + +- 原理： + - 为每个物体定义安全区域（通常是椭圆或矩形） + - 检测安全区域是否重叠 + - 考虑速度向量进行预测 + +- 优点： + - 直观易理解 + - 可以考虑物体实际尺寸 + - 适合不同类型交通工具 + +### 2.2 基于时空的检测算法 + +#### 2.2.1 时空立方体算法 + +- 原理： + - 构建 4D 时空轨迹（x, y, z, t） + - 检测时空轨迹的交叉 + - 计算到达同一位置的时间差 + +- 优点： + - 预测性强 + - 可以处理复杂场景 + - 考虑时间维度 + +#### 2.2.2 网格化检测算法 + +- 原理： + - 将场地划分为网格 + - 预测物体在未来时间点占用的网格 + - 检测网格占用冲突 + +- 优点： + - 计算效率高 + - 易于并行处理 + - 适合大规模场景 + +### 2.3 基于概率的检测算法 + +#### 2.3.1 Monte Carlo 模拟 + +- 原理： + - 考虑位置和速度的不确定性 + - 多次随机采样模拟 + - 计算碰撞概率 + +- 优点： + - 考虑不确定性 + - 结果更可靠 + - 适合复杂环境 + +#### 2.3.2 贝叶斯预测模型 + +- 原理： + - 建立运动状态概率模型 + - 实时更新碰撞概率 + - 动态调整预警阈值 + +- 优点： + - 可以学习历史数据 + - 适应性强 + - 预测准确度高 + +## 3. 实现方案 + +### 3.1 多层次检测策略 + +采用三层检测机制： + +1. 第一层：快速检测 + + ```cpp + // 使用简单的 CPA 算法进行快速筛查 + struct CPAResult { + double min_distance; // 最小距离 + double time_to_cpa; // 到达最近点的时间 + Point cpa_point; // 最近接近点 + }; + + CPAResult calculateCPA(const Vehicle& v1, const Vehicle& v2, double prediction_time) { + // 计算两个物体的最近接近点 + // 返回最小距离和时间信息 + } + ``` + +2. 第二层：精确检测 + + ```cpp + // 使用安全区域重叠检测进行精确判断 + struct SafetyZone { + Point center; + double length; + double width; + double orientation; + }; + + bool checkSafetyZoneOverlap(const SafetyZone& zone1, const SafetyZone& zone2) { + // 检查两个安全区域是否重叠 + // 考虑物体的实际尺寸和方向 + } + ``` + +3. 第三层：预测检测 + + ```cpp + // 使用时空网格进行路径预测 + struct TimeSpaceGrid { + int x, y; // 空间坐标 + double time; // 时间戳 + std::vector occupants; // 占用该格子的物体ID + }; + + bool checkPathConflict(const Vehicle& v1, const Vehicle& v2, + const std::vector& grids) { + // 检查未来时间窗口内的路径冲突 + // 考虑路网约束 + } + ``` + +### 3.2 综合判断 + +```cpp +struct ConflictResult { + bool has_conflict; // 是否存在冲突 + double risk_level; // 风险等级 + double time_to_conflict; // 到冲突的时间 + std::string conflict_type; // 冲突类型 +}; + +ConflictResult detectConflict(const Vehicle& v1, const Vehicle& v2) { + // 1. 快速 CPA 检测 + // 2. 如果可能存在冲突，进行安全区域检测 + // 3. 对于高风险情况，进行时空路径预测 + // 4. 综合三层结果，返回最终判断 +} +``` + +## 4. 实现步骤 + +1. 第一阶段：基础功能实现 + - 实现 CPA 算法 + - 实现基本的安全区域检测 + - 设计基础数据结构 + +2. 第二阶段：增强功能 + - 添加时空网格检测 + - 实现路径预测 + - 优化性能 + +3. 第三阶段：高级特性 + - 添加概率模型 + - 实现自适应阈值 + - 系统集成测试 + +## 5. 注意事项 + +1. 性能要求 + - 实时响应：检测延迟不超过 100ms + - CPU 占用率控制在 50% 以下 + - 内存使用不超过 1GB + +2. 可靠性要求 + - 误报率控制在 1% 以下 + - 漏报率控制在 0.1% 以下 + - 系统可用性 99.99% + +3. 扩展性要求 + - 支持新增检测算法 + - 支持参数配置调整 + - 支持不同类型交通工具 diff --git a/docs/requirements.md b/docs/requirements.md index 8e26ff8..2534900 100644 --- a/docs/requirements.md +++ b/docs/requirements.md @@ -8,9 +8,22 @@ ### 2.2 冲突检测 -### 2.3 红绿灯信号处理 + 1. 路径检测 + 2. 距离检测 + 3. 路口检测 -### 2.4 数据采集 +### 2.3 无人车辆控制 + + 1. 无人车控制 + 1. 控制指令 + 1. 冲突预警 + 2. 冲突告警 + 3. 安全停靠 + 2. + +### 2.4 红绿灯信号处理 + +### 2.5 数据采集 1. 航空器和车辆位置数据 1. 数据来源： diff --git a/src/collector/DataSourceConfig.cpp b/src/collector/DataSourceConfig.cpp index 2f8829d..19a389a 100644 --- a/src/collector/DataSourceConfig.cpp +++ b/src/collector/DataSourceConfig.cpp @@ -19,17 +19,17 @@ DataSourceConfig DataSourceConfig::fromSystemConfig(const SystemConfig& config) dataSourceConfig.position.auth.auth_required = config.data_source.position.auth.auth_required; // 转换无人车数据源配置 - dataSourceConfig.vehicle.host = config.data_source.unmanned_vehicle.host; - dataSourceConfig.vehicle.port = config.data_source.unmanned_vehicle.port; - dataSourceConfig.vehicle.status_path = config.data_source.unmanned_vehicle.status_path; - dataSourceConfig.vehicle.command_path = config.data_source.unmanned_vehicle.command_path; - dataSourceConfig.vehicle.refresh_interval_ms = config.data_source.unmanned_vehicle.refresh_interval_ms; - dataSourceConfig.vehicle.timeout_ms = config.data_source.unmanned_vehicle.timeout_ms; - dataSourceConfig.vehicle.read_timeout_ms = config.data_source.unmanned_vehicle.read_timeout_ms; - dataSourceConfig.vehicle.auth.username = config.data_source.unmanned_vehicle.auth.username; - dataSourceConfig.vehicle.auth.password = config.data_source.unmanned_vehicle.auth.password; - dataSourceConfig.vehicle.auth.auth_path = config.data_source.unmanned_vehicle.auth.auth_path; - dataSourceConfig.vehicle.auth.auth_required = config.data_source.unmanned_vehicle.auth.auth_required; + dataSourceConfig.vehicle.host = config.data_source.vehicle.host; + dataSourceConfig.vehicle.port = config.data_source.vehicle.port; + dataSourceConfig.vehicle.status_path = config.data_source.vehicle.status_path; + dataSourceConfig.vehicle.command_path = config.data_source.vehicle.command_path; + dataSourceConfig.vehicle.refresh_interval_ms = config.data_source.vehicle.refresh_interval_ms; + dataSourceConfig.vehicle.timeout_ms = config.data_source.vehicle.timeout_ms; + dataSourceConfig.vehicle.read_timeout_ms = config.data_source.vehicle.read_timeout_ms; + dataSourceConfig.vehicle.auth.username = config.data_source.vehicle.auth.username; + dataSourceConfig.vehicle.auth.password = config.data_source.vehicle.auth.password; + dataSourceConfig.vehicle.auth.auth_path = config.data_source.vehicle.auth.auth_path; + dataSourceConfig.vehicle.auth.auth_required = config.data_source.vehicle.auth.auth_required; // 转换红绿灯数据源配置 dataSourceConfig.traffic_light.host = config.data_source.traffic_light.host; diff --git a/src/config/SystemConfigJson.cpp b/src/config/SystemConfigJson.cpp deleted file mode 100644 index 0519ecb..0000000 --- a/src/config/SystemConfigJson.cpp +++ /dev/null @@ -1 +0,0 @@ - \ No newline at end of file diff --git a/src/core/System.cpp b/src/core/System.cpp index 6f790ae..2efc3f1 100644 --- a/src/core/System.cpp +++ b/src/core/System.cpp @@ -677,6 +677,7 @@ void System::broadcastVehicleCommand(const VehicleCommand& cmd) { case CommandType::ALERT: return "ALERT"; case CommandType::WARNING: return "WARNING"; case CommandType::RESUME: return "RESUME"; + case CommandType::PARKING: return "PARKING"; default: return "UNKNOWN"; } }()}, @@ -687,6 +688,7 @@ void System::broadcastVehicleCommand(const VehicleCommand& cmd) { case CommandReason::SPECIAL_VEHICLE: return "SPECIAL_VEHICLE"; case CommandReason::AIRCRAFT_PUSH: return "AIRCRAFT_PUSH"; case CommandReason::RESUME_TRAFFIC: return "RESUME_TRAFFIC"; + case CommandReason::PARKING_SIDE: return "PARKING_SIDE"; default: return "UNKNOWN"; } }()}, diff --git a/src/detector/CollisionDetector.cpp b/src/detector/CollisionDetector.cpp index 18b7219..0f4a9f2 100644 --- a/src/detector/CollisionDetector.cpp +++ b/src/detector/CollisionDetector.cpp @@ -8,7 +8,7 @@ CollisionDetector::CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles) : airportBounds_(bounds) , vehicleTree_(bounds.getAirportBounds(), 8) // 使用机场总边界初始化四叉树 - , controllableVehicles_(controllableVehicles) { + , controllableVehicles_(&controllableVehicles) { // 记录初始化信息 const auto& airportBounds = bounds.getAirportBounds(); @@ -37,7 +37,7 @@ void CollisionDetector::updateTraffic(const std::vector& aircraft, // 根据配置设置车辆类型 Vehicle updatedVehicle = vehicle; - const auto* config = controllableVehicles_.findVehicle(vehicle.vehicleNo); + const auto* config = controllableVehicles_->findVehicle(vehicle.vehicleNo); if (config) { if (config->type == "UNMANNED") { updatedVehicle.type = MovingObjectType::UNMANNED; diff --git a/src/detector/CollisionDetector.h b/src/detector/CollisionDetector.h index c0d75a5..27c3e7b 100644 --- a/src/detector/CollisionDetector.h +++ b/src/detector/CollisionDetector.h @@ -74,7 +74,7 @@ private: const AirportBounds& airportBounds_; QuadTree vehicleTree_; std::vector aircraftData_; - const ControllableVehicles& controllableVehicles_; + const ControllableVehicles* controllableVehicles_; // 冲突记录映射表：<(id1,id2), CollisionRecord> std::unordered_map, CollisionRecord, CollisionPairHash> collisionRecords_; diff --git a/src/mock/MockDataService.h b/src/mock/MockDataService.h index 0b8cad2..2852115 100644 --- a/src/mock/MockDataService.h +++ b/src/mock/MockDataService.h @@ -2,7 +2,7 @@ #define AIRPORT_MOCK_MOCKDATASERVICE_H #include "types/BasicTypes.h" -#include "spatial/AirportBounds.h" +#include "config/AirportBounds.h" #include "types/TrafficLightTypes.h" #include #include diff --git a/src/network/HTTPClient.cpp b/src/network/HTTPClient.cpp index 0935887..e236a1e 100644 --- a/src/network/HTTPClient.cpp +++ b/src/network/HTTPClient.cpp @@ -1,7 +1,22 @@ -#include "network/HTTPClient.h" +#include "HTTPClient.h" #include "utils/Logger.h" #include +namespace { +std::string getSignalStateString(SignalState state) { + switch (state) { + case SignalState::RED: + return "RED"; + case SignalState::YELLOW: + return "YELLOW"; + case SignalState::GREEN: + return "GREEN"; + default: + return "UNKNOWN"; + } +} +} // anonymous namespace + HTTPClient::HTTPClient() { curl_global_init(CURL_GLOBAL_ALL); curl_ = curl_easy_init(); @@ -49,6 +64,7 @@ bool HTTPClient::sendCommand(const std::string& ip, int port, const VehicleComma case CommandType::ALERT: return "ALERT"; case CommandType::WARNING: return "WARNING"; case CommandType::RESUME: return "RESUME"; + case CommandType::PARKING: return "PARKING"; default: return "UNKNOWN"; } }()}, @@ -59,28 +75,20 @@ bool HTTPClient::sendCommand(const std::string& ip, int port, const VehicleComma case CommandReason::SPECIAL_VEHICLE: return "SPECIAL_VEHICLE"; case CommandReason::AIRCRAFT_PUSH: return "AIRCRAFT_PUSH"; case CommandReason::RESUME_TRAFFIC: return "RESUME_TRAFFIC"; + case CommandReason::PARKING_SIDE: return "PARKING_SIDE"; default: return "UNKNOWN"; } }()}, {"latitude", command.latitude}, - {"longitude", command.longitude} + {"longitude", command.longitude}, + {"signalState", getSignalStateString(command.signalState)}, + {"intersectionId", command.intersectionId}, + {"relativeSpeed", command.relativeSpeed}, + {"relativeMotionX", command.relativeMotionX}, + {"relativeMotionY", command.relativeMotionY}, + {"minDistance", command.minDistance} }; - // 添加可选字段 - if (command.type == CommandType::SIGNAL) { - // 信号灯指令的可选字段 - request["signalState"] = command.signalState == SignalState::RED ? "RED" : "GREEN"; - request["intersectionId"] = command.intersectionId; - } - - if (command.type == CommandType::ALERT || command.type == CommandType::WARNING) { - // 告警/预警指令的可选字段 - request["relativeSpeed"] = command.relativeSpeed; - request["relativeMotionX"] = command.relativeMotionX; - request["relativeMotionY"] = command.relativeMotionY; - request["minDistance"] = command.minDistance; - } - std::string request_body = request.dump(); response_buffer_.clear(); @@ -131,4 +139,4 @@ bool HTTPClient::sendCommand(const std::string& ip, int port, const VehicleComma Logger::error("Command failed with HTTP code: ", http_code, " response: ", response_buffer_); return false; } -} \ No newline at end of file +} \ No newline at end of file diff --git a/src/network/HTTPDataSource.cpp b/src/network/HTTPDataSource.cpp index 4da56c1..c36785f 100644 --- a/src/network/HTTPDataSource.cpp +++ b/src/network/HTTPDataSource.cpp @@ -1,4 +1,5 @@ -#include "network/HTTPDataSource.h" +#include "HTTPDataSource.h" +#include "HTTPClient.h" #include "utils/Logger.h" #include #include @@ -264,6 +265,19 @@ bool HTTPDataSource::fetchTrafficLightSignals(std::vector& s return parseTrafficLightResponse(response, signals); } +std::string getSignalStateString(SignalState state) { + switch (state) { + case SignalState::RED: + return "RED"; + case SignalState::YELLOW: + return "YELLOW"; + case SignalState::GREEN: + return "GREEN"; + default: + return "GREEN"; // 默认为绿灯 + } +} + bool HTTPDataSource::sendUnmannedVehicleCommand(const std::string& vehicle_id, const VehicleCommand& command) { std::lock_guard lock(mutex_); @@ -310,14 +324,11 @@ bool HTTPDataSource::sendUnmannedVehicleCommand(const std::string& vehicle_id, c } }()}, {"latitude", command.latitude}, - {"longitude", command.longitude} + {"longitude", command.longitude}, + {"signalState", getSignalStateString(command.signalState)}, + {"intersectionId", command.intersectionId} }; - if (command.type == CommandType::SIGNAL) { - request["signalState"] = command.signalState == SignalState::RED ? "RED" : "GREEN"; - request["intersectionId"] = command.intersectionId; - } - if (command.type == CommandType::ALERT || command.type == CommandType::WARNING) { request["relativeSpeed"] = command.relativeSpeed; request["relativeMotionX"] = command.relativeMotionX; diff --git a/src/spatial/QuadTree.cpp b/src/spatial/QuadTree.cpp index ab6bd9c..61e7645 100644 --- a/src/spatial/QuadTree.cpp +++ b/src/spatial/QuadTree.cpp @@ -1,17 +1,5 @@ #include "QuadTree.h" -bool Bounds::contains(const Vector2D& point) const { - return point.x >= x && point.x <= (x + width) && - point.y >= y && point.y <= (y + height); -} - -bool Bounds::intersects(const Bounds& other) const { - return !(other.x > (x + width) || - (other.x + other.width) < x || - other.y > (y + height) || - (other.y + other.height) < y); -} - // 显式实例化常用类型 template class QuadTree; template class QuadTree; \ No newline at end of file diff --git a/src/types/VehicleCommand.h b/src/types/VehicleCommand.h index dde7ed4..e73e0c5 100644 --- a/src/types/VehicleCommand.h +++ b/src/types/VehicleCommand.h @@ -9,7 +9,8 @@ enum class CommandType { ALERT, // 告警指令 SIGNAL, // 信号灯指令 WARNING, // 预警指令 - RESUME // 恢复指令 + RESUME, // 恢复指令 + PARKING // 安全停靠 }; // 信号灯状态 @@ -25,7 +26,8 @@ enum class CommandReason { AIRCRAFT_CROSSING, // 航空器交叉 SPECIAL_VEHICLE, // 特勤车辆 AIRCRAFT_PUSH, // 航空器推出 - RESUME_TRAFFIC // 恢复通行 + RESUME_TRAFFIC, // 恢复通行 + PARKING_SIDE // 安全停靠 }; struct VehicleCommand { diff --git a/src/utils/Logger.cpp b/src/utils/Logger.cpp index d119efc..3a3fb36 100644 --- a/src/utils/Logger.cpp +++ b/src/utils/Logger.cpp @@ -1,32 +1,4 @@ #include "Logger.h" -#include -#include -#include - -LogLevel& Logger::currentLevel() { - static LogLevel level = LogLevel::INFO; - return level; -} - -void Logger::setLogLevel(LogLevel level) { - currentLevel() = level; -} - -template -void Logger::log(const char* level, Args... args) { - auto now = std::chrono::system_clock::now(); - auto now_c = std::chrono::system_clock::to_time_t(now); - auto now_ms = std::chrono::duration_cast( - now.time_since_epoch()) % 1000; - - std::stringstream ss; - ss << std::put_time(std::localtime(&now_c), "%Y-%m-%d %H:%M:%S") - << '.' << std::setfill('0') << std::setw(3) << now_ms.count() - << " [" << level << "] "; - - (ss << ... << args) << std::endl; - std::cerr << ss.str(); -} // 显式实例化模板，以支持常用类型 template void Logger::log(const char* level, const char* msg); diff --git a/src/utils/Logger.h b/src/utils/Logger.h index ecc29d7..ac045cb 100644 --- a/src/utils/Logger.h +++ b/src/utils/Logger.h @@ -84,6 +84,7 @@ private: << '.' << std::setfill('0') << std::setw(3) << now_ms.count() << " [" << level << "] "; + ss << std::fixed << std::setprecision(14); (ss << ... << args) << std::endl; std::lock_guard lock(getMutex()); diff --git a/src/vehicle/ControllableVehicles.cpp b/src/vehicle/ControllableVehicles.cpp index 219ff62..8a3fe34 100644 --- a/src/vehicle/ControllableVehicles.cpp +++ b/src/vehicle/ControllableVehicles.cpp @@ -93,12 +93,14 @@ bool ControllableVehicles::sendCommand(const std::string& vehicleNo, const Vehic Logger::info("Successfully sent command to vehicle ", vehicleNo, ": ", command.type == CommandType::SIGNAL ? "SIGNAL" : command.type == CommandType::ALERT ? "ALERT" : - command.type == CommandType::WARNING ? "WARNING" : "RESUME", + command.type == CommandType::WARNING ? "WARNING" : + command.type == CommandType::PARKING ? "PARKING" : "RESUME", "/", command.reason == CommandReason::TRAFFIC_LIGHT ? "TRAFFIC_LIGHT" : command.reason == CommandReason::AIRCRAFT_CROSSING ? "AIRCRAFT_CROSSING" : command.reason == CommandReason::SPECIAL_VEHICLE ? "SPECIAL_VEHICLE" : - command.reason == CommandReason::AIRCRAFT_PUSH ? "AIRCRAFT_PUSH" : "RESUME_TRAFFIC"); + command.reason == CommandReason::AIRCRAFT_PUSH ? "AIRCRAFT_PUSH" : + command.reason == CommandReason::PARKING_SIDE ? "PARKING_SIDE" : "RESUME_TRAFFIC"); } else { Logger::error("Failed to send command to vehicle ", vehicleNo); } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index f25529d..7afe78f 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -2,10 +2,11 @@ set(TEST_SOURCES BasicCollisionTest.cpp CollisionDetectorTest.cpp + TrafficLightDetectorTest.cpp + SafetyZoneTest.cpp BasicTypesTest.cpp HTTPDataSourceTest.cpp DataCollectorTest.cpp - TrafficLightDetectorTest.cpp ) # 创建测试可执行文件 @@ -14,7 +15,7 @@ add_executable(unit_tests ${TEST_SOURCES}) # 链接需要的库 target_link_libraries(unit_tests PRIVATE - airport_core + ${PROJECT_NAME}_lib GTest::gtest_main GTest::gmock_main ) diff --git a/tests/HTTPDataSourceTest.cpp b/tests/HTTPDataSourceTest.cpp index 7c1e16c..e4e914b 100644 --- a/tests/HTTPDataSourceTest.cpp +++ b/tests/HTTPDataSourceTest.cpp @@ -113,6 +113,16 @@ TEST_F(HTTPDataSourceTest, TrafficLightSignals) { DataSourceConfig config; config.position.host = "localhost"; config.position.port = 8081; + config.position.aircraft_path = "/openApi/getCurrentFlightPositions"; + config.position.vehicle_path = "/openApi/getCurrentVehiclePositions"; + config.position.auth.auth_required = true; + config.position.auth.auth_path = "/login"; + config.position.auth.username = "dianxin"; + config.position.auth.password = "dianxin@123"; + config.position.timeout_ms = 5000; + config.position.read_timeout_ms = 2000; + config.position.refresh_interval_ms = 100; + config.traffic_light.host = "localhost"; config.traffic_light.port = 8081; config.traffic_light.signal_path = "/api/VehicleCommandInfo"; @@ -123,7 +133,12 @@ TEST_F(HTTPDataSourceTest, TrafficLightSignals) { HTTPDataSource source(config); ASSERT_TRUE(source.connect()); - // 先发送 GREEN 指令解除 RED 状态 + // 先获取车辆状态来初始化车辆 + std::vector vehicles; + ASSERT_TRUE(source.fetchPositionVehicleData(vehicles)) << "获取车辆状态失败"; + ASSERT_FALSE(vehicles.empty()) << "车辆数据为空"; + + // 发送绿灯指令（优先级 2） VehicleCommand green_command; green_command.vehicleId = "QN001"; green_command.type = CommandType::SIGNAL; @@ -134,60 +149,65 @@ TEST_F(HTTPDataSourceTest, TrafficLightSignals) { std::chrono::system_clock::now().time_since_epoch()).count(); ASSERT_TRUE(source.sendUnmannedVehicleCommand(green_command.vehicleId, green_command)) << "发送绿灯指令失败"; - // 等待一小段时间确保 GREEN 指令生效 - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // 等待绿灯指令生效 + std::this_thread::sleep_for(std::chrono::milliseconds(500)); - // 再发送 RESUME 指令解除其他可能的状态 - VehicleCommand resume_command; - resume_command.vehicleId = "QN001"; - resume_command.type = CommandType::RESUME; - resume_command.reason = CommandReason::RESUME_TRAFFIC; - resume_command.timestamp = std::chrono::duration_cast( + // 发送红灯指令（优先级 4） + VehicleCommand red_command; + red_command.vehicleId = "QN001"; + red_command.type = CommandType::SIGNAL; + red_command.reason = CommandReason::TRAFFIC_LIGHT; + red_command.signalState = SignalState::RED; + red_command.intersectionId = "INTERSECTION_001"; + red_command.latitude = 36.36; + red_command.longitude = 120.08; + red_command.timestamp = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()).count(); - ASSERT_TRUE(source.sendUnmannedVehicleCommand(resume_command.vehicleId, resume_command)) << "发送恢复指令失败"; + ASSERT_TRUE(source.sendUnmannedVehicleCommand(red_command.vehicleId, red_command)) << "发送红灯指令失败"; - // 等待一小段时间确保 RESUME 指令生效 - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // 等待红灯指令生效 + std::this_thread::sleep_for(std::chrono::milliseconds(500)); - // 发送红绿灯信号指令 - VehicleCommand command; - command.vehicleId = "QN001"; - command.type = CommandType::SIGNAL; - command.reason = CommandReason::TRAFFIC_LIGHT; - command.signalState = SignalState::RED; - command.intersectionId = "INTERSECTION_001"; - command.latitude = 36.36; - command.longitude = 120.08; - command.timestamp = std::chrono::duration_cast( + + // 最后发送绿灯指令 + green_command.timestamp = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()).count(); - - ASSERT_TRUE(source.sendUnmannedVehicleCommand(command.vehicleId, command)) << "获取红绿灯信号失败"; + ASSERT_TRUE(source.sendUnmannedVehicleCommand(green_command.vehicleId, green_command)) << "发送最终绿灯指令失败"; } TEST_F(HTTPDataSourceTest, UnmannedVehicleCommand) { DataSourceConfig config; + config.position.host = "localhost"; + config.position.port = 8081; + config.position.aircraft_path = "/openApi/getCurrentFlightPositions"; + config.position.vehicle_path = "/openApi/getCurrentVehiclePositions"; + config.position.auth.auth_required = true; + config.position.auth.auth_path = "/login"; + config.position.auth.username = "dianxin"; + config.position.auth.password = "dianxin@123"; + config.position.timeout_ms = 5000; + config.position.read_timeout_ms = 2000; + config.position.refresh_interval_ms = 100; + config.vehicle.host = "localhost"; config.vehicle.port = 8081; config.vehicle.command_path = "/api/VehicleCommandInfo"; + config.vehicle.auth.auth_required = true; + config.vehicle.auth.auth_path = "/login"; + config.vehicle.auth.username = "dianxin"; + config.vehicle.auth.password = "dianxin@123"; + config.vehicle.timeout_ms = 5000; + config.vehicle.read_timeout_ms = 2000; HTTPDataSource source(config); - ASSERT_TRUE(source.connect()); + ASSERT_TRUE(source.connect()) << "连接失败"; - // 先发送 GREEN 指令解除 RED 状态 - VehicleCommand green_command; - green_command.vehicleId = "QN001"; - green_command.type = CommandType::SIGNAL; - green_command.reason = CommandReason::TRAFFIC_LIGHT; - green_command.signalState = SignalState::GREEN; - green_command.intersectionId = "INTERSECTION_001"; - green_command.timestamp = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()).count(); - ASSERT_TRUE(source.sendUnmannedVehicleCommand(green_command.vehicleId, green_command)) << "发送绿灯指令失败"; + // 先获取车辆状态来初始化车辆 + std::vector vehicles; + ASSERT_TRUE(source.fetchPositionVehicleData(vehicles)) << "获取车辆状态失败"; + ASSERT_FALSE(vehicles.empty()) << "车辆数据为空"; - // 等待一小段时间确保 GREEN 指令生效 - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - // 再发送 RESUME 指令解除其他可能的状态 + // 先发送 RESUME 指令清除所有状态 VehicleCommand resume_command; resume_command.vehicleId = "QN001"; resume_command.type = CommandType::RESUME; @@ -196,24 +216,43 @@ TEST_F(HTTPDataSourceTest, UnmannedVehicleCommand) { std::chrono::system_clock::now().time_since_epoch()).count(); ASSERT_TRUE(source.sendUnmannedVehicleCommand(resume_command.vehicleId, resume_command)) << "发送恢复指令失败"; - // 等待一小段时间确保 RESUME 指令生效 - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // 等待 RESUME 指令生效 + std::this_thread::sleep_for(std::chrono::milliseconds(500)); - // 发送告警指令 - VehicleCommand command; - command.vehicleId = "QN001"; - command.type = CommandType::ALERT; - command.reason = CommandReason::AIRCRAFT_CROSSING; - command.relativeSpeed = 100; - command.relativeMotionX = 100; - command.relativeMotionY = 100; - command.minDistance = 100; - command.latitude = 36.36; - command.longitude = 120.08; - command.timestamp = std::chrono::duration_cast( + // 发送 WARNING 指令（优先级 3） + VehicleCommand warning_command; + warning_command.vehicleId = "QN001"; + warning_command.type = CommandType::WARNING; + warning_command.reason = CommandReason::AIRCRAFT_CROSSING; + warning_command.relativeSpeed = 50; + warning_command.relativeMotionX = 50; + warning_command.relativeMotionY = 50; + warning_command.timestamp = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()).count(); + ASSERT_TRUE(source.sendUnmannedVehicleCommand(warning_command.vehicleId, warning_command)) << "发送预警指令失败"; - ASSERT_TRUE(source.sendUnmannedVehicleCommand(command.vehicleId, command)) << "发送无人车指令失败"; + // 等待 WARNING 指令生效 + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + // 发送 ALERT 指令（优先级 5） + VehicleCommand alert_command; + alert_command.vehicleId = "QN001"; + alert_command.type = CommandType::ALERT; + alert_command.reason = CommandReason::AIRCRAFT_CROSSING; + alert_command.relativeSpeed = 100; + alert_command.relativeMotionX = 100; + alert_command.relativeMotionY = 100; + alert_command.timestamp = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + ASSERT_TRUE(source.sendUnmannedVehicleCommand(alert_command.vehicleId, alert_command)) << "发送告警指令失败"; + + // 等待 ALERT 指令生效 + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + // 最后发送 RESUME 指令清除状态 + resume_command.timestamp = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + ASSERT_TRUE(source.sendUnmannedVehicleCommand(resume_command.vehicleId, resume_command)) << "发送最终恢复指令失败"; } TEST_F(HTTPDataSourceTest, TrafficLightAndUnmannedAuthFailure) { diff --git a/tests/TrafficLightDetectorTest.cpp b/tests/TrafficLightDetectorTest.cpp index 82a7594..dae325c 100644 --- a/tests/TrafficLightDetectorTest.cpp +++ b/tests/TrafficLightDetectorTest.cpp @@ -8,12 +8,20 @@ #include "utils/Logger.h" #include "types/VehicleCommand.h" #include "types/TrafficLightTypes.h" +#include "core/System.h" -class MockControllableVehicles : public ControllableVehicles { +// Mock System 类 +class MockSystem : public System { public: - explicit MockControllableVehicles() : ControllableVehicles("config/vehicle_speed_limits.json") {} - MOCK_METHOD(bool, isControllable, (const std::string& vehicleNo), (const, override)); - MOCK_METHOD(void, sendCommand, (const std::string& vehicleNo, const VehicleCommand& command), (override)); + MOCK_METHOD(void, broadcastTrafficLightCommand, (const std::string&, const VehicleCommand&)); + static MockSystem& getInstance() { + static MockSystem instance; + return instance; + } +private: + MockSystem() = default; + MockSystem(const MockSystem&) = delete; + MockSystem& operator=(const MockSystem&) = delete; }; class TrafficLightDetectorTest : public ::testing::Test { @@ -25,7 +33,8 @@ protected: intersection.name = "Test Intersection"; intersection.position.latitude = 36.36; intersection.position.longitude = 120.08; - intersection.radius = 50.0; + intersection.position.altitude = 9.5; + intersection.width = 50.0; intersection.trafficLightId = "TL001"; // 创建临时配置文件 @@ -37,18 +46,25 @@ protected: {"name", intersection.name}, {"position", { {"latitude", intersection.position.latitude}, - {"longitude", intersection.position.longitude} + {"longitude", intersection.position.longitude}, + {"altitude", intersection.position.altitude} }}, - {"radius", intersection.radius}, - {"trafficLightId", intersection.trafficLightId} + {"width", intersection.width}, + {"trafficLightId", intersection.trafficLightId}, + {"safetyZone", { + {"aircraftRadius", 50.0}, + {"vehicleRadius", 50.0} + }} }); config_file << j.dump(4); config_file.close(); intersectionConfig = IntersectionConfig::load("test_intersections.json"); - - mockControllableVehicles = std::make_shared<::testing::NiceMock>(); - detector = std::make_unique(intersectionConfig, *mockControllableVehicles); + detector = std::make_unique( + intersectionConfig, + ControllableVehicles::getInstance(), + MockSystem::getInstance() + ); } void TearDown() override { @@ -78,7 +94,6 @@ protected: std::unique_ptr detector; IntersectionConfig intersectionConfig; - std::shared_ptr mockControllableVehicles; }; // 测试红绿灯信号处理 @@ -89,19 +104,11 @@ TEST_F(TrafficLightDetectorTest, SignalProcessing) { // 测试红灯 TrafficLightSignal redSignal = createTestSignal("TL001", SignalStatus::RED); - EXPECT_CALL(*mockControllableVehicles, isControllable("V001")) - .WillOnce(::testing::Return(true)); - EXPECT_CALL(*mockControllableVehicles, sendCommand("V001", ::testing::_)) - .Times(1); detector->processSignal(redSignal, vehicles); // 测试绿灯 TrafficLightSignal greenSignal = createTestSignal("TL001", SignalStatus::GREEN); - EXPECT_CALL(*mockControllableVehicles, isControllable("V001")) - .WillOnce(::testing::Return(true)); - EXPECT_CALL(*mockControllableVehicles, sendCommand("V001", ::testing::_)) - .Times(1); detector->processSignal(greenSignal, vehicles); } @@ -113,10 +120,6 @@ TEST_F(TrafficLightDetectorTest, InvalidSignal) { // 测试无效的红绿灯ID TrafficLightSignal invalidSignal = createTestSignal("TL999", SignalStatus::RED); - EXPECT_CALL(*mockControllableVehicles, isControllable(::testing::_)) - .Times(0); - EXPECT_CALL(*mockControllableVehicles, sendCommand(::testing::_, ::testing::_)) - .Times(0); detector->processSignal(invalidSignal, vehicles); } @@ -126,20 +129,12 @@ TEST_F(TrafficLightDetectorTest, EdgeCases) { // 测试空车辆列表 std::vector emptyVehicles; TrafficLightSignal signal = createTestSignal("TL001", SignalStatus::RED); - EXPECT_CALL(*mockControllableVehicles, isControllable(::testing::_)) - .Times(0); - EXPECT_CALL(*mockControllableVehicles, sendCommand(::testing::_, ::testing::_)) - .Times(0); detector->processSignal(signal, emptyVehicles); // 测试交叉口范围外的车辆 Vehicle farVehicle = createTestVehicle("V002", 36.40, 120.12); std::vector vehicles = {farVehicle}; - EXPECT_CALL(*mockControllableVehicles, isControllable(::testing::_)) - .Times(0); - EXPECT_CALL(*mockControllableVehicles, sendCommand(::testing::_, ::testing::_)) - .Times(0); detector->processSignal(signal, vehicles); } \ No newline at end of file diff --git a/tools/map_websocket.html b/tools/map_websocket.html index 19de9ea..377cc6e 100644 --- a/tools/map_websocket.html +++ b/tools/map_websocket.html @@ -143,6 +143,21 @@ pointer-events: none; z-index: 1000; } + .safety-border { + width: 100%; + height: 100%; + border: 2px solid; + background: none; + } + .emergency-border { + border-color: rgba(255, 0, 0, 0.8); + } + .core-border { + border-color: rgba(255, 165, 0, 0.6); + } + .warning-border { + border-color: rgba(255, 255, 0, 0.4); + } @@ -275,11 +290,43 @@ // 根据ID前缀确定图标类型 if (data.object_type === 'aircraft') { - iconClass = 'aircraft-icon'; // 六形 + iconClass = 'aircraft-icon'; + + // 创建或更新安全边框 + const borders = [ + {size: 250, class: 'warning-border'}, // 外围预警区 + {size: 150, class: 'core-border'}, // 核心安全区 + {size: 100, class: 'emergency-border'} // 紧急制动区 + ]; + + borders.forEach(border => { + const borderId = id + '_' + border.class; + let borderMarker = markers.get(borderId); + + if (!borderMarker) { + // 创建边框标记 + borderMarker = L.marker(position, { + icon: L.divIcon({ + className: 'safety-border ' + border.class, + iconSize: [border.size, border.size], + iconAnchor: [border.size/2, border.size/2] + }), + rotationAngle: data.heading || 0, + rotationOrigin: 'center center' + }).addTo(map); + markers.set(borderId, borderMarker); + } else { + // 更新边框位置和方向 + borderMarker.setLatLng(position); + if (data.heading !== undefined) { + borderMarker.setRotationAngle(data.heading); + } + } + }); } else if (id.startsWith('TQ')) { - iconClass = 'special-vehicle-icon'; // 正方形 + iconClass = 'special-vehicle-icon'; } else { - iconClass = 'vehicle-icon'; // 三角形 + iconClass = 'vehicle-icon'; } let marker = markers.get(id); @@ -287,10 +334,10 @@ // 创建新标记 marker = L.marker(position, { icon: createIcon(iconClass), - rotationAngle: data.heading || 0, // 设置初始航向 - rotationOrigin: 'center center' // 设置旋转中心 + rotationAngle: data.heading || 0, + rotationOrigin: 'center center' }).addTo(map); - marker.bindTooltip(id); // 添加标签显示ID + marker.bindTooltip(id); markers.set(id, marker); } else { // 更新现有标记位置和航向 @@ -482,17 +529,19 @@ updateVehicleCommand(data.vehicleId, data.commandType); // 为控制指令添加中文描述 const commandTypes = { - 'SIGNAL': '信号灯指令', - 'ALERT': '告警指令', - 'WARNING': '预警指令', - 'RESUME': '恢复指令' + 'SIGNAL': '信号灯', + 'ALERT': '告警', + 'WARNING': '预警', + 'RESUME': '恢复', + 'PARKING': '安全停靠' }; const reasons = { 'TRAFFIC_LIGHT': '红绿灯控制', 'AIRCRAFT_CROSSING': '航空器交叉', 'SPECIAL_VEHICLE': '特勤车辆', 'AIRCRAFT_PUSH': '航空器推出', - 'RESUME_TRAFFIC': '恢复通行' + 'RESUME_TRAFFIC': '恢复通行', + 'PARKING_SIDE': '安全停靠' }; message = `收到车辆控制指令:\n车辆: ${data.vehicleId}\n` + `指令类型: ${commandTypes[data.commandType] || data.commandType}\n` + @@ -541,9 +590,12 @@ ws.close(); ws = null; - // 清除所有标记 - markers.forEach(marker => map.removeLayer(marker)); + // 清除所有标记（包括边框标记） + markers.forEach((marker, key) => { + map.removeLayer(marker); + }); markers.clear(); + trafficLights.forEach(light => map.removeLayer(light)); trafficLights.clear(); } diff --git a/tools/mock_server.py b/tools/mock_server.py index c26cf1b..f637d5a 100644 --- a/tools/mock_server.py +++ b/tools/mock_server.py @@ -147,7 +147,7 @@ vehicle_data = [ "longitude": POINT_T12["longitude"], "latitude": POINT_T12["latitude"], "time": int(time.time() * 1000), - "direction": 135.0, # 东南方向约为135度 + "direction": 90.0, # 向东方向为90度 "speed": DEFAULT_VEHICLE_SPEED }, { @@ -230,26 +230,17 @@ class VehicleState: # 如果是红绿灯状态，更新状态和指令 if command_type in ["RED", "GREEN", "YELLOW"]: self.traffic_light_state = command_type + self.current_command = command_type + self.command_priority = priority_map[command_type] + if command_type == "RED": - self.current_command = command_type - self.command_priority = priority_map[command_type] self.is_running = False print(f"车辆 {self.vehicle_no} 收到红灯指令，停止运行") elif command_type == "YELLOW": # 黄灯表示红绿灯故障，清除当前红绿灯状态，允许车辆继续行驶 - self.current_command = command_type - self.command_priority = priority_map[command_type] self.is_running = True print(f"车辆 {self.vehicle_no} 收到黄灯指令（红绿灯故障），继续行驶") else: # GREEN - # 清除 RED 指令 - if self.current_command == "RED": - print(f"车辆 {self.vehicle_no} 收到绿灯指令，清除红灯指令") - - # 设置为绿灯指令 - self.current_command = command_type - self.command_priority = priority_map[command_type] - # 如果没有其他阻塞性指令，允许移动 if self.current_command not in ["ALERT", "WARNING"]: self.is_running = True @@ -258,6 +249,8 @@ class VehicleState: # 其他指令正常更新 self.current_command = command_type self.command_priority = priority_map.get(command_type, 0) + if command_type == "RESUME": + self.traffic_light_state = None # 清除红绿灯状态 print(f"更新车辆 {self.vehicle_no} 状态: command={self.current_command}, traffic_light={self.traffic_light_state}, priority={self.command_priority}") @@ -325,10 +318,15 @@ def handle_vehicle_command(): print(f"收到车辆控制指令: vehicle_id={vehicle_id}, type={command_type}, reason={reason}, signal_state={signal_state}") print(f"完整请求数据: {data}") - # 将 SIGNAL 指令转换为 RED、YELLOW 或 GREEN - if command_type == "SIGNAL": - command_type = signal_state # 直接使用信号状态作为命令类型 - print(f"转换 SIGNAL 指令为: {command_type}") + # 检查 SIGNAL 类型命令必须包含 signalState + if command_type == "SIGNAL" and not signal_state: + print(f"SIGNAL 类型命令缺少 signalState") + return jsonify({ + "code": 400, + "msg": "SIGNAL command must include signalState", + "transId": data.get("transId"), + "timestamp": int(time.time() * 1000) + }), 400 # 检查车辆是否存在 vehicle_state = vehicle_states.get(vehicle_id) @@ -347,7 +345,7 @@ def handle_vehicle_command(): # 特勤车只响应红绿灯信号 if vehicle_id.startswith("TQ"): - if command_type not in ["RED", "GREEN"]: + if command_type != "SIGNAL": print(f"特勤车辆忽略非红绿灯指令: {vehicle_id}") return jsonify({ "code": 200, @@ -356,7 +354,7 @@ def handle_vehicle_command(): "timestamp": int(time.time() * 1000) }) # 更新红绿灯状态和指令状态 - vehicle_state.update_command(command_type, target_lat, target_lon) + vehicle_state.update_command(signal_state, target_lat, target_lon) print(f"特勤车 {vehicle_id} 更新状态: command={command_type}, traffic_light={vehicle_state.traffic_light_state}") return jsonify({ "code": 200, @@ -366,13 +364,14 @@ def handle_vehicle_command(): }) # 检查指令优先级并添加详细日志 - can_override = vehicle_state.can_be_overridden_by(command_type) + check_command = signal_state if command_type == "SIGNAL" else command_type + can_override = vehicle_state.can_be_overridden_by(check_command) print(f"指令优先级检查: vehicle={vehicle_id}, current_command={vehicle_state.current_command}, " - f"new_command={command_type}, can_override={can_override}") + f"new_command={check_command}, can_override={can_override}") if not can_override: print(f"指令优先级过低: vehicle={vehicle_id}, current_priority={vehicle_state.command_priority}, " - f"command={command_type}") + f"command={check_command}") return jsonify({ "code": 400, "msg": "Command priority too low", @@ -381,7 +380,17 @@ def handle_vehicle_command(): }) # 处理不同类型的指令 - if command_type in ["ALERT", "WARNING"]: + if command_type == "SIGNAL": + # 更新红绿灯状态和指令状态 + vehicle_state.update_command(signal_state, target_lat, target_lon) + print(f"车辆 {vehicle_id} 更新状态: command={command_type}, traffic_light={vehicle_state.traffic_light_state}") + return jsonify({ + "code": 200, + "msg": "Traffic light state updated", + "transId": data.get("transId"), + "timestamp": int(time.time() * 1000) + }) + elif command_type in ["ALERT", "WARNING"]: # 查找当前车辆 current_vehicle = None for v in vehicle_data: @@ -399,12 +408,15 @@ def handle_vehicle_command(): vehicle_state.target_lat = target_lat vehicle_state.target_lon = target_lon # 立即更新车辆速度 - current_vehicle["speed"] = 0 + if current_vehicle: + current_vehicle["speed"] = 0 - elif command_type in ["RED", "GREEN"]: - print(f"执行红绿灯指令: vehicle_id={vehicle_id}, state={command_type}") - # 红绿灯状态的更新由 update_command 处理 - pass + return jsonify({ + "code": 200, + "msg": "Command executed successfully", + "transId": data.get("transId"), + "timestamp": int(time.time() * 1000) + }) elif command_type == "RESUME": print(f"执行恢复指令: vehicle_id={vehicle_id}") @@ -428,7 +440,7 @@ def handle_vehicle_command(): # 更新车辆运行状态 vehicle_state.is_running = vehicle_state.can_move() - if vehicle_state.is_running: + if vehicle_state.is_running and current_vehicle: print(f"车辆 {vehicle_id} 恢复运行") current_vehicle["speed"] = DEFAULT_VEHICLE_SPEED @@ -448,21 +460,6 @@ def handle_vehicle_command(): "timestamp": int(time.time() * 1000) }) - # 更新车辆状态 - vehicle_state.update_command(command_type, target_lat, target_lon) - vehicle_state.command_reason = reason - vehicle_state.last_command_time = time.time() - - # 更新车辆运行状态 - vehicle_state.is_running = vehicle_state.can_move() - if not vehicle_state.is_running: - vehicle_state.target_speed = 0 - - print(f"Vehicle {vehicle_id} state updated: running={vehicle_state.is_running}, " - f"command={vehicle_state.current_command}, traffic_light={vehicle_state.traffic_light_state}, " - f"reason={reason}, priority={vehicle_state.command_priority}, " - f"target_speed={vehicle_state.target_speed}, brake_mode={vehicle_state.brake_mode}") - return jsonify({ "code": 200, "msg": "Command executed successfully", @@ -491,15 +488,14 @@ def calculate_distance_to_intersection(vehicle, intersection): lon_diff = (vehicle_lon - intersection_lon) * (111319.9 * math.cos(math.radians(vehicle_lat))) return math.sqrt(lat_diff * lat_diff + lon_diff * lon_diff) -def get_nearest_intersection(vehicle): - """获取车辆最近的路口及其红绿灯状态""" - west_dist = calculate_distance_to_intersection(vehicle, T2_INTERSECTION) - east_dist = calculate_distance_to_intersection(vehicle, T6_INTERSECTION) - - if west_dist <= east_dist: - return T2_INTERSECTION, traffic_light_data[0], west_dist # TL001 - else: - return T6_INTERSECTION, traffic_light_data[1], east_dist # TL002 +def calculate_path_direction(start_point, end_point): + """计算两点之间的方向角度（相对于正北方向，顺时针）""" + dlat = end_point["latitude"] - start_point["latitude"] + dlon = end_point["longitude"] - start_point["longitude"] + angle_rad = math.atan2(dlon, dlat) # 使用 atan2 计算角度 + angle_deg = math.degrees(angle_rad) + # 转换为顺时针方向（正北为0度） + return (90 - angle_deg) % 360 def get_front_traffic_light(vehicle, distance_to_west, distance_to_east): """获取车辆前方的红绿灯状态""" @@ -509,17 +505,46 @@ def get_front_traffic_light(vehicle, distance_to_west, distance_to_east): if "phase" not in vehicle: vehicle["phase"] = 0 - # QN001 的路线：西路口北侧 -> 东 -> 北 - if vehicle["phase"] == 0: # 南北移动 - # 在西路口以北时，向南移动需要判断西路口红绿灯 - if vehicle["direction"] == -1 and vehicle["latitude"] > T2_INTERSECTION["latitude"]: + # QN001 的路线：T1 -> T2 -> T4 + if vehicle["phase"] == 0: # T1 -> T2 阶段 + # 计算 T1 到 T2 的实际方向 + actual_direction = calculate_path_direction(POINT_T1, POINT_T2) + # 允许 15 度的误差范围 + direction_diff = abs(vehicle["direction"] - actual_direction) + direction_diff = min(direction_diff, 360 - direction_diff) + + # 在西路口以北时，向 T2 移动需要判断西路口红绿灯 + if direction_diff <= 15 and vehicle["latitude"] > T2_INTERSECTION["latitude"]: return traffic_light_data[0], distance_to_west # 西路口红绿灯 elif vehicle["vehicleNo"] == "QN002": - # 在东路口以西时，向东移动需要判断东路红绿灯 - if vehicle["direction"] == 1 and vehicle["longitude"] < T6_INTERSECTION["longitude"]: + # 计算 T12 到 T8 的实际方向 + actual_direction = calculate_path_direction(POINT_T12, POINT_T8) + # 允许 15 度的误差范围 + direction_diff = abs(vehicle["direction"] - actual_direction) + direction_diff = min(direction_diff, 360 - direction_diff) + + # 在东路口以西时，向 T8 移动需要判断东路红绿灯 + if direction_diff <= 15 and vehicle["longitude"] < T6_INTERSECTION["longitude"]: return traffic_light_data[1], distance_to_east # 东路口红绿灯 + elif vehicle["vehicleNo"] == "TQ001": + # 确保 phase 属性存在 + if "phase" not in vehicle: + vehicle["phase"] = 0 + + # TQ001 的路线：T4 -> T2 -> T3 + if vehicle["phase"] == 0: # T4 -> T2 阶段 + # 计算 T4 到 T2 的实际方向 + actual_direction = calculate_path_direction(POINT_T4, POINT_T2) + # 允许 15 度的误差范围 + direction_diff = abs(vehicle["direction"] - actual_direction) + direction_diff = min(direction_diff, 360 - direction_diff) + + # 在西路口以东时，向 T2 移动需要判断西路口红绿灯 + if direction_diff <= 15 and vehicle["longitude"] > T2_INTERSECTION["longitude"]: + return traffic_light_data[0], distance_to_west # 西路口红绿灯 + # 其他情况，表示车辆已经过路口或不需要判断红绿灯 return None, float('inf') @@ -630,19 +655,18 @@ def update_vehicle_position(vehicle, elapsed_time): print(f"特勤车 {vehicle['vehicleNo']} 正常行驶，速度={vehicle['speed']}km/h") else: # 普通车辆的移动逻辑 - # 先判断是否在红灯影响范围内（50米） + # 红灯且距离停止线小于等于 50 米时必须停车 if traffic_light and traffic_light["state"] == 0 and distance <= DIST_50M: - # 在红灯影响范围内，检查是否需要停车 - if not vehicle_state.can_move(): - vehicle["speed"] = 0 - print(f"车辆 {vehicle['vehicleNo']} 遇红灯在停止线前停车，距路口 {distance:.1f}米") - return - else: - # 不在红灯影响范围内，只检查其他指令 - if vehicle_state.current_command in ["ALERT", "WARNING"]: - vehicle["speed"] = 0 - print(f"车辆 {vehicle['vehicleNo']} 不能移动: command={vehicle_state.current_command}") - return + vehicle["speed"] = 0 + vehicle_state.current_command = "RED" # 设置当前指令为红灯 + vehicle_state.is_running = False # 设置为停止状态 + print(f"车辆 {vehicle['vehicleNo']} 遇红灯在停止线前停车，距路口 {distance:.1f}米") + return + elif vehicle_state.current_command in ["ALERT", "WARNING"]: + # 其他停车指令 + vehicle["speed"] = 0 + print(f"车辆 {vehicle['vehicleNo']} 不能移动: command={vehicle_state.current_command}") + return # 可以移动，设置正常速度 vehicle["speed"] = DEFAULT_VEHICLE_SPEED diff --git a/tools/test_websocket.html b/tools/test_websocket.html index 71735a5..908630a 100644 --- a/tools/test_websocket.html +++ b/tools/test_websocket.html @@ -87,17 +87,19 @@ type = 'command'; // 为控制指令添加中文描述 const commandTypes = { - 'SIGNAL': '信号灯指令', - 'ALERT': '告警指令', - 'WARNING': '预警指令', - 'RESUME': '恢复指令' + 'SIGNAL': '信号灯', + 'ALERT': '告警', + 'WARNING': '预警', + 'RESUME': '恢复', + 'PARKING': '安全停靠' }; const reasons = { 'TRAFFIC_LIGHT': '红绿灯控制', 'AIRCRAFT_CROSSING': '航空器交叉', 'SPECIAL_VEHICLE': '特勤车辆', 'AIRCRAFT_PUSH': '航空器推出', - 'RESUME_TRAFFIC': '恢复通行' + 'RESUME_TRAFFIC': '恢复通行', + 'PARKING_SIDE': '安全停靠' }; message = `收到车辆控制指令:\n车辆: ${data.vehicleId}\n` + `指令类型: ${commandTypes[data.commandType] || data.commandType}\n` +