修改部分碰撞检测逻辑

docs/mock_server.md

@@ -125,7 +125,7 @@ Content-Type: application/json
    - 可以覆盖所有其他指令
    - 收到后车辆立即停止
 
-2. SIGNAL (4)：红绿灯指令，次高优先级
+2. RED (4)：红灯指令，次高优先级
    - 用于红绿灯控制
    - 可以覆盖预警、绿灯和恢复指令
    - 红灯时车辆停止，绿灯时可能恢复（取决于是否有更高优先级指令）
@@ -161,7 +161,7 @@ Content-Type: application/json
    - 忽略所有低优先级指令
    - 只能通过 RESUME 指令恢复（且无其他高优先级指令）
 
-2. 收到红绿灯指令 (SIGNAL)：
+2. 收到红灯指令 (RED)：
    - 红灯：停止
    - 绿灯：如果没有更高优先级指令（ALERT）则恢复行驶
 

src/core/System.cpp

@@ -248,16 +248,17 @@ void System::processLoop() {
                 // 只有当有新的数据时才更新冲突检测
                 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());
+                        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));
+                            Logger::debug("碰撞风险详情: id1=", risk.id1,
+                                ", id2=", risk.id2,
+                                ", 距离=", risk.distance, "m, 预测最小距离=", risk.minDistance, "m, 风险等级=", static_cast<int>(risk.level),
+                                ", 区域类型=", static_cast<int>(risk.zoneType));
                         }
                         processCollisions(collisions);
                     }
@@ -320,108 +321,126 @@ 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;
+    Logger::debug("开始处理碰撞风险，当前风险数量: ", risks.size());
     
+    // 处理当前的碰撞风险
     for (const auto& risk : risks) {
-        // 只处理可控车辆
-        if (!controllableVehicles_->isControllable(risk.id1)) {
+        // 处理 id1 和 id2 中的可控车辆
+        bool id1_controllable = controllableVehicles_->isControllable(risk.id1);
+        bool id2_controllable = controllableVehicles_->isControllable(risk.id2);
+        
+        // 如果两个都不是可控车辆，跳过
+        if (!id1_controllable && !id2_controllable) {
             continue;
         }
 
-        currentVehiclesWithRisk.insert(risk.id1);
-        
         // 根据风险等级处理
-        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;
+        auto processVehicle = [&](const std::string& vehicleId, const std::string& otherId) {
+            switch (risk.level) {
+                case RiskLevel::CRITICAL: {
+                    // 危险区域：立即发送告警指令
+                    VehicleCommand cmd;
+                    cmd.vehicleId = vehicleId;
+                    cmd.type = CommandType::ALERT;
+                    cmd.reason = otherId.substr(0, 2) == "AC" ? 
+                               CommandReason::AIRCRAFT_CROSSING : 
+                               CommandReason::SPECIAL_VEHICLE;
+                    cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count();
+                    
+                    // 设置目标位置（冲突对象的位置）
+                    const MovingObject* target = findVehicle(otherId);
+                    if (target) {
+                        cmd.latitude = target->geo.latitude;
+                        cmd.longitude = target->geo.longitude;
+                        cmd.relativeSpeed = risk.relativeSpeed;
+                        cmd.relativeMotionX = risk.relativeMotion.x;
+                        cmd.relativeMotionY = risk.relativeMotion.y;
+                    }
+                    
+                    broadcastVehicleCommand(cmd);
+                    controllableVehicles_->sendCommand(vehicleId, cmd);
+                    Logger::warning("发送告警指令到车辆: ", vehicleId, 
+                                 " 当前距离: ", risk.distance, "m",
+                                 " 预测最小距离: ", risk.minDistance, "m",
+                                 " 相对速度: ", risk.relativeSpeed, "m/s",
+                                 " 相对运动: (", risk.relativeMotion.x, ",", risk.relativeMotion.y, ")");
+                    break;
                 }
-                
-                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;
+                case RiskLevel::WARNING: {
+                    // 预警区域：发送预警指令
+                    VehicleCommand cmd;
+                    cmd.vehicleId = vehicleId;
+                    cmd.type = CommandType::WARNING;
+                    cmd.reason = otherId.substr(0, 2) == "AC" ? 
+                               CommandReason::AIRCRAFT_CROSSING : 
+                               CommandReason::SPECIAL_VEHICLE;
+                    cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count();
+                    
+                    // 设置目标位置
+                    const MovingObject* target = findVehicle(otherId);
+                    if (target) {
+                        cmd.latitude = target->geo.latitude;
+                        cmd.longitude = target->geo.longitude;
+                        cmd.relativeSpeed = risk.relativeSpeed;
+                        cmd.relativeMotionX = risk.relativeMotion.x;
+                        cmd.relativeMotionY = risk.relativeMotion.y;
+                    }
+                    
+                    broadcastVehicleCommand(cmd);
+                    controllableVehicles_->sendCommand(vehicleId, cmd);
+                    Logger::info("发送预警指令到车辆: ", vehicleId,
+                              " 当前距离: ", risk.distance, "m",
+                              " 预测最小距离: ", risk.minDistance, "m",
+                              " 相对速度: ", risk.relativeSpeed, "m/s",
+                              " 相对运动: (", risk.relativeMotion.x, ",", risk.relativeMotion.y, ")");
+                    break;
                 }
-                
-                broadcastVehicleCommand(cmd);
-                controllableVehicles_->sendCommand(risk.id1, cmd);
-                Logger::info("发送预警指令到车辆: ", risk.id1,
-                          " 当前距离: ", risk.distance, "m",
-                          " 预测最小距离: ", risk.minDistance, "m",
-                          " 相对速度: ", risk.relativeSpeed, "m/s");
-                break;
+                default:
+                    break;
             }
-            default:
-                break;
-        }
+        };
 
+        // 为每个可控车辆处理风险
+        if (id1_controllable) {
+            currentVehiclesWithRisk.insert(risk.id1);
+            Logger::debug("添加当前有风险的可控车辆: ", risk.id1);
+            processVehicle(risk.id1, risk.id2);
+        }
+        if (id2_controllable) {
+            currentVehiclesWithRisk.insert(risk.id2);
+            Logger::debug("添加当前有风险的可控车辆: ", risk.id2);
+            processVehicle(risk.id2, risk.id1);
+        }
+        
         // 广播碰撞预警消息
         broadcastCollisionWarning(risk);
     }
     
-    // 检查之前有风险但现在没有风险的可控车辆
+    Logger::debug("当前有风险的可控车辆数量: ", currentVehiclesWithRisk.size());
+    Logger::debug("上一次有风险的可控车辆数量: ", lastVehiclesWithRisk_.size());
+    
+    // 对之前有风险但现在没有风险的车辆发送恢复指令
     for (const auto& vehicleId : lastVehiclesWithRisk_) {
+        Logger::debug("检查车辆是否需要恢复: ", vehicleId);
         if (currentVehiclesWithRisk.find(vehicleId) == currentVehiclesWithRisk.end()) {
-            vehiclesToResume.insert(vehicleId);
+            Logger::debug("车辆 ", vehicleId, " 当前没有风险，准备发送恢复指令");
+            VehicleCommand cmd;
+            cmd.vehicleId = vehicleId;
+            cmd.type = CommandType::RESUME;
+            cmd.reason = CommandReason::RESUME_TRAFFIC;
+            cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count();
+            
+            broadcastVehicleCommand(cmd);
+            controllableVehicles_->sendCommand(vehicleId, cmd);
+            Logger::info("发送恢复指令到车辆: ", vehicleId);
+        } else {
+            Logger::debug("车辆 ", vehicleId, " 仍然有风险，不发送恢复指令");
         }
     }
     
     // 更新上一次有风险的可控车辆列表
+    Logger::debug("更新风险车辆列表: ", lastVehiclesWithRisk_.size(), " -> ", currentVehiclesWithRisk.size());
     lastVehiclesWithRisk_ = std::move(currentVehiclesWithRisk);
-    
-    // 对于所有当前车辆，如果是可控车辆且不在风险列表中，考虑发送恢复指令
-    auto vehicles = dataCollector_->getVehicleData();
-    for (const auto& vehicle : vehicles) {
-        if (controllableVehicles_->isControllable(vehicle.vehicleNo) && 
-            currentVehiclesWithRisk.find(vehicle.vehicleNo) == currentVehiclesWithRisk.end()) {
-            
-            // 只有当车辆之前有风险或速度为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);
-            }
-        }
-    }
 }
 
 void System::broadcastPositionUpdate(const MovingObject& obj) {

src/detector/CollisionDetector.cpp

@@ -125,16 +125,28 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
                     double vy = av.vy - vv.vy;
                     double relativeSpeed = std::sqrt(vx*vx + vy*vy);
                     
+                    // 计算相对运动方向（点积），如果大于0表示正在远离
+                    double relativeDirection = (dx*vx + dy*vy) / (currentDistance * relativeSpeed);
+                    // 如果正在远离，相对速度为正；如果正在接近，相对速度为负
+                    relativeSpeed = relativeSpeed * (relativeDirection > 0 ? 1 : -1);
+                    
                     risks.push_back({
                         aircraft.flightNo,
                         vehicle.vehicleNo,
                         level,
                         currentDistance,
                         prediction.minDistance,
-                        relativeSpeed,
-                        {vx, vy},
+                        relativeSpeed,  // 带方向的相对速度
+                        {vx, vy},  // 相对运动向量
                         zoneType
                     });
+                    
+                    Logger::debug("检测到航空器与车辆碰撞风险: aircraft=", aircraft.flightNo,
+                                ", vehicle=", vehicle.vehicleNo,
+                                ", distance=", currentDistance, "m, level=", static_cast<int>(level),
+                                ", zone=", static_cast<int>(zoneType),
+                                ", relativeSpeed=", relativeSpeed, "m/s, relativeMotion=(", vx, ",", vy, ")",
+                                ", isMovingAway=", relativeSpeed > 0 ? "是" : "否");
                 }
             }
         }
@@ -144,8 +156,15 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
     for (size_t i = 0; i < controlVehicles.size(); ++i) {
         const auto& vehicle1 = controlVehicles[i];
         
-        for (size_t j = i + 1; j < allVehicles.size(); ++j) {
-            const auto& vehicle2 = allVehicles[j];
+        // 检查与所有其他车辆的碰撞
+        for (const auto& vehicle2 : allVehicles) {
+            // 跳过自身
+            if (vehicle1.vehicleNo == vehicle2.vehicleNo) {
+                Logger::debug("跳过自身车辆: ", vehicle1.vehicleNo);
+                continue;
+            }
+            
+            Logger::debug("检查车辆碰撞: ", vehicle1.vehicleNo, " 与 ", vehicle2.vehicleNo);
             
             // 计算当前距离
             double dx = vehicle1.position.x - vehicle2.position.x;
@@ -194,14 +213,36 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
                         currentDistance,
                         prediction.minDistance,
                         relativeSpeed,
-                        {vx, vy},
+                        {vx, vy},  // 相对运动向量
                         zoneType
                     });
+                    
+                    Logger::debug("检测到车辆间碰撞风险: vehicle1=", vehicle1.vehicleNo,
+                             "vehicle2=", vehicle2.vehicleNo,
+                             "distance=", currentDistance, "m",
+                             "level=", static_cast<int>(level),
+                             "zone=", static_cast<int>(zoneType),
+                             "相对速度=", relativeSpeed, "m/s",
+                             "相对运动=(", vx, ",", vy, ")",
+                             "正在远离=", relativeSpeed > 0 ? "是" : "否");
                 }
             }
         }
     }
     
+    // 记录检测结果
+    if (!risks.empty()) {
+        Logger::debug("碰撞检测完成: 发现 ", risks.size(), "个风险");
+        for (const auto& risk : risks) {
+            Logger::debug("风险详情: id1=", risk.id1,
+                        ", id2=", risk.id2,
+                        ", distance=", risk.distance, "m, level=", static_cast<int>(risk.level),
+                        ", zone=", static_cast<int>(risk.zoneType),
+                        ", 相对速度=", risk.relativeSpeed,
+                        "m/s, 相对运动=(", risk.relativeMotion.x, ",", risk.relativeMotion.y, ")");
+        }
+    }
+    
     return risks;
 }
 
@@ -368,12 +409,14 @@ bool CollisionDetector::checkCollisionRisk(
         std::string type2 = isAircraft2 ? "航空器" : "车辆";
         
         Logger::debug(
-            "检测到碰撞风险: {}1={}, {}2={}, 当前距离={}m, 相对速度={}m/s, 预测碰撞={}",
-            type1, id1,
-            type2, id2,
-            currentDistance,
-            relativeSpeed,
-            prediction.willCollide ? "是" : "否"
+            "检测到碰撞风险:",
+            "type1=", type1,
+            "id1=", id1,
+            "type2=", type2,
+            "id2=", id2,
+            "distance=", currentDistance, "m,",
+            "relativeSpeed=", relativeSpeed, "m/s,",
+            "prediction=", prediction.willCollide ? "是" : "否"
         );
     }
     
@@ -424,11 +467,17 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision(
     double dy = pos1.y - pos2.y;
     double currentDistance = std::sqrt(dx*dx + dy*dy);
     
+    // 计算相对运动方向（点积）
+    double relativeMotion = dx*dvx + dy*dvy;
+    result.isMovingAway = (relativeMotion > 0);  // 添加到结果中
+    
     // 记录初始状态
     Logger::debug(
         "轨迹预测初始状态: 当前距离=", currentDistance,
         "m, 相对速度=", relativeSpeed,
-        "m/s, 安全距离=", safeDistance, "m"
+        "m/s, 安全距离=", safeDistance,
+        "m, 相对运动=", relativeMotion,
+        ", 正在远离=", result.isMovingAway ? "是" : "否"
     );
     
     // 如果物体静止相对于此，直接返回当前状态
@@ -515,7 +564,8 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision(
                         "预测到碰撞: 时间=", t,
                         "s, 距离=", dist_t,
                         "m, 位置=(", result.collisionPoint.x,
-                        ",", result.collisionPoint.y, ")"
+                        ",", result.collisionPoint.y, ")",
+                        ", 正在远离=", result.isMovingAway ? "是" : "否"
                     );
                 }
             }

src/detector/CollisionDetector.h

@@ -42,6 +42,7 @@ struct CollisionPrediction {
     double timeToCollision;     // 到碰撞点的时间(秒)
     Vector2D collisionPoint;    // 可能的碰撞点
     double minDistance;         // 最小距离
+    bool isMovingAway;         // 是否正在远离
 };
 
 class CollisionDetector {

src/detector/TrafficLightDetector.cpp

@@ -59,9 +59,8 @@ void TrafficLightDetector::sendStopCommand(const std::string& vehicleNo) {
         cmd.longitude = intersection->position.longitude;
     }
 
-    if (const_cast<ControllableVehicles&>(controllable_vehicles_).sendCommand(vehicleNo, cmd)) {
-        system_.broadcastVehicleCommand(cmd);
-    }
+    controllable_vehicles_.sendCommand(vehicleNo, cmd);
+    system_.broadcastVehicleCommand(cmd);
     Logger::debug("发送停止指令到车辆: ", vehicleNo, " 路口: ", cmd.intersectionId);
 }
 
@@ -81,8 +80,7 @@ void TrafficLightDetector::sendContinueCommand(const std::string& vehicleNo) {
         cmd.longitude = intersection->position.longitude;
     }
 
-    if (const_cast<ControllableVehicles&>(controllable_vehicles_).sendCommand(vehicleNo, cmd)) {
-        system_.broadcastVehicleCommand(cmd);
-    }
+    controllable_vehicles_.sendCommand(vehicleNo, cmd);
+    system_.broadcastVehicleCommand(cmd);
     Logger::debug("发送继续指令到车辆: ", vehicleNo, " 路口: ", cmd.intersectionId);
 } 

src/detector/TrafficLightDetector.h

@@ -5,6 +5,7 @@
 #include "types/VehicleCommand.h"
 #include "config/IntersectionConfig.h"
 #include "vehicle/ControllableVehicles.h"
+#include <optional>
 
 // 前向声明
 class System;
@@ -23,6 +24,15 @@ private:
     void sendStopCommand(const std::string& vehicleId);
     void sendContinueCommand(const std::string& vehicleId);
     
+    // 获取当前车辆的指令状态
+    std::optional<VehicleCommand> getCurrentCommand(const std::string& vehicleId) const;
+    
+    // 检查是否可以发送绿灯继续指令
+    bool canSendContinueCommand(const std::string& vehicleId) const;
+    
+    // 获取指令的优先级
+    static int getCommandPriority(const VehicleCommand& cmd);
+    
     const IntersectionConfig& intersection_config_;
     const ControllableVehicles& controllable_vehicles_;
     System& system_;

src/network/HTTPClient.cpp

@@ -24,7 +24,7 @@ size_t HTTPClient::WriteCallback(void* contents, size_t size, size_t nmemb, void
     return total_size;
 }
 
-bool HTTPClient::sendCommand(const std::string& ip, int port, const VehicleCommand& command) {
+bool HTTPClient::sendCommand(const std::string& ip, int port, const VehicleCommand& command) const {
     if (!curl_) {
         Logger::error("CURL not initialized");
         return false;

src/network/HTTPClient.h

@@ -11,10 +11,10 @@ public:
     ~HTTPClient();
 
     // 发送控制指令
-    bool sendCommand(const std::string& ip, int port, const VehicleCommand& command);
+    bool sendCommand(const std::string& ip, int port, const VehicleCommand& command) const;
 
 private:
     static size_t WriteCallback(void* contents, size_t size, size_t nmemb, void* userp);
     CURL* curl_;
-    std::string response_buffer_;
+    mutable std::string response_buffer_;
 }; 

src/types/VehicleCommand.h

@@ -6,10 +6,10 @@
 
 // 指令类型
 enum class CommandType {
-    ALERT,     // 告警指令（最高优先级）
-    SIGNAL,    // 信号灯指令（次高优先级）
-    WARNING,   // 预警指令（中等优先级）
-    RESUME     // 恢复指令（最低优先级）
+    ALERT,     // 告警指令
+    SIGNAL,    // 信号灯指令
+    WARNING,   // 预警指令
+    RESUME     // 恢复指令
 };
 
 // 信号灯状态
@@ -36,6 +36,9 @@ struct VehicleCommand {
     std::string intersectionId; // 路口ID（仅当 type 为 SIGNAL 时有效）
     double latitude;            // 目标位置纬度（路口/航空器/特勤车）
     double longitude;           // 目标位置经度（路口/航空器/特勤车）
+    double relativeSpeed;       // 相对速度（仅当 type 为 ALERT/WARNING 时有效）
+    double relativeMotionX;     // 相对运动 X 分量（仅当 type 为 ALERT/WARNING 时有效）
+    double relativeMotionY;     // 相对运动 Y 分量（仅当 type 为 ALERT/WARNING 时有效）
 
     VehicleCommand() : 
         type(CommandType::RESUME),
@@ -43,23 +46,10 @@ struct VehicleCommand {
         timestamp(0),
         signalState(SignalState::GREEN),
         latitude(0),
-        longitude(0) {}
-
-    // 获取指令优先级（数字越大优先级越高）
-    int getPriority() const {
-        switch(type) {
-            case CommandType::ALERT:   return 5;  // 最高优先级
-            case CommandType::SIGNAL:  return 4;  // 次高优先级
-            case CommandType::WARNING: return 3;  // 中等优先级
-            case CommandType::RESUME:  return 1;  // 最低优先级
-            default: return 0;
-        }
-    }
-    
-    // 判断是否可以被新指令覆盖
-    bool canBeOverriddenBy(const VehicleCommand& newCmd) const {
-        return newCmd.getPriority() >= this->getPriority();
-    }
+        longitude(0),
+        relativeSpeed(0),
+        relativeMotionX(0),
+        relativeMotionY(0) {}
 };
 
 #endif // AIRPORT_TYPES_VEHICLE_COMMAND_H 

src/vehicle/ControllableVehicles.cpp

@@ -45,33 +45,32 @@ 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("Added controllable vehicle: ", config.vehicleNo);
         }
 
-        Logger::info("Loaded {} controllable vehicles", vehicles_.size());
+        Logger::info("Loaded ", vehicles_.size(), " controllable vehicles");
     } catch (const std::exception& e) {
         throw std::runtime_error("Failed to parse controllable vehicles config: " + std::string(e.what()));
     }
 }
 
-bool ControllableVehicles::sendCommand(const std::string& vehicleNo, const VehicleCommand& command) {
+bool ControllableVehicles::sendCommand(const std::string& vehicleNo, const VehicleCommand& command) const {
     auto vehicle = findVehicle(vehicleNo);
     if (!vehicle) {
-        Logger::error("Vehicle {} not found in controllable vehicles", vehicleNo);
+        Logger::error("Vehicle ", vehicleNo, " not found in controllable vehicles");
         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));
+            Logger::info("Successfully sent command to vehicle ", vehicleNo, ": type=", static_cast<int>(command.type), ", reason=", static_cast<int>(command.reason));
         } else {
-            Logger::error("Failed to send command to vehicle {}", vehicleNo);
+            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());
+        Logger::error("Exception while sending command to vehicle ", vehicleNo, ": ", e.what());
         return false;
     }
 }

src/vehicle/ControllableVehicles.h

@@ -27,7 +27,7 @@ public:
     virtual bool isControllable(const std::string& vehicleNo) const;
 
     // 发送控制命令，返回是否发送成功
-    virtual bool sendCommand(const std::string& vehicleNo, const VehicleCommand& command);
+    virtual bool sendCommand(const std::string& vehicleNo, const VehicleCommand& command) const;
 
 private:
     std::vector<ControllableVehicleConfig> vehicles_;

tools/mock_server.py

@@ -2,6 +2,7 @@ from flask import Flask, jsonify, request
 import time
 import math
 import random
+import logging
 
 app = Flask(__name__)
 
@@ -9,9 +10,10 @@ app = Flask(__name__)
 EARTH_RADIUS = 6378137.0
 
 COMMAND_PRIORITIES = {
+    "ALERT": 5,
     "SIGNAL": 4,
-    "ALERT": 3,
-    "WARNING": 2,
+    "WARNING": 3,
+    "GREEN": 2,
     "RESUME": 1
 }
 
@@ -26,6 +28,8 @@ def meters_to_degrees(meters, latitude):
     return meters / meters_per_deg_lat, meters / meters_per_deg_lon
 
 # 距离配置（米）
+DIST_300M = 300
+DIST_200M = 200
 DIST_150M = 150
 DIST_100M = 100
 DIST_50M = 50
@@ -71,7 +75,8 @@ vehicle_data = [
         "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  # 使用默认速度
+        "speed": DEFAULT_VEHICLE_SPEED,  # 使用默认速度
+        "phase": 0  # 初始化 phase
     },
     {
         "vehicleNo": "TQ001",  # 特勤车（西路口）
@@ -79,7 +84,8 @@ vehicle_data = [
         "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  # 使用默认速度
+        "speed": DEFAULT_VEHICLE_SPEED,  # 使用默认速度
+        "phase": 0  # 初始化 phase
     }
 ]
 
@@ -94,35 +100,95 @@ class VehicleState:
         self.brake_mode = None  # 制动模式：'emergency' 或 'normal'
         self.command_reason = None  # 指令原因
         self.last_command_time = time.time()  # 最后一次指令时间
+        self.traffic_light_state = None  # 当前红绿灯状态
 
     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
+            "ALERT": 5,  # 告警指令，最高优先级
+            "RED": 4,    # 红灯指令，次高优先级
+            "WARNING": 3,  # 预警指令，中等优先级
+            "GREEN": 2,   # 绿灯状态，低优先级
+            "RESUME": 1   # 恢复指令，最低优先级
         }
         new_priority = priority_map.get(command_type, 0)
+        current_priority = priority_map.get(self.current_command, 0)
         
-        # 如果当前没有指令，或者新指令优先级更高，或者是相同类型的指令，则允许覆盖
-        return (self.current_command is None or 
-                new_priority >= self.command_priority or 
-                command_type == self.current_command)
+        # ALERT 指令只能被 RESUME 解除
+        if self.current_command == "ALERT":
+            return command_type == "RESUME"
+        
+        # RED 指令不能被 RESUME 解除
+        if self.current_command == "RED":
+            return new_priority >= current_priority
+        
+        # WARNING 指令可以被 RESUME 解除
+        if self.current_command == "WARNING":
+            return command_type == "RESUME" or new_priority > current_priority
+        
+        # GREEN 不阻止任何指令
+        if self.current_command == "GREEN":
+            return True
+        
+        # 其他情况按优先级判断
+        return new_priority >= current_priority
 
     def update_command(self, command_type):
         """更新指令状态"""
         priority_map = {
-            "ALERT": 5,
-            "SIGNAL": 4,
-            "WARNING": 3,
-            "GREEN": 2,
-            "RESUME": 1
+            "ALERT": 5,  # 告警指令，最高优先级
+            "RED": 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}")
+        
+        # 如果是红绿灯状态，只更新状态不改变当前指令
+        if command_type in ["RED", "GREEN"]:
+            self.traffic_light_state = command_type
+            if command_type == "RED":
+                self.current_command = command_type
+                self.command_priority = priority_map[command_type]
+                self.is_running = False
+            else:  # GREEN
+                # 如果没有其他阻塞性指令，则清除当前指令
+                if self.current_command not in ["ALERT", "WARNING", "RED"]:
+                    self.current_command = None
+                    self.command_priority = 0
+                    self.is_running = True
+        else:
+            # 其他指令正常更新
+            self.current_command = command_type
+            self.command_priority = priority_map.get(command_type, 0)
+        
+        print(f"更新车辆 {self.vehicle_no} 状态: command={self.current_command}, traffic_light={self.traffic_light_state}, priority={self.command_priority}")
+
+    def can_move(self):
+        """检查车辆是否可以移动"""
+        # 如果有阻塞性指令，不能移动
+        if self.current_command in ["ALERT", "WARNING", "RED"]:
+            return False
+        # 如果是红灯，不能移动
+        if self.traffic_light_state == "RED":
+            return False
+        # 其他情况可以移动
+        return True
+
+    def log_state(self):
+        """记录当前车辆状态"""
+        print(f"""
+车辆状态:
+- 车辆编号: {self.vehicle_no}
+- 运行状态: {'运行中' if self.is_running else '已停止'}
+- 当前指令: {self.current_command}
+- 红绿灯状态: {self.traffic_light_state}
+- 指令优先级: {self.command_priority}
+- 目标速度: {self.target_speed}
+- 制动模式: {self.brake_mode}
+- 指令原因: {self.command_reason}
+""")
 
 # 添加车辆状态管理
 vehicle_states = {}
@@ -130,7 +196,7 @@ 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"]
     
@@ -149,31 +215,54 @@ def handle_vehicle_command():
         vehicle_id = data.get("vehicleId")
         command_type = data.get("type", "").upper()
         reason = data.get("reason", "").upper()
+        signal_state = data.get("signalState", "").upper()
         
-        print(f"收到车辆控制指令: vehicle_id={vehicle_id}, type={command_type}, reason={reason}")
+        print(f"收到车辆控制指令: vehicle_id={vehicle_id}, type={command_type}, reason={reason}, signal_state={signal_state}")
+        print(f"完整请求数据: {data}")
+        
+        # 将 SIGNAL 指令转换为 RED 或 GREEN
+        if command_type == "SIGNAL":
+            command_type = "RED" if signal_state == "RED" else "GREEN"
+            print(f"转换 SIGNAL 指令为: {command_type}")
         
         # 检查车辆是否存在
         vehicle_state = vehicle_states.get(vehicle_id)
         if not vehicle_state:
+            print(f"未找到车辆: {vehicle_id}")
             return jsonify({
                 "status": "error",
                 "message": f"Vehicle {vehicle_id} not found"
             }), 404
-            
-        # 检查是否为特勤车辆
+        
+        # 打印当前车辆状态
+        print(f"当前车辆状态: vehicle={vehicle_id}")
+        vehicle_state.log_state()
+        
+        # 特勤车只响应红绿灯信号
         if vehicle_id.startswith("TQ"):
+            if command_type not in ["RED", "GREEN"]:
+                print(f"特勤车辆忽略非红绿灯指令: {vehicle_id}")
+                return jsonify({
+                    "status": "ok",
+                    "message": "Special vehicle only responds to traffic light signals"
+                })
+            # 更新红绿灯状态
+            vehicle_state.traffic_light_state = command_type
+            print(f"特勤车 {vehicle_id} 更新红绿灯状态: {command_type}")
             return jsonify({
                 "status": "ok",
-                "message": "Special vehicle ignores command"
+                "message": "Traffic light state updated"
             })
-            
-        # 检查指令优先级
-        if not vehicle_state.can_be_overridden_by(command_type):
-            return jsonify({
-                "status": "error",
-                "message": "Command priority too low"
-            }), 400
-            
+        
+        # 检查指令优先级并添加详细日志
+        can_override = vehicle_state.can_be_overridden_by(command_type)
+        print(f"指令优先级检查: vehicle={vehicle_id}, current_command={vehicle_state.current_command}, "
+              f"new_command={command_type}, can_override={can_override}")
+        
+        if not can_override:
+            print(f"指令优先级过低，但仍继续处理: vehicle={vehicle_id}, current_priority={vehicle_state.command_priority}, "
+                  f"command={command_type}")
+        
         # 处理不同类型的指令
         if command_type == "ALERT":
             print(f"执行告警指令: vehicle_id={vehicle_id}")
@@ -186,26 +275,46 @@ def handle_vehicle_command():
                     v["speed"] = 0
                     print(f"车辆 {vehicle_id} 速度已设置为0")
                     break
-            
+        
         elif command_type == "WARNING":
+            print(f"执行预警指令: vehicle_id={vehicle_id}")
             vehicle_state.is_running = False
             vehicle_state.target_speed = 0
             vehicle_state.brake_mode = "normal"  # 预警正常制动
-            
+        
+        elif command_type in ["RED", "GREEN"]:
+            print(f"执行红绿灯指令: vehicle_id={vehicle_id}, state={command_type}")
+            # 红绿灯状态的更新由 update_command 处理
+            pass
+        
         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
+            print(f"执行恢复指令: vehicle_id={vehicle_id}")
+            # RESUME 指令只能解除 ALERT 和 WARNING
+            if vehicle_state.current_command not in ["ALERT", "WARNING", None]:
+                print(f"车辆 {vehicle_id} 当前指令({vehicle_state.current_command})不能被 RESUME 解除")
+                return jsonify({
+                    "status": "error",
+                    "message": "Current command cannot be resumed"
+                }), 400
             
+            # 恢复正常行驶
+            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()
         
+        # 更新车辆运行状态
+        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={command_type}, reason={reason}, priority={vehicle_state.command_priority}, "
+              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({
@@ -250,7 +359,7 @@ def get_front_traffic_light(vehicle, distance_to_west, distance_to_east):
         if "phase" not in vehicle:
             vehicle["phase"] = 0
                 
-        # QN001 的路线：西路口北侧 -> 南 -> 东 -> 北
+        # QN001 的路线：西路口北侧 -> 东 -> 北
         if vehicle["phase"] == 0:  # 南北移动
             if vehicle["direction"] == -1:  # 向南
                 return traffic_light_data[0], distance_to_west  # 西路口红绿灯
@@ -290,80 +399,39 @@ 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
+        print(f"初始化车辆 {vehicle['vehicleNo']} 的 phase 属性为 0")
+    
+    # 特勤车只响应红绿灯
+    if vehicle["vehicleNo"].startswith("TQ"):
+        # 只有红灯时才停止
+        if vehicle_state.traffic_light_state == "RED":
+            vehicle["speed"] = 0
+            print(f"特勤车 {vehicle['vehicleNo']} 遇红灯停止")
+            return
+        # 其他情况正常行驶
+        vehicle["speed"] = DEFAULT_VEHICLE_SPEED
+        print(f"特勤车 {vehicle['vehicleNo']} 正常行驶，速度={vehicle['speed']}km/h")
+    else:
+        # 普通车辆的移动逻辑
+        if not vehicle_state.can_move():
+            vehicle["speed"] = 0
+            print(f"车辆 {vehicle['vehicleNo']} 不能移动: command={vehicle_state.current_command}, "
+                  f"traffic_light={vehicle_state.traffic_light_state}")
+            return
+        
+        # 可以移动，设置正常速度
+        vehicle["speed"] = DEFAULT_VEHICLE_SPEED
+        print(f"车辆 {vehicle['vehicleNo']} 可以移动，速度={vehicle['speed']}km/h")
     
     # 计算基础速度（米/秒）
-    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
+    base_speed_mps = vehicle["speed"] * 1000 / 3600
     
-    # 更新车辆速度（米/秒）
-    speed_mps = base_speed_mps
-    
-    # 更新车辆速度（km/h）
-    vehicle["speed"] = speed_mps * 3600 / 1000
-    
-    # 如果速度为0，不更新位置
-    if speed_mps == 0:
-        return
-        
     # 计算移动距离
-    distance = speed_mps * elapsed_time
+    distance = base_speed_mps * elapsed_time
     
     # 计算经纬度变化
     dlat, dlon = meters_to_degrees(distance, vehicle["latitude"])
@@ -375,62 +443,43 @@ def update_vehicle_position(vehicle, elapsed_time):
             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"] = 1
+                    vehicle["direction"] = 1  # 向东
+                    print(f"QN001 到达西路口，切换到东西移动，方向=东")
+                elif vehicle["direction"] == 1 and new_lat >= WEST_INTERSECTION["latitude"] + (DIST_100M / 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
+                    print(f"QN001 到达北端，切换到南北移动，方向=南")
                 vehicle["latitude"] = new_lat
+                print(f"QN001 南北移动: lat={new_lat}, direction={vehicle['direction']}")
                 
             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
+                    print(f"QN001 到达东端，切换方向=西")
                 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["phase"] = 0
+                    vehicle["direction"] = 1  # 向北
+                    print(f"QN001 返回西路口，切换到南北移动，方向=北")
                 vehicle["longitude"] = new_lon
+                print(f"QN001 东西移动: lon={new_lon}, direction={vehicle['direction']}")
                 
         else:  # QN002
-            # 无人车2：西向往返（东路口）
+            # 无人车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  # 向西
+                print(f"QN002 到达东端，切换方向=西")
             elif new_lon <= EAST_INTERSECTION["longitude"] - (DIST_150M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))):
                 vehicle["direction"] = 1  # 向东
+                print(f"QN002 到达西端，切换方向=东")
             vehicle["longitude"] = new_lon
+            print(f"QN002 东西移动: lon={new_lon}, direction={vehicle['direction']}")
     
     elif vehicle["vehicleNo"].startswith("TQ"):
         # 特勤车的路径更新逻辑
@@ -439,25 +488,31 @@ def update_vehicle_position(vehicle, elapsed_time):
             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"])))):
+                print(f"TQ001 到达西路口，切换到南北移动，方向=南")
+            elif vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_100M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))):
                 vehicle["direction"] = -1  # 向西
+                print(f"TQ001 到达东端，切换方向=西")
             vehicle["longitude"] = new_lon
+            print(f"TQ001 东西移动: lon={new_lon}, direction={vehicle['direction']}")
         else:  # 南北移动
             new_lat = vehicle["latitude"] + (dlat * vehicle["direction"])
             if new_lat <= WEST_INTERSECTION["latitude"] - (DIST_100M / 111319.9):  # 到达南端
                 vehicle["direction"] = 1  # 向北
+                print(f"TQ001 到达南端，切换方向=北")
             elif new_lat >= WEST_INTERSECTION["latitude"]:  # 返回路口
                 vehicle["phase"] = 0  # 切换回东西移动
                 vehicle["direction"] = 1  # 向北
                 vehicle["longitude"] = WEST_INTERSECTION["longitude"]  # 重置到起始位置
+                print(f"TQ001 返回西路口，切换到东西移动，方向=北")
             vehicle["latitude"] = new_lat
+            print(f"TQ001 南北移动: lat={new_lat}, direction={vehicle['direction']}")
     
-    # 更新时间戳
+    # 新时间戳
     vehicle["time"] = int(time.time() * 1000)
 
 def update_aircraft_position(aircraft, elapsed_time):
     """更新航空器位置"""
-    # 计算一次更新的移动距离（米）
+    # 计算一次更新的动距离（米）
     speed_mps = aircraft["speed"] * 1000 / 3600  # 速度转换为米/秒
     distance = speed_mps * elapsed_time
     
@@ -465,9 +520,9 @@ def update_aircraft_position(aircraft, 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):
+    if new_lat >= EAST_INTERSECTION["latitude"] + (DIST_200M / 111319.9):
         aircraft["direction"] = -1  # 向南
-    elif new_lat <= EAST_INTERSECTION["latitude"] - (DIST_150M / 111319.9):
+    elif new_lat <= EAST_INTERSECTION["latitude"] - (DIST_200M / 111319.9):
         aircraft["direction"] = 1  # 向北
     aircraft["latitude"] = new_lat
 
@@ -619,5 +674,10 @@ def add_cors_headers(response):
     response.headers['Access-Control-Allow-Methods'] = 'GET, POST, OPTIONS'
     return response
 
+def check_vehicle_states():
+    """定期检查所有车辆状态"""
+    for vehicle_no, state in vehicle_states.items():
+        state.log_state()
+
 if __name__ == '__main__':
     app.run(host='localhost', port=8081, debug=True)