修改了碰撞检测逻辑。
This commit is contained in:
parent
49f40cc56e
commit
4954605295
@ -43,4 +43,5 @@
|
||||
- 先询问代码的用途
|
||||
- 理解代码的功能和作用
|
||||
- 确认是否可以修改或删除
|
||||
- 如果不确定就直接问我
|
||||
- 如果不确定就直接问我
|
||||
- 不要随便修改原有代码中的中文注释
|
||||
@ -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
|
||||
)
|
||||
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)
|
||||
@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
},
|
||||
|
||||
@ -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,
|
||||
|
||||
14
config/vehicle_control.json
Normal file
14
config/vehicle_control.json
Normal file
@ -0,0 +1,14 @@
|
||||
{
|
||||
"vehicles": [
|
||||
{
|
||||
"vehicleNo": "QN001",
|
||||
"ip": "localhost",
|
||||
"port": 8081
|
||||
},
|
||||
{
|
||||
"vehicleNo": "QN002",
|
||||
"ip": "localhost",
|
||||
"port": 8081
|
||||
}
|
||||
]
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
]
|
||||
}
|
||||
301
docs/mock_server.md
Normal file
301
docs/mock_server.md
Normal file
@ -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 服务：<http://localhost:8081>
|
||||
|
||||
## 8. 注意事项
|
||||
|
||||
1. 位置更新
|
||||
- 位置更新频率：1秒一次
|
||||
- 坐标系统：WGS84
|
||||
- 速度单位：km/h（内部计算使用 m/s）
|
||||
|
||||
2. 车辆控制
|
||||
- 车辆收到停止指令后会立即停止位置更新
|
||||
- 只有收到 RESUME 指令且没有更高优先级的指令时才会恢复运动
|
||||
- 每个指令都必须包含时间戳
|
||||
|
||||
3. 错误处理
|
||||
- 无效的车辆ID：返回 404 错误
|
||||
- 无效的指令格式：返回 400 错误
|
||||
- 服务器内部错误：返回 500 错误
|
||||
76
include/core/System.h
Normal file
76
include/core/System.h
Normal file
@ -0,0 +1,76 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <unordered_map>
|
||||
#include <unordered_set>
|
||||
#include <chrono>
|
||||
#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<bool> running_{false};
|
||||
std::thread processThread_;
|
||||
std::thread ws_thread_;
|
||||
|
||||
// 系统组件
|
||||
std::unique_ptr<AirportBounds> airportBounds_;
|
||||
std::unique_ptr<ControllableVehicles> controllableVehicles_;
|
||||
std::unique_ptr<CollisionDetector> collisionDetector_;
|
||||
std::unique_ptr<TrafficLightDetector> trafficLightDetector_;
|
||||
std::unique_ptr<DataCollector> dataCollector_;
|
||||
std::unique_ptr<network::WebSocketServer> ws_server_;
|
||||
|
||||
// 配置
|
||||
SystemConfig system_config_;
|
||||
IntersectionConfig intersection_config_;
|
||||
|
||||
// 记录车辆停止的时间
|
||||
std::unordered_map<std::string, std::chrono::system_clock::time_point> vehicleStopTimes_;
|
||||
|
||||
// 记录上一次有风险的可控车辆列表
|
||||
std::unordered_set<std::string> lastVehiclesWithRisk_;
|
||||
};
|
||||
@ -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<Aircraft>& getAircraftRef() const {
|
||||
std::lock_guard<std::mutex> lock(cacheMutex_);
|
||||
return aircraftCache_;
|
||||
}
|
||||
|
||||
const std::vector<Vehicle>& getVehicleRef() const {
|
||||
std::lock_guard<std::mutex> lock(cacheMutex_);
|
||||
return vehicleCache_;
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<DataSource> dataSource_;
|
||||
|
||||
@ -23,12 +23,20 @@ IntersectionConfig IntersectionConfig::load(const std::string& configFile) {
|
||||
info.name = item["name"].get<std::string>();
|
||||
info.trafficLightId = item["trafficLightId"].get<std::string>();
|
||||
|
||||
// 加载位置信息
|
||||
// 加载位置信息，检查是否为 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<double>();
|
||||
info.position.latitude = item["position"]["latitude"].get<double>();
|
||||
info.position.altitude = item["position"]["altitude"].get<double>();
|
||||
|
||||
// 加载距离阈值
|
||||
// 加载距离阈值，检查是否为 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<double>();
|
||||
info.stopDistance = item["stopDistance"].get<double>();
|
||||
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
#include "utils/Logger.h"
|
||||
#include <fstream>
|
||||
|
||||
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());
|
||||
|
||||
@ -3,7 +3,23 @@
|
||||
#include <string>
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
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 类访问私有成员
|
||||
};
|
||||
@ -2,7 +2,7 @@
|
||||
#include <typeinfo>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#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<network::WebSocketServer>(system_config_.websocket.port);
|
||||
ws_server_ = std::make_unique<network::WebSocketServer>(system_config.websocket.port);
|
||||
ws_thread_ = std::thread([this]() {
|
||||
ws_server_->start();
|
||||
});
|
||||
@ -46,31 +46,32 @@ bool System::initialize() {
|
||||
airportBounds_ = std::make_unique<AirportBounds>("config/airport_bounds.json");
|
||||
|
||||
// 加载可控车辆配置
|
||||
controllableVehicles_ = std::make_unique<ControllableVehicles>("config/vehicle_speed_limits.json");
|
||||
controllableVehicles_ = std::make_unique<ControllableVehicles>("config/vehicle_control.json");
|
||||
Logger::info("Loaded controllable vehicles configuration");
|
||||
|
||||
// 初始化冲突检测器
|
||||
collisionDetector_ = std::make_unique<CollisionDetector>(*airportBounds_, *controllableVehicles_);
|
||||
|
||||
// 初始化红绿灯检测器
|
||||
trafficLightDetector_ = std::make_unique<TrafficLightDetector>(intersection_config_, *controllableVehicles_);
|
||||
trafficLightDetector_ = std::make_unique<TrafficLightDetector>(intersection_config_, *controllableVehicles_, *this);
|
||||
|
||||
// 创建数据采集器
|
||||
dataCollector_ = std::make_unique<DataCollector>();
|
||||
|
||||
// 数据采集器初始化并启动
|
||||
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<std::chrono::milliseconds>(
|
||||
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<std::chrono::milliseconds>(
|
||||
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<std::chrono::milliseconds>(
|
||||
now.time_since_epoch()).count());
|
||||
Logger::debug("上次更新时间: ", std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
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<std::chrono::milliseconds>(
|
||||
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) {
|
||||
// 更新冲突检测器
|
||||
// 更<EFBFBD><EFBFBD>冲突检测器
|
||||
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<int>(risk.level), static_cast<int>(risk.zoneType));
|
||||
}
|
||||
processCollisions(collisions);
|
||||
}
|
||||
|
||||
@ -230,7 +270,7 @@ void System::processLoop() {
|
||||
// 处理红绿灯信号
|
||||
auto traffic_light_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
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<std::chrono::milliseconds>(next_collection - now).count();
|
||||
|
||||
if (wait_time > 0) {
|
||||
@ -275,29 +315,110 @@ void System::processLoop() {
|
||||
}
|
||||
}
|
||||
|
||||
void System::processCollisions(const std::vector<CollisionRisk>& collisions) {
|
||||
if (!collisions.empty()) {
|
||||
Logger::info("检测到 ", collisions.size(), " 个冲突风险");
|
||||
void System::processCollisions(const std::vector<CollisionRisk>& risks) {
|
||||
// 记录当前有风险的可控车辆
|
||||
std::unordered_set<std::string> currentVehiclesWithRisk;
|
||||
const auto& config = SystemConfig::instance().collision_detection;
|
||||
|
||||
// 记录需要发送恢复指令的车辆
|
||||
std::unordered_set<std::string> 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<int>(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<int>(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;
|
||||
}
|
||||
@ -4,6 +4,7 @@
|
||||
#include <thread>
|
||||
#include <string>
|
||||
#include <atomic>
|
||||
#include <unordered_set>
|
||||
#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<CollisionRisk>& collisions);
|
||||
|
||||
|
||||
|
||||
std::atomic<bool> running_{false};
|
||||
std::thread processThread_;
|
||||
|
||||
@ -64,4 +64,10 @@ private:
|
||||
IntersectionConfig intersection_config_;
|
||||
|
||||
static System* instance_;
|
||||
|
||||
// 记录上一次有风险的车辆列表
|
||||
std::unordered_set<std::string> lastVehiclesWithRisk_;
|
||||
|
||||
// 辅助函数
|
||||
const MovingObject* findVehicle(const std::string& vehicleId) const;
|
||||
};
|
||||
@ -1,34 +1,80 @@
|
||||
#include "detector/CollisionDetector.h"
|
||||
#include "utils/Logger.h"
|
||||
#include "config/SystemConfig.h"
|
||||
#include <cmath>
|
||||
#include <format>
|
||||
|
||||
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>& aircraft,
|
||||
const std::vector<Vehicle>& 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<CollisionRisk> CollisionDetector::detectCollisions() {
|
||||
std::vector<CollisionRisk> risks;
|
||||
|
||||
// 获取配置参数
|
||||
const auto& predictionConfig = SystemConfig::instance().collision_detection.prediction;
|
||||
|
||||
// 获取所有车辆
|
||||
auto allVehicles = vehicleTree_.queryRange(vehicleTree_.getBounds());
|
||||
|
||||
// 过滤出可控车辆
|
||||
|
||||
// 可控车辆
|
||||
std::vector<Vehicle> controlVehicles;
|
||||
for (const auto& vehicle : allVehicles) {
|
||||
if (controllableVehicles_->isControllable(vehicle.vehicleNo)) {
|
||||
@ -38,92 +84,120 @@ std::vector<CollisionRisk> 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<CollisionRisk> 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<RiskLevel, WarningZoneType> 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<int>(level),
|
||||
", 区域类型=", static_cast<int>(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<double>::infinity();
|
||||
result.minDistance = std::numeric_limits<double>::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;
|
||||
}
|
||||
@ -5,15 +5,24 @@
|
||||
#include "spatial/QuadTree.h"
|
||||
#include "spatial/AirportBounds.h"
|
||||
#include "vehicle/ControllableVehicles.h"
|
||||
#include "config/SystemConfig.h"
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
#include <limits>
|
||||
#include <utils/Logger.h>
|
||||
|
||||
// 碰撞风险等级
|
||||
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<CollisionRisk> 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<Vehicle> 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<RiskLevel, WarningZoneType> 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,
|
||||
|
||||
@ -1,12 +1,15 @@
|
||||
#include "detector/TrafficLightDetector.h"
|
||||
#include "utils/Logger.h"
|
||||
#include "types/BasicTypes.h"
|
||||
#include "core/System.h"
|
||||
#include <chrono>
|
||||
|
||||
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<ControllableVehicles&>(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<ControllableVehicles&>(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<ControllableVehicles&>(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<ControllableVehicles&>(controllable_vehicles_).sendCommand(vehicleNo, cmd)) {
|
||||
system_.broadcastVehicleCommand(cmd);
|
||||
}
|
||||
Logger::debug("发送继续指令到车辆: ", vehicleNo, " 路口: ", cmd.intersectionId);
|
||||
}
|
||||
@ -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<Vehicle>& 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_; // 当前处理的信号
|
||||
};
|
||||
@ -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;
|
||||
|
||||
109
src/network/HTTPClient.cpp
Normal file
109
src/network/HTTPClient.cpp
Normal file
@ -0,0 +1,109 @@
|
||||
#include "network/HTTPClient.h"
|
||||
#include "utils/Logger.h"
|
||||
#include <sstream>
|
||||
|
||||
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<std::string*>(userp);
|
||||
response->append(static_cast<char*>(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;
|
||||
}
|
||||
}
|
||||
20
src/network/HTTPClient.h
Normal file
20
src/network/HTTPClient.h
Normal file
@ -0,0 +1,20 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <curl/curl.h>
|
||||
#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_;
|
||||
};
|
||||
@ -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<double>(),
|
||||
config["aircraft_ground_radius"].get<double>(),
|
||||
config["height_threshold"].get<double>()
|
||||
config["height_threshold"].get<double>(),
|
||||
config["warning_zone_radius"].get<double>(),
|
||||
config["alert_zone_radius"].get<double>()
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
@ -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 // 警报距离
|
||||
};
|
||||
}
|
||||
};
|
||||
|
||||
@ -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 {
|
||||
|
||||
7
src/types/RiskLevel.h
Normal file
7
src/types/RiskLevel.h
Normal file
@ -0,0 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
enum class RiskLevel {
|
||||
NONE, // 无风险
|
||||
WARNING, // 预警
|
||||
CRITICAL // 告警
|
||||
};
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -4,7 +4,8 @@
|
||||
#include <fstream>
|
||||
#include <stdexcept>
|
||||
|
||||
ControllableVehicles::ControllableVehicles(const std::string& configFile) {
|
||||
ControllableVehicles::ControllableVehicles(const std::string& configFile)
|
||||
: http_client_(std::make_unique<HTTPClient>()) {
|
||||
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<std::string>();
|
||||
config.port = item["port"].get<int>();
|
||||
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<int>(command.type), static_cast<int>(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;
|
||||
}
|
||||
}
|
||||
@ -4,6 +4,7 @@
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#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<ControllableVehicleConfig> vehicles_;
|
||||
std::unique_ptr<HTTPClient> http_client_;
|
||||
void loadConfig(const std::string& configFile);
|
||||
};
|
||||
@ -62,7 +62,7 @@ TEST_F(CollisionDetectorTest, DetectControllableVehicleAircraftCollision) {
|
||||
.WillRepeatedly(testing::Return(true));
|
||||
Logger::info("Set mock expectation: VEH001 is controllable");
|
||||
|
||||
// 设置测试数<EFBFBD><EFBFBD><EFBFBD>
|
||||
// 设置<EFBFBD><EFBFBD><EFBFBD>试数据
|
||||
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) << "碰撞时间应该接近预期值";
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
396
tools/map_websocket.html
Normal file
396
tools/map_websocket.html
Normal file
@ -0,0 +1,396 @@
|
||||
<!DOCTYPE html>
|
||||
<html>
|
||||
<head>
|
||||
<title>机场车辆监控</title>
|
||||
<link rel="stylesheet" href="https://unpkg.com/leaflet@1.9.4/dist/leaflet.css"/>
|
||||
<script src="https://unpkg.com/leaflet@1.9.4/dist/leaflet.js"></script>
|
||||
<style>
|
||||
body {
|
||||
margin: 0;
|
||||
padding: 20px;
|
||||
font-family: Arial, sans-serif;
|
||||
}
|
||||
.container {
|
||||
display: flex;
|
||||
gap: 20px;
|
||||
}
|
||||
#map {
|
||||
height: 800px;
|
||||
width: 60%;
|
||||
border: 1px solid #ccc;
|
||||
}
|
||||
#messages {
|
||||
width: 40%;
|
||||
height: 800px;
|
||||
overflow-y: auto;
|
||||
border: 1px solid #ccc;
|
||||
padding: 10px;
|
||||
font-family: monospace;
|
||||
}
|
||||
.controls {
|
||||
margin-top: 10px;
|
||||
}
|
||||
.error { color: red; }
|
||||
.success { color: green; }
|
||||
.info { color: blue; }
|
||||
.position { color: #666; }
|
||||
.warning { color: #f90; }
|
||||
.command { color: #800080; }
|
||||
.vehicle-icon {
|
||||
width: 10px;
|
||||
height: 10px;
|
||||
background-color: black;
|
||||
clip-path: polygon(0% 0%, 100% 0%, 100% 100%, 0% 100%);
|
||||
border: 2px solid white;
|
||||
}
|
||||
.aircraft-icon {
|
||||
width: 50px;
|
||||
height: 50px;
|
||||
background-color: purple;
|
||||
clip-path: polygon(0% 0%, 100% 0%, 100% 100%, 0% 100%);
|
||||
border: 2px solid white;
|
||||
}
|
||||
.special-vehicle-icon {
|
||||
width: 10px;
|
||||
height: 10px;
|
||||
background-color: orange;
|
||||
clip-path: polygon(0% 0%, 100% 0%, 100% 100%, 0% 100%);
|
||||
border: 2px solid white;
|
||||
}
|
||||
.intersection-icon {
|
||||
width: 30px;
|
||||
height: 30px;
|
||||
background-color: #666;
|
||||
clip-path: polygon(40% 0%, 60% 0%, 60% 40%, 100% 40%, 100% 60%, 60% 60%, 60% 100%, 40% 100%, 40% 60%, 0% 60%, 0% 40%, 40% 40%);
|
||||
border: 2px solid white;
|
||||
}
|
||||
.traffic-light {
|
||||
width: 10px;
|
||||
height: 10px;
|
||||
border-radius: 50%;
|
||||
border: 1px solid white;
|
||||
z-index: 1000;
|
||||
}
|
||||
.traffic-light-red {
|
||||
background-color: red;
|
||||
}
|
||||
.traffic-light-green {
|
||||
background-color: green;
|
||||
}
|
||||
.distance-label {
|
||||
background: none;
|
||||
border: none;
|
||||
color: #666;
|
||||
font-size: 12px;
|
||||
text-align: center;
|
||||
white-space: nowrap;
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<h2>机场车辆监控系统</h2>
|
||||
<div class="container">
|
||||
<div id="map"></div>
|
||||
<div id="messages"></div>
|
||||
</div>
|
||||
<div class="controls">
|
||||
<button onclick="connect()">连接</button>
|
||||
<button onclick="disconnect()">断开</button>
|
||||
<button onclick="clearMessages()">清空日志</button>
|
||||
</div>
|
||||
|
||||
<script>
|
||||
let ws = null;
|
||||
const messagesDiv = document.getElementById('messages');
|
||||
|
||||
// 初始化地图
|
||||
const map = L.map('map').setView([36.361999, 120.088503], 17);
|
||||
L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
|
||||
maxZoom: 19,
|
||||
attribution: '© OpenStreetMap contributors'
|
||||
}).addTo(map);
|
||||
|
||||
// 定义路口坐标
|
||||
const WEST_INTERSECTION = {
|
||||
latitude: 36.361999,
|
||||
longitude: 120.086003
|
||||
};
|
||||
const EAST_INTERSECTION = {
|
||||
latitude: 36.361999,
|
||||
longitude: 120.089003
|
||||
};
|
||||
|
||||
// 存储所有标记
|
||||
const markers = new Map();
|
||||
const trafficLights = new Map();
|
||||
const intersections = new Map();
|
||||
|
||||
// 创建自定义图标
|
||||
function createIcon(className) {
|
||||
let size;
|
||||
if (className.includes('aircraft')) {
|
||||
size = [50, 50]; // 50米正方形
|
||||
} else if (className.includes('vehicle')) {
|
||||
size = [10, 10]; // 10米正方形
|
||||
} else if (className.includes('traffic-light')) {
|
||||
size = [10, 10]; // 10像素的红绿灯
|
||||
} else {
|
||||
size = [20, 20]; // 其他图标保持原样
|
||||
}
|
||||
|
||||
return L.divIcon({
|
||||
className: className,
|
||||
iconSize: size,
|
||||
iconAnchor: [size[0]/2, size[1]/2] // 设置图标锚点为中心
|
||||
});
|
||||
}
|
||||
|
||||
function log(message, type = 'info') {
|
||||
const div = document.createElement('div');
|
||||
div.className = type;
|
||||
div.textContent = `${new Date().toLocaleTimeString()} - ${message}`;
|
||||
messagesDiv.appendChild(div);
|
||||
messagesDiv.scrollTop = messagesDiv.scrollHeight;
|
||||
}
|
||||
|
||||
function clearMessages() {
|
||||
messagesDiv.innerHTML = '';
|
||||
}
|
||||
|
||||
function updatePosition(data) {
|
||||
const id = data.object_id;
|
||||
const position = [data.position.latitude, data.position.longitude];
|
||||
let iconClass;
|
||||
|
||||
// 根据ID前缀确定图标类型
|
||||
if (data.object_type === 'aircraft') {
|
||||
iconClass = 'aircraft-icon'; // 六形
|
||||
} else if (id.startsWith('TQ')) {
|
||||
iconClass = 'special-vehicle-icon'; // 正方形
|
||||
} else {
|
||||
iconClass = 'vehicle-icon'; // 三角形
|
||||
}
|
||||
|
||||
let marker = markers.get(id);
|
||||
if (!marker) {
|
||||
// 创建新标记
|
||||
marker = L.marker(position, {
|
||||
icon: createIcon(iconClass)
|
||||
}).addTo(map);
|
||||
marker.bindTooltip(id); // 添加标签显示ID
|
||||
markers.set(id, marker);
|
||||
} else {
|
||||
// 更新现有标记位置
|
||||
marker.setLatLng(position);
|
||||
}
|
||||
|
||||
// 更新标记旋转（根据航向）
|
||||
if (data.heading !== undefined) {
|
||||
marker.setRotationAngle(data.heading);
|
||||
}
|
||||
}
|
||||
|
||||
function updateTrafficLight(data) {
|
||||
const id = data.id;
|
||||
const position = [data.position.latitude, data.position.longitude];
|
||||
const state = data.status === 0 ? 'red' : 'green';
|
||||
|
||||
let light = trafficLights.get(id);
|
||||
if (!light) {
|
||||
light = L.marker(position, {
|
||||
icon: createIcon(`traffic-light traffic-light-${state}`)
|
||||
}).addTo(map);
|
||||
light.bindTooltip(id);
|
||||
trafficLights.set(id, light);
|
||||
} else {
|
||||
light.setIcon(createIcon(`traffic-light traffic-light-${state}`));
|
||||
}
|
||||
}
|
||||
|
||||
function connect() {
|
||||
if (ws) {
|
||||
log('已经连接，请先断开', 'error');
|
||||
return;
|
||||
}
|
||||
|
||||
try {
|
||||
ws = new WebSocket('ws://localhost:8010');
|
||||
|
||||
ws.onopen = () => {
|
||||
log('连接成功', 'success');
|
||||
};
|
||||
|
||||
ws.onclose = () => {
|
||||
log('连接关闭', 'info');
|
||||
ws = null;
|
||||
};
|
||||
|
||||
ws.onerror = (error) => {
|
||||
log('发生错误: ' + error, 'error');
|
||||
};
|
||||
|
||||
ws.onmessage = (event) => {
|
||||
try {
|
||||
const data = JSON.parse(event.data);
|
||||
const formattedData = JSON.stringify(data, null, 2);
|
||||
|
||||
// 根据消息类型使用不同的样式和处理方式
|
||||
let type = 'info';
|
||||
let message = '收到消息:\n' + formattedData;
|
||||
|
||||
switch (data.type) {
|
||||
case 'position_update':
|
||||
type = 'position';
|
||||
updatePosition(data);
|
||||
break;
|
||||
case 'traffic_light_status':
|
||||
updateTrafficLight(data);
|
||||
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(message, type);
|
||||
} catch (e) {
|
||||
log('收到消息: ' + event.data, 'info');
|
||||
}
|
||||
};
|
||||
} catch (error) {
|
||||
log('连接失败: ' + error, 'error');
|
||||
}
|
||||
}
|
||||
|
||||
function disconnect() {
|
||||
if (!ws) {
|
||||
log('未连接', 'error');
|
||||
return;
|
||||
}
|
||||
|
||||
ws.close();
|
||||
ws = null;
|
||||
|
||||
// 清除所有标记
|
||||
markers.forEach(marker => map.removeLayer(marker));
|
||||
markers.clear();
|
||||
trafficLights.forEach(light => map.removeLayer(light));
|
||||
trafficLights.clear();
|
||||
}
|
||||
|
||||
// 添加道路刻度标记函数
|
||||
function addRoadMarks(startPoint, endPoint, isLatitude = false) {
|
||||
const start = isLatitude ? startPoint[0] : startPoint[1];
|
||||
const end = isLatitude ? endPoint[0] : endPoint[1];
|
||||
const fixed = isLatitude ? startPoint[1] : startPoint[0];
|
||||
const step = 0.0005; // 约50米
|
||||
const direction = start < end ? 1 : -1;
|
||||
|
||||
for (let i = 0; Math.abs(i) <= Math.abs(end - start); i += step * direction) {
|
||||
const position = isLatitude ?
|
||||
[start + i, fixed] :
|
||||
[fixed, start + i];
|
||||
|
||||
// 添加刻度线
|
||||
const markLine = isLatitude ?
|
||||
[[position[0], position[1] - 0.0001], [position[0], position[1] + 0.0001]] :
|
||||
[[position[0] - 0.0001, position[1]], [position[0] + 0.0001, position[1]]];
|
||||
|
||||
L.polyline(markLine, {
|
||||
color: '#666',
|
||||
weight: 2
|
||||
}).addTo(map);
|
||||
|
||||
// 添加距离标签（以米为单位）
|
||||
const distance = Math.abs(Math.round(i / step) * 50);
|
||||
if (distance > 0) { // 只显示非零<E99D9E><E99BB6><EFBFBD>离
|
||||
const label = L.divIcon({
|
||||
className: 'distance-label',
|
||||
html: distance + 'm',
|
||||
iconSize: [40, 20],
|
||||
iconAnchor: [20, 10]
|
||||
});
|
||||
|
||||
L.marker(position, {
|
||||
icon: label,
|
||||
interactive: false
|
||||
}).addTo(map);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 添加道路
|
||||
// 东西向主路
|
||||
const mainRoadEW = L.polyline([
|
||||
[WEST_INTERSECTION.latitude, WEST_INTERSECTION.longitude - 0.002],
|
||||
[EAST_INTERSECTION.latitude, EAST_INTERSECTION.longitude + 0.002]
|
||||
], {
|
||||
color: '#999',
|
||||
weight: 8
|
||||
}).addTo(map);
|
||||
|
||||
// 西路口南北向道路
|
||||
const westRoadNS = L.polyline([
|
||||
[WEST_INTERSECTION.latitude + 0.002, WEST_INTERSECTION.longitude],
|
||||
[WEST_INTERSECTION.latitude - 0.002, WEST_INTERSECTION.longitude]
|
||||
], {
|
||||
color: '#999',
|
||||
weight: 8
|
||||
}).addTo(map);
|
||||
|
||||
// 东路口南北向道路
|
||||
const eastRoadNS = L.polyline([
|
||||
[EAST_INTERSECTION.latitude + 0.002, EAST_INTERSECTION.longitude],
|
||||
[EAST_INTERSECTION.latitude - 0.002, EAST_INTERSECTION.longitude]
|
||||
], {
|
||||
color: '#999',
|
||||
weight: 8
|
||||
}).addTo(map);
|
||||
|
||||
// 添加刻度标记
|
||||
// 西路口南北向刻度
|
||||
addRoadMarks(
|
||||
[WEST_INTERSECTION.latitude - 0.002, WEST_INTERSECTION.longitude],
|
||||
[WEST_INTERSECTION.latitude + 0.002, WEST_INTERSECTION.longitude],
|
||||
true
|
||||
);
|
||||
|
||||
// 东路口南北向刻度
|
||||
addRoadMarks(
|
||||
[EAST_INTERSECTION.latitude - 0.002, EAST_INTERSECTION.longitude],
|
||||
[EAST_INTERSECTION.latitude + 0.002, EAST_INTERSECTION.longitude],
|
||||
true
|
||||
);
|
||||
|
||||
// 东西向刻度
|
||||
addRoadMarks(
|
||||
[WEST_INTERSECTION.latitude, WEST_INTERSECTION.longitude - 0.002],
|
||||
[EAST_INTERSECTION.latitude, EAST_INTERSECTION.longitude + 0.002],
|
||||
false
|
||||
);
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
@ -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)
|
||||
|
||||
|
||||
538
tools/mock_server.py.bak
Normal file
538
tools/mock_server.py.bak
Normal file
@ -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') # 没有<E6B2A1><E69C89><EFBFBD>绿灯
|
||||
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
|
||||
|
||||
# 红绿灯<E7BBBF><E781AF><EFBFBD>据
|
||||
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)
|
||||
@ -17,6 +17,7 @@
|
||||
.info { color: blue; }
|
||||
.position { color: #666; }
|
||||
.warning { color: #f90; }
|
||||
.command { color: #800080; } /* 紫色显示控制指令 */
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
@ -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');
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user