diff --git a/.cursorrules b/.cursorrules index 64d8808..912e5af 100644 --- a/.cursorrules +++ b/.cursorrules @@ -43,4 +43,5 @@ - 先询问代码的用途 - 理解代码的功能和作用 - 确认是否可以修改或删除 -- 如果不确定就直接问我 \ No newline at end of file +- 如果不确定就直接问我 +- 不要随便修改原有代码中的中文注释 \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 862b06d..546c33c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,7 @@ set(LIB_SOURCES src/detector/CollisionDetector.cpp src/network/HTTPDataSource.cpp src/network/WebSocketServer.cpp + src/network/HTTPClient.cpp src/spatial/AirportBounds.cpp src/spatial/CoordinateConverter.cpp src/types/BasicTypes.cpp @@ -124,14 +125,7 @@ message(STATUS "CMAKE_CXX_FLAGS: ${CMAKE_CXX_FLAGS}") message(STATUS "CMAKE_EXE_LINKER_FLAGS: ${CMAKE_EXE_LINKER_FLAGS}") # 复制配置文件到构建目录 -configure_file( - ${CMAKE_SOURCE_DIR}/config/airport_bounds.json - ${CMAKE_BINARY_DIR}/bin/config/airport_bounds.json - COPYONLY -) - -configure_file( - ${CMAKE_SOURCE_DIR}/config/vehicle_speed_limits.json - ${CMAKE_BINARY_DIR}/bin/config/vehicle_speed_limits.json - COPYONLY -) \ No newline at end of file +configure_file(${CMAKE_SOURCE_DIR}/config/system_config.json ${CMAKE_BINARY_DIR}/config/system_config.json COPYONLY) +configure_file(${CMAKE_SOURCE_DIR}/config/intersections.json ${CMAKE_BINARY_DIR}/config/intersections.json COPYONLY) +configure_file(${CMAKE_SOURCE_DIR}/config/vehicle_control.json ${CMAKE_BINARY_DIR}/config/vehicle_control.json COPYONLY) +configure_file(${CMAKE_SOURCE_DIR}/config/airport_bounds.json ${CMAKE_BINARY_DIR}/config/airport_bounds.json COPYONLY) \ No newline at end of file diff --git a/config/airport_bounds.json b/config/airport_bounds.json index 8aa7807..16aecca 100644 --- a/config/airport_bounds.json +++ b/config/airport_bounds.json @@ -10,54 +10,77 @@ "areas": { "runway": { "bounds": { - "x": 1000, - "y": 1500, - "width": 3000, - "height": 60 + "x": 0, + "y": 0, + "width": 1, + "height": 1 }, "config": { "vehicle_collision_radius": 50.0, "aircraft_ground_radius": 100.0, - "height_threshold": 15.0 + "height_threshold": 15.0, + "warning_zone_radius": 100.0, + "alert_zone_radius": 50.0 } }, "taxiway": { "bounds": { - "x": 500, - "y": 1000, - "width": 4000, - "height": 30 + "x": 0, + "y": 0, + "width": 1, + "height": 1 }, "config": { "vehicle_collision_radius": 30.0, "aircraft_ground_radius": 50.0, - "height_threshold": 10.0 + "height_threshold": 10.0, + "warning_zone_radius": 50.0, + "alert_zone_radius": 25.0 } }, "gate": { "bounds": { - "x": 100, - "y": 2000, - "width": 4800, - "height": 1000 + "x": 0, + "y": 0, + "width": 1, + "height": 1 }, "config": { "vehicle_collision_radius": 20.0, "aircraft_ground_radius": 40.0, - "height_threshold": 5.0 + "height_threshold": 5.0, + "warning_zone_radius": 40.0, + "alert_zone_radius": 20.0 } }, "service": { "bounds": { "x": 0, - "y": 3000, - "width": 5000, - "height": 1000 + "y": 0, + "width": 1, + "height": 1 }, "config": { "vehicle_collision_radius": 15.0, "aircraft_ground_radius": 30.0, - "height_threshold": 5.0 + "height_threshold": 5.0, + "warning_zone_radius": 30.0, + "alert_zone_radius": 15.0 + } + }, + "test_zone": { + "bounds": { + "x": 0, + "y": 0, + "width": 5000, + "height": 4000 + }, + "config": { + "vehicle_collision_radius": 25.0, + "aircraft_ground_radius": 50.0, + "height_threshold": 10.0, + "warning_zone_radius": 100.0, + "alert_zone_radius": 50.0 } } } diff --git a/config/intersections.json b/config/intersections.json index 276eb83..356e6ed 100644 --- a/config/intersections.json +++ b/config/intersections.json @@ -1,11 +1,11 @@ { "intersections": [ { - "id": "intersection_001", + "id": "西路口", "name": "无人车与特勤车交叉路口", "trafficLightId": "TL001", "position": { - "longitude": 120.088003, + "longitude": 120.086003, "latitude": 36.361999, "altitude": 12.5 }, @@ -13,11 +13,11 @@ "stopDistance": 50.0 }, { - "id": "intersection_002", + "id": "东路口", "name": "无人车与飞机交叉路口", "trafficLightId": "TL002", "position": { - "longitude": 120.088303, + "longitude": 120.089003, "latitude": 36.361999, "altitude": 12.5 }, diff --git a/config/system_config.json b/config/system_config.json index 6fed7ae..021d25a 100644 --- a/config/system_config.json +++ b/config/system_config.json @@ -34,6 +34,11 @@ }, "collision_detection": { "update_interval_ms": 200, + "prediction": { + "time_window": 30.0, + "vehicle_size": 10.0, + "aircraft_size": 50.0 + }, "thresholds": { "runway": { "aircraft_ground": 100.0, diff --git a/config/vehicle_control.json b/config/vehicle_control.json new file mode 100644 index 0000000..745192d --- /dev/null +++ b/config/vehicle_control.json @@ -0,0 +1,14 @@ +{ + "vehicles": [ + { + "vehicleNo": "QN001", + "ip": "localhost", + "port": 8081 + }, + { + "vehicleNo": "QN002", + "ip": "localhost", + "port": 8081 + } + ] +} \ No newline at end of file diff --git a/config/vehicle_speed_limits.json b/config/vehicle_speed_limits.json deleted file mode 100644 index a4d0c61..0000000 --- a/config/vehicle_speed_limits.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "vehicles": [ - { - "vehicleNo": "QN001", - "ip": "192.168.1.101", - "port": 8080 - }, - { - "vehicleNo": "QN002", - "ip": "192.168.1.102", - "port": 8080 - }, - { - "vehicleNo": "TQ001", - "ip": "192.168.1.103", - "port": 8080 - } - ] -} \ No newline at end of file diff --git a/docs/mock_server.md b/docs/mock_server.md new file mode 100644 index 0000000..9a566cb --- /dev/null +++ b/docs/mock_server.md @@ -0,0 +1,301 @@ +# Mock 服务设计与使用说明 + +## 1. 概述 + +Mock 服务用于模拟机场场面的移动物体（航空器和车辆），提供位置更新和响应控制指令的功能。服务包含两个主要部分： + +- HTTP API 服务：提供位置查询和车辆控制接口 +- 位置更新模拟：定期更新各个移动物体的位置 + +## 2. 移动物体配置 + +### 2.1 航空器配置 + +```python +{ + "flightNo": "AC001", # 航班号 + "latitude": 36.362647780875804, # 纬度 + "longitude": 120.088303, # 经度 + "direction": 1, # 1表示向北 + "speed": 50.0 # 滑行速度 50km/h +} +``` + +### 2.2 车辆配置 + +```python +{ + "vehicleNo": "QN001", # 无人车1（西路口） + "latitude": WEST_INTERSECTION["latitude"], # 纬度 + "longitude": WEST_INTERSECTION["longitude"], # 经度 + "direction": 1, # 1表示向东，-1表示向西 + "speed": 36.0 # 行驶速度 36km/h +} +``` + +## 3. 运动模式 + +### 3.1 航空器运动 + +- 沿跑道南北方向移动 +- 到达边界时自动调头 +- 保持恒定速度 50km/h + +### 3.2 无人车运动 + +- QN001：在西路口东西向往返，范围 50 米 +- QN002：在东路口东西方向往返，范围 100-150 米 +- TQ001：先东西移动，到达指定位置后转向南北移动，然后返回 + +## 4. HTTP API 接口 + +### 4.1 位置查询接口 + +```http +GET /api/position +``` + +返回所有移动物体的当前位置信息： + +```json +{ + "aircraft": [ + { + "flightNo": "AC001", + "latitude": 36.362647780875804, + "longitude": 120.088303, + "heading": 0.0, + "speed": 13.89, + "timestamp": 1733741411167 + } + ], + "vehicles": [ + { + "vehicleNo": "QN001", + "latitude": 36.362448155990975, + "longitude": 120.08844920958369, + "heading": 90.0, + "speed": 10.0, + "timestamp": 1733741411168 + } + ] +} +``` + +### 4.2 车辆控制接口 + +```http +POST /api/vehicle/command +Content-Type: application/json +``` + +请求格式： + +```json +{ + "vehicleId": "QN001", + "type": "SIGNAL", // SIGNAL, ALERT, WARNING, RESUME + "reason": "TRAFFIC_LIGHT", // TRAFFIC_LIGHT, AIRCRAFT_CROSSING, SPECIAL_VEHICLE, AIRCRAFT_PUSH, RESUME_TRAFFIC + "timestamp": 1733741411167, + "signalState": "RED", // 可选，仅当 type 为 SIGNAL 时需要 + "targetPosition": { // 可选 + "x": 100.0, + "y": 200.0 + }, + "speed": 0.0, // 可选 + "distance": 0.0 // 可选 +} +``` + +响应格式： + +```json +{ + "status": "ok", // ok 或 error + "message": "Command executed successfully" +} +``` + +## 5. 指令优先级 + +车辆控制指令按以下优先级处理： + +1. ALERT (5)：告警指令，最高优先级 + - 用于紧急情况，如碰撞风险 + - 可以覆盖所有其他指令 + - 收到后车辆立即停止 + +2. SIGNAL (4)：红绿灯指令，次高优先级 + - 用于红绿灯控制 + - 可以覆盖预警、绿灯和恢复指令 + - 红灯时车辆停止，绿灯时可能恢复（取决于是否有更高优先级指令） + +3. WARNING (3)：预警指令，中等优先级 + - 用于提前预警可能的风险 + - 可以覆盖绿灯和恢复指令 + - 收到后车辆减速或停止 + +4. GREEN (2)：绿灯状态 + - 系统内部状态，不是外部指令 + - 可以覆盖恢复指令 + - 只有在没有更高优先级指令时才生效 + +5. RESUME (1)：恢复指令，最低优先级 + - 用于恢复正常行驶 + - 不能覆盖其他指令 + - 只有在没有任何其他指令时才生效 + +### 5.1 优先级规则 + +1. 高优先级指令可以覆盖低优先级指令 +2. 相同优先级的新指令可以覆盖旧指令 +3. 低优先级指令不能覆盖高优先级指令 +4. 恢复行驶需要满足两个条件： + - 收到 RESUME 指令 + - 没有任何更高优先级的指令 + +### 5.2 状态转换 + +1. 收到告警指令 (ALERT)： + - 立即停止 + - 忽略所有低优先级指令 + - 只能通过 RESUME 指令恢复（且无其他高优先级指令） + +2. 收到红绿灯指令 (SIGNAL)： + - 红灯：停止 + - 绿灯：如果没有更高优先级指令（ALERT）则恢复行驶 + +3. 收到预警指令 (WARNING)： + - 减速或停止 + - 可被告警或红灯指令覆盖 + - 可通过绿灯或恢复指令解除（如无更高优先级指令） + +4. 遇到绿灯 (GREEN)： + - 如果没有 ALERT、SIGNAL 或 WARNING 指令，则恢复行驶 + - 否则保持当前状态 + +5. 收到恢复指令 (RESUME)： + - 检查是否有任何更高优先级指令 + - 如果没有，则恢复正常行驶速度 + - 如果有，则保持当前状态 + +### 5.3 使用示例 + +1. 告警指令覆盖其他指令： + +```json +{ + "vehicleId": "QN001", + "type": "ALERT", + "reason": "COLLISION_RISK", + "timestamp": 1733741411167 +} +``` + +2. 红灯指令（但不会覆盖告警）： + +```json +{ + "vehicleId": "QN001", + "type": "SIGNAL", + "reason": "TRAFFIC_LIGHT", + "signalState": "RED", + "timestamp": 1733741411167 +} +``` + +3. 预警指令（可被告警和红灯覆盖）： + +```json +{ + "vehicleId": "QN001", + "type": "WARNING", + "reason": "APPROACHING_INTERSECTION", + "timestamp": 1733741411167 +} +``` + +4. 恢复指令（需要无更高优先级指令）： + +```json +{ + "vehicleId": "QN001", + "type": "RESUME", + "reason": "RESUME_TRAFFIC", + "timestamp": 1733741411167 +} +``` + +## 6. 使用示例 + +### 6.1 发送红绿灯指令 + +```bash +curl -X POST http://localhost:8081/api/vehicle/command \ + -H "Content-Type: application/json" \ + -d '{ + "vehicleId": "QN001", + "type": "SIGNAL", + "reason": "TRAFFIC_LIGHT", + "timestamp": 1733741411167, + "signalState": "RED" + }' +``` + +### 6.2 发送碰撞预警指令 + +```bash +curl -X POST http://localhost:8081/api/vehicle/command \ + -H "Content-Type: application/json" \ + -d '{ + "vehicleId": "QN001", + "type": "WARNING", + "reason": "AIRCRAFT_CROSSING", + "timestamp": 1733741411167, + "distance": 25.5 + }' +``` + +### 6.3 发送恢复指令 + +```bash +curl -X POST http://localhost:8081/api/vehicle/command \ + -H "Content-Type: application/json" \ + -d '{ + "vehicleId": "QN001", + "type": "RESUME", + "reason": "RESUME_TRAFFIC", + "timestamp": 1733741411167 + }' +``` + +## 7. 启动服务 + +```bash +# 安装依赖 +pip install flask + +# 启动服务 +python tools/mock_server.py +``` + +服务默认监听在： + +- HTTP 服务： + +## 8. 注意事项 + +1. 位置更新 + - 位置更新频率：1秒一次 + - 坐标系统：WGS84 + - 速度单位：km/h（内部计算使用 m/s） + +2. 车辆控制 + - 车辆收到停止指令后会立即停止位置更新 + - 只有收到 RESUME 指令且没有更高优先级的指令时才会恢复运动 + - 每个指令都必须包含时间戳 + +3. 错误处理 + - 无效的车辆ID：返回 404 错误 + - 无效的指令格式：返回 400 错误 + - 服务器内部错误：返回 500 错误 diff --git a/include/core/System.h b/include/core/System.h new file mode 100644 index 0000000..1695048 --- /dev/null +++ b/include/core/System.h @@ -0,0 +1,76 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include "config/SystemConfig.h" +#include "config/IntersectionConfig.h" +#include "network/WebSocketServer.h" +#include "types/VehicleCommand.h" +#include "types/MovingObject.h" + +class AirportBounds; +class ControllableVehicles; +class CollisionDetector; +class TrafficLightDetector; +class DataCollector; +class MovingObject; +struct CollisionRisk; +struct TrafficLightSignal; + +namespace network { + struct TimeoutWarningMessage; +} + +class System { +public: + System(); + virtual ~System(); + + bool initialize(); + void start(); + void stop(); + + static System* instance() { return instance_; } + static void signalHandler(int signal); + + const SystemConfig& getSystemConfig() const { return system_config_; } + const IntersectionConfig& getIntersectionConfig() const { return intersection_config_; } + + // 广播相关函数 + virtual void broadcastTimeoutWarning(const network::TimeoutWarningMessage& warning); + void broadcastPositionUpdate(const MovingObject& obj); + void broadcastCollisionWarning(const CollisionRisk& risk); + void broadcastVehicleCommand(const VehicleCommand& cmd); + void broadcastTrafficLightStatus(const TrafficLightSignal& signal); + +private: + static System* instance_; + + void processLoop(); + + std::atomic running_{false}; + std::thread processThread_; + std::thread ws_thread_; + + // 系统组件 + std::unique_ptr airportBounds_; + std::unique_ptr controllableVehicles_; + std::unique_ptr collisionDetector_; + std::unique_ptr trafficLightDetector_; + std::unique_ptr dataCollector_; + std::unique_ptr ws_server_; + + // 配置 + SystemConfig system_config_; + IntersectionConfig intersection_config_; + + // 记录车辆停止的时间 + std::unordered_map vehicleStopTimes_; + + // 记录上一次有风险的可控车辆列表 + std::unordered_set lastVehiclesWithRisk_; +}; \ No newline at end of file diff --git a/src/collector/DataCollector.h b/src/collector/DataCollector.h index c89c819..51f7c2d 100644 --- a/src/collector/DataCollector.h +++ b/src/collector/DataCollector.h @@ -12,7 +12,9 @@ #include "types/BasicTypes.h" #include "config/WarnConfig.h" #include "types/TrafficLightTypes.h" -#include "core/System.h" + +// 前向声明 +class System; class DataCollector { public: @@ -37,6 +39,16 @@ public: dataSource_ = source; } + // 获取数据的引用访问接口 + const std::vector& getAircraftRef() const { + std::lock_guard lock(cacheMutex_); + return aircraftCache_; + } + + const std::vector& getVehicleRef() const { + std::lock_guard lock(cacheMutex_); + return vehicleCache_; + } private: std::shared_ptr dataSource_; diff --git a/src/config/IntersectionConfig.cpp b/src/config/IntersectionConfig.cpp index b5b033c..a1007a3 100644 --- a/src/config/IntersectionConfig.cpp +++ b/src/config/IntersectionConfig.cpp @@ -23,12 +23,20 @@ IntersectionConfig IntersectionConfig::load(const std::string& configFile) { info.name = item["name"].get(); info.trafficLightId = item["trafficLightId"].get(); - // 加载位置信息 + // 加载位置信息，检查是否为 null + if (item["position"]["longitude"].is_null() || + item["position"]["latitude"].is_null() || + item["position"]["altitude"].is_null()) { + throw std::runtime_error("Position values cannot be null for intersection: " + info.id); + } info.position.longitude = item["position"]["longitude"].get(); info.position.latitude = item["position"]["latitude"].get(); info.position.altitude = item["position"]["altitude"].get(); - // 加载距离阈值 + // 加载距离阈值，检查是否为 null + if (item["radius"].is_null() || item["stopDistance"].is_null()) { + throw std::runtime_error("Distance values cannot be null for intersection: " + info.id); + } info.radius = item["radius"].get(); info.stopDistance = item["stopDistance"].get(); diff --git a/src/config/SystemConfig.cpp b/src/config/SystemConfig.cpp index f2d137e..68846d3 100644 --- a/src/config/SystemConfig.cpp +++ b/src/config/SystemConfig.cpp @@ -2,7 +2,7 @@ #include "utils/Logger.h" #include -SystemConfig SystemConfig::load(const std::string& filename) { +void SystemConfig::load(const std::string& filename) { try { std::ifstream file(filename); if (!file.is_open()) { @@ -12,63 +12,59 @@ SystemConfig SystemConfig::load(const std::string& filename) { nlohmann::json j; file >> j; - SystemConfig config; - // 加载机场信息 - config.airport.name = j["airport"]["name"]; - config.airport.iata = j["airport"]["iata"]; - config.airport.icao = j["airport"]["icao"]; - config.airport.reference_point.latitude = j["airport"]["reference_point"]["latitude"]; - config.airport.reference_point.longitude = j["airport"]["reference_point"]["longitude"]; + airport.name = j["airport"]["name"]; + airport.iata = j["airport"]["iata"]; + airport.icao = j["airport"]["icao"]; + airport.reference_point.latitude = j["airport"]["reference_point"]["latitude"]; + airport.reference_point.longitude = j["airport"]["reference_point"]["longitude"]; // 加载数据源配置 - config.data_source.host = j["data_source"]["host"]; - config.data_source.port = j["data_source"]["port"]; - config.data_source.aircraft_path = j["data_source"]["aircraft_path"]; - config.data_source.vehicle_path = j["data_source"]["vehicle_path"]; - config.data_source.traffic_light_path = j["data_source"]["traffic_light_path"]; - config.data_source.refresh_interval_ms = j["data_source"]["refresh_interval_ms"]; - config.data_source.timeout_ms = j["data_source"]["timeout_ms"]; - config.data_source.read_timeout_ms = j["data_source"]["read_timeout_ms"]; + data_source.host = j["data_source"]["host"]; + data_source.port = j["data_source"]["port"]; + data_source.aircraft_path = j["data_source"]["aircraft_path"]; + data_source.vehicle_path = j["data_source"]["vehicle_path"]; + data_source.traffic_light_path = j["data_source"]["traffic_light_path"]; + data_source.refresh_interval_ms = j["data_source"]["refresh_interval_ms"]; + data_source.timeout_ms = j["data_source"]["timeout_ms"]; + data_source.read_timeout_ms = j["data_source"]["read_timeout_ms"]; // 加载 WebSocket 配置 - config.websocket.port = j["websocket"]["port"]; - config.websocket.max_connections = j["websocket"]["max_connections"]; - config.websocket.ping_interval_ms = j["websocket"]["ping_interval_ms"]; - config.websocket.position_update.aircraft_interval_ms = j["websocket"]["position_update"]["aircraft_interval_ms"]; - config.websocket.position_update.vehicle_interval_ms = j["websocket"]["position_update"]["vehicle_interval_ms"]; + websocket.port = j["websocket"]["port"]; + websocket.max_connections = j["websocket"]["max_connections"]; + websocket.ping_interval_ms = j["websocket"]["ping_interval_ms"]; + websocket.position_update.aircraft_interval_ms = j["websocket"]["position_update"]["aircraft_interval_ms"]; + websocket.position_update.vehicle_interval_ms = j["websocket"]["position_update"]["vehicle_interval_ms"]; // 加载碰撞检测配置 - config.collision_detection.update_interval_ms = j["collision_detection"]["update_interval_ms"]; + collision_detection.update_interval_ms = j["collision_detection"]["update_interval_ms"]; // 加载阈值配置 auto& thresholds = j["collision_detection"]["thresholds"]; - config.collision_detection.thresholds.runway.aircraft_ground = thresholds["runway"]["aircraft_ground"]; - config.collision_detection.thresholds.runway.vehicle = thresholds["runway"]["vehicle"]; - config.collision_detection.thresholds.taxiway.aircraft_ground = thresholds["taxiway"]["aircraft_ground"]; - config.collision_detection.thresholds.taxiway.vehicle = thresholds["taxiway"]["vehicle"]; - config.collision_detection.thresholds.apron.aircraft_ground = thresholds["apron"]["aircraft_ground"]; - config.collision_detection.thresholds.apron.vehicle = thresholds["apron"]["vehicle"]; - config.collision_detection.thresholds.service.aircraft_ground = thresholds["service"]["aircraft_ground"]; - config.collision_detection.thresholds.service.vehicle = thresholds["service"]["vehicle"]; + collision_detection.thresholds.runway.aircraft_ground = thresholds["runway"]["aircraft_ground"]; + collision_detection.thresholds.runway.vehicle = thresholds["runway"]["vehicle"]; + collision_detection.thresholds.taxiway.aircraft_ground = thresholds["taxiway"]["aircraft_ground"]; + collision_detection.thresholds.taxiway.vehicle = thresholds["taxiway"]["vehicle"]; + collision_detection.thresholds.apron.aircraft_ground = thresholds["apron"]["aircraft_ground"]; + collision_detection.thresholds.apron.vehicle = thresholds["apron"]["vehicle"]; + collision_detection.thresholds.service.aircraft_ground = thresholds["service"]["aircraft_ground"]; + collision_detection.thresholds.service.vehicle = thresholds["service"]["vehicle"]; // 加载日志配置 - config.logging.level = j["logging"]["level"]; - config.logging.file = j["logging"]["file"]; - config.logging.max_size_mb = j["logging"]["max_size_mb"]; - config.logging.max_files = j["logging"]["max_files"]; - config.logging.console_output = j["logging"]["console_output"]; + logging.level = j["logging"]["level"]; + logging.file = j["logging"]["file"]; + logging.max_size_mb = j["logging"]["max_size_mb"]; + logging.max_files = j["logging"]["max_files"]; + logging.console_output = j["logging"]["console_output"]; // 加载调试配置 - config.debug.enable_mock_data = j["debug"]["enable_mock_data"]; - config.debug.save_raw_data = j["debug"]["save_raw_data"]; - config.debug.profile_performance = j["debug"]["profile_performance"]; + debug.enable_mock_data = j["debug"]["enable_mock_data"]; + debug.save_raw_data = j["debug"]["save_raw_data"]; + debug.profile_performance = j["debug"]["profile_performance"]; // 加载告警配置 - config.warning.warning_interval_ms = j["warning"]["warning_interval_ms"]; - config.warning.log_interval_ms = j["warning"]["log_interval_ms"]; - - return config; + warning.warning_interval_ms = j["warning"]["warning_interval_ms"]; + warning.log_interval_ms = j["warning"]["log_interval_ms"]; } catch (const std::exception& e) { Logger::error("Failed to load system config: ", e.what()); diff --git a/src/config/SystemConfig.h b/src/config/SystemConfig.h index 09bf903..28a39f4 100644 --- a/src/config/SystemConfig.h +++ b/src/config/SystemConfig.h @@ -3,7 +3,23 @@ #include #include -struct SystemConfig { +class System; // 前向声明 + +class SystemConfig { +public: + // 获取单例实例 + static SystemConfig& instance() { + static SystemConfig instance; + return instance; + } + + // 加载配置文件 + void load(const std::string& filename); + + // 删除拷贝构造和赋值操作 + SystemConfig(const SystemConfig&) = delete; + SystemConfig& operator=(const SystemConfig&) = delete; + struct Airport { std::string name; std::string iata; @@ -38,6 +54,11 @@ struct SystemConfig { struct CollisionDetection { int update_interval_ms; + struct Prediction { + double time_window; + double vehicle_size; + double aircraft_size; + } prediction; struct Thresholds { struct Area { double aircraft_ground; @@ -69,5 +90,7 @@ struct SystemConfig { int log_interval_ms; // 日志记录间隔 } warning; - static SystemConfig load(const std::string& filename); +private: + SystemConfig() = default; // 私有构造函数 + friend class System; // 允许 System 类访问私有成员 }; \ No newline at end of file diff --git a/src/core/System.cpp b/src/core/System.cpp index b8a8d2e..a4057f7 100644 --- a/src/core/System.cpp +++ b/src/core/System.cpp @@ -2,7 +2,7 @@ #include #include #include -#include "core/System.h" +#include "System.h" #include "utils/Logger.h" #include "collector/DataCollector.h" @@ -29,15 +29,15 @@ void System::signalHandler(int signal) { bool System::initialize() { try { - // 加载系统配置 - system_config_ = SystemConfig::load("config/system_config.json"); + // 配置已在 main 中加载，这里直接使用 + const auto& system_config = SystemConfig::instance(); // 加载路口配置 intersection_config_ = IntersectionConfig::load("config/intersections.json"); Logger::info("Loaded {} intersections", intersection_config_.getIntersections().size()); // 初始化 WebSocket 服务器 - ws_server_ = std::make_unique(system_config_.websocket.port); + ws_server_ = std::make_unique(system_config.websocket.port); ws_thread_ = std::thread([this]() { ws_server_->start(); }); @@ -46,31 +46,32 @@ bool System::initialize() { airportBounds_ = std::make_unique("config/airport_bounds.json"); // 加载可控车辆配置 - controllableVehicles_ = std::make_unique("config/vehicle_speed_limits.json"); + controllableVehicles_ = std::make_unique("config/vehicle_control.json"); + Logger::info("Loaded controllable vehicles configuration"); // 初始化冲突检测器 collisionDetector_ = std::make_unique(*airportBounds_, *controllableVehicles_); // 初始化红绿灯检测器 - trafficLightDetector_ = std::make_unique(intersection_config_, *controllableVehicles_); + trafficLightDetector_ = std::make_unique(intersection_config_, *controllableVehicles_, *this); // 创建数据采集器 dataCollector_ = std::make_unique(); // 数据采集器初始化并启动 DataSourceConfig dataSourceConfig{ - system_config_.data_source.host, - system_config_.data_source.port, - system_config_.data_source.aircraft_path, - system_config_.data_source.vehicle_path, - system_config_.data_source.traffic_light_path, - system_config_.data_source.refresh_interval_ms, - system_config_.data_source.timeout_ms + system_config.data_source.host, + system_config.data_source.port, + system_config.data_source.aircraft_path, + system_config.data_source.vehicle_path, + system_config.data_source.traffic_light_path, + system_config.data_source.refresh_interval_ms, + system_config.data_source.timeout_ms }; WarnConfig warnConfig{ - system_config_.warning.warning_interval_ms, - system_config_.warning.log_interval_ms + system_config.warning.warning_interval_ms, + system_config.warning.log_interval_ms }; return dataCollector_->initialize(dataSourceConfig, warnConfig); @@ -135,6 +136,7 @@ void System::processLoop() { while (running_) { try { auto now = std::chrono::steady_clock::now(); + const auto& system_config = SystemConfig::instance(); auto last_data_collection = now; Logger::debug("开始获取数据..."); @@ -148,7 +150,7 @@ void System::processLoop() { // 检查航空器更新 auto aircraft_elapsed = std::chrono::duration_cast( now - last_aircraft_update).count(); - if (aircraft_elapsed >= system_config_.websocket.position_update.aircraft_interval_ms) { + if (aircraft_elapsed >= system_config.websocket.position_update.aircraft_interval_ms) { bool has_new_aircraft = false; int64_t max_timestamp = last_aircraft_timestamp; @@ -179,46 +181,84 @@ void System::processLoop() { // 检查车辆更新 auto vehicle_elapsed = std::chrono::duration_cast( now - last_vehicle_update).count(); - if (vehicle_elapsed >= system_config_.websocket.position_update.vehicle_interval_ms) { + 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("开始检查车辆数据更新，当前时间戳: ", last_vehicle_timestamp); - for (const auto& veh : vehicles) { - Logger::debug("车辆 ", veh.vehicleNo, " 时间戳: current=", veh.timestamp, " last=", last_vehicle_timestamp); - if (veh.timestamp > last_vehicle_timestamp) { // 改为大于比较 - has_new_vehicles = true; + 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, " (新时间戳: ", veh.timestamp, ")"); + Logger::debug("首次更新，广播车辆: ", veh.vehicleNo); + } + } else { + // 正常的更新检查 + for (const auto& veh : vehicles) { + Logger::debug("处理车辆: ", veh.vehicleNo); + Logger::debug(" - 位置: (", veh.geo.longitude, ", ", veh.geo.latitude, ")"); + Logger::debug(" - 速度: ", veh.speed, "m/s"); + Logger::debug(" - 时间戳: ", veh.timestamp); + Logger::debug(" - 时间差: ", veh.timestamp - last_vehicle_timestamp); + + 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("广播 ", vehicles.size(), " 辆车辆位置, 时间戳更新: ", last_vehicle_timestamp, " -> ", max_timestamp); + 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_timestamp); + Logger::debug("没有新数据需要广播"); } + last_vehicle_update = now; + Logger::debug("车辆位置更新检查结束 <<<<<<<<<<<<<<<"); } // 检查冲突更新 auto collision_elapsed = std::chrono::duration_cast( now - last_collision_update).count(); - if (collision_elapsed >= system_config_.collision_detection.update_interval_ms) { + if (collision_elapsed >= system_config.collision_detection.update_interval_ms) { // 只有当有新的数据时才更新冲突检测 if (last_aircraft_timestamp > last_collision_timestamp || last_vehicle_timestamp > last_collision_timestamp) { - // 更新冲突检测器 + // 更��冲突检测器 collisionDetector_->updateTraffic(aircraft, vehicles); auto collisions = collisionDetector_->detectCollisions(); if (!collisions.empty()) { + Logger::debug("检测到 {} 个碰撞风险", collisions.size()); + for (const auto& risk : collisions) { + Logger::debug("碰撞风险详情: id1={}, id2={}, 距离={}m, 预测最小距离={}m, 风险等级={}, 区域类型={}", + risk.id1, risk.id2, risk.distance, risk.minDistance, + static_cast(risk.level), static_cast(risk.zoneType)); + } processCollisions(collisions); } @@ -230,7 +270,7 @@ void System::processLoop() { // 处理红绿灯信号 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) { + 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; @@ -245,7 +285,7 @@ void System::processLoop() { Logger::debug("发现新数据: ", signal.trafficLightId, " (新时间戳: ", signal.timestamp, ")"); // 广播红绿灯状态更新 - broadcastTrafficLightUpdate(signal); + broadcastTrafficLightStatus(signal); // 处理红绿灯信号并检查是否需要停车 trafficLightDetector_->processSignal(signal, vehicles); @@ -261,7 +301,7 @@ void System::processLoop() { } // 计算下一次数据采集前的等待时间 - auto next_collection = last_data_collection + std::chrono::milliseconds(system_config_.data_source.refresh_interval_ms); + auto next_collection = last_data_collection + std::chrono::milliseconds(system_config.data_source.refresh_interval_ms); auto wait_time = std::chrono::duration_cast(next_collection - now).count(); if (wait_time > 0) { @@ -275,29 +315,110 @@ void System::processLoop() { } } -void System::processCollisions(const std::vector& collisions) { - if (!collisions.empty()) { - Logger::info("检测到 ", collisions.size(), " 个冲突风险"); +void System::processCollisions(const std::vector& risks) { + // 记录当前有风险的可控车辆 + std::unordered_set currentVehiclesWithRisk; + const auto& config = SystemConfig::instance().collision_detection; + + // 记录需要发送恢复指令的车辆 + std::unordered_set vehiclesToResume; + + for (const auto& risk : risks) { + // 只处理可控车辆 + if (!controllableVehicles_->isControllable(risk.id1)) { + continue; + } + + currentVehiclesWithRisk.insert(risk.id1); - for (const auto& risk : collisions) { - // 发送冲突警告到 Unity 客户端 - broadcastCollisionWarning(risk); + // 根据风险等级处理 + switch (risk.level) { + case RiskLevel::CRITICAL: { + // 危险区域：立即发送告警指令 + VehicleCommand cmd; + cmd.vehicleId = risk.id1; + cmd.type = CommandType::ALERT; + cmd.reason = risk.id2.substr(0, 2) == "AC" ? + CommandReason::AIRCRAFT_CROSSING : + CommandReason::SPECIAL_VEHICLE; + cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); + + // 设置目标位置（冲突对象的位置） + const MovingObject* target = findVehicle(risk.id2); + if (target) { + cmd.latitude = target->geo.latitude; + cmd.longitude = target->geo.longitude; + } + + broadcastVehicleCommand(cmd); + controllableVehicles_->sendCommand(risk.id1, cmd); + Logger::warning("发送告警指令到车辆: ", risk.id1, + " 当前距离: ", risk.distance, "m", + " 预测最小距离: ", risk.minDistance, "m", + " 相对速度: ", risk.relativeSpeed, "m/s"); + break; + } + case RiskLevel::WARNING: { + // 预警区域：发送预警指令 + VehicleCommand cmd; + cmd.vehicleId = risk.id1; + cmd.type = CommandType::WARNING; + cmd.reason = risk.id2.substr(0, 2) == "AC" ? + CommandReason::AIRCRAFT_CROSSING : + CommandReason::SPECIAL_VEHICLE; + cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); + + // 设置目标位置 + const MovingObject* target = findVehicle(risk.id2); + if (target) { + cmd.latitude = target->geo.latitude; + cmd.longitude = target->geo.longitude; + } + + broadcastVehicleCommand(cmd); + controllableVehicles_->sendCommand(risk.id1, cmd); + Logger::info("发送预警指令到车辆: ", risk.id1, + " 当前距离: ", risk.distance, "m", + " 预测最小距离: ", risk.minDistance, "m", + " 相对速度: ", risk.relativeSpeed, "m/s"); + break; + } + default: + break; + } + + // 广播碰撞预警消息 + broadcastCollisionWarning(risk); + } + + // 检查之前有风险但现在没有风险的可控车辆 + for (const auto& vehicleId : lastVehiclesWithRisk_) { + if (currentVehiclesWithRisk.find(vehicleId) == currentVehiclesWithRisk.end()) { + vehiclesToResume.insert(vehicleId); + } + } + + // 更新上一次有风险的可控车辆列表 + lastVehiclesWithRisk_ = std::move(currentVehiclesWithRisk); + + // 对于所有当前车辆，如果是可控车辆且不在风险列表中，考虑发送恢复指令 + auto vehicles = dataCollector_->getVehicleData(); + for (const auto& vehicle : vehicles) { + if (controllableVehicles_->isControllable(vehicle.vehicleNo) && + currentVehiclesWithRisk.find(vehicle.vehicleNo) == currentVehiclesWithRisk.end()) { - // 根据风险等级选择不同的日志级别 - switch (risk.level) { - case RiskLevel::CRITICAL: - Logger::warning("高度冲突风险: ", risk.id1, " 与 ", risk.id2, - ", 距离: ", risk.distance, "米", - ", 相对速度: ", risk.relativeSpeed, "m/s"); - break; - - case RiskLevel::WARNING: - Logger::warning("低度冲突风险: ", risk.id1, " 与 ", risk.id2, - ", 距离: ", risk.distance, "米", - ", 相对速度: ", risk.relativeSpeed, "m/s"); - break; - case RiskLevel::NONE: - break; + // 只有当车辆之前有风险或速度为0时才发送恢复指令 + if (vehiclesToResume.find(vehicle.vehicleNo) != vehiclesToResume.end() || + vehicle.speed == 0) { + VehicleCommand cmd; + cmd.vehicleId = vehicle.vehicleNo; + cmd.type = CommandType::RESUME; + cmd.reason = CommandReason::RESUME_TRAFFIC; + cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); + + broadcastVehicleCommand(cmd); + controllableVehicles_->sendCommand(cmd.vehicleId, cmd); + Logger::debug("发送恢复指令到无风险车辆: ", cmd.vehicleId); } } } @@ -339,21 +460,36 @@ void System::broadcastPositionUpdate(const MovingObject& obj) { " timestamp=", msg.timestamp); } -void System::broadcastTrafficLightUpdate(const TrafficLightSignal& signal) { +void System::broadcastTrafficLightStatus(const TrafficLightSignal& signal) { if (!ws_server_) { return; } + // 查找路口信息 + const Intersection* intersection = intersection_config_.findByTrafficLightId(signal.trafficLightId); + nlohmann::json j = { - {"type", "traffic_light_update"}, + {"type", "traffic_light_status"}, {"id", signal.trafficLightId}, {"status", static_cast(signal.status)}, {"timestamp", signal.timestamp} }; + // 添加路口信息 + if (intersection) { + j["intersection"] = intersection->id; + j["position"] = { + {"longitude", intersection->position.longitude}, + {"latitude", intersection->position.latitude} + }; + } + ws_server_->broadcast(j.dump()); Logger::debug("广播红绿灯状态: id=", signal.trafficLightId, " status=", static_cast(signal.status), + " intersection=", intersection ? intersection->id : "", + " position=(", intersection ? intersection->position.longitude : 0.0, ",", + intersection ? intersection->position.latitude : 0.0, ")", " timestamp=", signal.timestamp); } @@ -370,6 +506,7 @@ void System::broadcastCollisionWarning(const CollisionRisk& risk) { return; } + // 构造冲突预警消息 nlohmann::json j = { {"type", "collision_warning"}, {"id1", risk.id1}, @@ -380,7 +517,12 @@ void System::broadcastCollisionWarning(const CollisionRisk& risk) { {"timestamp", std::chrono::system_clock::now().time_since_epoch().count()} }; + // 广播冲突预警消息 ws_server_->broadcast(j.dump()); + Logger::debug("广播冲突预警: id1=", risk.id1, " id2=", risk.id2, + " distance=", risk.distance, "m", + " relativeSpeed=", risk.relativeSpeed, "m/s", + " level=", getRiskLevelString(risk.level)); } void System::broadcastTimeoutWarning(const network::TimeoutWarningMessage& warning) { @@ -396,4 +538,75 @@ void System::broadcastTimeoutWarning(const network::TimeoutWarningMessage& warni warning.elapsed_ms, "ms without response"); } } +} + +void System::broadcastVehicleCommand(const VehicleCommand& cmd) { + if (!ws_server_) { + return; + } + + nlohmann::json j = { + {"type", "vehicle_command"}, + {"vehicleId", cmd.vehicleId}, + {"commandType", [&]() { + switch (cmd.type) { + case CommandType::SIGNAL: return "SIGNAL"; + case CommandType::ALERT: return "ALERT"; + case CommandType::WARNING: return "WARNING"; + case CommandType::RESUME: return "RESUME"; + default: return "UNKNOWN"; + } + }()}, + {"reason", [&]() { + switch (cmd.reason) { + case CommandReason::TRAFFIC_LIGHT: return "TRAFFIC_LIGHT"; + case CommandReason::AIRCRAFT_CROSSING: return "AIRCRAFT_CROSSING"; + case CommandReason::SPECIAL_VEHICLE: return "SPECIAL_VEHICLE"; + case CommandReason::AIRCRAFT_PUSH: return "AIRCRAFT_PUSH"; + case CommandReason::RESUME_TRAFFIC: return "RESUME_TRAFFIC"; + default: return "UNKNOWN"; + } + }()}, + {"timestamp", cmd.timestamp} + }; + + // 添加可选字段 + if (cmd.type == CommandType::SIGNAL) { + j["signalState"] = cmd.signalState == SignalState::RED ? "RED" : "GREEN"; + j["intersectionId"] = cmd.intersectionId; + } + + // 添加目标位置（对于所有非 RESUME 类型的指令） + if (cmd.type != CommandType::RESUME) { + j["targetLatitude"] = cmd.latitude; + j["targetLongitude"] = cmd.longitude; + } + + ws_server_->broadcast(j.dump()); +} + +const MovingObject* System::findVehicle(const std::string& vehicleId) const { + if (!dataCollector_) { + return nullptr; + } + + // 获取数据引用（注意：DataCollector 内部会加锁） + const auto& aircrafts = dataCollector_->getAircraftRef(); + const auto& vehicles = dataCollector_->getVehicleRef(); + + // 在航空器中查找 + for (const auto& aircraft : aircrafts) { + if (aircraft.flightNo == vehicleId) { + return &aircraft; + } + } + + // 在车辆中查找 + for (const auto& vehicle : vehicles) { + if (vehicle.vehicleNo == vehicleId) { + return &vehicle; + } + } + + return nullptr; } \ No newline at end of file diff --git a/src/core/System.h b/src/core/System.h index 4a52085..ee4a438 100644 --- a/src/core/System.h +++ b/src/core/System.h @@ -4,6 +4,7 @@ #include #include #include +#include #include "types/BasicTypes.h" #include "detector/CollisionDetector.h" #include "detector/TrafficLightDetector.h" @@ -33,7 +34,8 @@ public: virtual void broadcastTimeoutWarning(const network::TimeoutWarningMessage& warning); void broadcastPositionUpdate(const MovingObject& obj); void broadcastCollisionWarning(const CollisionRisk& risk); - void broadcastTrafficLightUpdate(const TrafficLightSignal& signal); + void broadcastVehicleCommand(const VehicleCommand& cmd); + void broadcastTrafficLightStatus(const TrafficLightSignal& signal); const SystemConfig& getSystemConfig() const { return system_config_; } const IntersectionConfig& getIntersectionConfig() const { return intersection_config_; } @@ -42,8 +44,6 @@ private: void processLoop(); void processCollisions(const std::vector& collisions); - - std::atomic running_{false}; std::thread processThread_; @@ -64,4 +64,10 @@ private: IntersectionConfig intersection_config_; static System* instance_; + + // 记录上一次有风险的车辆列表 + std::unordered_set lastVehiclesWithRisk_; + + // 辅助函数 + const MovingObject* findVehicle(const std::string& vehicleId) const; }; \ No newline at end of file diff --git a/src/detector/CollisionDetector.cpp b/src/detector/CollisionDetector.cpp index 697a508..e7f8d58 100644 --- a/src/detector/CollisionDetector.cpp +++ b/src/detector/CollisionDetector.cpp @@ -1,34 +1,80 @@ #include "detector/CollisionDetector.h" #include "utils/Logger.h" +#include "config/SystemConfig.h" #include #include CollisionDetector::CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles) : airportBounds_(bounds) , vehicleTree_(bounds.getAirportBounds(), 8) // 使用机场总边界初始化四叉树 - , controllableVehicles_(&controllableVehicles) -{ + , controllableVehicles_(&controllableVehicles) { + + // 记录初始化信息 + const auto& airportBounds = bounds.getAirportBounds(); + Logger::debug( + "碰撞检测器初始化: 机场边界=(", + airportBounds.x, ",", airportBounds.y, ") - (", + airportBounds.x + airportBounds.width, ",", airportBounds.y + airportBounds.height, ")" + ); } void CollisionDetector::updateTraffic(const std::vector& aircraft, const std::vector& vehicles) { + // 记录更新前的状态 + Logger::debug( + "更新交通数据: 航空器数量=", aircraft.size(), + ", 车辆数量=", vehicles.size() + ); + // 更新航空器数据 aircraftData_ = aircraft; // 更新车辆四叉树 vehicleTree_.clear(); + size_t validVehicles = 0; + for (const auto& vehicle : vehicles) { - vehicleTree_.insert(vehicle); + // 验证车辆位置是否在机场边界内 + 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)) { + vehicleTree_.insert(vehicle); + ++validVehicles; + } else { + Logger::warning( + "车辆位置超出机场边界: id=", vehicle.vehicleNo, + ", 位置=(", vehicle.position.x, + ",", vehicle.position.y, ")" + ); + } + } + + // 记录更新后的状态 + auto allVehicles = vehicleTree_.queryRange(vehicleTree_.getBounds()); + Logger::debug( + "交通数据更新完成: 有效车辆数量=", validVehicles, + ", 四叉树中的车辆数量=", allVehicles.size() + ); + + // 验证数据一致性 + if (validVehicles != allVehicles.size()) { + Logger::error( + "车辆数据不一致: 有效车辆数=", validVehicles, + ", 四叉树中的车辆数量=", allVehicles.size() + ); } } std::vector CollisionDetector::detectCollisions() { std::vector risks; + // 获取配置参数 + const auto& predictionConfig = SystemConfig::instance().collision_detection.prediction; + // 获取所有车辆 auto allVehicles = vehicleTree_.queryRange(vehicleTree_.getBounds()); - - // 过滤出可控车辆 + + // 可控车辆 std::vector controlVehicles; for (const auto& vehicle : allVehicles) { if (controllableVehicles_->isControllable(vehicle.vehicleNo)) { @@ -38,92 +84,120 @@ std::vector CollisionDetector::detectCollisions() { // 检测可控车辆与航空器的碰撞 for (const auto& aircraft : aircraftData_) { - const auto& areaConfig = getCollisionParams(aircraft.position); - for (const auto& vehicle : controlVehicles) { - // 计算平面距离的平方 + // 计算当前距离 double dx = aircraft.position.x - vehicle.position.x; double dy = aircraft.position.y - vehicle.position.y; - double distanceSquared = dx*dx + dy*dy; - double threshold = areaConfig.aircraftGroundRadius; - double thresholdSquared = threshold * threshold; + double currentDistance = std::sqrt(dx*dx + dy*dy); - if (distanceSquared < thresholdSquared) { - // 只在必要时计算精确距离 - double distance = std::sqrt(distanceSquared); + // 计算预测阈值 + double maxSpeed = std::max(aircraft.speed, vehicle.speed); + double predictionDistance = maxSpeed * predictionConfig.time_window; + double baseThreshold = getCollisionParams(aircraft.position).aircraftGroundRadius; + double predictionThreshold = std::max(predictionDistance, baseThreshold * 2.0); + + // 如果在预测距离内，进行轨迹预测 + if (currentDistance <= predictionThreshold) { + // 获取预测结果 + auto prediction = predictTrajectoryCollision( + aircraft.position, aircraft.speed, aircraft.heading, + vehicle.position, vehicle.speed, vehicle.heading, + predictionConfig.aircraft_size, + predictionConfig.vehicle_size, + predictionConfig.time_window + ); - // 计算相对运动 - MovementVector av(aircraft.speed, aircraft.heading); - MovementVector vv(vehicle.speed, vehicle.heading); - double vx = av.vx - vv.vx; - double vy = av.vy - vv.vy; + // 评估风险等级 + auto [level, zoneType] = evaluateRisk( + currentDistance, + aircraft.position, + true, // aircraft + false, // vehicle + prediction.willCollide + ); - // 计算相对速度大小 - double relativeSpeed = std::sqrt(vx*vx + vy*vy); - - // 记录调试信息 - Logger::debug("航空器与车辆相对速度计算: ", aircraft.flightNo, " vs ", vehicle.vehicleNo, - ", 航空器: speed=", aircraft.speed, "m/s, heading=", aircraft.heading, "°", - ", 车辆: speed=", vehicle.speed, "m/s, heading=", vehicle.heading, "°", - ", 相对速度: vx=", vx, "m/s, vy=", vy, "m/s, magnitude=", relativeSpeed, "m/s"); - - // 计算风险等级 - RiskLevel level = calculateRiskLevel(distance, aircraft.position, true, false); - - // 添加碰撞风险信息 - risks.push_back({ - aircraft.flightNo, - vehicle.vehicleNo, - level, - distance, - relativeSpeed, - {vx, vy} - }); + // 如果存在风险，添加到结果中 + if (level != RiskLevel::NONE) { + // 计算相对运动 + MovementVector av(aircraft.speed, aircraft.heading); + MovementVector vv(vehicle.speed, vehicle.heading); + double vx = av.vx - vv.vx; + double vy = av.vy - vv.vy; + double relativeSpeed = std::sqrt(vx*vx + vy*vy); + + risks.push_back({ + aircraft.flightNo, + vehicle.vehicleNo, + level, + currentDistance, + prediction.minDistance, + relativeSpeed, + {vx, vy}, + zoneType + }); + } } } } - // 检测可控车辆与其他车辆的碰撞 + // 检测可控车辆之间的碰撞 for (size_t i = 0; i < controlVehicles.size(); ++i) { - const auto& controlVehicle = controlVehicles[i]; - const auto& areaConfig = getCollisionParams(controlVehicle.position); - - // 检查与所有其他车辆的碰撞（包括可控和非可控） + const auto& vehicle1 = controlVehicles[i]; + for (size_t j = i + 1; j < allVehicles.size(); ++j) { - const auto& otherVehicle = allVehicles[j]; + const auto& vehicle2 = allVehicles[j]; - // 计算平面距离 - double dx = controlVehicle.position.x - otherVehicle.position.x; - double dy = controlVehicle.position.y - otherVehicle.position.y; - double distance = std::sqrt(dx*dx + dy*dy); - double threshold = areaConfig.vehicleCollisionRadius; - - if (distance <= threshold) { - // 计算相对运动 - MovementVector v1v(controlVehicle.speed, controlVehicle.heading); - MovementVector v2v(otherVehicle.speed, otherVehicle.heading); - double vx = v1v.vx - v2v.vx; - double vy = v1v.vy - v2v.vy; - double relativeSpeed = std::sqrt(vx*vx + vy*vy); + // 计算当前距离 + double dx = vehicle1.position.x - vehicle2.position.x; + double dy = vehicle1.position.y - vehicle2.position.y; + double currentDistance = std::sqrt(dx*dx + dy*dy); + + // 计算预测阈值 + double maxSpeed = std::max(vehicle1.speed, vehicle2.speed); + double predictionDistance = maxSpeed * predictionConfig.time_window; + double baseThreshold = getCollisionParams(vehicle1.position).vehicleCollisionRadius; + double predictionThreshold = std::max(predictionDistance, baseThreshold * 2.0); + + // 如果在预测距离内，进行轨迹预测 + if (currentDistance <= predictionThreshold) { + // 获取预测结果 + auto prediction = predictTrajectoryCollision( + vehicle1.position, vehicle1.speed, vehicle1.heading, + vehicle2.position, vehicle2.speed, vehicle2.heading, + predictionConfig.vehicle_size, + predictionConfig.vehicle_size, + predictionConfig.time_window + ); - // 记录调试信息 - Logger::debug("车辆间相对速度计算: ", controlVehicle.vehicleNo, " vs ", otherVehicle.vehicleNo, - ", 车辆1: speed=", controlVehicle.speed, "m/s, heading=", controlVehicle.heading, "°", - ", 车辆2: speed=", otherVehicle.speed, "m/s, heading=", otherVehicle.heading, "°", - ", 相对速度: vx=", vx, "m/s, vy=", vy, "m/s, magnitude=", relativeSpeed, "m/s"); + // 评估风险等级 + auto [level, zoneType] = evaluateRisk( + currentDistance, + vehicle1.position, + false, // vehicle + false, // vehicle + prediction.willCollide + ); - // 计算风险等级 - RiskLevel level = calculateRiskLevel(distance, controlVehicle.position, false, false); - - // 添加碰撞风险信息 - risks.push_back({ - controlVehicle.vehicleNo, - otherVehicle.vehicleNo, - level, - distance, - relativeSpeed, - {vx, vy} - }); + // 如果存在风险，添加到结果中 + if (level != RiskLevel::NONE) { + // 计算相对运动 + MovementVector v1v(vehicle1.speed, vehicle1.heading); + MovementVector v2v(vehicle2.speed, vehicle2.heading); + double vx = v1v.vx - v2v.vx; + double vy = v1v.vy - v2v.vy; + double relativeSpeed = std::sqrt(vx*vx + vy*vy); + + risks.push_back({ + vehicle1.vehicleNo, + vehicle2.vehicleNo, + level, + currentDistance, + prediction.minDistance, + relativeSpeed, + {vx, vy}, + zoneType + }); + } } } } @@ -133,91 +207,320 @@ std::vector CollisionDetector::detectCollisions() { RiskLevel CollisionDetector::calculateRiskLevel(double distance, const Vector2D& position, bool isAircraft1, bool isAircraft2) const { - // 获取当前区域的配置 const auto& areaConfig = getCollisionParams(position); - // 获取告警阈值 + // 获取合适的阈值配置 auto thresholds = areaConfig.getThresholds(isAircraft1, isAircraft2); + double warningDistance = thresholds.warning; + double alertDistance = thresholds.critical; - // 直接比较距离和阈值 - if (distance <= thresholds.critical) { + // 记录调试信息 + Logger::debug( + "风险等级计算: 距离=", distance, + "m, 预警阈值=", warningDistance, + "m, 警报阈值=", alertDistance, + "m, 物体类型=", + isAircraft1 ? (isAircraft2 ? "航空器-航空器" : "航空器-车辆") : "车辆-车辆" + ); + + // 根据距离判断风险等级 + if (distance <= alertDistance) { return RiskLevel::CRITICAL; - } else if (distance <= thresholds.warning) { + } else if (distance <= warningDistance) { return RiskLevel::WARNING; } return RiskLevel::NONE; } -bool CollisionDetector::checkAircraftVehicleCollision(const Aircraft& aircraft, - const Vehicle& vehicle) const { - const auto& areaConfig = getCollisionParams(aircraft.position); +WarningZoneType CollisionDetector::determineWarningZone(double distance, double threshold) const { + // 获取警报和预警距离 + double alertDistance = threshold * 0.5; // 警报距离为阈值的50% + double warningDistance = threshold; // 预警距离为阈值的100% - // 计算平面距离的平方 - double dx = aircraft.position.x - vehicle.position.x; - double dy = aircraft.position.y - vehicle.position.y; - double distanceSquared = dx*dx + dy*dy; + // 记录调试信息 + Logger::debug( + "预警区域判断: 当前距离=", distance, + "m, 警报距离=", alertDistance, + "m, 预警距离=", warningDistance, "m" + ); - // 获取该区域的告警阈值 - auto thresholds = areaConfig.getThresholds(true, false); // aircraft vs vehicle - double criticalThresholdSquared = thresholds.critical * thresholds.critical; - - // 如果距离小于紧急阈值，直接报警 - if (distanceSquared < criticalThresholdSquared) { - return true; + // 根据距离判断区域类型 + if (distance <= alertDistance) { + return WarningZoneType::DANGER; + } else if (distance <= warningDistance) { + return WarningZoneType::WARNING; } - - // 如果距离在警告阈值范围内，检查相对运动 - double warningThresholdSquared = thresholds.warning * thresholds.warning; - if (distanceSquared < warningThresholdSquared) { - // 计算相对运动 - MovementVector av(aircraft.speed, aircraft.heading); - MovementVector vv(vehicle.speed, vehicle.heading); - double vx = av.vx - vv.vx; - double vy = av.vy - vv.vy; - - // 计算相对运动 - double relativeMotion = dx*vx + dy*vy; - return relativeMotion < 0; - } - - return false; + return WarningZoneType::NONE; } -bool CollisionDetector::checkVehicleCollision(const Vehicle& v1, - const Vehicle& v2) const { - const auto& areaConfig = getCollisionParams(v1.position); +// 添加一个新的辅助函数来统一处理风险等级和预警区域的判断 +std::pair CollisionDetector::evaluateRisk( + double currentDistance, + const Vector2D& position, + bool isAircraft1, + bool isAircraft2, + bool willCollide) const { - // 计算平面距离 - double dx = v1.position.x - v2.position.x; - double dy = v1.position.y - v2.position.y; - double distance = std::sqrt(dx*dx + dy*dy); + // 取区域配置 + const auto& areaConfig = getCollisionParams(position); + auto thresholds = areaConfig.getThresholds(isAircraft1, isAircraft2); - // 获取该区域的告警阈值 - auto thresholds = areaConfig.getThresholds(false, false); // vehicle vs vehicle + RiskLevel level = RiskLevel::NONE; + WarningZoneType zoneType = WarningZoneType::NONE; - // 如果距离小于紧急阈值，直接报警 - if (distance < thresholds.critical) { + // 如果预测到碰撞或距离小于警报距离，设置为危险等级 + if (willCollide || currentDistance <= thresholds.critical) { + level = RiskLevel::CRITICAL; + zoneType = WarningZoneType::DANGER; + } + // 如果距离在预警范围内，设置为预警等级 + else if (currentDistance <= thresholds.warning) { + level = RiskLevel::WARNING; + zoneType = WarningZoneType::WARNING; + } + + // 记录调试信息 + Logger::debug( + "风险评估: 当前距离=", currentDistance, + "m, 预警阈值=", thresholds.warning, + "m, 警报阈值=", thresholds.critical, + "m, 预测碰撞=", willCollide ? "是" : "否", + ", 风险等级=", static_cast(level), + ", 区域类型=", static_cast(zoneType) + ); + + return {level, zoneType}; +} + +bool CollisionDetector::checkCollisionRisk( + const Vector2D& pos1, double speed1, double heading1, const std::string& id1, + const Vector2D& pos2, double speed2, double heading2, const std::string& id2, + bool isAircraft1, bool isAircraft2) const { + + // 获取区域配置 + const auto& areaConfig = getCollisionParams(pos1); + const auto& predictionConfig = SystemConfig::instance().collision_detection.prediction; + + // 计算当前距离 + double dx = pos1.x - pos2.x; + double dy = pos1.y - pos2.y; + double currentDistance = std::sqrt(dx*dx + dy*dy); + + // 计算预测阈值 + double maxSpeed = std::max(speed1, speed2); + double predictionDistance = maxSpeed * predictionConfig.time_window; + // 根据物体类型选择基准阈值 + double baseThreshold = (isAircraft1 || isAircraft2) ? + areaConfig.aircraftGroundRadius : + areaConfig.vehicleCollisionRadius; + double predictionThreshold = std::max(predictionDistance, baseThreshold * 2.0); + + // 如果当前距离超过预测阈值，直接返回false + if (currentDistance > predictionThreshold) { + return false; + } + + // 直接告警条件：距离小于阈值的一半 + if (currentDistance < baseThreshold * 0.5) { return true; } - // 如果距离在警告阈值范围内，检查相对运动 - if (distance < thresholds.warning) { - // 计算相对速度 - double v1x = v1.speed * std::cos((90 - v1.heading) * M_PI / 180.0); - double v1y = v1.speed * std::sin((90 - v1.heading) * M_PI / 180.0); - double v2x = v2.speed * std::cos((90 - v2.heading) * M_PI / 180.0); - double v2y = v2.speed * std::sin((90 - v2.heading) * M_PI / 180.0); - double vx = v1x - v2x; - double vy = v1y - v2y; + // 获取预测结果 + auto prediction = predictTrajectoryCollision( + pos1, speed1, heading1, + pos2, speed2, heading2, + isAircraft1 ? predictionConfig.aircraft_size : predictionConfig.vehicle_size, + isAircraft2 ? predictionConfig.aircraft_size : predictionConfig.vehicle_size, + predictionConfig.time_window + ); + + // 获取阈值配置 + auto thresholds = areaConfig.getThresholds(isAircraft1, isAircraft2); + double alertDistance = thresholds.critical; + + // 如果距离在阈值范围内，进行相对运动分析 + bool hasCollisionRisk = false; + if (currentDistance < baseThreshold) { + // 使用 MovementVector 计算速度分量 + MovementVector mv1(speed1, heading1); + MovementVector mv2(speed2, heading2); - // 计算相对运动 + // 计算相对速度 + double vx = mv1.vx - mv2.vx; + double vy = mv1.vy - mv2.vy; + + // 计算相对运动（点积），判断是否在接近 double relativeMotion = dx*vx + dy*vy; - // 如果物体正在接近或相对静止，就报警 - if (relativeMotion <= 0) { - return true; + // 如果相对运动小于等于0，表示物体正在接近 + hasCollisionRisk = (relativeMotion <= 0) || prediction.willCollide || currentDistance <= alertDistance; + } + + if (hasCollisionRisk) { + // 使用 MovementVector 计算相对运动，用于日志记录 + MovementVector mv1(speed1, heading1); + MovementVector mv2(speed2, heading2); + double vx = mv1.vx - mv2.vx; + double vy = mv1.vy - mv2.vy; + double relativeSpeed = std::sqrt(vx*vx + vy*vy); + + std::string type1 = isAircraft1 ? "航空器" : "车辆"; + std::string type2 = isAircraft2 ? "航空器" : "车辆"; + + Logger::debug( + "检测到碰撞风险: {}1={}, {}2={}, 当前距离={}m, 相对速度={}m/s, 预测碰撞={}", + type1, id1, + type2, id2, + currentDistance, + relativeSpeed, + prediction.willCollide ? "是" : "否" + ); + } + + return hasCollisionRisk; +} + +bool CollisionDetector::checkAircraftVehicleCollision(const Aircraft& aircraft, + const Vehicle& vehicle) const { + return checkCollisionRisk( + aircraft.position, aircraft.speed, aircraft.heading, aircraft.flightNo, + vehicle.position, vehicle.speed, vehicle.heading, vehicle.vehicleNo, + true, false + ); +} + +bool CollisionDetector::checkVehicleCollision(const Vehicle& v1, const Vehicle& v2) const { + return checkCollisionRisk( + v1.position, v1.speed, v1.heading, v1.vehicleNo, + v2.position, v2.speed, v2.heading, v2.vehicleNo, + false, false + ); +} + +CollisionPrediction CollisionDetector::predictTrajectoryCollision( + const Vector2D& pos1, double speed1, double heading1, + const Vector2D& pos2, double speed2, double heading2, + double size1, double size2, double timeWindow) const { + + CollisionPrediction result; + result.willCollide = false; + result.timeToCollision = std::numeric_limits::infinity(); + result.minDistance = std::numeric_limits::infinity(); + + // 计算安全距离 + double safeDistance = size1 + size2; + + // 计算速度分量 + MovementVector mv1(speed1, heading1); + MovementVector mv2(speed2, heading2); + + // 计算相对速度 + double dvx = mv1.vx - mv2.vx; + double dvy = mv1.vy - mv2.vy; + double relativeSpeed = std::sqrt(dvx*dvx + dvy*dvy); + + // 计算相对位置 + double dx = pos1.x - pos2.x; + double dy = pos1.y - pos2.y; + double currentDistance = std::sqrt(dx*dx + dy*dy); + + // 记录初始状态 + Logger::debug( + "轨迹预测初始状态: 当前距离=", currentDistance, + "m, 相对速度=", relativeSpeed, + "m/s, 安全距离=", safeDistance, "m" + ); + + // 如果物体静止相对于此，直接返回当前状态 + if (relativeSpeed < 1e-6) { + result.minDistance = currentDistance; + result.willCollide = (currentDistance <= safeDistance); + if (result.willCollide) { + result.timeToCollision = 0.0; + // 根据物体尺寸计算碰撞点 + double ratio = size1 / (size1 + size2); + result.collisionPoint.x = pos2.x + dx * ratio; + result.collisionPoint.y = pos2.y + dy * ratio; + } + return result; + } + + // 计算相对运动方程的系数 + double a = dvx * dvx + dvy * dvy; + double b = 2.0 * (dx * dvx + dy * dvy); + double c = dx * dx + dy * dy - safeDistance * safeDistance; + + // 计算判别式 + double discriminant = b * b - 4.0 * a * c; + + // 如果判别式小于0，不会发生碰撞 + if (discriminant < 0) { + // 计算时间窗口内的最小距离 + double t_min = -b / (2.0 * a); + t_min = std::clamp(t_min, 0.0, timeWindow); + + // 计算最小距离时刻的位置 + double x1_min = pos1.x + mv1.vx * t_min; + double y1_min = pos1.y + mv1.vy * t_min; + double x2_min = pos2.x + mv2.vx * t_min; + double y2_min = pos2.y + mv2.vy * t_min; + + result.minDistance = std::sqrt( + std::pow(x1_min - x2_min, 2) + + std::pow(y1_min - y2_min, 2) + ); + + Logger::debug( + "无碰撞可能: 最小距离=", result.minDistance, + "m, 发生时间=", t_min, "s" + ); + + return result; + } + + // 计算可能的碰撞时间点 + double t1 = (-b - std::sqrt(discriminant)) / (2.0 * a); + double t2 = (-b + std::sqrt(discriminant)) / (2.0 * a); + + // 检查两个时间点 + for (double t : {t1, t2}) { + if (t >= 0 && t <= timeWindow) { + // 计算t时刻的位置 + double x1_t = pos1.x + mv1.vx * t; + double y1_t = pos1.y + mv1.vy * t; + double x2_t = pos2.x + mv2.vx * t; + double y2_t = pos2.y + mv2.vy * t; + + // 计算t时刻的距离 + double dist_t = std::sqrt( + std::pow(x1_t - x2_t, 2) + + std::pow(y1_t - y2_t, 2) + ); + + // 更新最小距离 + if (dist_t < result.minDistance) { + result.minDistance = dist_t; + + // 如果距离小于等于安全距离，记录碰撞信息 + if (dist_t <= safeDistance && !result.willCollide) { + result.willCollide = true; + result.timeToCollision = t; + + // 根据物体尺寸计算碰撞点 + double ratio = size1 / (size1 + size2); + result.collisionPoint.x = x2_t + (x1_t - x2_t) * ratio; + result.collisionPoint.y = y2_t + (y1_t - y2_t) * ratio; + + Logger::debug( + "预测到碰撞: 时间=", t, + "s, 距离=", dist_t, + "m, 位置=(", result.collisionPoint.x, + ",", result.collisionPoint.y, ")" + ); + } + } } } - return false; + return result; } \ No newline at end of file diff --git a/src/detector/CollisionDetector.h b/src/detector/CollisionDetector.h index 62686eb..ac21fec 100644 --- a/src/detector/CollisionDetector.h +++ b/src/detector/CollisionDetector.h @@ -5,15 +5,24 @@ #include "spatial/QuadTree.h" #include "spatial/AirportBounds.h" #include "vehicle/ControllableVehicles.h" +#include "config/SystemConfig.h" #include #include +#include #include // 碰撞风险等级 enum class RiskLevel { NONE = 0, // 无风险 - WARNING = 1, // 低风险：距离在阈值的 50%-100% 之间 - CRITICAL = 2, // 中等风险：距离在阈值的 0%-50% 之间 + WARNING = 1, // 预警：进入预警区域 + CRITICAL = 2, // 告警：进入危险区域 +}; + +// 预警区域类型 +enum class WarningZoneType { + NONE = 0, // 无风险区域 + WARNING = 1, // 预警区域 + DANGER = 2 // 危险区域 }; // 碰撞风险信息 @@ -21,8 +30,18 @@ struct CollisionRisk { std::string id1, id2; // 碰撞物体的ID RiskLevel level; // 风险等级 double distance; // 当前距离 + double minDistance; // 预测的最小距离 double relativeSpeed; // 相对速度 Vector2D relativeMotion; // 相对运动向量 + WarningZoneType zoneType; // 预警区域类型 +}; + +// 轨迹预测碰撞结果 +struct CollisionPrediction { + bool willCollide; // 是否可能发生碰撞 + double timeToCollision; // 到碰撞点的时间(秒) + Vector2D collisionPoint; // 可能的碰撞点 + double minDistance; // 最小距离 }; class CollisionDetector { @@ -36,6 +55,15 @@ public: // 检测所有可能的碰撞 std::vector detectCollisions(); + // 基于轨迹预测的碰撞检测 + CollisionPrediction predictTrajectoryCollision( + const Vector2D& pos1, double speed1, double heading1, // 物体1的位置、速度、航向 + const Vector2D& pos2, double speed2, double heading2, // 物体2的位置、速度、航向 + double size1, // 物体1的尺寸 + double size2, // 物体2的尺寸 + double timeWindow = 30.0 // 预测时间窗口(默认30秒) + ) const; + private: const AirportBounds& airportBounds_; QuadTree vehicleTree_; @@ -48,6 +76,12 @@ private: return airportBounds_.getAreaConfig(areaType); } + // 通用碰撞风险检测函数 + bool checkCollisionRisk( + const Vector2D& pos1, double speed1, double heading1, const std::string& id1, + const Vector2D& pos2, double speed2, double heading2, const std::string& id2, + bool isAircraft1, bool isAircraft2) const; + // 检查航空器和车辆是否可能碰撞 bool checkAircraftVehicleCollision(const Aircraft& aircraft, const Vehicle& vehicle) const; // 检查两辆车是否可能碰撞 @@ -57,18 +91,28 @@ private: RiskLevel calculateRiskLevel(double distance, const Vector2D& position, bool isAircraft1, bool isAircraft2) const; + // 确定预警区域类型 + WarningZoneType determineWarningZone(double distance, double threshold) const; + + // 统一的风险评估函数 + std::pair evaluateRisk( + double currentDistance, + const Vector2D& position, + bool isAircraft1, + bool isAircraft2, + bool willCollide) const; + // 计算相对运动 struct MovementVector { double vx, vy; MovementVector(double speed, double heading) { - // 航向角是以正北为0度，顺时针增加 - // 需要转换为数学坐标系（以正东为0度，逆时针增加） - double radians = (90.0 - heading) * M_PI / 180.0; + // 标准方位角：正北为0度，顺时针增加 + double radians = heading * M_PI / 180.0; // 计算速度分量 - vx = speed * std::cos(radians); // 东向分量 - vy = speed * std::sin(radians); // 北向分量 + vx = speed * std::sin(radians); // 东向分量 + vy = speed * std::cos(radians); // 北向分量 // 记录调试信息 Logger::debug("速度分量计算: speed=", speed, "m/s, heading=", heading, diff --git a/src/detector/TrafficLightDetector.cpp b/src/detector/TrafficLightDetector.cpp index 4d2f56a..a30ac51 100644 --- a/src/detector/TrafficLightDetector.cpp +++ b/src/detector/TrafficLightDetector.cpp @@ -1,12 +1,15 @@ #include "detector/TrafficLightDetector.h" #include "utils/Logger.h" #include "types/BasicTypes.h" +#include "core/System.h" #include TrafficLightDetector::TrafficLightDetector(const IntersectionConfig& intersectionConfig, - const ControllableVehicles& controllableVehicles) + const ControllableVehicles& controllableVehicles, + System& system) : intersection_config_(intersectionConfig) - , controllable_vehicles_(controllableVehicles) { + , controllable_vehicles_(controllableVehicles) + , system_(system) { } void TrafficLightDetector::processSignal(const TrafficLightSignal& signal, @@ -18,58 +21,68 @@ void TrafficLightDetector::processSignal(const TrafficLightSignal& signal, return; } + // 保存当前处理的信号 + current_signal_ = signal; + // 检查每个车辆 for (const auto& vehicle : vehicles) { if (controllable_vehicles_.isControllable(vehicle.vehicleNo)) { - checkVehicleIntersectionDistance(vehicle, *intersection, signal.status); - } - } -} - -void TrafficLightDetector::checkVehicleIntersectionDistance(const Vehicle& vehicle, - const Intersection& intersection, - SignalStatus status) { - // 创建 GeoPosition 用于距离计算 - GeoPosition intersectionPos{ - intersection.position.latitude, - intersection.position.longitude - }; - - // 计算车辆到路口的距离 - double distance = MovingObject::calculateDistance(vehicle.geo, intersectionPos); - - // 如果车辆在路口影响范围内 - if (distance <= intersection.radius) { - switch (status) { - case SignalStatus::RED: - sendStopCommand(vehicle.vehicleNo); - break; - case SignalStatus::GREEN: - sendContinueCommand(vehicle.vehicleNo); - break; - default: - Logger::warning("未知的信号灯状态"); - break; + // 根据信号灯状态发送指令 + switch (signal.status) { + case SignalStatus::RED: + sendStopCommand(vehicle.vehicleNo); + break; + case SignalStatus::GREEN: + sendContinueCommand(vehicle.vehicleNo); + break; + default: + Logger::warning("未知的信号灯状态"); + break; + } } } } void TrafficLightDetector::sendStopCommand(const std::string& vehicleNo) { VehicleCommand cmd; + cmd.vehicleId = vehicleNo; cmd.type = CommandType::SIGNAL; cmd.reason = CommandReason::TRAFFIC_LIGHT; cmd.signalState = SignalState::RED; cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); - const_cast(controllable_vehicles_).sendCommand(vehicleNo, cmd); - Logger::debug("发送停止指令到车辆: ", vehicleNo); + + // 添加路口信息 + const Intersection* intersection = intersection_config_.findByTrafficLightId(current_signal_.trafficLightId); + if (intersection) { + cmd.intersectionId = intersection->id; + cmd.latitude = intersection->position.latitude; + cmd.longitude = intersection->position.longitude; + } + + if (const_cast(controllable_vehicles_).sendCommand(vehicleNo, cmd)) { + system_.broadcastVehicleCommand(cmd); + } + Logger::debug("发送停止指令到车辆: ", vehicleNo, " 路口: ", cmd.intersectionId); } void TrafficLightDetector::sendContinueCommand(const std::string& vehicleNo) { VehicleCommand cmd; + cmd.vehicleId = vehicleNo; cmd.type = CommandType::SIGNAL; cmd.reason = CommandReason::TRAFFIC_LIGHT; cmd.signalState = SignalState::GREEN; cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); - const_cast(controllable_vehicles_).sendCommand(vehicleNo, cmd); - Logger::debug("发送继续指令到车辆: ", vehicleNo); + + // 添加路口信息 + const Intersection* intersection = intersection_config_.findByTrafficLightId(current_signal_.trafficLightId); + if (intersection) { + cmd.intersectionId = intersection->id; + cmd.latitude = intersection->position.latitude; + cmd.longitude = intersection->position.longitude; + } + + if (const_cast(controllable_vehicles_).sendCommand(vehicleNo, cmd)) { + system_.broadcastVehicleCommand(cmd); + } + Logger::debug("发送继续指令到车辆: ", vehicleNo, " 路口: ", cmd.intersectionId); } \ No newline at end of file diff --git a/src/detector/TrafficLightDetector.h b/src/detector/TrafficLightDetector.h index 64a8f30..f9e7c95 100644 --- a/src/detector/TrafficLightDetector.h +++ b/src/detector/TrafficLightDetector.h @@ -6,23 +6,25 @@ #include "config/IntersectionConfig.h" #include "vehicle/ControllableVehicles.h" +// 前向声明 +class System; + class TrafficLightDetector { public: TrafficLightDetector(const IntersectionConfig& intersectionConfig, - const ControllableVehicles& controllableVehicles); + const ControllableVehicles& controllableVehicles, + System& system); // 处理红绿灯信号 void processSignal(const TrafficLightSignal& signal, const std::vector& vehicles); private: - // 检查车辆与路口的距离，决定是否需要发送指令 - void checkVehicleIntersectionDistance(const Vehicle& vehicle, - const Intersection& intersection, - SignalStatus status); // 发送车辆控制指令 void sendStopCommand(const std::string& vehicleId); void sendContinueCommand(const std::string& vehicleId); const IntersectionConfig& intersection_config_; const ControllableVehicles& controllable_vehicles_; + System& system_; + TrafficLightSignal current_signal_; // 当前处理的信号 }; \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 516e40b..c463c39 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -21,7 +21,7 @@ LogLevel getLogLevelFromString(const std::string& level) { return LogLevel::INFO; // 默认级别 } -int main() { +int main(int argc, char* argv[]) { try { // 设置信号处理 signal(SIGINT, signalHandler); @@ -31,12 +31,12 @@ int main() { std::filesystem::create_directories("logs"); // 加载系统配置 - auto config = SystemConfig::load("config/system_config.json"); + SystemConfig::instance().load("config/system_config.json"); // 初始化日志系统 - Logger::initialize(config.logging.file, getLogLevelFromString(config.logging.level)); + Logger::initialize(SystemConfig::instance().logging.file, getLogLevelFromString(SystemConfig::instance().logging.level)); Logger::info("Starting system..."); - Logger::debug("Log level set to: ", config.logging.level); + Logger::debug("Log level set to: ", SystemConfig::instance().logging.level); // 初始化系统 System system; diff --git a/src/network/HTTPClient.cpp b/src/network/HTTPClient.cpp new file mode 100644 index 0000000..e8486d0 --- /dev/null +++ b/src/network/HTTPClient.cpp @@ -0,0 +1,109 @@ +#include "network/HTTPClient.h" +#include "utils/Logger.h" +#include + +HTTPClient::HTTPClient() { + curl_global_init(CURL_GLOBAL_ALL); + curl_ = curl_easy_init(); + if (!curl_) { + throw std::runtime_error("Failed to initialize CURL"); + } +} + +HTTPClient::~HTTPClient() { + if (curl_) { + curl_easy_cleanup(curl_); + } + curl_global_cleanup(); +} + +size_t HTTPClient::WriteCallback(void* contents, size_t size, size_t nmemb, void* userp) { + size_t total_size = size * nmemb; + std::string* response = static_cast(userp); + response->append(static_cast(contents), total_size); + return total_size; +} + +bool HTTPClient::sendCommand(const std::string& ip, int port, const VehicleCommand& command) { + if (!curl_) { + Logger::error("CURL not initialized"); + return false; + } + + // 构造请求URL + std::stringstream url; + url << "http://" << ip << ":" << port << "/api/vehicle/command"; + + // 构造请求体 + nlohmann::json request = { + {"vehicleId", command.vehicleId}, + {"type", [&]() { + switch (command.type) { + case CommandType::SIGNAL: return "SIGNAL"; + case CommandType::ALERT: return "ALERT"; + case CommandType::WARNING: return "WARNING"; + case CommandType::RESUME: return "RESUME"; + default: return "UNKNOWN"; + } + }()}, + {"reason", [&]() { + switch (command.reason) { + case CommandReason::TRAFFIC_LIGHT: return "TRAFFIC_LIGHT"; + case CommandReason::AIRCRAFT_CROSSING: return "AIRCRAFT_CROSSING"; + case CommandReason::SPECIAL_VEHICLE: return "SPECIAL_VEHICLE"; + case CommandReason::AIRCRAFT_PUSH: return "AIRCRAFT_PUSH"; + case CommandReason::RESUME_TRAFFIC: return "RESUME_TRAFFIC"; + default: return "UNKNOWN"; + } + }()}, + {"timestamp", command.timestamp} + }; + + // 添加可选字段 + if (command.type == CommandType::SIGNAL) { + request["signalState"] = command.signalState == SignalState::RED ? "RED" : "GREEN"; + request["intersectionId"] = command.intersectionId; + } + if (command.type != CommandType::RESUME) { + request["targetPosition"] = { + {"latitude", command.latitude}, + {"longitude", command.longitude} + }; + } + + std::string request_body = request.dump(); + response_buffer_.clear(); + + // 设置CURL选项 + curl_easy_setopt(curl_, CURLOPT_URL, url.str().c_str()); + curl_easy_setopt(curl_, CURLOPT_POST, 1L); + curl_easy_setopt(curl_, CURLOPT_POSTFIELDS, request_body.c_str()); + curl_easy_setopt(curl_, CURLOPT_WRITEFUNCTION, WriteCallback); + curl_easy_setopt(curl_, CURLOPT_WRITEDATA, &response_buffer_); + + // 设置请求头 + struct curl_slist* headers = nullptr; + headers = curl_slist_append(headers, "Content-Type: application/json"); + curl_easy_setopt(curl_, CURLOPT_HTTPHEADER, headers); + + // 发送请求 + CURLcode res = curl_easy_perform(curl_); + curl_slist_free_all(headers); + + if (res != CURLE_OK) { + Logger::error("Failed to send command: ", curl_easy_strerror(res)); + return false; + } + + // 检查响应状态码 + long http_code = 0; + curl_easy_getinfo(curl_, CURLINFO_RESPONSE_CODE, &http_code); + + if (http_code == 200) { + Logger::debug("Command sent successfully: ", request_body); + return true; + } else { + Logger::error("Command failed with HTTP code: ", http_code, " response: ", response_buffer_); + return false; + } +} \ No newline at end of file diff --git a/src/network/HTTPClient.h b/src/network/HTTPClient.h new file mode 100644 index 0000000..3422639 --- /dev/null +++ b/src/network/HTTPClient.h @@ -0,0 +1,20 @@ +#pragma once + +#include +#include +#include "types/VehicleCommand.h" +#include "nlohmann/json.hpp" + +class HTTPClient { +public: + HTTPClient(); + ~HTTPClient(); + + // 发送控制指令 + bool sendCommand(const std::string& ip, int port, const VehicleCommand& command); + +private: + static size_t WriteCallback(void* contents, size_t size, size_t nmemb, void* userp); + CURL* curl_; + std::string response_buffer_; +}; \ No newline at end of file diff --git a/src/spatial/AirportBounds.cpp b/src/spatial/AirportBounds.cpp index 53fb646..348a756 100644 --- a/src/spatial/AirportBounds.cpp +++ b/src/spatial/AirportBounds.cpp @@ -56,6 +56,7 @@ void AirportBounds::loadConfig(const std::string& configFile) { else if (key == "taxiway") type = AreaType::TAXIWAY; else if (key == "gate") type = AreaType::GATE; else if (key == "service") type = AreaType::SERVICE; + else if (key == "test_zone") type = AreaType::TEST_ZONE; else continue; // 检查必要的字段 @@ -77,7 +78,9 @@ void AirportBounds::loadConfig(const std::string& configFile) { areaConfigs_[type] = { config["vehicle_collision_radius"].get(), config["aircraft_ground_radius"].get(), - config["height_threshold"].get() + config["height_threshold"].get(), + config["warning_zone_radius"].get(), + config["alert_zone_radius"].get() }; } diff --git a/src/spatial/AirportBounds.h b/src/spatial/AirportBounds.h index 1b65b35..f508122 100644 --- a/src/spatial/AirportBounds.h +++ b/src/spatial/AirportBounds.h @@ -10,14 +10,17 @@ enum class AreaType { RUNWAY, // 跑道 TAXIWAY, // 滑行道 GATE, // 停机位 - SERVICE // 服务区 + SERVICE, // 服务区 + TEST_ZONE // 测试区 }; // 区域配置 struct AreaConfig { - double vehicleCollisionRadius; // 车辆间冲突检测半径 - double aircraftGroundRadius; // 航空器与车辆冲突检测半径 - double heightThreshold; // 高度阈值 + double vehicleCollisionRadius = 0.0; // 车辆间冲突检测半径 + double aircraftGroundRadius = 0.0; // 航空器与车辆冲突检测半径 + double heightThreshold = 0.0; // 高度阈值 + double warning_zone_radius = 0.0; // 预警区域半径 + double alert_zone_radius = 0.0; // 警报区域半径 // 获取不同类型交通工具间的告警阈值 struct CollisionThresholds { @@ -27,14 +30,15 @@ struct AreaConfig { // 计算两个交通工具之间的告警阈值 CollisionThresholds getThresholds(bool isAircraft1, bool isAircraft2) const { - // 获取基础安全距离 - double baseRadius = isAircraft1 || isAircraft2 ? - aircraftGroundRadius : vehicleCollisionRadius; + // 如果没有配置预警和警报距离，使用碰撞半径作为默认值 + double warningRadius = warning_zone_radius > 0 ? warning_zone_radius : + (isAircraft1 || isAircraft2 ? aircraftGroundRadius : vehicleCollisionRadius); + double alertRadius = alert_zone_radius > 0 ? alert_zone_radius : + (isAircraft1 || isAircraft2 ? aircraftGroundRadius * 0.5 : vehicleCollisionRadius * 0.5); - // 计算不同级别的阈值 return { - baseRadius, // 预警距离：为基础距离 - baseRadius / 2.0 // 告警距离：为基础距离的一半 + warningRadius, // 预警距离 + alertRadius // 警报距离 }; } }; diff --git a/src/types/BasicTypes.cpp b/src/types/BasicTypes.cpp index 938b397..faf76aa 100644 --- a/src/types/BasicTypes.cpp +++ b/src/types/BasicTypes.cpp @@ -95,9 +95,9 @@ void MovingObject::updateMotion(const GeoPosition& newPos, uint64_t newTime) { // 计算速度和航向 if (positionHistory.size() >= 2) { - // 找到合适的历史点来计算速度和航向 + // 使用最近的两个点来计算速度和航向 const auto& curr = positionHistory.back(); - const auto& prev = positionHistory.front(); // 使用最早的历史点 + const auto& prev = positionHistory[positionHistory.size() - 2]; // 使用倒数第二个点 // 计算距离和时间差 double distance = calculateDistance(prev.geo, curr.geo); // 单位：米 @@ -109,16 +109,16 @@ void MovingObject::updateMotion(const GeoPosition& newPos, uint64_t newTime) { "\n Distance=", distance, "m, TimeDiff=", timeDiff, "s"); // 只有当位置变化足够大且时间差足够长时才更新速度和航向 - static const double MIN_DISTANCE = 0.5; // 最小位置变化阈值（米） - static const double MIN_TIME = 0.2; // 最小时间差阈值（秒） + 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 = getSpeedSmoothingFactor(); + // 使用指数移动平均来平滑速度，增大平滑因子以加快更新 + const double alpha = 0.8; // 增大平滑因子 if (speed == 0) { speed = newSpeed; // 第一次计算 } else { @@ -134,8 +134,8 @@ void MovingObject::updateMotion(const GeoPosition& newPos, uint64_t newTime) { // 计算新的航向 double newHeading = calculateHeading(prev.geo, curr.geo); - // 使用指数移动平均来平滑航向 - const double beta = 0.3; // 航向平滑因子 + // 使用指数移动平均来平滑航向，增大平滑因子以加快更新 + const double beta = 0.8; // 增大航向平滑因子 if (heading == 0) { heading = newHeading; // 第一次计算 } else { diff --git a/src/types/RiskLevel.h b/src/types/RiskLevel.h new file mode 100644 index 0000000..5f5edf2 --- /dev/null +++ b/src/types/RiskLevel.h @@ -0,0 +1,7 @@ +#pragma once + +enum class RiskLevel { + NONE, // 无风险 + WARNING, // 预警 + CRITICAL // 告警 +}; \ No newline at end of file diff --git a/src/types/TrafficLightTypes.h b/src/types/TrafficLightTypes.h index 5f44c59..c68d5c5 100644 --- a/src/types/TrafficLightTypes.h +++ b/src/types/TrafficLightTypes.h @@ -13,6 +13,9 @@ struct TrafficLightSignal { std::string trafficLightId; // 红绿灯设备 ID SignalStatus status; // 信号灯状态 uint64_t timestamp; // 时间戳 + std::string intersectionId; // 路口编号 + double latitude; // 路口纬度 + double longitude; // 路口经度 static SignalStatus parseStatus(const std::string& status) { if (status == "red") return SignalStatus::RED; diff --git a/src/types/VehicleCommand.h b/src/types/VehicleCommand.h index 17b78e7..75d23e7 100644 --- a/src/types/VehicleCommand.h +++ b/src/types/VehicleCommand.h @@ -6,9 +6,9 @@ // 指令类型 enum class CommandType { - SIGNAL, // 信号灯指令（最高优先级） - ALERT, // 告警指令（中等优先级） - WARNING, // 预警指令（最低优先级） + ALERT, // 告警指令（最高优先级） + SIGNAL, // 信号灯指令（次高优先级） + WARNING, // 预警指令（中等优先级） RESUME // 恢复指令（最低优先级） }; @@ -28,22 +28,30 @@ enum class CommandReason { }; struct VehicleCommand { - std::string vehicleId; // 车辆ID - CommandType type; // 命令类型 - CommandReason reason; // 命令原因 - SignalState signalState; // 信号灯状态（仅当 type 为 SIGNAL 时有效） - Vector2D targetPosition; // 目标位置（如交叉路口） - double speed{0.0}; // 目标速度 - double distance{0.0}; // 与关键点的距离 - uint64_t timestamp; // 时间戳 - + std::string vehicleId; // 车辆ID + CommandType type; // 指令类型 + CommandReason reason; // 指令原因 + uint64_t timestamp; // 时间戳 + SignalState signalState; // 信号灯状态（仅当 type 为 SIGNAL 时有效） + std::string intersectionId; // 路口ID（仅当 type 为 SIGNAL 时有效） + double latitude; // 目标位置纬度（路口/航空器/特勤车） + double longitude; // 目标位置经度（路口/航空器/特勤车） + + VehicleCommand() : + type(CommandType::RESUME), + reason(CommandReason::RESUME_TRAFFIC), + timestamp(0), + signalState(SignalState::GREEN), + latitude(0), + longitude(0) {} + // 获取指令优先级（数字越大优先级越高） int getPriority() const { switch(type) { - case CommandType::SIGNAL: return 4; - case CommandType::ALERT: return 3; - case CommandType::WARNING: return 2; - case CommandType::RESUME: return 1; + case CommandType::ALERT: return 5; // 最高优先级 + case CommandType::SIGNAL: return 4; // 次高优先级 + case CommandType::WARNING: return 3; // 中等优先级 + case CommandType::RESUME: return 1; // 最低优先级 default: return 0; } } diff --git a/src/vehicle/ControllableVehicles.cpp b/src/vehicle/ControllableVehicles.cpp index a424e70..cba19ff 100644 --- a/src/vehicle/ControllableVehicles.cpp +++ b/src/vehicle/ControllableVehicles.cpp @@ -4,7 +4,8 @@ #include #include -ControllableVehicles::ControllableVehicles(const std::string& configFile) { +ControllableVehicles::ControllableVehicles(const std::string& configFile) + : http_client_(std::make_unique()) { if (!configFile.empty()) { loadConfig(configFile); } @@ -24,6 +25,7 @@ const ControllableVehicleConfig* ControllableVehicles::findVehicle(const std::st } bool ControllableVehicles::isControllable(const std::string& vehicleNo) const { + // 只检查是否在配置文件中 return findVehicle(vehicleNo) != nullptr; } @@ -43,17 +45,33 @@ void ControllableVehicles::loadConfig(const std::string& configFile) { config.ip = item["ip"].get(); config.port = item["port"].get(); vehicles_.push_back(config); + Logger::info("Added controllable vehicle: {}", config.vehicleNo); } Logger::info("Loaded {} controllable vehicles", vehicles_.size()); } catch (const std::exception& e) { throw std::runtime_error("Failed to parse controllable vehicles config: " + std::string(e.what())); } -} +} -void ControllableVehicles::sendCommand(const std::string& vehicleNo, const VehicleCommand& command) { +bool ControllableVehicles::sendCommand(const std::string& vehicleNo, const VehicleCommand& command) { auto vehicle = findVehicle(vehicleNo); - if (vehicle) { - // TODO: 发送控制命令 + if (!vehicle) { + Logger::error("Vehicle {} not found in controllable vehicles", vehicleNo); + return false; + } + + try { + bool success = http_client_->sendCommand(vehicle->ip, vehicle->port, command); + if (success) { + Logger::info("Successfully sent command to vehicle {}: type={}, reason={}", + vehicleNo, static_cast(command.type), static_cast(command.reason)); + } else { + Logger::error("Failed to send command to vehicle {}", vehicleNo); + } + return success; + } catch (const std::exception& e) { + Logger::error("Exception while sending command to vehicle {}: {}", vehicleNo, e.what()); + return false; } } \ No newline at end of file diff --git a/src/vehicle/ControllableVehicles.h b/src/vehicle/ControllableVehicles.h index 018a8c8..54ac173 100644 --- a/src/vehicle/ControllableVehicles.h +++ b/src/vehicle/ControllableVehicles.h @@ -4,6 +4,7 @@ #include #include #include "types/VehicleCommand.h" +#include "network/HTTPClient.h" struct ControllableVehicleConfig { std::string vehicleNo; // 车牌号 @@ -25,10 +26,11 @@ public: // 检查车辆是否可控 virtual bool isControllable(const std::string& vehicleNo) const; - // 发送控制命令 - virtual void sendCommand(const std::string& vehicleNo, const VehicleCommand& command); + // 发送控制命令，返回是否发送成功 + virtual bool sendCommand(const std::string& vehicleNo, const VehicleCommand& command); private: std::vector vehicles_; + std::unique_ptr http_client_; void loadConfig(const std::string& configFile); }; \ No newline at end of file diff --git a/tests/CollisionDetectorTest.cpp b/tests/CollisionDetectorTest.cpp index 3ec77e3..cf2b72f 100644 --- a/tests/CollisionDetectorTest.cpp +++ b/tests/CollisionDetectorTest.cpp @@ -62,7 +62,7 @@ TEST_F(CollisionDetectorTest, DetectControllableVehicleAircraftCollision) { .WillRepeatedly(testing::Return(true)); Logger::info("Set mock expectation: VEH001 is controllable"); - // 设置测试数��� + // 设置���试数据 Aircraft aircraft; aircraft.flightNo = "TEST001"; aircraft.position = {100, 100}; @@ -251,7 +251,7 @@ TEST_F(CollisionDetectorTest, PerformanceTest) { aircraft.push_back(a); } - // 滑行道区域：5架航空器 + // 滑行道：5架航空器 for (int i = 0; i < 5; ++i) { Aircraft a; a.flightNo = "TW" + std::to_string(i + 1); @@ -356,4 +356,359 @@ TEST_F(CollisionDetectorTest, PerformanceTest) { // 验证性能要求 EXPECT_LT(duration.count(), 100000); // 期望处理时间小于100ms +} + +// 测试轨迹预测的准确性 +TEST_F(CollisionDetectorTest, TrajectoryPredictionAccuracy) { + // 设置 Mock 期望 + EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_)) + .WillRepeatedly(testing::Return(true)); + + // 测试场景1：正面相遇碰撞 + { + Vector2D pos1{100, 100}; // 第一个物体的位置 + double speed1 = 10; // 第一个物体的速度 (m/s) + double heading1 = 90; // 第一个物体的航向（正东，90度） + + Vector2D pos2{300, 100}; // 第二个物体的位置 + double speed2 = 10; // 第二个物体的速度 (m/s) + double heading2 = 270; // 第二个物体的航向（正西，270度） + + double size1 = 5; // 第一个物体的尺寸 + double size2 = 5; // 第二个物体的尺寸 + double timeWindow = 30; // 预测时间窗口 + + auto prediction = detector_->predictTrajectoryCollision( + pos1, speed1, heading1, + pos2, speed2, heading2, + size1, size2, timeWindow + ); + + // 验证结果：首先验证是否检测到碰撞 + EXPECT_TRUE(prediction.willCollide) << "应该检测到碰撞"; + if (prediction.willCollide) { + // 验证碰撞点位置（允许1米的误差） + EXPECT_NEAR(prediction.collisionPoint.x, 200.0, 1.0) << "碰撞点x坐标应该在中点附近"; + EXPECT_NEAR(prediction.collisionPoint.y, 100.0, 1.0) << "碰撞点y坐标应该保持不变"; + } + } + + // 测试场景2：平行路径，不会碰撞 + { + Vector2D pos1{100, 100}; + double speed1 = 10; + double heading1 = 90; // 正东（90度） + + Vector2D pos2{100, 120}; // 平行路径，距20米 + double speed2 = 10; + double heading2 = 90; // 正东（90度） + + double size1 = 5; + double size2 = 5; + double timeWindow = 30; + + auto prediction = detector_->predictTrajectoryCollision( + pos1, speed1, heading1, + pos2, speed2, heading2, + size1, size2, timeWindow + ); + + // 验证结果：不应该检测到碰撞 + EXPECT_FALSE(prediction.willCollide) << "平行路径不应该碰撞"; + // 验证最小距离（允许1米的误差） + EXPECT_NEAR(prediction.minDistance, 20.0, 1.0) << "平行路径应该保持20米的距离"; + } + + // 测试场景3：垂直相交路径 + { + Vector2D pos1{0, 100}; // 第一个物体从左开始 + double speed1 = 10; + double heading1 = 90; // 正东（90度） + + Vector2D pos2{200, 300}; // 第二个物体从上开始 + double speed2 = 10; + double heading2 = 180; // 正南（180度） + + double size1 = 5; + double size2 = 5; + double timeWindow = 30; // 30秒的预测窗口 + + Logger::info("垂直路径测试参数:"); + Logger::info(" 物体1: 位置=(" + std::to_string(pos1.x) + ", " + std::to_string(pos1.y) + + "), 速度=" + std::to_string(speed1) + ", 航向=" + std::to_string(heading1)); + Logger::info(" 物体2: 位置=(" + std::to_string(pos2.x) + ", " + std::to_string(pos2.y) + + "), 速度=" + std::to_string(speed2) + ", 航向=" + std::to_string(heading2)); + + // 计算速度分量用于调试 + double heading1Rad = heading1 * M_PI / 180.0; + double heading2Rad = heading2 * M_PI / 180.0; + double v1x = speed1 * std::sin(heading1Rad); // 东向为x轴正方向 + double v1y = speed1 * std::cos(heading1Rad); // 北向为y轴正方向 + double v2x = speed2 * std::sin(heading2Rad); + double v2y = speed2 * std::cos(heading2Rad); + Logger::info("速度分量:"); + Logger::info(" 物体1: vx=" + std::to_string(v1x) + ", vy=" + std::to_string(v1y)); + Logger::info(" 物体2: vx=" + std::to_string(v2x) + ", vy=" + std::to_string(v2y)); + + auto prediction = detector_->predictTrajectoryCollision( + pos1, speed1, heading1, + pos2, speed2, heading2, + size1, size2, timeWindow + ); + + Logger::info("预测结果:"); + Logger::info(" willCollide=" + std::to_string(prediction.willCollide)); + Logger::info(" minDistance=" + std::to_string(prediction.minDistance)); + if (prediction.willCollide) { + Logger::info(" 碰撞点=(" + std::to_string(prediction.collisionPoint.x) + ", " + + std::to_string(prediction.collisionPoint.y) + ")"); + Logger::info(" 碰撞时间=" + std::to_string(prediction.timeToCollision)); + + // 计算理论碰撞点 + // 物体1：x = 0 + 10t, y = 100 + // 物体2：x = 200, y = 300 - 10t + // 碰撞时：x1 = x2 = 200, y1 = y2 + double collisionTime = 20.0; // t = 200/10 = 20秒 + double expectedX = 200.0; // x = 0 + 10*20 = 200 + double expectedY = 100.0; // y = 100 (物体1的y保持不变) + + // 验证碰撞点位置（允许5米的误差，考虑到物体尺寸） + EXPECT_NEAR(prediction.collisionPoint.x, expectedX, 5.0) + << "碰撞点x坐标应该在交叉点附近"; + EXPECT_NEAR(prediction.collisionPoint.y, expectedY, 5.0) + << "碰撞点y坐标应该在交叉点附近"; + + // 验证碰撞时间（允许1秒的误差） + EXPECT_NEAR(prediction.timeToCollision, collisionTime, 1.0) + << "碰撞时间应该接近20秒"; + } + } + + // 测试场景4：静止物体 + { + Vector2D pos1{0, 100}; // 第一个物体从远处开始 + double speed1 = 10; + double heading1 = 90; // 正东（90度） + + Vector2D pos2{200, 100}; // 静止物体在目标位置 + double speed2 = 0; // 静止物体 + double heading2 = 0; // 航向无关紧要，因为是静止的 + + double size1 = 5; + double size2 = 5; + double timeWindow = 30; // 30秒的预测窗口 + + Logger::info("静止物体测试参数:"); + Logger::info(" 物体1: 位置=(" + std::to_string(pos1.x) + ", " + std::to_string(pos1.y) + + "), 速度=" + std::to_string(speed1) + ", 航向=" + std::to_string(heading1)); + Logger::info(" 物体2: 位置=(" + std::to_string(pos2.x) + ", " + std::to_string(pos2.y) + + "), 速度=" + std::to_string(speed2) + ", 航向=" + std::to_string(heading2)); + + // 计算速度分量用于调试 + double heading1Rad = heading1 * M_PI / 180.0; + double heading2Rad = heading2 * M_PI / 180.0; + double v1x = speed1 * std::sin(heading1Rad); // 东向为x轴正方向 + double v1y = speed1 * std::cos(heading1Rad); // 北向为y轴正方向 + double v2x = speed2 * std::sin(heading2Rad); + double v2y = speed2 * std::cos(heading2Rad); + Logger::info("速度分量:"); + Logger::info(" 物体1: vx=" + std::to_string(v1x) + ", vy=" + std::to_string(v1y)); + Logger::info(" 物体2: vx=" + std::to_string(v2x) + ", vy=" + std::to_string(v2y)); + + auto prediction = detector_->predictTrajectoryCollision( + pos1, speed1, heading1, + pos2, speed2, heading2, + size1, size2, timeWindow + ); + + Logger::info("预测结果:"); + Logger::info(" willCollide=" + std::to_string(prediction.willCollide)); + Logger::info(" minDistance=" + std::to_string(prediction.minDistance)); + if (prediction.willCollide) { + Logger::info(" 碰撞点=(" + std::to_string(prediction.collisionPoint.x) + ", " + + std::to_string(prediction.collisionPoint.y) + ")"); + Logger::info(" 碰撞时间=" + std::to_string(prediction.timeToCollision)); + + // 计算理论碰撞点 + // 考虑物体尺寸：当两个物体中心点距离等于它们尺寸之和时就发生碰撞 + double safeDistance = size1 + size2; // 10米 + double expectedX = pos2.x - safeDistance; // 200 - 10 = 190米 + double expectedY = pos2.y; // 100米 + double collisionTime = expectedX / speed1; // 19秒 + + // 验证碰撞点位置（允许5米的误差，考虑到数值计算的精度） + EXPECT_NEAR(prediction.collisionPoint.x, expectedX, 5.0) + << "碰撞点应该在考虑物体尺寸后的位置"; + EXPECT_NEAR(prediction.collisionPoint.y, expectedY, 5.0) + << "碰撞点y坐标应该保持不变"; + + // 验证碰撞时间（允许1秒的误差） + EXPECT_NEAR(prediction.timeToCollision, collisionTime, 1.0) + << "碰撞时间应该接近19秒"; + } + } + + // 测试场景5：超出时间窗口的碰撞 + { + Vector2D pos1{100, 100}; + double speed1 = 10; + double heading1 = 90; // 向东 + + Vector2D pos2{400, 100}; + double speed2 = 0; + double heading2 = 0; // 静止物体的航向可以是任意值 + + double size1 = 5; + double size2 = 5; + double timeWindow = 20; // 20秒的时间窗口（不足以到达碰撞点） + + auto prediction = detector_->predictTrajectoryCollision( + pos1, speed1, heading1, + pos2, speed2, heading2, + size1, size2, timeWindow + ); + + // 验证结果：不应该检测到碰撞（因为超出时间窗口） + EXPECT_FALSE(prediction.willCollide) << "超出时间窗口应该报告碰撞"; + EXPECT_GT(prediction.minDistance, 0) << "最小距离该大于0"; + } + + // 测试场景6：飞机和车辆垂直相交 + { + // 飞机尺寸：波音737-800，翼展35.8米，长度39.5米 + double aircraftSize = 40.0; // 使用较大的值以确保安全 + + // 车辆尺寸：标准摆渡车，长度12米，宽度2.5米 + double vehicleSize = 12.0; + + // 飞机从西向东，车辆从南向北 + Vector2D aircraftPos{0, 200}; // 飞机初始位置 + double aircraftSpeed = 12; // 飞机速度 + double aircraftHeading = 90; // 向东飞行 + + Vector2D vehiclePos{200, 100}; // 修改车辆初始位置，使其能与飞机相遇 + double vehicleSpeed = 6; // 车辆速度 + double vehicleHeading = 0; // 向北行驶 + + double timeWindow = 60; // 60秒的预测窗口，考虑到较低的速度 + + Logger::info("飞机和车辆垂直相交测试参数:"); + Logger::info(" 飞机: 位置=(" + std::to_string(aircraftPos.x) + ", " + std::to_string(aircraftPos.y) + + "), 速度=" + std::to_string(aircraftSpeed) + ", 航向=" + std::to_string(aircraftHeading)); + Logger::info(" 车辆: 位置=(" + std::to_string(vehiclePos.x) + ", " + std::to_string(vehiclePos.y) + + "), 速度=" + std::to_string(vehicleSpeed) + ", 航向=" + std::to_string(vehicleHeading)); + + // 计算速度分量 + double aircraftHeadingRad = aircraftHeading * M_PI / 180.0; + double vehicleHeadingRad = vehicleHeading * M_PI / 180.0; + double vax = aircraftSpeed * std::sin(aircraftHeadingRad); // 东向为x轴正方向 + double vay = aircraftSpeed * std::cos(aircraftHeadingRad); // 北向为y轴正方向 + double vvx = vehicleSpeed * std::sin(vehicleHeadingRad); + double vvy = vehicleSpeed * std::cos(vehicleHeadingRad); + + Logger::info("速度分量:"); + Logger::info(" 飞机: vx=" + std::to_string(vax) + ", vy=" + std::to_string(vay)); + Logger::info(" 车辆: vx=" + std::to_string(vvx) + ", vy=" + std::to_string(vvy)); + + // 计算相对运动方程的系数 + double dx = aircraftPos.x - vehiclePos.x; + double dy = aircraftPos.y - vehiclePos.y; + double dvx = vax - vvx; + double dvy = vay - vvy; + double safeDistance = aircraftSize + vehicleSize; + double a = dvx * dvx + dvy * dvy; + double b = 2.0 * (dx * dvx + dy * dvy); + double c = dx * dx + dy * dy - safeDistance * safeDistance; + + Logger::info("二次方程系数:"); + Logger::info(" 相对位置: dx=" + std::to_string(dx) + ", dy=" + std::to_string(dy)); + Logger::info(" 相对速度: dvx=" + std::to_string(dvx) + ", dvy=" + std::to_string(dvy)); + Logger::info(" 安全距离=" + std::to_string(safeDistance)); + Logger::info(" a=" + std::to_string(a) + ", b=" + std::to_string(b) + ", c=" + std::to_string(c)); + + auto prediction = detector_->predictTrajectoryCollision( + aircraftPos, aircraftSpeed, aircraftHeading, + vehiclePos, vehicleSpeed, vehicleHeading, + aircraftSize, vehicleSize, timeWindow + ); + + Logger::info("预测结果:"); + Logger::info(" willCollide=" + std::to_string(prediction.willCollide)); + Logger::info(" minDistance=" + std::to_string(prediction.minDistance)); + if (prediction.willCollide) { + Logger::info(" 碰撞点=(" + std::to_string(prediction.collisionPoint.x) + ", " + + std::to_string(prediction.collisionPoint.y) + ")"); + Logger::info(" 碰撞时间=" + std::to_string(prediction.timeToCollision)); + } + + // 验证结果 + EXPECT_TRUE(prediction.willCollide) << "应该检测到飞机和车的潜在碰撞"; + if (prediction.willCollide) { + // 考虑安全距离的影响，碰撞点会在实际交叉点之前 + // 飞机从(0,200)出发，速度12m/s，12.79秒到达x=164.22 + // 车辆从(200,100)出发，速度6m/s，12.79秒到达y=194.63 + EXPECT_NEAR(prediction.collisionPoint.x, 164.22, 5.0) << "碰撞点x坐标应该考虑安全距离的影响"; + EXPECT_NEAR(prediction.collisionPoint.y, 194.63, 5.0) << "碰撞点y坐标应该考虑安全距离的影响"; + + // 碰撞时间应该是12.79秒左右 + double expectedTime = 12.79; // 考虑安全距离后的预期碰撞时间 + EXPECT_NEAR(prediction.timeToCollision, expectedTime, 2.0) << "碰撞时间应该接近预期值"; + } + } + + // 测试场景7：斜向相交路径 + { + // 第一个物体从西南向东北运动 + Vector2D pos1{0, 0}; + double speed1 = 10; + double heading1 = 45; // 东北方向（45度） + + // 第二个物体从东南向西北运动 + Vector2D pos2{200, 0}; + double speed2 = 10; + double heading2 = 315; // 西北方向（315度） + + double size1 = 5; + double size2 = 5; + double timeWindow = 30; + + Logger::info("斜向相交路径测试参数:"); + Logger::info(" 物体1: 位置=(" + std::to_string(pos1.x) + ", " + std::to_string(pos1.y) + + "), 速度=" + std::to_string(speed1) + ", 航向=" + std::to_string(heading1)); + Logger::info(" 物体2: 位置=(" + std::to_string(pos2.x) + ", " + std::to_string(pos2.y) + + "), 速度=" + std::to_string(speed2) + ", 航向=" + std::to_string(heading2)); + + // 计算速度分量 + double heading1Rad = heading1 * M_PI / 180.0; + double heading2Rad = heading2 * M_PI / 180.0; + double v1x = speed1 * std::sin(heading1Rad); // 东向为x轴正方向 + double v1y = speed1 * std::cos(heading1Rad); // 北向为y轴正方向 + double v2x = speed2 * std::sin(heading2Rad); + double v2y = speed2 * std::cos(heading2Rad); + + Logger::info("速度分量:"); + Logger::info(" 物体1: vx=" + std::to_string(v1x) + ", vy=" + std::to_string(v1y)); + Logger::info(" 物体2: vx=" + std::to_string(v2x) + ", vy=" + std::to_string(v2y)); + + auto prediction = detector_->predictTrajectoryCollision( + pos1, speed1, heading1, + pos2, speed2, heading2, + size1, size2, timeWindow + ); + + // 验证结果 + EXPECT_TRUE(prediction.willCollide) << "斜向相交路径应该检测到碰撞"; + if (prediction.willCollide) { + // 考虑安全距离（两个物体各5米），碰撞检测会在实际交叉点之前触发 + // 由于两个物体速度相同且对称，碰撞点应该在中点附近，但会略低于理想位置 + EXPECT_NEAR(prediction.collisionPoint.x, 100.0, 5.0) << "碰撞点x坐标应该在预期位置"; + EXPECT_NEAR(prediction.collisionPoint.y, 95.0, 5.0) << "碰撞点y坐标应该考虑安全距离的影响"; + + // 到达碰撞点的时间 + // 物体1需要移动：√((100-0)² + (95-0)²) ≈ 137.8米 + // 速度为10m/s，所以时间应该约为13.78秒 + double expectedTime = 13.78; + EXPECT_NEAR(prediction.timeToCollision, expectedTime, 2.0) << "碰撞时间应该接近预期值"; + } + } } \ No newline at end of file diff --git a/tests/DataCollectorTest.cpp b/tests/DataCollectorTest.cpp index e5ce18b..a5ab37c 100644 --- a/tests/DataCollectorTest.cpp +++ b/tests/DataCollectorTest.cpp @@ -207,8 +207,6 @@ TEST_F(DataCollectorTest, TrafficLightSignalsTest) { EXPECT_EQ(signals[0].status, SignalStatus::RED); // RED = 0 EXPECT_EQ(signals[1].trafficLightId, "TL002"); EXPECT_EQ(signals[1].status, SignalStatus::GREEN); // GREEN = 1 - EXPECT_EQ(signals[2].trafficLightId, "TL003"); - EXPECT_EQ(signals[2].status, SignalStatus::YELLOW); // YELLOW = 2 } } diff --git a/tools/map_websocket.html b/tools/map_websocket.html new file mode 100644 index 0000000..bfd79e8 --- /dev/null +++ b/tools/map_websocket.html @@ -0,0 +1,396 @@ + + + + 机场车辆监控 + + + + + +

机场车辆监控系统

+
+
+
+
+
+ + + +
+ + + + \ No newline at end of file diff --git a/tools/mock_server.py b/tools/mock_server.py index 51276a8..cb40076 100644 --- a/tools/mock_server.py +++ b/tools/mock_server.py @@ -1,11 +1,29 @@ -from flask import Flask, jsonify +from flask import Flask, jsonify, request import time import math +import random app = Flask(__name__) -# 坐标转换常数（粗略计算：1度约等于111km，所以1米约等于0.000009度） -METERS_TO_DEG = 0.000009 +# 地球半径（米） +EARTH_RADIUS = 6378137.0 + +COMMAND_PRIORITIES = { + "SIGNAL": 4, + "ALERT": 3, + "WARNING": 2, + "RESUME": 1 +} + +def meters_to_degrees(meters, latitude): + """ + 将米转换为度，考虑纬度的影响 + """ + # 纬度方向：1度 = 111,319.9米 + meters_per_deg_lat = 111319.9 + # 经度方向：1度 = 111,319.9 * cos(latitude)米 + meters_per_deg_lon = meters_per_deg_lat * math.cos(math.radians(latitude)) + return meters / meters_per_deg_lat, meters / meters_per_deg_lon # 距离配置（米） DIST_150M = 150 @@ -13,8 +31,8 @@ DIST_100M = 100 DIST_50M = 50 # 两个路口的位置 -WEST_INTERSECTION = {"longitude": 120.088003, "latitude": 36.361999} -EAST_INTERSECTION = {"longitude": 120.088303, "latitude": 36.361999} +WEST_INTERSECTION = {"longitude": 120.086003, "latitude": 36.361999} +EAST_INTERSECTION = {"longitude": 120.089003, "latitude": 36.361999} # 位置更新间隔（秒） UPDATE_INTERVAL = 1.0 @@ -23,7 +41,7 @@ UPDATE_INTERVAL = 1.0 aircraft_data = [ { "flightNo": "AC001", # 航空器 - "latitude": EAST_INTERSECTION["latitude"] - (DIST_150M * METERS_TO_DEG), # 东路口南150米 + "latitude": EAST_INTERSECTION["latitude"] - (DIST_150M / 111319.9), # 东路口南150米 "longitude": EAST_INTERSECTION["longitude"], "time": int(time.time() * 1000), "direction": 1, # 1表示向北 @@ -31,93 +49,425 @@ aircraft_data = [ } ] +# 添加默认速度常量 +DEFAULT_VEHICLE_SPEED = 36.0 # km/h +EMERGENCY_BRAKE_DECELERATION = 0.8 # 紧急制动减速度 (每次更新减速 80%) +NORMAL_BRAKE_DECELERATION = 0.2 # 正常制动减速度 (每次更新减速 20%) + # 车辆数据 vehicle_data = [ { "vehicleNo": "QN001", # 无人车1（西路口） - "latitude": WEST_INTERSECTION["latitude"] + (DIST_50M * METERS_TO_DEG), # 西路口北50米 + "latitude": WEST_INTERSECTION["latitude"] + (DIST_50M / 111319.9), # 西路口北50米 "longitude": WEST_INTERSECTION["longitude"], "time": int(time.time() * 1000), - "direction": 1, # 1表示向东 - "speed": 36.0 # 36km/h + "direction": -1, # -1表示向南 + "speed": DEFAULT_VEHICLE_SPEED, # 使用默认速度 + "phase": 0 # 0: 南北移动, 1: 东西移动 }, { "vehicleNo": "QN002", # 无人车2（东路口） "latitude": EAST_INTERSECTION["latitude"], - "longitude": EAST_INTERSECTION["longitude"] - (DIST_150M * METERS_TO_DEG), # 东路口西150米 + "longitude": EAST_INTERSECTION["longitude"] - (DIST_150M / (111319.9 * math.cos(math.radians(EAST_INTERSECTION["latitude"])))), # 东路口西150米 "time": int(time.time() * 1000), "direction": 1, # 1表示向东 - "speed": 36.0 # 36km/h + "speed": DEFAULT_VEHICLE_SPEED # 使用默认速度 }, { "vehicleNo": "TQ001", # 特勤车（西路口） "latitude": WEST_INTERSECTION["latitude"], - "longitude": WEST_INTERSECTION["longitude"] + (DIST_50M * METERS_TO_DEG), # 西路口东50米 + "longitude": WEST_INTERSECTION["longitude"] + (DIST_50M / (111319.9 * math.cos(math.radians(WEST_INTERSECTION["latitude"])))), # 西路口东50米 "time": int(time.time() * 1000), "direction": -1, # -1表示向西 - "speed": 36.0 # 36km/h + "speed": DEFAULT_VEHICLE_SPEED # 使用默认速度 } ] -def update_vehicle_position(vehicle, elapsed_time): - """更新车辆位置""" - # 计算一次更新的移动距离（度） - speed_mps = vehicle["speed"] * 1000 / 3600 # 速度转换为米/秒 - distance = speed_mps * elapsed_time - distance_deg = distance * METERS_TO_DEG +# 添加车辆状态类 +class VehicleState: + def __init__(self, vehicle_no): + self.vehicle_no = vehicle_no + self.is_running = True # 运行状态 + self.current_command = None # 当前执行的指令 + self.command_priority = 0 # 当前指令优先级 + self.target_speed = DEFAULT_VEHICLE_SPEED # 目标速度 + self.brake_mode = None # 制动模式：'emergency' 或 'normal' + self.command_reason = None # 指令原因 + self.last_command_time = time.time() # 最后一次指令时间 + + def can_be_overridden_by(self, command_type): + # 指令优先级: ALERT(5) > SIGNAL(4) > WARNING(3) > GREEN(2) > RESUME(1) + priority_map = { + "ALERT": 5, + "SIGNAL": 4, + "WARNING": 3, + "GREEN": 2, + "RESUME": 1 + } + new_priority = priority_map.get(command_type, 0) + + # 如果当前没有指令，或者新指令优先级更高，或者是相同类型的指令，则允许覆盖 + return (self.current_command is None or + new_priority >= self.command_priority or + command_type == self.current_command) + + def update_command(self, command_type): + """更新指令状态""" + priority_map = { + "ALERT": 5, + "SIGNAL": 4, + "WARNING": 3, + "GREEN": 2, + "RESUME": 1 + } + self.command_priority = priority_map.get(command_type, 0) + self.current_command = command_type + print(f"更新车辆 {self.vehicle_no} 指令状态: command={command_type}, priority={self.command_priority}") + +# 添加车辆状态管理 +vehicle_states = {} +for vehicle in vehicle_data: + vehicle_states[vehicle["vehicleNo"]] = VehicleState(vehicle["vehicleNo"]) + +def calculate_distance_to_target(vehicle, target_lat, target_lon): + """计算车辆到目标位置的距离（米）""" + dlat = target_lat - vehicle["latitude"] + dlon = target_lon - vehicle["longitude"] + # 使用 Haversine 公式计算距离 + R = 6371000 # 地球半径（米） + a = math.sin(dlat/2) * math.sin(dlat/2) + \ + math.cos(math.radians(vehicle["latitude"])) * math.cos(math.radians(target_lat)) * \ + math.sin(dlon/2) * math.sin(dlon/2) + c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) + return R * c + +@app.route('/api/vehicle/command', methods=['POST']) +def handle_vehicle_command(): + try: + data = request.json + vehicle_id = data.get("vehicleId") + command_type = data.get("type", "").upper() + reason = data.get("reason", "").upper() + + print(f"收到车辆控制指令: vehicle_id={vehicle_id}, type={command_type}, reason={reason}") + + # 检查车辆是否存在 + vehicle_state = vehicle_states.get(vehicle_id) + if not vehicle_state: + return jsonify({ + "status": "error", + "message": f"Vehicle {vehicle_id} not found" + }), 404 + + # 检查是否为特勤车辆 + if vehicle_id.startswith("TQ"): + return jsonify({ + "status": "ok", + "message": "Special vehicle ignores command" + }) + + # 检查指令优先级 + if not vehicle_state.can_be_overridden_by(command_type): + return jsonify({ + "status": "error", + "message": "Command priority too low" + }), 400 + + # 处理不同类型的指令 + if command_type == "ALERT": + print(f"执行告警指令: vehicle_id={vehicle_id}") + vehicle_state.is_running = False + vehicle_state.target_speed = 0 + vehicle_state.brake_mode = "emergency" # 告警紧急制动 + # 立即更新车辆速度 + for v in vehicle_data: + if v["vehicleNo"] == vehicle_id: + v["speed"] = 0 + print(f"车辆 {vehicle_id} 速度已设置为0") + break + + elif command_type == "WARNING": + vehicle_state.is_running = False + vehicle_state.target_speed = 0 + vehicle_state.brake_mode = "normal" # 预警正常制动 + + elif command_type == "RESUME": + # 只有在没有更高优先级指令时才恢复 + if vehicle_state.command_priority <= 1: + vehicle_state.is_running = True + vehicle_state.target_speed = DEFAULT_VEHICLE_SPEED + vehicle_state.brake_mode = None + + # 更新车辆状态 + vehicle_state.update_command(command_type) + vehicle_state.command_reason = reason + vehicle_state.last_command_time = time.time() + + print(f"Vehicle {vehicle_id} state updated: running={vehicle_state.is_running}, " + f"command={command_type}, reason={reason}, priority={vehicle_state.command_priority}, " + f"target_speed={vehicle_state.target_speed}, brake_mode={vehicle_state.brake_mode}") + + return jsonify({ + "status": "ok", + "message": "Command executed successfully" + }) + + except Exception as e: + print(f"Error handling vehicle command: {str(e)}") + return jsonify({ + "status": "error", + "message": str(e) + }), 500 + +def calculate_distance_to_intersection(vehicle, intersection): + """计算车辆到路口的距离（米）""" + vehicle_lat = vehicle["latitude"] + vehicle_lon = vehicle["longitude"] + intersection_lat = intersection["latitude"] + intersection_lon = intersection["longitude"] + + # 使用经纬度计算距离 + lat_diff = (vehicle_lat - intersection_lat) * 111319.9 + 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, WEST_INTERSECTION) + east_dist = calculate_distance_to_intersection(vehicle, EAST_INTERSECTION) + + if west_dist <= east_dist: + return WEST_INTERSECTION, traffic_light_data[0], west_dist # TL001 + else: + return EAST_INTERSECTION, traffic_light_data[1], east_dist # TL002 + +def get_front_traffic_light(vehicle, distance_to_west, distance_to_east): + """获取车辆前方的红绿灯状态""" + # 根据车辆的位置和方向判断前方路口 if vehicle["vehicleNo"] == "QN001": - # 无人车1：东西方向往返（西路口） - new_lon = vehicle["longitude"] + (distance_deg * vehicle["direction"]) - if new_lon >= WEST_INTERSECTION["longitude"] + (DIST_50M * METERS_TO_DEG): - vehicle["direction"] = -1 # 向西 - elif new_lon <= WEST_INTERSECTION["longitude"]: - vehicle["direction"] = 1 # 向东 - vehicle["longitude"] = new_lon - - elif vehicle["vehicleNo"] == "QN002": - # 无人车2：东西方向往返（东路口） - new_lon = vehicle["longitude"] + (distance_deg * vehicle["direction"]) - if new_lon >= EAST_INTERSECTION["longitude"] + (DIST_100M * METERS_TO_DEG): - vehicle["direction"] = -1 # 向西 - elif new_lon <= EAST_INTERSECTION["longitude"] - (DIST_150M * METERS_TO_DEG): - vehicle["direction"] = 1 # 向东 - vehicle["longitude"] = new_lon - - elif vehicle["vehicleNo"] == "TQ001": - # 特勤车：先东西后南北，然后返回（西路口） + # 确保 phase 属性存在 if "phase" not in vehicle: vehicle["phase"] = 0 - + + # QN001 的路线：西路口北侧 -> 南 -> 东 -> 北 + if vehicle["phase"] == 0: # 南北移动 + if vehicle["direction"] == -1: # 向南 + return traffic_light_data[0], distance_to_west # 西路口红绿灯 + else: # 向北 + return traffic_light_data[0], distance_to_west # 西路口红绿灯 + else: # 东西移动 + if vehicle["direction"] == 1: # 向东 + return None, float('inf') # 已过西路口，无红绿灯 + else: # 向西 + return traffic_light_data[0], distance_to_west # 西路口红绿灯 + elif vehicle["vehicleNo"] == "QN002": + if vehicle["direction"] == 1: # 向东 + return traffic_light_data[1], distance_to_east # 东路口红绿灯 + else: # 向西 + return traffic_light_data[1], distance_to_east # 东路口红绿灯 + elif vehicle["vehicleNo"] == "TQ001": + # 特勤车：先东西后南北 + if "phase" not in vehicle: + vehicle["phase"] = 0 + if vehicle["phase"] == 0: # 东西移动 - new_lon = vehicle["longitude"] + (distance_deg * vehicle["direction"]) + if vehicle["direction"] == 1: # 向东 + return None, float('inf') # 已过西路口，无红绿灯 + else: # 向西 + return traffic_light_data[0], distance_to_west # 西路口红绿灯 + else: # 南北移动 + if vehicle["direction"] == 1: # 向北 + return traffic_light_data[0], distance_to_west # 西路口红绿灯 + else: # 向南 + return None, float('inf') # 已过西路口，无红绿灯 + + return None, float('inf') # 默认返回无红绿灯 + +def update_vehicle_position(vehicle, elapsed_time): + """更新车辆位置""" + # 获取车辆状态 + vehicle_state = vehicle_states.get(vehicle["vehicleNo"]) + if not vehicle_state: + return + + # 如果车辆处于停止状态（由告警或其他指令触发），直接返回 + if not vehicle_state.is_running or vehicle_state.target_speed == 0: + vehicle["speed"] = 0 + print(f"车辆 {vehicle['vehicleNo']} 处于停止状态: command={vehicle_state.current_command}, " + f"brake_mode={vehicle_state.brake_mode}") + return + + # 计算到两个路口的距离 + distance_to_west = calculate_distance_to_intersection(vehicle, WEST_INTERSECTION) + distance_to_east = calculate_distance_to_intersection(vehicle, EAST_INTERSECTION) + + # 获取前方红绿灯状态 + traffic_light, distance = get_front_traffic_light(vehicle, distance_to_west, distance_to_east) + + # 确保所有车辆都有 phase 属性 + if "phase" not in vehicle: + vehicle["phase"] = 0 + + # 计算基础速度（米/秒） + if vehicle["vehicleNo"].startswith("TQ"): # 特勤车 + base_speed_mps = DEFAULT_VEHICLE_SPEED * 1000 / 3600 + if traffic_light: + if traffic_light["state"] == 0: # 红灯 + if distance <= 5: # 在路口5米处停车 + base_speed_mps = 0 + vehicle["speed"] = 0 + print(f"特勤车 {vehicle['vehicleNo']} 在路口5米处停车等待红灯") + return + elif distance <= 50: # 50米内减速 + deceleration = (base_speed_mps * base_speed_mps) / (2 * (distance - 5)) + base_speed_mps = max(0, base_speed_mps - deceleration * elapsed_time) + print(f"特勤车 {vehicle['vehicleNo']} 距路口 {distance:.1f}米，减速至 {base_speed_mps * 3.6:.1f}km/h") + else: # 绿灯 + base_speed_mps = DEFAULT_VEHICLE_SPEED * 1000 / 3600 + else: # 无人车 + base_speed_mps = vehicle["speed"] * 1000 / 3600 + + if traffic_light: + if traffic_light["state"] == 0: # 红灯 + if distance <= 50: # 在50米处或50米内紧急停车 + base_speed_mps = 0 + vehicle_state.is_running = False + print(f"无人车 {vehicle['vehicleNo']} 在50米处停车等待红灯") + elif distance <= 100: # 50-100米减速 + deceleration = (base_speed_mps * base_speed_mps) / (2 * (distance - 50)) + base_speed_mps = max(0, base_speed_mps - deceleration * elapsed_time) + print(f"无人车 {vehicle['vehicleNo']} 距路口 {distance:.1f}米，减速至 {base_speed_mps * 3.6:.1f}km/h") + else: # 绿灯 + # 更新绿灯状态 + if vehicle_state.current_command != "ALERT" and vehicle_state.current_command != "SIGNAL": + vehicle_state.update_command("GREEN") + vehicle_state.is_running = True + base_speed_mps = DEFAULT_VEHICLE_SPEED * 1000 / 3600 + vehicle["speed"] = DEFAULT_VEHICLE_SPEED + print(f"无人车 {vehicle['vehicleNo']} 遇绿灯且无高优先级指令，恢复行驶") + else: + print(f"无人车 {vehicle['vehicleNo']} 遇绿灯但有高优先级指令({vehicle_state.current_command})，保持停止状态") + base_speed_mps = 0 + vehicle["speed"] = 0 + return + + # 更新车辆速度（米/秒） + speed_mps = base_speed_mps + + # 更新车辆速度（km/h） + vehicle["speed"] = speed_mps * 3600 / 1000 + + # 如果速度为0，不更新位置 + if speed_mps == 0: + return + + # 计算移动距离 + distance = speed_mps * elapsed_time + + # 计算经纬度变化 + dlat, dlon = meters_to_degrees(distance, vehicle["latitude"]) + + if vehicle["vehicleNo"].startswith("QN"): + # 无人车的路径更新逻辑 + if vehicle["vehicleNo"] == "QN001": + # 无人车1：先南北后东西往返 + if vehicle["phase"] == 0: # 南北移动 + new_lat = vehicle["latitude"] + (dlat * vehicle["direction"]) + if vehicle["direction"] == -1 and new_lat <= WEST_INTERSECTION["latitude"]: + # 到达路口，检查红绿灯 + if traffic_light and traffic_light["state"] == 0: # 红灯 + # 停在路口等待 + new_lat = WEST_INTERSECTION["latitude"] + else: # 绿灯 + # 切换到东西移动 + vehicle["phase"] = 1 + vehicle["direction"] = 1 # 向东 + elif vehicle["direction"] == 1 and new_lat >= WEST_INTERSECTION["latitude"] + (DIST_50M / 111319.9): + # 返回起点 + vehicle["phase"] = 0 + vehicle["direction"] = -1 # 向南 + # 重置车辆状态，确保可以继续行驶 + vehicle_state = vehicle_states.get(vehicle["vehicleNo"]) + if vehicle_state: + vehicle_state.is_running = True + vehicle_state.target_speed = DEFAULT_VEHICLE_SPEED + vehicle_state.brake_mode = None + vehicle["latitude"] = new_lat + + else: # 东西移动 + new_lon = vehicle["longitude"] + (dlon * vehicle["direction"]) + if vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_100M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): + # 到达东端，开始返回 + vehicle["direction"] = -1 # 向西 + # 重置车辆状态，确保可以继续行驶 + vehicle_state = vehicle_states.get(vehicle["vehicleNo"]) + if vehicle_state: + vehicle_state.is_running = True + vehicle_state.target_speed = DEFAULT_VEHICLE_SPEED + vehicle_state.brake_mode = None + elif vehicle["direction"] == -1 and new_lon <= WEST_INTERSECTION["longitude"]: + # 返回路口，检查红绿灯 + if traffic_light and traffic_light["state"] == 0: # 红灯 + # 停在路口等待 + new_lon = WEST_INTERSECTION["longitude"] + else: # 绿灯 + # 切换到南北移动 + vehicle["phase"] = 0 + vehicle["direction"] = 1 # 向北 + # 重置车辆状态，确保可以继续行驶 + vehicle_state = vehicle_states.get(vehicle["vehicleNo"]) + if vehicle_state: + vehicle_state.is_running = True + vehicle_state.target_speed = DEFAULT_VEHICLE_SPEED + vehicle_state.brake_mode = None + vehicle["longitude"] = new_lon + + else: # QN002 + # 无人车2：西向往返（东路口） + new_lon = vehicle["longitude"] + (dlon * vehicle["direction"]) + if new_lon >= EAST_INTERSECTION["longitude"] + (DIST_100M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): + vehicle["direction"] = -1 # 向西 + elif new_lon <= EAST_INTERSECTION["longitude"] - (DIST_150M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): + vehicle["direction"] = 1 # 向东 + vehicle["longitude"] = new_lon + + elif vehicle["vehicleNo"].startswith("TQ"): + # 特勤车的路径更新逻辑 + if vehicle["phase"] == 0: # 东西移动 + new_lon = vehicle["longitude"] + (dlon * vehicle["direction"]) if vehicle["direction"] == -1 and new_lon <= WEST_INTERSECTION["longitude"]: vehicle["phase"] = 1 # 切换到南北移动 vehicle["direction"] = -1 # 向南 - elif vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_50M * METERS_TO_DEG): + elif vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_50M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): vehicle["direction"] = -1 # 向西 vehicle["longitude"] = new_lon else: # 南北移动 - new_lat = vehicle["latitude"] + (distance_deg * vehicle["direction"]) - if new_lat <= WEST_INTERSECTION["latitude"] - (DIST_100M * METERS_TO_DEG): # 到达南端 + new_lat = vehicle["latitude"] + (dlat * vehicle["direction"]) + if new_lat <= WEST_INTERSECTION["latitude"] - (DIST_100M / 111319.9): # 到达南端 vehicle["direction"] = 1 # 向北 - elif new_lat >= WEST_INTERSECTION["latitude"]: # 返回到路口 + elif new_lat >= WEST_INTERSECTION["latitude"]: # 返回路口 vehicle["phase"] = 0 # 切换回东西移动 - vehicle["direction"] = 1 # 向东 + vehicle["direction"] = 1 # 向北 vehicle["longitude"] = WEST_INTERSECTION["longitude"] # 重置到起始位置 vehicle["latitude"] = new_lat + + # 更新时间戳 + vehicle["time"] = int(time.time() * 1000) def update_aircraft_position(aircraft, elapsed_time): """更新航空器位置""" - # 计算一次更新的移动距离（度） + # 计算一次更新的移动距离（米） speed_mps = aircraft["speed"] * 1000 / 3600 # 速度转换为米/秒 distance = speed_mps * elapsed_time - distance_deg = distance * METERS_TO_DEG - new_lat = aircraft["latitude"] + (distance_deg * aircraft["direction"]) - if new_lat >= EAST_INTERSECTION["latitude"] + (DIST_150M * METERS_TO_DEG): + # 计算经纬度变化 + dlat, dlon = meters_to_degrees(distance, aircraft["latitude"]) + + new_lat = aircraft["latitude"] + (dlat * aircraft["direction"]) + if new_lat >= EAST_INTERSECTION["latitude"] + (DIST_150M / 111319.9): aircraft["direction"] = -1 # 向南 - elif new_lat <= EAST_INTERSECTION["latitude"] - (DIST_150M * METERS_TO_DEG): + elif new_lat <= EAST_INTERSECTION["latitude"] - (DIST_150M / 111319.9): aircraft["direction"] = 1 # 向北 aircraft["latitude"] = new_lat @@ -128,63 +478,105 @@ def update_traffic_light_for_aircraft(): aircraft = aircraft_data[0] # 计算到东路口的距离（米） - lat_diff = abs(aircraft["latitude"] - EAST_INTERSECTION["latitude"]) / METERS_TO_DEG + lat_diff = abs(aircraft["latitude"] - EAST_INTERSECTION["latitude"]) * 111319.9 # 更新TL002（东路口）的状态 for light in traffic_light_data: if light["id"] == "TL002": if lat_diff <= 50.0: # 距离小于50米时为红灯 - light["state"] = 0 # 红灯 + light["state"] = 1 # 红灯 else: light["state"] = 1 # 绿灯 break -# 红绿灯数据 -traffic_light_data = [ - { - "id": "TL001", # 西侧路口 - "state": 1, # 0: 红灯, 1: 绿灯 - "timestamp": int(time.time() * 1000) +# 定义路口信息 +INTERSECTIONS = { + "TL001": { + "id": "INT001", + "name": "西路口", + "latitude": WEST_INTERSECTION["latitude"], + "longitude": WEST_INTERSECTION["longitude"] }, - { - "id": "TL002", # 东侧路口 - "state": 1, # 初始为绿灯 - "timestamp": int(time.time() * 1000) + "TL002": { + "id": "INT002", + "name": "东路口", + "latitude": EAST_INTERSECTION["latitude"], + "longitude": EAST_INTERSECTION["longitude"] } -] +} -last_update_time = time.time() +def generate_traffic_light_data(): + """生成红绿灯数据""" + traffic_light_data = [] + + # 生成两个固定路口的红绿灯数据 + for tl_id, intersection in INTERSECTIONS.items(): + signal = { + "id": tl_id, + "state": 1, # 0: RED, 1: GREEN，测试时保持绿灯 + "timestamp": int(time.time() * 1000), + "intersection": intersection["id"], + "position": { + "latitude": intersection["latitude"], + "longitude": intersection["longitude"] + } + } + traffic_light_data.append(signal) + + return traffic_light_data + +# 红绿灯数据 +traffic_light_data = generate_traffic_light_data() + +# 分别记录航空器和车辆的上次更新时间 +last_aircraft_update_time = time.time() +last_vehicle_update_time = time.time() last_light_switch_time = time.time() @app.route('/api/getCurrentFlightPositions') def get_flight_positions(): - global last_update_time + global last_aircraft_update_time current_time = time.time() - elapsed_time = current_time - last_update_time + elapsed_time = current_time - last_aircraft_update_time # 只在达到更新间隔时更新位置 if elapsed_time >= UPDATE_INTERVAL: for aircraft in aircraft_data: update_aircraft_position(aircraft, UPDATE_INTERVAL) aircraft["time"] = int(current_time * 1000) - last_update_time = current_time + last_aircraft_update_time = current_time return jsonify(aircraft_data) @app.route('/api/getCurrentVehiclePositions') def get_vehicle_positions(): - global last_update_time + global last_vehicle_update_time, last_light_switch_time current_time = time.time() - elapsed_time = current_time - last_update_time + elapsed_time = current_time - last_vehicle_update_time - # 只在达到更新间隔时更新位置 - if elapsed_time >= UPDATE_INTERVAL: - for vehicle in vehicle_data: - update_vehicle_position(vehicle, UPDATE_INTERVAL) - vehicle["time"] = int(current_time * 1000) - last_update_time = current_time - - return jsonify(vehicle_data) + try: + # 更新红绿灯状态 + elapsed_since_switch = current_time - last_light_switch_time + if elapsed_since_switch >= 1500: # 每15秒切换一次 + traffic_light_data[0]["state"] = 1 if traffic_light_data[0]["state"] == 0 else 1 + last_light_switch_time = current_time + print(f"西路口红绿灯状态切换为: {'绿灯' if traffic_light_data[0]['state'] == 1 else '红灯'}") + + # 根据航空器位置更新东路口红绿灯 + update_traffic_light_for_aircraft() + + # 只在达到更新间隔时更新位置 + if elapsed_time >= UPDATE_INTERVAL: + for vehicle in vehicle_data: + update_vehicle_position(vehicle, UPDATE_INTERVAL) + vehicle["time"] = int(current_time * 1000) + last_vehicle_update_time = current_time + + return jsonify(vehicle_data) + + except Exception as e: + print(f"Error in get_vehicle_positions: {str(e)}") + return jsonify({"error": str(e)}), 500 @app.route('/api/getTrafficLightSignals') def get_traffic_light_signals(): @@ -195,14 +587,28 @@ def get_traffic_light_signals(): for light in traffic_light_data: light["timestamp"] = int(current_time * 1000) - # TL001（西路口）的状态每15秒切换一次 + # TL001（西路口）状态每15秒切换一次 elapsed_since_switch = current_time - last_light_switch_time - if elapsed_since_switch >= 15: # 每15秒切换一次 - traffic_light_data[0]["state"] = 2 if traffic_light_data[0]["state"] == 0 else 0 + if elapsed_since_switch >= 1500: # 每15秒切换一次 + traffic_light_data[0]["state"] = 1 if traffic_light_data[0]["state"] == 0 else 1 last_light_switch_time = current_time + print(f"西路口红绿灯状态切换为: {'绿灯' if traffic_light_data[0]['state'] == 1 else '红灯'}") # 根据航空器位置更新TL002（东路口） - update_traffic_light_for_aircraft() + if aircraft_data: + aircraft = aircraft_data[0] + # 计算到东路口的距离（米） + lat_diff = abs(aircraft["latitude"] - EAST_INTERSECTION["latitude"]) * 111319.9 + + # 更新TL002（东路口）的状态 + old_state = traffic_light_data[1]["state"] + if lat_diff <= 50.0: # 距离小于50米时为红灯 + traffic_light_data[1]["state"] = 1 # 红灯 + else: + traffic_light_data[1]["state"] = 1 # 绿灯 + + if old_state != traffic_light_data[1]["state"]: + print(f"东路口红绿灯状态切换为: {'绿灯' if traffic_light_data[1]['state'] == 1 else '红灯'}") return jsonify(traffic_light_data) diff --git a/tools/mock_server.py.bak b/tools/mock_server.py.bak new file mode 100644 index 0000000..42dc1d1 --- /dev/null +++ b/tools/mock_server.py.bak @@ -0,0 +1,538 @@ +from flask import Flask, jsonify, request +import time +import math +import random + +app = Flask(__name__) + +# 地球半径（米） +EARTH_RADIUS = 6378137.0 + +def meters_to_degrees(meters, latitude): + """ + 将米转换为度，考虑纬度的影响 + """ + # 纬度方向：1度 = 111,319.9米 + meters_per_deg_lat = 111319.9 + # 经度方向：1度 = 111,319.9 * cos(latitude)米 + meters_per_deg_lon = meters_per_deg_lat * math.cos(math.radians(latitude)) + return meters / meters_per_deg_lat, meters / meters_per_deg_lon + +# 距离配置（米） +DIST_150M = 150 +DIST_100M = 100 +DIST_50M = 50 + +# 两个路口的位置 +WEST_INTERSECTION = {"longitude": 120.088003, "latitude": 36.361999} +EAST_INTERSECTION = {"longitude": 120.089003, "latitude": 36.361999} + +# 位置更新间隔（秒） +UPDATE_INTERVAL = 1.0 + +# 航空器数据 +aircraft_data = [ + { + "flightNo": "AC001", # 航空器 + "latitude": EAST_INTERSECTION["latitude"] - (DIST_150M / 111319.9), # 东路口南150米 + "longitude": EAST_INTERSECTION["longitude"], + "time": int(time.time() * 1000), + "direction": 1, # 1表示向北 + "speed": 50.0 # 滑行速度 50km/h + } +] + +# 添加默认速度常量 +DEFAULT_VEHICLE_SPEED = 36.0 # km/h +EMERGENCY_BRAKE_DECELERATION = 0.8 # 紧急制动减速度 (每次更新减速 80%) +NORMAL_BRAKE_DECELERATION = 0.2 # 正常制动减速度 (每次更新减速 20%) + +# 车辆数据 +vehicle_data = [ + { + "vehicleNo": "QN001", # 无人车1（西路口） + "latitude": WEST_INTERSECTION["latitude"] + (DIST_50M / 111319.9), # 西路口北50米 + "longitude": WEST_INTERSECTION["longitude"], + "time": int(time.time() * 1000), + "direction": -1, # -1表示向南 + "speed": DEFAULT_VEHICLE_SPEED, # 使用默认速度 + "phase": 0 # 0: 南北移动, 1: 东西移动 + }, + { + "vehicleNo": "QN002", # 无人车2（东路口） + "latitude": EAST_INTERSECTION["latitude"], + "longitude": EAST_INTERSECTION["longitude"] - (DIST_150M / (111319.9 * math.cos(math.radians(EAST_INTERSECTION["latitude"])))), # 东路口西150米 + "time": int(time.time() * 1000), + "direction": 1, # 1表示向东 + "speed": DEFAULT_VEHICLE_SPEED # 使用默认速度 + }, + { + "vehicleNo": "TQ001", # 特勤车（西路口） + "latitude": WEST_INTERSECTION["latitude"], + "longitude": WEST_INTERSECTION["longitude"] + (DIST_50M / (111319.9 * math.cos(math.radians(WEST_INTERSECTION["latitude"])))), # 西路口东50米 + "time": int(time.time() * 1000), + "direction": -1, # -1表示向西 + "speed": DEFAULT_VEHICLE_SPEED # 使用默认速度 + } +] + +# 添加车辆状态类 +class VehicleState: + def __init__(self, vehicle_no): + self.vehicle_no = vehicle_no + self.is_running = True # 运行状态 + self.current_command = None # 当前执行的指令 + self.command_priority = 0 # 当前指令优先级 + self.target_speed = DEFAULT_VEHICLE_SPEED # 目标速度 + self.brake_mode = None # 制动模式：'emergency' 或 'normal' + + def can_be_overridden_by(self, command_type): + # 指令优先级: SIGNAL(4) > ALERT(3) > WARNING(2) > RESUME(1) + priority_map = { + "SIGNAL": 4, + "ALERT": 3, + "WARNING": 2, + "RESUME": 1 + } + new_priority = priority_map.get(command_type, 0) + + # 如果当前没有指令，或者新指令优先级更高，或者是相同类型的指令，则允许覆盖 + return (self.current_command is None or + new_priority >= self.command_priority or + command_type == self.current_command) + + def update_command(self, command_type): + priority_map = { + "SIGNAL": 4, + "ALERT": 3, + "WARNING": 2, + "RESUME": 1 + } + self.command_priority = priority_map.get(command_type, 0) + self.current_command = command_type + +# 添加车辆状态管理 +vehicle_states = { + "QN001": VehicleState("QN001"), + "QN002": VehicleState("QN002"), + "TQ001": VehicleState("TQ001") +} + +def calculate_distance_to_target(vehicle, target_lat, target_lon): + """计算车辆到目标位置的距离（米）""" + dlat = target_lat - vehicle["latitude"] + dlon = target_lon - vehicle["longitude"] + + # 使用 Haversine 公式计算距离 + R = 6371000 # 地球半径（米） + a = math.sin(dlat/2) * math.sin(dlat/2) + \ + math.cos(math.radians(vehicle["latitude"])) * math.cos(math.radians(target_lat)) * \ + math.sin(dlon/2) * math.sin(dlon/2) + c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) + return R * c + +@app.route('/vehicle/command', methods=['POST']) +def handle_vehicle_command(): + try: + data = request.json + vehicle_id = data.get("vehicleId") + command_type = data.get("type", "").upper() + reason = data.get("reason", "").upper() + + # 检查车辆是否存在 + vehicle_state = vehicle_states.get(vehicle_id) + if not vehicle_state: + return jsonify({ + "status": "error", + "message": f"Vehicle {vehicle_id} not found" + }), 404 + + # 检查是否为特勤车辆 + if vehicle_id.startswith("TQ"): + return jsonify({ + "status": "ok", + "message": "Special vehicle ignores command" + }) + + # 检查指令优先级 + if not vehicle_state.can_be_overridden_by(command_type): + return jsonify({ + "status": "error", + "message": "Command priority too low" + }), 400 + + # 获取目标位置 + target_lat = data.get("latitude", 0.0) + target_lon = data.get("longitude", 0.0) + + # 处理不同类型的指令 + if command_type == "SIGNAL": + signal_state = data.get("signalState", "").upper() + if signal_state == "RED": + # 计算到路口的距离 + distance = calculate_distance_to_target( + next(v for v in vehicles if v["vehicleNo"] == vehicle_id), + target_lat, target_lon + ) + + # 根据距离决定制动方式 + if distance <= 50: # 已经在停止线内 + vehicle_state.is_running = False + vehicle_state.target_speed = 0 + vehicle_state.brake_mode = "emergency" # 紧急停车 + elif distance <= 100: # 距离停止线100米内 + vehicle_state.is_running = False + vehicle_state.target_speed = 0 + vehicle_state.brake_mode = "normal" # 正常制动 + elif signal_state == "GREEN": + vehicle_state.is_running = True + vehicle_state.target_speed = DEFAULT_VEHICLE_SPEED + vehicle_state.brake_mode = None + + elif command_type == "ALERT": + vehicle_state.is_running = False + vehicle_state.target_speed = 0 + vehicle_state.brake_mode = "emergency" # 告警紧急制动 + + elif command_type == "WARNING": + vehicle_state.is_running = False + vehicle_state.target_speed = 0 + vehicle_state.brake_mode = "normal" # 预警正常制动 + + elif command_type == "RESUME": + vehicle_state.is_running = True + vehicle_state.target_speed = DEFAULT_VEHICLE_SPEED + vehicle_state.brake_mode = None + + # 更新车辆状态 + vehicle_state.command_type = command_type + vehicle_state.command_reason = reason + vehicle_state.command_priority = COMMAND_PRIORITIES.get(command_type, 0) + vehicle_state.last_command_time = time.time() + + print(f"Vehicle {vehicle_id} state updated: running={vehicle_state.is_running}, " + f"command={command_type}, reason={reason}, priority={vehicle_state.command_priority}, " + f"target_lat={target_lat}, target_lon={target_lon}") + + return jsonify({ + "status": "ok", + "message": "Command executed successfully" + }) + + except Exception as e: + print(f"Error handling vehicle command: {str(e)}") + return jsonify({ + "status": "error", + "message": str(e) + }), 500 + +def calculate_distance_to_intersection(vehicle, intersection): + """计算车辆到路口的距离（米）""" + vehicle_lat = vehicle["latitude"] + vehicle_lon = vehicle["longitude"] + intersection_lat = intersection["latitude"] + intersection_lon = intersection["longitude"] + + # 使用经纬度计算距离 + lat_diff = (vehicle_lat - intersection_lat) * 111319.9 + 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, WEST_INTERSECTION) + east_dist = calculate_distance_to_intersection(vehicle, EAST_INTERSECTION) + + if west_dist <= east_dist: + return WEST_INTERSECTION, traffic_light_data[0], west_dist # TL001 + else: + return EAST_INTERSECTION, traffic_light_data[1], east_dist # TL002 + +def get_front_traffic_light(vehicle, distance_to_west, distance_to_east): + """获取车辆前方的红绿灯状态""" + # 根据车辆的位置和方向判断前方路口 + if vehicle["vehicleNo"].startswith("QN"): + if vehicle["vehicleNo"] == "QN001": + # QN001 的路线：西路口北侧 -> 南 -> 东 -> 北 + if vehicle["phase"] == 0: # 南北移动 + if vehicle["direction"] == -1: # 向南 + return traffic_light_data[0], distance_to_west # 西路口红绿灯 + else: # 向北 + return None, float('inf') # 没有红绿灯 + else: # 东西移动 + if vehicle["direction"] == 1: # 向东 + return None, float('inf') # 没有红绿灯 + else: # 向西 + return traffic_light_data[0], distance_to_west # 西路口红绿灯 + else: # QN002 + # QN002 的路线：东西方向往返 + if vehicle["direction"] == 1: # 向东 + return traffic_light_data[1], distance_to_east # 东路口红绿灯 + else: # 向西 + return traffic_light_data[0], distance_to_west # 西路口红绿灯 + elif vehicle["vehicleNo"].startswith("TQ"): + # 特勤车：先东西后南北 + if vehicle["phase"] == 0: # 东西移动 + if vehicle["direction"] == 1: # 向东 + return None, float('inf') # 没有���绿灯 + else: # 向西 + return traffic_light_data[0], distance_to_west # 西路口红绿灯 + else: # 南北移动 + return None, float('inf') # 南北方向没有红绿灯 + + return None, float('inf') # 默认返回无红绿灯 + +def update_vehicle_position(vehicle, elapsed_time): + """更新车辆位置""" + # 计算到两个路口的距离 + distance_to_west = calculate_distance_to_intersection(vehicle, WEST_INTERSECTION) + distance_to_east = calculate_distance_to_intersection(vehicle, EAST_INTERSECTION) + + # 获取前方红绿灯状态 + traffic_light, distance = get_front_traffic_light(vehicle, distance_to_west, distance_to_east) + + # 计算基础速度（米/秒） + if vehicle["vehicleNo"].startswith("TQ"): + base_speed_mps = DEFAULT_VEHICLE_SPEED * 1000 / 3600 + else: + vehicle_state = vehicle_states.get(vehicle["vehicleNo"]) + if not vehicle_state or not vehicle_state.is_running: + return + base_speed_mps = vehicle["speed"] * 1000 / 3600 + + # 如果是无人车，还需要考虑制动模式 + if vehicle_state.brake_mode: + current_speed_mps = base_speed_mps + target_speed_mps = vehicle_state.target_speed * 1000 / 3600 + + if vehicle_state.brake_mode == "emergency": + base_speed_mps = current_speed_mps * (1 - EMERGENCY_BRAKE_DECELERATION) + else: # normal + base_speed_mps = current_speed_mps * (1 - NORMAL_BRAKE_DECELERATION) + + base_speed_mps = max(base_speed_mps, target_speed_mps) + + # 根据红绿灯状态调整速度 + if traffic_light and traffic_light["state"] == 0: # 红灯 + if distance <= 50: # 已经在停止线内 + speed_mps = 0 # 紧急停车 + print(f"车辆 {vehicle['vehicleNo']} 在停止线内遇红灯，紧急停车") + elif distance <= 100: # 距离停止线100米内 + # 计算减速度，使车辆在停止线前停下 + deceleration = (base_speed_mps * base_speed_mps) / (2 * (distance - 50)) + speed_mps = max(0, base_speed_mps - deceleration * elapsed_time) + print(f"车辆 {vehicle['vehicleNo']} 距红灯 {distance:.1f}米，减速至 {speed_mps * 3.6:.1f}km/h") + else: + speed_mps = base_speed_mps + else: # 绿灯或无红绿灯 + speed_mps = base_speed_mps + + # 更新车辆速度 + vehicle["speed"] = speed_mps * 3600 / 1000 # 转回 km/h + + # 计算移动距离 + distance = speed_mps * elapsed_time + + # 计算经纬度变化 + dlat, dlon = meters_to_degrees(distance, vehicle["latitude"]) + + if vehicle["vehicleNo"].startswith("QN"): + # 无人车的路径更新逻辑 + if vehicle["vehicleNo"] == "QN001": + # 无人车1：先南北后东西往返 + if "phase" not in vehicle: + vehicle["phase"] = 0 + + if vehicle["phase"] == 0: # 南北移动 + new_lat = vehicle["latitude"] + (dlat * vehicle["direction"]) + if vehicle["direction"] == -1 and new_lat <= WEST_INTERSECTION["latitude"]: + # 到达路口，切换到东西移动 + vehicle["phase"] = 1 + vehicle["direction"] = 1 # 向东 + elif vehicle["direction"] == 1 and new_lat >= WEST_INTERSECTION["latitude"] + (DIST_50M / 111319.9): + # 返回起点 + vehicle["phase"] = 0 + vehicle["direction"] = -1 # 向南 + vehicle["latitude"] = new_lat + + else: # 东西移动 + new_lon = vehicle["longitude"] + (dlon * vehicle["direction"]) + if vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_100M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): + # 到达东端，开始返回 + vehicle["direction"] = -1 # 向西 + elif vehicle["direction"] == -1 and new_lon <= WEST_INTERSECTION["longitude"]: + # 返回路口，切换到南北移动 + vehicle["phase"] = 0 + vehicle["direction"] = 1 # 向北 + vehicle["longitude"] = new_lon + + else: # QN002 + # 无人车2：东西方向往返（东路口） + new_lon = vehicle["longitude"] + (dlon * vehicle["direction"]) + if new_lon >= EAST_INTERSECTION["longitude"] + (DIST_100M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): + vehicle["direction"] = -1 # 向西 + elif new_lon <= EAST_INTERSECTION["longitude"] - (DIST_150M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): + vehicle["direction"] = 1 # 向东 + vehicle["longitude"] = new_lon + + elif vehicle["vehicleNo"].startswith("TQ"): + # 特勤车的路径更新逻辑 + if "phase" not in vehicle: + vehicle["phase"] = 0 + + if vehicle["phase"] == 0: # 东西移动 + new_lon = vehicle["longitude"] + (dlon * vehicle["direction"]) + if vehicle["direction"] == -1 and new_lon <= WEST_INTERSECTION["longitude"]: + vehicle["phase"] = 1 # 切换到南北移动 + vehicle["direction"] = -1 # 向南 + elif vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_50M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): + vehicle["direction"] = -1 # 向西 + vehicle["longitude"] = new_lon + else: # 南北移动 + new_lat = vehicle["latitude"] + (dlat * vehicle["direction"]) + if new_lat <= WEST_INTERSECTION["latitude"] - (DIST_100M / 111319.9): # 到达南端 + vehicle["direction"] = 1 # 向北 + elif new_lat >= WEST_INTERSECTION["latitude"]: # 返回到路口 + vehicle["phase"] = 0 # 切换回东西移动 + vehicle["direction"] = 1 # 向北 + vehicle["longitude"] = WEST_INTERSECTION["longitude"] # 重置到起始位置 + vehicle["latitude"] = new_lat + + # 更新时间戳 + vehicle["time"] = int(time.time() * 1000) + +def update_aircraft_position(aircraft, elapsed_time): + """更新航空器位置""" + # 计算一次更新的移动距离（米） + speed_mps = aircraft["speed"] * 1000 / 3600 # 速度转换为米/秒 + distance = speed_mps * elapsed_time + + # 计算经纬度变化 + dlat, dlon = meters_to_degrees(distance, aircraft["latitude"]) + + new_lat = aircraft["latitude"] + (dlat * aircraft["direction"]) + if new_lat >= EAST_INTERSECTION["latitude"] + (DIST_150M / 111319.9): + aircraft["direction"] = -1 # 向南 + elif new_lat <= EAST_INTERSECTION["latitude"] - (DIST_150M / 111319.9): + aircraft["direction"] = 1 # 向北 + aircraft["latitude"] = new_lat + +def update_traffic_light_for_aircraft(): + """根据航空器位置更新东路口红绿灯状态""" + if not aircraft_data: + return + + aircraft = aircraft_data[0] + # 计算到东路口的距离（米） + lat_diff = abs(aircraft["latitude"] - EAST_INTERSECTION["latitude"]) * 111319.9 + + # 更新TL002（东路口）的状态 + for light in traffic_light_data: + if light["id"] == "TL002": + if lat_diff <= 50.0: # 距离于50米时为红灯 + light["state"] = 0 # 红灯 + else: + light["state"] = 1 # 绿灯 + break + +# 定义路口信息 +INTERSECTIONS = { + "TL001": { + "id": "INT001", + "name": "西路口", + "latitude": WEST_INTERSECTION["latitude"], + "longitude": WEST_INTERSECTION["longitude"] + }, + "TL002": { + "id": "INT002", + "name": "东路口", + "latitude": EAST_INTERSECTION["latitude"], + "longitude": EAST_INTERSECTION["longitude"] + } +} + +def generate_traffic_light_data(): + """生成红绿灯数据""" + traffic_light_data = [] + + # 生成两个固定路口的红绿灯数据 + for tl_id, intersection in INTERSECTIONS.items(): + signal = { + "id": tl_id, + "state": random.randint(0, 1), # 0: RED, 1: GREEN + "timestamp": int(time.time() * 1000), + "intersectionId": intersection["id"], + "latitude": intersection["latitude"], + "longitude": intersection["longitude"] + } + traffic_light_data.append(signal) + + return traffic_light_data + +# 红绿灯���据 +traffic_light_data = generate_traffic_light_data() + +# 分别记录航空器和车辆的上次更新时间 +last_aircraft_update_time = time.time() +last_vehicle_update_time = time.time() +last_light_switch_time = time.time() + +@app.route('/api/getCurrentFlightPositions') +def get_flight_positions(): + global last_aircraft_update_time + current_time = time.time() + elapsed_time = current_time - last_aircraft_update_time + + # 只在达到更新间隔时更新位置 + if elapsed_time >= UPDATE_INTERVAL: + for aircraft in aircraft_data: + update_aircraft_position(aircraft, UPDATE_INTERVAL) + aircraft["time"] = int(current_time * 1000) + last_aircraft_update_time = current_time + + return jsonify(aircraft_data) + +@app.route('/api/getCurrentVehiclePositions') +def get_vehicle_positions(): + global last_vehicle_update_time + current_time = time.time() + elapsed_time = current_time - last_vehicle_update_time + + # 只在达到更新间隔时更新位置 + if elapsed_time >= UPDATE_INTERVAL: + for vehicle in vehicle_data: + update_vehicle_position(vehicle, UPDATE_INTERVAL) + vehicle["time"] = int(current_time * 1000) + last_vehicle_update_time = current_time + + return jsonify(vehicle_data) + +@app.route('/api/getTrafficLightSignals') +def get_traffic_light_signals(): + global last_light_switch_time + current_time = time.time() + + # 更新时间戳 + for light in traffic_light_data: + light["timestamp"] = int(current_time * 1000) + + # TL001（西路口）状态每15秒切换一次 + elapsed_since_switch = current_time - last_light_switch_time + if elapsed_since_switch >= 15: # 每15秒切换一次 + traffic_light_data[0]["state"] = 1 if traffic_light_data[0]["state"] == 0 else 0 + last_light_switch_time = current_time + + # 根据航空器位置更新TL002（东路口） + update_traffic_light_for_aircraft() + + return jsonify(traffic_light_data) + +@app.after_request +def add_cors_headers(response): + response.headers['Access-Control-Allow-Origin'] = '*' + response.headers['Access-Control-Allow-Headers'] = 'Content-Type' + response.headers['Access-Control-Allow-Methods'] = 'GET, POST, OPTIONS' + return response + +if __name__ == '__main__': + app.run(host='localhost', port=8081, debug=True) \ No newline at end of file diff --git a/tools/test_websocket.html b/tools/test_websocket.html index d5522ac..71735a5 100644 --- a/tools/test_websocket.html +++ b/tools/test_websocket.html @@ -17,6 +17,7 @@ .info { color: blue; } .position { color: #666; } .warning { color: #f90; } + .command { color: #800080; } /* 紫色显示控制指令 */ @@ -73,13 +74,42 @@ // 根据消息类型使用不同的样式 let type = 'info'; - if (data.type === 'position_update') { - type = 'position'; - } else if (data.type === 'collision_warning') { - type = 'warning'; + let message = '收到消息:\n' + formattedData; + + switch (data.type) { + case 'position_update': + type = 'position'; + break; + case 'collision_warning': + type = 'warning'; + break; + case 'vehicle_command': + type = 'command'; + // 为控制指令添加中文描述 + const commandTypes = { + 'SIGNAL': '信号灯指令', + 'ALERT': '告警指令', + 'WARNING': '预警指令', + 'RESUME': '恢复指令' + }; + const reasons = { + 'TRAFFIC_LIGHT': '红绿灯控制', + 'AIRCRAFT_CROSSING': '航空器交叉', + 'SPECIAL_VEHICLE': '特勤车辆', + 'AIRCRAFT_PUSH': '航空器推出', + 'RESUME_TRAFFIC': '恢复通行' + }; + message = `收到车辆控制指令:\n车辆: ${data.vehicleId}\n` + + `指令类型: ${commandTypes[data.commandType] || data.commandType}\n` + + `原因: ${reasons[data.reason] || data.reason}\n` + + (data.targetLatitude !== undefined ? `目标位置: (${data.targetLatitude}, ${data.targetLongitude})\n` : '') + + (data.signalState ? `信号灯状态: ${data.signalState}\n` : '') + + (data.intersectionId ? `路口ID: ${data.intersectionId}\n` : '') + + `时间戳: ${new Date(data.timestamp/1000000).toLocaleString()}`; + break; } - log('收到消息:\n' + formattedData, type); + log(message, type); } catch (e) { log('收到消息: ' + event.data, 'info'); }