增加了安全停靠指令，删除了无用的代码

CMakeLists.txt

@@ -129,26 +129,8 @@ if(BUILD_TESTS AND EXISTS "${CMAKE_SOURCE_DIR}/tests")
         FetchContent_MakeAvailable(googletest)
     endif()
 
-    # 测试源文件列表
-    set(TEST_SOURCES
-        tests/BasicCollisionTest.cpp
-        tests/CollisionDetectorTest.cpp
-        tests/BasicTypesTest.cpp
-        tests/HTTPDataSourceTest.cpp
-        tests/DataCollectorTest.cpp
-        tests/SafetyZoneTest.cpp
-    )
-
-    # 创建测试可执行文件
-    add_executable(unit_tests ${TEST_SOURCES})
-
-    # 统一 macOS 和 CentOS 的链接设置
-    target_link_libraries(unit_tests
-        PRIVATE
-        ${PROJECT_NAME}_lib
-        GTest::gtest_main
-        GTest::gmock_main
-    )
+    # 添加 tests 子目录
+    add_subdirectory(tests)
 endif()
 
 # 打印配置信息

HTTPClient.cpp

@@ -0,0 +1,12 @@
+std::string getSignalStateString(SignalState state) {
+    switch (state) {
+        case SignalState::RED:
+            return "RED";
+        case SignalState::YELLOW:
+            return "YELLOW";
+        case SignalState::GREEN:
+            return "GREEN";
+        default:
+            return "UNKNOWN";
+    }
+} 

docs/conflict_detection_algorithms.md

@@ -0,0 +1,197 @@
+# 机场地面冲突检测算法方案
+
+## 1. 算法概述
+
+机场地面冲突检测系统采用多层次检测策略，结合不同算法的优势，实现快速、准确、可靠的冲突检测。
+
+## 2. 常用冲突检测算法
+
+### 2.1 基于几何的检测算法
+
+#### 2.1.1 CPA (Closest Point of Approach) 算法
+
+- 原理：
+  - 计算两个移动物体之间的最近接近点
+  - 预测未来一段时间内的最小距离
+  - 判断是否小于安全阈值
+
+- 优点：
+  - 计算简单快速
+  - 适合实时系统
+  - 易于实现
+
+- 缺点：
+  - 不考虑路网约束
+  - 预测精度有限
+
+#### 2.1.2 安全区域重叠检测
+
+- 原理：
+  - 为每个物体定义安全区域（通常是椭圆或矩形）
+  - 检测安全区域是否重叠
+  - 考虑速度向量进行预测
+
+- 优点：
+  - 直观易理解
+  - 可以考虑物体实际尺寸
+  - 适合不同类型交通工具
+
+### 2.2 基于时空的检测算法
+
+#### 2.2.1 时空立方体算法
+
+- 原理：
+  - 构建 4D 时空轨迹（x, y, z, t）
+  - 检测时空轨迹的交叉
+  - 计算到达同一位置的时间差
+
+- 优点：
+  - 预测性强
+  - 可以处理复杂场景
+  - 考虑时间维度
+
+#### 2.2.2 网格化检测算法
+
+- 原理：
+  - 将场地划分为网格
+  - 预测物体在未来时间点占用的网格
+  - 检测网格占用冲突
+
+- 优点：
+  - 计算效率高
+  - 易于并行处理
+  - 适合大规模场景
+
+### 2.3 基于概率的检测算法
+
+#### 2.3.1 Monte Carlo 模拟
+
+- 原理：
+  - 考虑位置和速度的不确定性
+  - 多次随机采样模拟
+  - 计算碰撞概率
+
+- 优点：
+  - 考虑不确定性
+  - 结果更可靠
+  - 适合复杂环境
+
+#### 2.3.2 贝叶斯预测模型
+
+- 原理：
+  - 建立运动状态概率模型
+  - 实时更新碰撞概率
+  - 动态调整预警阈值
+
+- 优点：
+  - 可以学习历史数据
+  - 适应性强
+  - 预测准确度高
+
+## 3. 实现方案
+
+### 3.1 多层次检测策略
+
+采用三层检测机制：
+
+1. 第一层：快速检测
+
+   ```cpp
+   // 使用简单的 CPA 算法进行快速筛查
+   struct CPAResult {
+       double min_distance;    // 最小距离
+       double time_to_cpa;    // 到达最近点的时间
+       Point cpa_point;       // 最近接近点
+   };
+
+   CPAResult calculateCPA(const Vehicle& v1, const Vehicle& v2, double prediction_time) {
+       // 计算两个物体的最近接近点
+       // 返回最小距离和时间信息
+   }
+   ```
+
+2. 第二层：精确检测
+
+   ```cpp
+   // 使用安全区域重叠检测进行精确判断
+   struct SafetyZone {
+       Point center;
+       double length;
+       double width;
+       double orientation;
+   };
+
+   bool checkSafetyZoneOverlap(const SafetyZone& zone1, const SafetyZone& zone2) {
+       // 检查两个安全区域是否重叠
+       // 考虑物体的实际尺寸和方向
+   }
+   ```
+
+3. 第三层：预测检测
+
+   ```cpp
+   // 使用时空网格进行路径预测
+   struct TimeSpaceGrid {
+       int x, y;              // 空间坐标
+       double time;           // 时间戳
+       std::vector<int> occupants;  // 占用该格子的物体ID
+   };
+
+   bool checkPathConflict(const Vehicle& v1, const Vehicle& v2, 
+                         const std::vector<TimeSpaceGrid>& grids) {
+       // 检查未来时间窗口内的路径冲突
+       // 考虑路网约束
+   }
+   ```
+
+### 3.2 综合判断
+
+```cpp
+struct ConflictResult {
+    bool has_conflict;     // 是否存在冲突
+    double risk_level;     // 风险等级
+    double time_to_conflict; // 到冲突的时间
+    std::string conflict_type; // 冲突类型
+};
+
+ConflictResult detectConflict(const Vehicle& v1, const Vehicle& v2) {
+    // 1. 快速 CPA 检测
+    // 2. 如果可能存在冲突，进行安全区域检测
+    // 3. 对于高风险情况，进行时空路径预测
+    // 4. 综合三层结果，返回最终判断
+}
+```
+
+## 4. 实现步骤
+
+1. 第一阶段：基础功能实现
+   - 实现 CPA 算法
+   - 实现基本的安全区域检测
+   - 设计基础数据结构
+
+2. 第二阶段：增强功能
+   - 添加时空网格检测
+   - 实现路径预测
+   - 优化性能
+
+3. 第三阶段：高级特性
+   - 添加概率模型
+   - 实现自适应阈值
+   - 系统集成测试
+
+## 5. 注意事项
+
+1. 性能要求
+   - 实时响应：检测延迟不超过 100ms
+   - CPU 占用率控制在 50% 以下
+   - 内存使用不超过 1GB
+
+2. 可靠性要求
+   - 误报率控制在 1% 以下
+   - 漏报率控制在 0.1% 以下
+   - 系统可用性 99.99%
+
+3. 扩展性要求
+   - 支持新增检测算法
+   - 支持参数配置调整
+   - 支持不同类型交通工具

docs/requirements.md

@@ -8,9 +8,22 @@
 
 ### 2.2 冲突检测
 
-### 2.3 红绿灯信号处理
+    1. 路径检测
+    2. 距离检测
+    3. 路口检测
 
-### 2.4 数据采集
+### 2.3 无人车辆控制
+
+    1. 无人车控制
+        1. 控制指令
+           1. 冲突预警
+           2. 冲突告警
+           3. 安全停靠
+        2. 
+
+### 2.4 红绿灯信号处理
+
+### 2.5 数据采集
 
 1. 航空器和车辆位置数据
     1. 数据来源：

src/collector/DataSourceConfig.cpp

@@ -19,17 +19,17 @@ DataSourceConfig DataSourceConfig::fromSystemConfig(const SystemConfig& config)
     dataSourceConfig.position.auth.auth_required = config.data_source.position.auth.auth_required;
 
     // 转换无人车数据源配置
-    dataSourceConfig.vehicle.host = config.data_source.unmanned_vehicle.host;
-    dataSourceConfig.vehicle.port = config.data_source.unmanned_vehicle.port;
-    dataSourceConfig.vehicle.status_path = config.data_source.unmanned_vehicle.status_path;
-    dataSourceConfig.vehicle.command_path = config.data_source.unmanned_vehicle.command_path;
-    dataSourceConfig.vehicle.refresh_interval_ms = config.data_source.unmanned_vehicle.refresh_interval_ms;
-    dataSourceConfig.vehicle.timeout_ms = config.data_source.unmanned_vehicle.timeout_ms;
-    dataSourceConfig.vehicle.read_timeout_ms = config.data_source.unmanned_vehicle.read_timeout_ms;
-    dataSourceConfig.vehicle.auth.username = config.data_source.unmanned_vehicle.auth.username;
-    dataSourceConfig.vehicle.auth.password = config.data_source.unmanned_vehicle.auth.password;
-    dataSourceConfig.vehicle.auth.auth_path = config.data_source.unmanned_vehicle.auth.auth_path;
-    dataSourceConfig.vehicle.auth.auth_required = config.data_source.unmanned_vehicle.auth.auth_required;
+    dataSourceConfig.vehicle.host = config.data_source.vehicle.host;
+    dataSourceConfig.vehicle.port = config.data_source.vehicle.port;
+    dataSourceConfig.vehicle.status_path = config.data_source.vehicle.status_path;
+    dataSourceConfig.vehicle.command_path = config.data_source.vehicle.command_path;
+    dataSourceConfig.vehicle.refresh_interval_ms = config.data_source.vehicle.refresh_interval_ms;
+    dataSourceConfig.vehicle.timeout_ms = config.data_source.vehicle.timeout_ms;
+    dataSourceConfig.vehicle.read_timeout_ms = config.data_source.vehicle.read_timeout_ms;
+    dataSourceConfig.vehicle.auth.username = config.data_source.vehicle.auth.username;
+    dataSourceConfig.vehicle.auth.password = config.data_source.vehicle.auth.password;
+    dataSourceConfig.vehicle.auth.auth_path = config.data_source.vehicle.auth.auth_path;
+    dataSourceConfig.vehicle.auth.auth_required = config.data_source.vehicle.auth.auth_required;
 
     // 转换红绿灯数据源配置
     dataSourceConfig.traffic_light.host = config.data_source.traffic_light.host;

src/config/SystemConfigJson.cpp

@@ -1 +0,0 @@
- 

src/core/System.cpp

@@ -677,6 +677,7 @@ void System::broadcastVehicleCommand(const VehicleCommand& cmd) {
                 case CommandType::ALERT: return "ALERT";
                 case CommandType::WARNING: return "WARNING";
                 case CommandType::RESUME: return "RESUME";
+                case CommandType::PARKING: return "PARKING";
                 default: return "UNKNOWN";
             }
         }()},
@@ -687,6 +688,7 @@ void System::broadcastVehicleCommand(const VehicleCommand& cmd) {
                 case CommandReason::SPECIAL_VEHICLE: return "SPECIAL_VEHICLE";
                 case CommandReason::AIRCRAFT_PUSH: return "AIRCRAFT_PUSH";
                 case CommandReason::RESUME_TRAFFIC: return "RESUME_TRAFFIC";
+                case CommandReason::PARKING_SIDE: return "PARKING_SIDE";
                 default: return "UNKNOWN";
             }
         }()},

src/detector/CollisionDetector.cpp

@@ -8,7 +8,7 @@
 CollisionDetector::CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles)
     : airportBounds_(bounds)
     , vehicleTree_(bounds.getAirportBounds(), 8)  // 使用机场总边界初始化四叉树
-    , controllableVehicles_(controllableVehicles) {
+    , controllableVehicles_(&controllableVehicles) {
     
     // 记录初始化信息
     const auto& airportBounds = bounds.getAirportBounds();
@@ -37,7 +37,7 @@ void CollisionDetector::updateTraffic(const std::vector<Aircraft>& aircraft,
             
             // 根据配置设置车辆类型
             Vehicle updatedVehicle = vehicle;
-            const auto* config = controllableVehicles_.findVehicle(vehicle.vehicleNo);
+            const auto* config = controllableVehicles_->findVehicle(vehicle.vehicleNo);
             if (config) {
                 if (config->type == "UNMANNED") {
                     updatedVehicle.type = MovingObjectType::UNMANNED;

src/detector/CollisionDetector.h

@@ -74,7 +74,7 @@ private:
     const AirportBounds& airportBounds_;
     QuadTree<Vehicle> vehicleTree_;
     std::vector<Aircraft> aircraftData_;
-    const ControllableVehicles& controllableVehicles_;
+    const ControllableVehicles* controllableVehicles_;
     
     // 冲突记录映射表：<(id1,id2), CollisionRecord>
     std::unordered_map<std::pair<std::string, std::string>, CollisionRecord, CollisionPairHash> collisionRecords_;

src/mock/MockDataService.h

@@ -2,7 +2,7 @@
 #define AIRPORT_MOCK_MOCKDATASERVICE_H
 
 #include "types/BasicTypes.h"
-#include "spatial/AirportBounds.h"
+#include "config/AirportBounds.h"
 #include "types/TrafficLightTypes.h"
 #include <vector>
 #include <random>

src/network/HTTPClient.cpp

@@ -1,7 +1,22 @@
-#include "network/HTTPClient.h"
+#include "HTTPClient.h"
 #include "utils/Logger.h"
 #include <sstream>
 
+namespace {
+std::string getSignalStateString(SignalState state) {
+    switch (state) {
+        case SignalState::RED:
+            return "RED";
+        case SignalState::YELLOW:
+            return "YELLOW";
+        case SignalState::GREEN:
+            return "GREEN";
+        default:
+            return "UNKNOWN";
+    }
+}
+} // anonymous namespace
+
 HTTPClient::HTTPClient() {
     curl_global_init(CURL_GLOBAL_ALL);
     curl_ = curl_easy_init();
@@ -49,6 +64,7 @@ bool HTTPClient::sendCommand(const std::string& ip, int port, const VehicleComma
                 case CommandType::ALERT: return "ALERT";
                 case CommandType::WARNING: return "WARNING";
                 case CommandType::RESUME: return "RESUME";
+                case CommandType::PARKING: return "PARKING";
                 default: return "UNKNOWN";
             }
         }()},
@@ -59,28 +75,20 @@ bool HTTPClient::sendCommand(const std::string& ip, int port, const VehicleComma
                 case CommandReason::SPECIAL_VEHICLE: return "SPECIAL_VEHICLE";
                 case CommandReason::AIRCRAFT_PUSH: return "AIRCRAFT_PUSH";
                 case CommandReason::RESUME_TRAFFIC: return "RESUME_TRAFFIC";
+                case CommandReason::PARKING_SIDE: return "PARKING_SIDE";
                 default: return "UNKNOWN";
             }
         }()},
         {"latitude", command.latitude},
-        {"longitude", command.longitude}
+        {"longitude", command.longitude},
+        {"signalState", getSignalStateString(command.signalState)},
+        {"intersectionId", command.intersectionId},
+        {"relativeSpeed", command.relativeSpeed},
+        {"relativeMotionX", command.relativeMotionX},
+        {"relativeMotionY", command.relativeMotionY},
+        {"minDistance", command.minDistance}
     };
 
-    // 添加可选字段
-    if (command.type == CommandType::SIGNAL) {
-        // 信号灯指令的可选字段
-        request["signalState"] = command.signalState == SignalState::RED ? "RED" : "GREEN";
-        request["intersectionId"] = command.intersectionId;
-    }
-    
-    if (command.type == CommandType::ALERT || command.type == CommandType::WARNING) {
-        // 告警/预警指令的可选字段
-        request["relativeSpeed"] = command.relativeSpeed;
-        request["relativeMotionX"] = command.relativeMotionX;
-        request["relativeMotionY"] = command.relativeMotionY;
-        request["minDistance"] = command.minDistance;
-    }
-
     std::string request_body = request.dump();
     response_buffer_.clear();
 
@@ -131,4 +139,4 @@ bool HTTPClient::sendCommand(const std::string& ip, int port, const VehicleComma
         Logger::error("Command failed with HTTP code: ", http_code, " response: ", response_buffer_);
         return false;
     }
-} 
+}

src/network/HTTPDataSource.cpp

@@ -1,4 +1,5 @@
-#include "network/HTTPDataSource.h"
+#include "HTTPDataSource.h"
+#include "HTTPClient.h"
 #include "utils/Logger.h"
 #include <nlohmann/json.hpp>
 #include <sstream>
@@ -264,6 +265,19 @@ bool HTTPDataSource::fetchTrafficLightSignals(std::vector<TrafficLightSignal>& s
     return parseTrafficLightResponse(response, signals);
 }
 
+std::string getSignalStateString(SignalState state) {
+    switch (state) {
+        case SignalState::RED:
+            return "RED";
+        case SignalState::YELLOW:
+            return "YELLOW";
+        case SignalState::GREEN:
+            return "GREEN";
+        default:
+            return "GREEN";  // 默认为绿灯
+    }
+}
+
 bool HTTPDataSource::sendUnmannedVehicleCommand(const std::string& vehicle_id, const VehicleCommand& command) {
     std::lock_guard<std::mutex> lock(mutex_);
     
@@ -310,14 +324,11 @@ bool HTTPDataSource::sendUnmannedVehicleCommand(const std::string& vehicle_id, c
             }
         }()},
         {"latitude", command.latitude},
-        {"longitude", command.longitude}
+        {"longitude", command.longitude},
+        {"signalState", getSignalStateString(command.signalState)},
+        {"intersectionId", command.intersectionId}
     };
 
-    if (command.type == CommandType::SIGNAL) {
-        request["signalState"] = command.signalState == SignalState::RED ? "RED" : "GREEN";
-        request["intersectionId"] = command.intersectionId;
-    }
-    
     if (command.type == CommandType::ALERT || command.type == CommandType::WARNING) {
         request["relativeSpeed"] = command.relativeSpeed;
         request["relativeMotionX"] = command.relativeMotionX;

src/spatial/QuadTree.cpp

@@ -1,17 +1,5 @@
 #include "QuadTree.h"
 
-bool Bounds::contains(const Vector2D& point) const {
-    return point.x >= x && point.x <= (x + width) &&
-           point.y >= y && point.y <= (y + height);
-}
-
-bool Bounds::intersects(const Bounds& other) const {
-    return !(other.x > (x + width) ||
-            (other.x + other.width) < x ||
-            other.y > (y + height) ||
-            (other.y + other.height) < y);
-}
-
 // 显式实例化常用类型
 template class QuadTree<Vehicle>;
 template class QuadTree<Aircraft>; 

src/types/VehicleCommand.h

@@ -9,7 +9,8 @@ enum class CommandType {
     ALERT,     // 告警指令
     SIGNAL,    // 信号灯指令
     WARNING,   // 预警指令
-    RESUME     // 恢复指令
+    RESUME,    // 恢复指令
+    PARKING    // 安全停靠
 };
 
 // 信号灯状态
@@ -25,7 +26,8 @@ enum class CommandReason {
     AIRCRAFT_CROSSING,  // 航空器交叉
     SPECIAL_VEHICLE,    // 特勤车辆
     AIRCRAFT_PUSH,      // 航空器推出
-    RESUME_TRAFFIC      // 恢复通行
+    RESUME_TRAFFIC,     // 恢复通行
+    PARKING_SIDE        // 安全停靠
 };
 
 struct VehicleCommand {

src/utils/Logger.cpp

@@ -1,32 +1,4 @@
 #include "Logger.h"
-#include <chrono>
-#include <iomanip>
-#include <sstream>
-
-LogLevel& Logger::currentLevel() {
-    static LogLevel level = LogLevel::INFO;
-    return level;
-}
-
-void Logger::setLogLevel(LogLevel level) {
-    currentLevel() = level;
-}
-
-template<typename... Args>
-void Logger::log(const char* level, Args... args) {
-    auto now = std::chrono::system_clock::now();
-    auto now_c = std::chrono::system_clock::to_time_t(now);
-    auto now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
-        now.time_since_epoch()) % 1000;
-    
-    std::stringstream ss;
-    ss << std::put_time(std::localtime(&now_c), "%Y-%m-%d %H:%M:%S")
-       << '.' << std::setfill('0') << std::setw(3) << now_ms.count()
-       << " [" << level << "] ";
-    
-    (ss << ... << args) << std::endl;
-    std::cerr << ss.str();
-}
 
 // 显式实例化模板，以支持常用类型
 template void Logger::log(const char* level, const char* msg);

src/utils/Logger.h

@@ -84,6 +84,7 @@ private:
            << '.' << std::setfill('0') << std::setw(3) << now_ms.count()
            << " [" << level << "] ";
         
+        ss << std::fixed << std::setprecision(14);
         (ss << ... << args) << std::endl;
         
         std::lock_guard<std::mutex> lock(getMutex());

src/vehicle/ControllableVehicles.cpp

@@ -93,12 +93,14 @@ bool ControllableVehicles::sendCommand(const std::string& vehicleNo, const Vehic
             Logger::info("Successfully sent command to vehicle ", vehicleNo, ": ", 
                 command.type == CommandType::SIGNAL ? "SIGNAL" :
                 command.type == CommandType::ALERT ? "ALERT" :
-                command.type == CommandType::WARNING ? "WARNING" : "RESUME",
+                command.type == CommandType::WARNING ? "WARNING" :
+                command.type == CommandType::PARKING ? "PARKING" : "RESUME",
                 "/",
                 command.reason == CommandReason::TRAFFIC_LIGHT ? "TRAFFIC_LIGHT" :
                 command.reason == CommandReason::AIRCRAFT_CROSSING ? "AIRCRAFT_CROSSING" :
                 command.reason == CommandReason::SPECIAL_VEHICLE ? "SPECIAL_VEHICLE" :
-                command.reason == CommandReason::AIRCRAFT_PUSH ? "AIRCRAFT_PUSH" : "RESUME_TRAFFIC");
+                command.reason == CommandReason::AIRCRAFT_PUSH ? "AIRCRAFT_PUSH" :
+                command.reason == CommandReason::PARKING_SIDE ? "PARKING_SIDE" : "RESUME_TRAFFIC");
         } else {
             Logger::error("Failed to send command to vehicle ", vehicleNo);
         }

tests/CMakeLists.txt

@@ -2,10 +2,11 @@
 set(TEST_SOURCES
     BasicCollisionTest.cpp
     CollisionDetectorTest.cpp
+    TrafficLightDetectorTest.cpp
+    SafetyZoneTest.cpp
     BasicTypesTest.cpp
     HTTPDataSourceTest.cpp
     DataCollectorTest.cpp
-    TrafficLightDetectorTest.cpp
 )
 
 # 创建测试可执行文件
@@ -14,7 +15,7 @@ add_executable(unit_tests ${TEST_SOURCES})
 # 链接需要的库
 target_link_libraries(unit_tests
     PRIVATE
-    airport_core
+    ${PROJECT_NAME}_lib
     GTest::gtest_main
     GTest::gmock_main
 )

tests/HTTPDataSourceTest.cpp

@@ -113,6 +113,16 @@ TEST_F(HTTPDataSourceTest, TrafficLightSignals) {
     DataSourceConfig config;
     config.position.host = "localhost";
     config.position.port = 8081;
+    config.position.aircraft_path = "/openApi/getCurrentFlightPositions";
+    config.position.vehicle_path = "/openApi/getCurrentVehiclePositions";
+    config.position.auth.auth_required = true;
+    config.position.auth.auth_path = "/login";
+    config.position.auth.username = "dianxin";
+    config.position.auth.password = "dianxin@123";
+    config.position.timeout_ms = 5000;
+    config.position.read_timeout_ms = 2000;
+    config.position.refresh_interval_ms = 100;
+    
     config.traffic_light.host = "localhost";
     config.traffic_light.port = 8081;
     config.traffic_light.signal_path = "/api/VehicleCommandInfo";
@@ -123,7 +133,12 @@ TEST_F(HTTPDataSourceTest, TrafficLightSignals) {
     HTTPDataSource source(config);
     ASSERT_TRUE(source.connect());
 
-    // 先发送 GREEN 指令解除 RED 状态
+    // 先获取车辆状态来初始化车辆
+    std::vector<Vehicle> vehicles;
+    ASSERT_TRUE(source.fetchPositionVehicleData(vehicles)) << "获取车辆状态失败";
+    ASSERT_FALSE(vehicles.empty()) << "车辆数据为空";
+
+    // 发送绿灯指令（优先级 2）
     VehicleCommand green_command;
     green_command.vehicleId = "QN001";
     green_command.type = CommandType::SIGNAL;
@@ -134,60 +149,65 @@ TEST_F(HTTPDataSourceTest, TrafficLightSignals) {
         std::chrono::system_clock::now().time_since_epoch()).count();
     ASSERT_TRUE(source.sendUnmannedVehicleCommand(green_command.vehicleId, green_command)) << "发送绿灯指令失败";
 
-    // 等待一小段时间确保 GREEN 指令生效
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    // 等待绿灯指令生效
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
 
-    // 再发送 RESUME 指令解除其他可能的状态
-    VehicleCommand resume_command;
-    resume_command.vehicleId = "QN001";
-    resume_command.type = CommandType::RESUME;
-    resume_command.reason = CommandReason::RESUME_TRAFFIC;
-    resume_command.timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
+    // 发送红灯指令（优先级 4）
+    VehicleCommand red_command;
+    red_command.vehicleId = "QN001";
+    red_command.type = CommandType::SIGNAL;
+    red_command.reason = CommandReason::TRAFFIC_LIGHT;
+    red_command.signalState = SignalState::RED;
+    red_command.intersectionId = "INTERSECTION_001";
+    red_command.latitude = 36.36;
+    red_command.longitude = 120.08;
+    red_command.timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
         std::chrono::system_clock::now().time_since_epoch()).count();
-    ASSERT_TRUE(source.sendUnmannedVehicleCommand(resume_command.vehicleId, resume_command)) << "发送恢复指令失败";
+    ASSERT_TRUE(source.sendUnmannedVehicleCommand(red_command.vehicleId, red_command)) << "发送红灯指令失败";
 
-    // 等待一小段时间确保 RESUME 指令生效
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    // 等待红灯指令生效
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
 
-    // 发送红绿灯信号指令
-    VehicleCommand command;
-    command.vehicleId = "QN001";
-    command.type = CommandType::SIGNAL;
-    command.reason = CommandReason::TRAFFIC_LIGHT;
-    command.signalState = SignalState::RED;
-    command.intersectionId = "INTERSECTION_001";
-    command.latitude = 36.36;
-    command.longitude = 120.08;
-    command.timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
+
+    // 最后发送绿灯指令
+    green_command.timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
         std::chrono::system_clock::now().time_since_epoch()).count();
-
-    ASSERT_TRUE(source.sendUnmannedVehicleCommand(command.vehicleId, command)) << "获取红绿灯信号失败";
+    ASSERT_TRUE(source.sendUnmannedVehicleCommand(green_command.vehicleId, green_command)) << "发送最终绿灯指令失败";
 }
 
 TEST_F(HTTPDataSourceTest, UnmannedVehicleCommand) {
     DataSourceConfig config;
+    config.position.host = "localhost";
+    config.position.port = 8081;
+    config.position.aircraft_path = "/openApi/getCurrentFlightPositions";
+    config.position.vehicle_path = "/openApi/getCurrentVehiclePositions";
+    config.position.auth.auth_required = true;
+    config.position.auth.auth_path = "/login";
+    config.position.auth.username = "dianxin";
+    config.position.auth.password = "dianxin@123";
+    config.position.timeout_ms = 5000;
+    config.position.read_timeout_ms = 2000;
+    config.position.refresh_interval_ms = 100;
+    
     config.vehicle.host = "localhost";
     config.vehicle.port = 8081;
     config.vehicle.command_path = "/api/VehicleCommandInfo";
+    config.vehicle.auth.auth_required = true;
+    config.vehicle.auth.auth_path = "/login";
+    config.vehicle.auth.username = "dianxin";
+    config.vehicle.auth.password = "dianxin@123";
+    config.vehicle.timeout_ms = 5000;
+    config.vehicle.read_timeout_ms = 2000;
 
     HTTPDataSource source(config);
-    ASSERT_TRUE(source.connect());
+    ASSERT_TRUE(source.connect()) << "连接失败";
 
-    // 先发送 GREEN 指令解除 RED 状态
-    VehicleCommand green_command;
-    green_command.vehicleId = "QN001";
-    green_command.type = CommandType::SIGNAL;
-    green_command.reason = CommandReason::TRAFFIC_LIGHT;
-    green_command.signalState = SignalState::GREEN;
-    green_command.intersectionId = "INTERSECTION_001";
-    green_command.timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
-        std::chrono::system_clock::now().time_since_epoch()).count();
-    ASSERT_TRUE(source.sendUnmannedVehicleCommand(green_command.vehicleId, green_command)) << "发送绿灯指令失败";
+    // 先获取车辆状态来初始化车辆
+    std::vector<Vehicle> vehicles;
+    ASSERT_TRUE(source.fetchPositionVehicleData(vehicles)) << "获取车辆状态失败";
+    ASSERT_FALSE(vehicles.empty()) << "车辆数据为空";
 
-    // 等待一小段时间确保 GREEN 指令生效
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-
-    // 再发送 RESUME 指令解除其他可能的状态
+    // 先发送 RESUME 指令清除所有状态
     VehicleCommand resume_command;
     resume_command.vehicleId = "QN001";
     resume_command.type = CommandType::RESUME;
@@ -196,24 +216,43 @@ TEST_F(HTTPDataSourceTest, UnmannedVehicleCommand) {
         std::chrono::system_clock::now().time_since_epoch()).count();
     ASSERT_TRUE(source.sendUnmannedVehicleCommand(resume_command.vehicleId, resume_command)) << "发送恢复指令失败";
 
-    // 等待一小段时间确保 RESUME 指令生效
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    // 等待 RESUME 指令生效
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
 
-    // 发送告警指令
-    VehicleCommand command;
-    command.vehicleId = "QN001";
-    command.type = CommandType::ALERT;
-    command.reason = CommandReason::AIRCRAFT_CROSSING;
-    command.relativeSpeed = 100;
-    command.relativeMotionX = 100;
-    command.relativeMotionY = 100;
-    command.minDistance = 100;
-    command.latitude = 36.36;
-    command.longitude = 120.08;
-    command.timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
+    // 发送 WARNING 指令（优先级 3）
+    VehicleCommand warning_command;
+    warning_command.vehicleId = "QN001";
+    warning_command.type = CommandType::WARNING;
+    warning_command.reason = CommandReason::AIRCRAFT_CROSSING;
+    warning_command.relativeSpeed = 50;
+    warning_command.relativeMotionX = 50;
+    warning_command.relativeMotionY = 50;
+    warning_command.timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
         std::chrono::system_clock::now().time_since_epoch()).count();
+    ASSERT_TRUE(source.sendUnmannedVehicleCommand(warning_command.vehicleId, warning_command)) << "发送预警指令失败";
 
-    ASSERT_TRUE(source.sendUnmannedVehicleCommand(command.vehicleId, command)) << "发送无人车指令失败";
+    // 等待 WARNING 指令生效
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+
+    // 发送 ALERT 指令（优先级 5）
+    VehicleCommand alert_command;
+    alert_command.vehicleId = "QN001";
+    alert_command.type = CommandType::ALERT;
+    alert_command.reason = CommandReason::AIRCRAFT_CROSSING;
+    alert_command.relativeSpeed = 100;
+    alert_command.relativeMotionX = 100;
+    alert_command.relativeMotionY = 100;
+    alert_command.timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
+        std::chrono::system_clock::now().time_since_epoch()).count();
+    ASSERT_TRUE(source.sendUnmannedVehicleCommand(alert_command.vehicleId, alert_command)) << "发送告警指令失败";
+
+    // 等待 ALERT 指令生效
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+
+    // 最后发送 RESUME 指令清除状态
+    resume_command.timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
+        std::chrono::system_clock::now().time_since_epoch()).count();
+    ASSERT_TRUE(source.sendUnmannedVehicleCommand(resume_command.vehicleId, resume_command)) << "发送最终恢复指令失败";
 }
 
 TEST_F(HTTPDataSourceTest, TrafficLightAndUnmannedAuthFailure) {

tests/TrafficLightDetectorTest.cpp

@@ -8,12 +8,20 @@
 #include "utils/Logger.h"
 #include "types/VehicleCommand.h"
 #include "types/TrafficLightTypes.h"
+#include "core/System.h"
 
-class MockControllableVehicles : public ControllableVehicles {
+// Mock System 类
+class MockSystem : public System {
 public:
-    explicit MockControllableVehicles() : ControllableVehicles("config/vehicle_speed_limits.json") {}
-    MOCK_METHOD(bool, isControllable, (const std::string& vehicleNo), (const, override));
-    MOCK_METHOD(void, sendCommand, (const std::string& vehicleNo, const VehicleCommand& command), (override));
+    MOCK_METHOD(void, broadcastTrafficLightCommand, (const std::string&, const VehicleCommand&));
+    static MockSystem& getInstance() {
+        static MockSystem instance;
+        return instance;
+    }
+private:
+    MockSystem() = default;
+    MockSystem(const MockSystem&) = delete;
+    MockSystem& operator=(const MockSystem&) = delete;
 };
 
 class TrafficLightDetectorTest : public ::testing::Test {
@@ -25,7 +33,8 @@ protected:
         intersection.name = "Test Intersection";
         intersection.position.latitude = 36.36;
         intersection.position.longitude = 120.08;
-        intersection.radius = 50.0;
+        intersection.position.altitude = 9.5;
+        intersection.width = 50.0;
         intersection.trafficLightId = "TL001";
         
         // 创建临时配置文件
@@ -37,18 +46,25 @@ protected:
             {"name", intersection.name},
             {"position", {
                 {"latitude", intersection.position.latitude},
-                {"longitude", intersection.position.longitude}
+                {"longitude", intersection.position.longitude},
+                {"altitude", intersection.position.altitude}
             }},
-            {"radius", intersection.radius},
-            {"trafficLightId", intersection.trafficLightId}
+            {"width", intersection.width},
+            {"trafficLightId", intersection.trafficLightId},
+            {"safetyZone", {
+                {"aircraftRadius", 50.0},
+                {"vehicleRadius", 50.0}
+            }}
         });
         config_file << j.dump(4);
         config_file.close();
         
         intersectionConfig = IntersectionConfig::load("test_intersections.json");
-        
-        mockControllableVehicles = std::make_shared<::testing::NiceMock<MockControllableVehicles>>();
-        detector = std::make_unique<TrafficLightDetector>(intersectionConfig, *mockControllableVehicles);
+        detector = std::make_unique<TrafficLightDetector>(
+            intersectionConfig, 
+            ControllableVehicles::getInstance(),
+            MockSystem::getInstance()
+        );
     }
 
     void TearDown() override {
@@ -78,7 +94,6 @@ protected:
 
     std::unique_ptr<TrafficLightDetector> detector;
     IntersectionConfig intersectionConfig;
-    std::shared_ptr<MockControllableVehicles> mockControllableVehicles;
 };
 
 // 测试红绿灯信号处理
@@ -89,19 +104,11 @@ TEST_F(TrafficLightDetectorTest, SignalProcessing) {
     
     // 测试红灯
     TrafficLightSignal redSignal = createTestSignal("TL001", SignalStatus::RED);
-    EXPECT_CALL(*mockControllableVehicles, isControllable("V001"))
-        .WillOnce(::testing::Return(true));
-    EXPECT_CALL(*mockControllableVehicles, sendCommand("V001", ::testing::_))
-        .Times(1);
     
     detector->processSignal(redSignal, vehicles);
     
     // 测试绿灯
     TrafficLightSignal greenSignal = createTestSignal("TL001", SignalStatus::GREEN);
-    EXPECT_CALL(*mockControllableVehicles, isControllable("V001"))
-        .WillOnce(::testing::Return(true));
-    EXPECT_CALL(*mockControllableVehicles, sendCommand("V001", ::testing::_))
-        .Times(1);
     
     detector->processSignal(greenSignal, vehicles);
 }
@@ -113,10 +120,6 @@ TEST_F(TrafficLightDetectorTest, InvalidSignal) {
     
     // 测试无效的红绿灯ID
     TrafficLightSignal invalidSignal = createTestSignal("TL999", SignalStatus::RED);
-    EXPECT_CALL(*mockControllableVehicles, isControllable(::testing::_))
-        .Times(0);
-    EXPECT_CALL(*mockControllableVehicles, sendCommand(::testing::_, ::testing::_))
-        .Times(0);
     
     detector->processSignal(invalidSignal, vehicles);
 }
@@ -126,20 +129,12 @@ TEST_F(TrafficLightDetectorTest, EdgeCases) {
     // 测试空车辆列表
     std::vector<Vehicle> emptyVehicles;
     TrafficLightSignal signal = createTestSignal("TL001", SignalStatus::RED);
-    EXPECT_CALL(*mockControllableVehicles, isControllable(::testing::_))
-        .Times(0);
-    EXPECT_CALL(*mockControllableVehicles, sendCommand(::testing::_, ::testing::_))
-        .Times(0);
     
     detector->processSignal(signal, emptyVehicles);
     
     // 测试交叉口范围外的车辆
     Vehicle farVehicle = createTestVehicle("V002", 36.40, 120.12);
     std::vector<Vehicle> vehicles = {farVehicle};
-    EXPECT_CALL(*mockControllableVehicles, isControllable(::testing::_))
-        .Times(0);
-    EXPECT_CALL(*mockControllableVehicles, sendCommand(::testing::_, ::testing::_))
-        .Times(0);
     
     detector->processSignal(signal, vehicles);
 } 

tools/map_websocket.html

@@ -143,6 +143,21 @@
             pointer-events: none;
             z-index: 1000;
         }
+        .safety-border {
+            width: 100%;
+            height: 100%;
+            border: 2px solid;
+            background: none;
+        }
+        .emergency-border {
+            border-color: rgba(255, 0, 0, 0.8);
+        }
+        .core-border {
+            border-color: rgba(255, 165, 0, 0.6);
+        }
+        .warning-border {
+            border-color: rgba(255, 255, 0, 0.4);
+        }
     </style>
 </head>
 <body>
@@ -275,11 +290,43 @@
             
             // 根据ID前缀确定图标类型
             if (data.object_type === 'aircraft') {
-                iconClass = 'aircraft-icon';  // 六形
+                iconClass = 'aircraft-icon';
+                
+                // 创建或更新安全边框
+                const borders = [
+                    {size: 250, class: 'warning-border'},  // 外围预警区
+                    {size: 150, class: 'core-border'},      // 核心安全区
+                    {size: 100, class: 'emergency-border'}  // 紧急制动区
+                ];
+                
+                borders.forEach(border => {
+                    const borderId = id + '_' + border.class;
+                    let borderMarker = markers.get(borderId);
+                    
+                    if (!borderMarker) {
+                        // 创建边框标记
+                        borderMarker = L.marker(position, {
+                            icon: L.divIcon({
+                                className: 'safety-border ' + border.class,
+                                iconSize: [border.size, border.size],
+                                iconAnchor: [border.size/2, border.size/2]
+                            }),
+                            rotationAngle: data.heading || 0,
+                            rotationOrigin: 'center center'
+                        }).addTo(map);
+                        markers.set(borderId, borderMarker);
+                    } else {
+                        // 更新边框位置和方向
+                        borderMarker.setLatLng(position);
+                        if (data.heading !== undefined) {
+                            borderMarker.setRotationAngle(data.heading);
+                        }
+                    }
+                });
             } else if (id.startsWith('TQ')) {
-                iconClass = 'special-vehicle-icon';  // 正方形
+                iconClass = 'special-vehicle-icon';
             } else {
-                iconClass = 'vehicle-icon';  // 三角形
+                iconClass = 'vehicle-icon';
             }
 
             let marker = markers.get(id);
@@ -287,10 +334,10 @@
                 // 创建新标记
                 marker = L.marker(position, {
                     icon: createIcon(iconClass),
-                    rotationAngle: data.heading || 0,  // 设置初始航向
-                    rotationOrigin: 'center center'    // 设置旋转中心
+                    rotationAngle: data.heading || 0,
+                    rotationOrigin: 'center center'
                 }).addTo(map);
-                marker.bindTooltip(id);  // 添加标签显示ID
+                marker.bindTooltip(id);
                 markers.set(id, marker);
             } else {
                 // 更新现有标记位置和航向
@@ -482,17 +529,19 @@
                                 updateVehicleCommand(data.vehicleId, data.commandType);
                                 // 为控制指令添加中文描述
                                 const commandTypes = {
-                                    'SIGNAL': '信号灯指令',
-                                    'ALERT': '告警指令',
-                                    'WARNING': '预警指令',
-                                    'RESUME': '恢复指令'
+                                    'SIGNAL': '信号灯',
+                                    'ALERT': '告警',
+                                    'WARNING': '预警',
+                                    'RESUME': '恢复',
+                                    'PARKING': '安全停靠'
                                 };
                                 const reasons = {
                                     'TRAFFIC_LIGHT': '红绿灯控制',
                                     'AIRCRAFT_CROSSING': '航空器交叉',
                                     'SPECIAL_VEHICLE': '特勤车辆',
                                     'AIRCRAFT_PUSH': '航空器推出',
-                                    'RESUME_TRAFFIC': '恢复通行'
+                                    'RESUME_TRAFFIC': '恢复通行',
+                                    'PARKING_SIDE': '安全停靠'
                                 };
                                 message = `收到车辆控制指令:\n车辆: ${data.vehicleId}\n` +
                                         `指令类型: ${commandTypes[data.commandType] || data.commandType}\n` +
@@ -541,9 +590,12 @@
             ws.close();
             ws = null;
             
-            // 清除所有标记
-            markers.forEach(marker => map.removeLayer(marker));
+            // 清除所有标记（包括边框标记）
+            markers.forEach((marker, key) => {
+                map.removeLayer(marker);
+            });
             markers.clear();
+            
             trafficLights.forEach(light => map.removeLayer(light));
             trafficLights.clear();
         }

tools/mock_server.py

@@ -147,7 +147,7 @@ vehicle_data = [
         "longitude": POINT_T12["longitude"],
         "latitude": POINT_T12["latitude"],
         "time": int(time.time() * 1000),
-        "direction": 135.0,  # 东南方向约为135度
+        "direction": 90.0,  # 向东方向为90度
         "speed": DEFAULT_VEHICLE_SPEED
     },
     {
@@ -230,26 +230,17 @@ class VehicleState:
         # 如果是红绿灯状态，更新状态和指令
         if command_type in ["RED", "GREEN", "YELLOW"]:
             self.traffic_light_state = command_type
+            self.current_command = command_type
+            self.command_priority = priority_map[command_type]
+            
             if command_type == "RED":
-                self.current_command = command_type
-                self.command_priority = priority_map[command_type]
                 self.is_running = False
                 print(f"车辆 {self.vehicle_no} 收到红灯指令，停止运行")
             elif command_type == "YELLOW":
                 # 黄灯表示红绿灯故障，清除当前红绿灯状态，允许车辆继续行驶
-                self.current_command = command_type
-                self.command_priority = priority_map[command_type]
                 self.is_running = True
                 print(f"车辆 {self.vehicle_no} 收到黄灯指令（红绿灯故障），继续行驶")
             else:  # GREEN
-                # 清除 RED 指令
-                if self.current_command == "RED":
-                    print(f"车辆 {self.vehicle_no} 收到绿灯指令，清除红灯指令")
-                
-                # 设置为绿灯指令
-                self.current_command = command_type
-                self.command_priority = priority_map[command_type]
-                
                 # 如果没有其他阻塞性指令，允许移动
                 if self.current_command not in ["ALERT", "WARNING"]:
                     self.is_running = True
@@ -258,6 +249,8 @@ class VehicleState:
             # 其他指令正常更新
             self.current_command = command_type
             self.command_priority = priority_map.get(command_type, 0)
+            if command_type == "RESUME":
+                self.traffic_light_state = None  # 清除红绿灯状态
         
         print(f"更新车辆 {self.vehicle_no} 状态: command={self.current_command}, traffic_light={self.traffic_light_state}, priority={self.command_priority}")
 
@@ -325,10 +318,15 @@ def handle_vehicle_command():
         print(f"收到车辆控制指令: vehicle_id={vehicle_id}, type={command_type}, reason={reason}, signal_state={signal_state}")
         print(f"完整请求数据: {data}")
         
-        # 将 SIGNAL 指令转换为 RED、YELLOW 或 GREEN
-        if command_type == "SIGNAL":
-            command_type = signal_state  # 直接使用信号状态作为命令类型
-            print(f"转换 SIGNAL 指令为: {command_type}")
+        # 检查 SIGNAL 类型命令必须包含 signalState
+        if command_type == "SIGNAL" and not signal_state:
+            print(f"SIGNAL 类型命令缺少 signalState")
+            return jsonify({
+                "code": 400,
+                "msg": "SIGNAL command must include signalState",
+                "transId": data.get("transId"),
+                "timestamp": int(time.time() * 1000)
+            }), 400
         
         # 检查车辆是否存在
         vehicle_state = vehicle_states.get(vehicle_id)
@@ -347,7 +345,7 @@ def handle_vehicle_command():
         
         # 特勤车只响应红绿灯信号
         if vehicle_id.startswith("TQ"):
-            if command_type not in ["RED", "GREEN"]:
+            if command_type != "SIGNAL":
                 print(f"特勤车辆忽略非红绿灯指令: {vehicle_id}")
                 return jsonify({
                     "code": 200,
@@ -356,7 +354,7 @@ def handle_vehicle_command():
                     "timestamp": int(time.time() * 1000)
                 })
             # 更新红绿灯状态和指令状态
-            vehicle_state.update_command(command_type, target_lat, target_lon)
+            vehicle_state.update_command(signal_state, target_lat, target_lon)
             print(f"特勤车 {vehicle_id} 更新状态: command={command_type}, traffic_light={vehicle_state.traffic_light_state}")
             return jsonify({
                 "code": 200,
@@ -366,13 +364,14 @@ def handle_vehicle_command():
             })
         
         # 检查指令优先级并添加详细日志
-        can_override = vehicle_state.can_be_overridden_by(command_type)
+        check_command = signal_state if command_type == "SIGNAL" else command_type
+        can_override = vehicle_state.can_be_overridden_by(check_command)
         print(f"指令优先级检查: vehicle={vehicle_id}, current_command={vehicle_state.current_command}, "
-              f"new_command={command_type}, can_override={can_override}")
+              f"new_command={check_command}, can_override={can_override}")
         
         if not can_override:
             print(f"指令优先级过低: vehicle={vehicle_id}, current_priority={vehicle_state.command_priority}, "
-                  f"command={command_type}")
+                  f"command={check_command}")
             return jsonify({
                 "code": 400,
                 "msg": "Command priority too low",
@@ -381,7 +380,17 @@ def handle_vehicle_command():
             })
         
         # 处理不同类型的指令
-        if command_type in ["ALERT", "WARNING"]:
+        if command_type == "SIGNAL":
+            # 更新红绿灯状态和指令状态
+            vehicle_state.update_command(signal_state, target_lat, target_lon)
+            print(f"车辆 {vehicle_id} 更新状态: command={command_type}, traffic_light={vehicle_state.traffic_light_state}")
+            return jsonify({
+                "code": 200,
+                "msg": "Traffic light state updated",
+                "transId": data.get("transId"),
+                "timestamp": int(time.time() * 1000)
+            })
+        elif command_type in ["ALERT", "WARNING"]:
             # 查找当前车辆
             current_vehicle = None
             for v in vehicle_data:
@@ -399,12 +408,15 @@ def handle_vehicle_command():
             vehicle_state.target_lat = target_lat
             vehicle_state.target_lon = target_lon
             # 立即更新车辆速度
-            current_vehicle["speed"] = 0
+            if current_vehicle:
+                current_vehicle["speed"] = 0
             
-        elif command_type in ["RED", "GREEN"]:
-            print(f"执行红绿灯指令: vehicle_id={vehicle_id}, state={command_type}")
-            # 红绿灯状态的更新由 update_command 处理
-            pass
+            return jsonify({
+                "code": 200,
+                "msg": "Command executed successfully",
+                "transId": data.get("transId"),
+                "timestamp": int(time.time() * 1000)
+            })
         
         elif command_type == "RESUME":
             print(f"执行恢复指令: vehicle_id={vehicle_id}")
@@ -428,7 +440,7 @@ def handle_vehicle_command():
                 
                 # 更新车辆运行状态
                 vehicle_state.is_running = vehicle_state.can_move()
-                if vehicle_state.is_running:
+                if vehicle_state.is_running and current_vehicle:
                     print(f"车辆 {vehicle_id} 恢复运行")
                     current_vehicle["speed"] = DEFAULT_VEHICLE_SPEED
                 
@@ -448,21 +460,6 @@ def handle_vehicle_command():
                     "timestamp": int(time.time() * 1000)
                 })
         
-        # 更新车辆状态
-        vehicle_state.update_command(command_type, target_lat, target_lon)
-        vehicle_state.command_reason = reason
-        vehicle_state.last_command_time = time.time()
-        
-        # 更新车辆运行状态
-        vehicle_state.is_running = vehicle_state.can_move()
-        if not vehicle_state.is_running:
-            vehicle_state.target_speed = 0
-        
-        print(f"Vehicle {vehicle_id} state updated: running={vehicle_state.is_running}, "
-              f"command={vehicle_state.current_command}, traffic_light={vehicle_state.traffic_light_state}, "
-              f"reason={reason}, priority={vehicle_state.command_priority}, "
-              f"target_speed={vehicle_state.target_speed}, brake_mode={vehicle_state.brake_mode}")
-        
         return jsonify({
             "code": 200,
             "msg": "Command executed successfully",
@@ -491,15 +488,14 @@ def calculate_distance_to_intersection(vehicle, intersection):
     lon_diff = (vehicle_lon - intersection_lon) * (111319.9 * math.cos(math.radians(vehicle_lat)))
     return math.sqrt(lat_diff * lat_diff + lon_diff * lon_diff)
 
-def get_nearest_intersection(vehicle):
-    """获取车辆最近的路口及其红绿灯状态"""
-    west_dist = calculate_distance_to_intersection(vehicle, T2_INTERSECTION)
-    east_dist = calculate_distance_to_intersection(vehicle, T6_INTERSECTION)
-    
-    if west_dist <= east_dist:
-        return T2_INTERSECTION, traffic_light_data[0], west_dist  # TL001
-    else:
-        return T6_INTERSECTION, traffic_light_data[1], east_dist  # TL002
+def calculate_path_direction(start_point, end_point):
+    """计算两点之间的方向角度（相对于正北方向，顺时针）"""
+    dlat = end_point["latitude"] - start_point["latitude"]
+    dlon = end_point["longitude"] - start_point["longitude"]
+    angle_rad = math.atan2(dlon, dlat)  # 使用 atan2 计算角度
+    angle_deg = math.degrees(angle_rad)
+    # 转换为顺时针方向（正北为0度）
+    return (90 - angle_deg) % 360
 
 def get_front_traffic_light(vehicle, distance_to_west, distance_to_east):
     """获取车辆前方的红绿灯状态"""
@@ -509,17 +505,46 @@ def get_front_traffic_light(vehicle, distance_to_west, distance_to_east):
         if "phase" not in vehicle:
             vehicle["phase"] = 0
                 
-        # QN001 的路线：西路口北侧 -> 东 -> 北
-        if vehicle["phase"] == 0:  # 南北移动
-            # 在西路口以北时，向南移动需要判断西路口红绿灯
-            if vehicle["direction"] == -1 and vehicle["latitude"] > T2_INTERSECTION["latitude"]:
+        # QN001 的路线：T1 -> T2 -> T4
+        if vehicle["phase"] == 0:  # T1 -> T2 阶段
+            # 计算 T1 到 T2 的实际方向
+            actual_direction = calculate_path_direction(POINT_T1, POINT_T2)
+            # 允许 15 度的误差范围
+            direction_diff = abs(vehicle["direction"] - actual_direction)
+            direction_diff = min(direction_diff, 360 - direction_diff)
+            
+            # 在西路口以北时，向 T2 移动需要判断西路口红绿灯
+            if direction_diff <= 15 and vehicle["latitude"] > T2_INTERSECTION["latitude"]:
                 return traffic_light_data[0], distance_to_west  # 西路口红绿灯
     
     elif vehicle["vehicleNo"] == "QN002":
-        # 在东路口以西时，向东移动需要判断东路红绿灯
-        if vehicle["direction"] == 1 and vehicle["longitude"] < T6_INTERSECTION["longitude"]:
+        # 计算 T12 到 T8 的实际方向
+        actual_direction = calculate_path_direction(POINT_T12, POINT_T8)
+        # 允许 15 度的误差范围
+        direction_diff = abs(vehicle["direction"] - actual_direction)
+        direction_diff = min(direction_diff, 360 - direction_diff)
+        
+        # 在东路口以西时，向 T8 移动需要判断东路红绿灯
+        if direction_diff <= 15 and vehicle["longitude"] < T6_INTERSECTION["longitude"]:
             return traffic_light_data[1], distance_to_east  # 东路口红绿灯
     
+    elif vehicle["vehicleNo"] == "TQ001":
+        # 确保 phase 属性存在
+        if "phase" not in vehicle:
+            vehicle["phase"] = 0
+                
+        # TQ001 的路线：T4 -> T2 -> T3
+        if vehicle["phase"] == 0:  # T4 -> T2 阶段
+            # 计算 T4 到 T2 的实际方向
+            actual_direction = calculate_path_direction(POINT_T4, POINT_T2)
+            # 允许 15 度的误差范围
+            direction_diff = abs(vehicle["direction"] - actual_direction)
+            direction_diff = min(direction_diff, 360 - direction_diff)
+            
+            # 在西路口以东时，向 T2 移动需要判断西路口红绿灯
+            if direction_diff <= 15 and vehicle["longitude"] > T2_INTERSECTION["longitude"]:
+                return traffic_light_data[0], distance_to_west  # 西路口红绿灯
+    
     # 其他情况，表示车辆已经过路口或不需要判断红绿灯
     return None, float('inf')
 
@@ -630,19 +655,18 @@ def update_vehicle_position(vehicle, elapsed_time):
         print(f"特勤车 {vehicle['vehicleNo']} 正常行驶，速度={vehicle['speed']}km/h")
     else:
         # 普通车辆的移动逻辑
-        # 先判断是否在红灯影响范围内（50米）
+        # 红灯且距离停止线小于等于 50 米时必须停车
         if traffic_light and traffic_light["state"] == 0 and distance <= DIST_50M:
-            # 在红灯影响范围内，检查是否需要停车
-            if not vehicle_state.can_move():
-                vehicle["speed"] = 0
-                print(f"车辆 {vehicle['vehicleNo']} 遇红灯在停止线前停车，距路口 {distance:.1f}米")
-                return
-        else:
-            # 不在红灯影响范围内，只检查其他指令
-            if vehicle_state.current_command in ["ALERT", "WARNING"]:
-                vehicle["speed"] = 0
-                print(f"车辆 {vehicle['vehicleNo']} 不能移动: command={vehicle_state.current_command}")
-                return
+            vehicle["speed"] = 0
+            vehicle_state.current_command = "RED"  # 设置当前指令为红灯
+            vehicle_state.is_running = False  # 设置为停止状态
+            print(f"车辆 {vehicle['vehicleNo']} 遇红灯在停止线前停车，距路口 {distance:.1f}米")
+            return
+        elif vehicle_state.current_command in ["ALERT", "WARNING"]:
+            # 其他停车指令
+            vehicle["speed"] = 0
+            print(f"车辆 {vehicle['vehicleNo']} 不能移动: command={vehicle_state.current_command}")
+            return
         
         # 可以移动，设置正常速度
         vehicle["speed"] = DEFAULT_VEHICLE_SPEED

tools/test_websocket.html

@@ -87,17 +87,19 @@
                                 type = 'command';
                                 // 为控制指令添加中文描述
                                 const commandTypes = {
-                                    'SIGNAL': '信号灯指令',
-                                    'ALERT': '告警指令',
-                                    'WARNING': '预警指令',
-                                    'RESUME': '恢复指令'
+                                    'SIGNAL': '信号灯',
+                                    'ALERT': '告警',
+                                    'WARNING': '预警',
+                                    'RESUME': '恢复',
+                                    'PARKING': '安全停靠'
                                 };
                                 const reasons = {
                                     'TRAFFIC_LIGHT': '红绿灯控制',
                                     'AIRCRAFT_CROSSING': '航空器交叉',
                                     'SPECIAL_VEHICLE': '特勤车辆',
                                     'AIRCRAFT_PUSH': '航空器推出',
-                                    'RESUME_TRAFFIC': '恢复通行'
+                                    'RESUME_TRAFFIC': '恢复通行',
+                                    'PARKING_SIDE': '安全停靠'
                                 };
                                 message = `收到车辆控制指令:\n车辆: ${data.vehicleId}\n` +
                                         `指令类型: ${commandTypes[data.commandType] || data.commandType}\n` +