修复了红绿灯问题

CMakeLists.txt

@@ -90,6 +90,11 @@ find_package(CURL REQUIRED)
 # 或安装系统包提供 nlohmann_json::nlohmann_json
 set(LOCAL_NLOHMANN_JSON_SOURCE_DIR "" CACHE PATH "Local nlohmann/json source dir (offline fallback). Should contain CMakeLists.txt")
 
+# 若仓库内已带 json 子目录，则默认优先使用它（避免 FetchContent 联网/卡住）
+if(NOT LOCAL_NLOHMANN_JSON_SOURCE_DIR AND EXISTS "${CMAKE_SOURCE_DIR}/json/CMakeLists.txt")
+    set(LOCAL_NLOHMANN_JSON_SOURCE_DIR "${CMAKE_SOURCE_DIR}/json")
+endif()
+
 find_package(nlohmann_json QUIET)
 
 if(NOT TARGET nlohmann_json::nlohmann_json)

src/collector/DataCollector.cpp

@@ -360,6 +360,8 @@ bool DataCollector::fetchPositionData() {
                 a.geo.latitude, a.geo.longitude);
             // 2. 再转换为机场坐标
             a.position = airportBounds_.toAirportCoordinate(a.position);
+            // 3. 航向同样需要旋转到机场坐标系（位置已旋转，否则会产生预测偏转）
+            a.heading = airportBounds_.toAirportHeading(a.heading);
         }
 
         for (auto& v : newVehicles) {
@@ -368,6 +370,8 @@ bool DataCollector::fetchPositionData() {
                 v.geo.latitude, v.geo.longitude);
             // 2. 再转换为机场坐标
             v.position = airportBounds_.toAirportCoordinate(v.position);
+            // 3. 航向同样需要旋转到机场坐标系
+            v.heading = airportBounds_.toAirportHeading(v.heading);
         }
 
         // 过滤数据
@@ -422,6 +426,11 @@ bool DataCollector::fetchTrafficLightData() {
     bool success = dataSource_->fetchTrafficLightSignals(newSignals);
 
     if (success) {
+        if (!newSignals.empty()) {
+            Logger::info("获取红绿灯成功: count=", newSignals.size(),
+                ", firstId=", newSignals[0].trafficLightId,
+                ", firstTs=", newSignals[0].timestamp);
+        }
         std::lock_guard<std::mutex> lock(cacheMutex_);
         trafficLightCache_ = std::move(newSignals);
         // 更新时间戳

src/config/AirportBounds.cpp

@@ -139,6 +139,17 @@ Vector2D AirportBounds::toAirportCoordinate(const Vector2D& point) const {
     return result;
 }
 
+double AirportBounds::toAirportHeading(double headingDeg) const {
+    // 坐标系做了逆时针 rotationAngle_ 旋转：
+    // 向量方向角（以 x 轴逆时针为正）会 +rotationAngle_；
+    // 航向角（以正北为0、顺时针为正）等价于减去 rotationAngle_。
+    double rotDeg = rotationAngle_ * 180.0 / M_PI;
+    double h = headingDeg - rotDeg;
+    while (h >= 360.0) h -= 360.0;
+    while (h < 0.0) h += 360.0;
+    return h;
+}
+
 AreaType AirportBounds::getAreaType(const Vector2D& position) const {
 
     // 按优先级检查位置所在区域

src/config/AirportBounds.h

@@ -49,6 +49,11 @@ public:
     // 输出：机场坐标系中的点，原点为机场参考点，方向为机场旋转后的方向
     Vector2D toAirportCoordinate(const Vector2D& point) const;
 
+    // 将航向角（正北为0度，顺时针增加）从世界坐标系转换到机场坐标系
+    // 机场坐标系相对世界坐标系做了 rotation_angle 的逆时针旋转。
+    // 因此航向需要减去该旋转角度，保证“位置旋转、速度方向也旋转”一致。
+    double toAirportHeading(double headingDeg) const;
+
 protected:
     Bounds airportBounds_;              // 整个机场边界
     std::unordered_map<AreaType, Bounds> areaBounds_;        // 各区域边界

src/core/System.cpp

@@ -119,6 +119,11 @@ void System::initializeSafetyZones() {
             intersection.position.latitude,
             intersection.position.longitude);
 
+        // 与车辆/航空器位置保持同一坐标系：转换到机场坐标（包含 rotation_angle）
+        if (airportBounds_) {
+            center = airportBounds_->toAirportCoordinate(center);
+        }
+
         // 创建安全区
         auto safetyZone = std::make_unique<SafetyZone>(
             center,
@@ -326,7 +331,8 @@ void System::checkUnmannedVehicleSafetyZones(
 
     // 遍历所有无人车（可控车辆）
     for (const auto& vehicle : vehicles) {
-        if (!controllableVehicles_.isControllable(vehicle.id)) {
+        // 安全区风险指令下发范围：WUREN + TEQIN
+        if (!controllableVehicles_.isManagedVehicle(vehicle.id)) {
             continue;
         }
         // 遍历所有路口安全区
@@ -365,16 +371,17 @@ void System::checkUnmannedVehicleSafetyZones(
                         risk.level = riskLevel;
                         risk.distance = distance;
                         risk.minDistance = distance;
-                        risk.relativeSpeed = std::sqrt(
-                            std::pow(obj->speed * std::cos(obj->heading) -
-                                vehicle.speed * std::cos(vehicle.heading),
-                                2) +
-                            std::pow(obj->speed * std::sin(obj->heading) -
-                                vehicle.speed * std::sin(vehicle.heading),
-                                2));
-                        risk.relativeMotion = {
-                            obj->position.x - vehicle.position.x,
-                            obj->position.y - vehicle.position.y };
+                        // 统一语义：relativeSpeed/relativeMotion 为“相对速度向量”（m/s）
+                        const double h_obj = obj->heading * M_PI / 180.0;
+                        const double h_veh = vehicle.heading * M_PI / 180.0;
+                        const double obj_vx = obj->speed * std::sin(h_obj);
+                        const double obj_vy = obj->speed * std::cos(h_obj);
+                        const double veh_vx = vehicle.speed * std::sin(h_veh);
+                        const double veh_vy = vehicle.speed * std::cos(h_veh);
+                        const double rel_vx = obj_vx - veh_vx;
+                        const double rel_vy = obj_vy - veh_vy;
+                        risk.relativeSpeed = std::sqrt(rel_vx * rel_vx + rel_vy * rel_vy);
+                        risk.relativeMotion = { rel_vx, rel_vy };
 
                         detectedRisks.push_back(risk);
 
@@ -415,8 +422,8 @@ void System::processCollisions(
     // 处理当前的碰撞风险
     for (const auto& risk : detectedRisks) {
         // 处理 id1 和 id2 中的可控车辆
-        bool id1_controllable = controllableVehicles_.isControllable(risk.id1);
-        bool id2_controllable = controllableVehicles_.isControllable(risk.id2);
+        bool id1_controllable = controllableVehicles_.isManagedVehicle(risk.id1);
+        bool id2_controllable = controllableVehicles_.isManagedVehicle(risk.id2);
 
         auto processVehicle = [&](const std::string& vehicleId, const std::string& otherId) {
             // 发送指令
@@ -519,12 +526,6 @@ void System::broadcastPositionUpdate(const MovingObject& obj) {
         {"speed", msg.speed} };
 
     ws_server_->broadcast(j.dump());
-    Logger::debug("广播位置更新: id=", msg.objectId,
-        " type=", msg.objectType,
-        " pos=(", msg.longitude, ",", msg.latitude, ")",
-        " heading=", msg.heading,
-        " speed=", msg.speed,
-        " timestamp=", msg.timestamp);
 }
 
 // 新增辅助函数：将 SignalStatus 转换为字符串
@@ -567,7 +568,7 @@ void System::broadcastTrafficLightStatus(const TrafficLightSignal& signal) {
     ws_server_->broadcast(j.dump());
 
     // 修改日志记录
-    Logger::debug("广播红绿灯状态: id=", signal.trafficLightId,
+    Logger::info("广播红绿灯状态: id=", signal.trafficLightId,
         ", NS_Status=", signalStatusToString(signal.ns_status),
         ", EW_Status=", signalStatusToString(signal.ew_status),
         ", intersection=", intersection ? intersection->id : "",
@@ -719,12 +720,6 @@ void System::updateSafetyZoneStates(const std::vector<MovingObject*>& objects) {
 
     // 检查所有移动物体
     for (const auto& obj : objects) {
-        Logger::debug("检查移动物体: id=", obj->id,
-            ", type=", obj->type == MovingObjectType::UNMANNED ? "无人车" : (obj->type == MovingObjectType::SPECIAL ? "特勤车" : "飞机"),
-            ", isAircraft=", obj->isAircraft(),
-            ", isSpecialVehicle=", obj->isSpecialVehicle(),
-            ", isUnmannedVehicle=", obj->isUnmannedVehicle());
-
         // 只检查飞机和特勤车
         if (obj->isAircraft() || obj->isSpecialVehicle()) {
             checkSafetyZoneIntrusion(*obj);
@@ -733,9 +728,14 @@ void System::updateSafetyZoneStates(const std::vector<MovingObject*>& objects) {
 }
 
 void System::checkSafetyZoneIntrusion(const MovingObject& obj) {
-    Logger::debug("检查安全区入侵: id=", obj.id,
-        ", type=", obj.isAircraft() ? "飞机" : (obj.isSpecialVehicle() ? "特勤车" : "其他"),
-        ", position=(", obj.position.x, ",", obj.position.y, ")");
+    // 高度过滤：飞机在高度阈值以上时不参与地面安全区触发
+    if (obj.isAircraft() && airportBounds_) {
+        const auto cfg = airportBounds_->getAreaConfig(airportBounds_->getAreaType(obj.position));
+        const auto& ac = static_cast<const Aircraft&>(obj);
+        if (std::isfinite(ac.altitude) && ac.altitude > cfg.height_threshold) {
+            return;
+        }
+    }
 
     // 检查每个安全区
     for (auto& [id, zone] : safetyZones_) {
@@ -760,7 +760,7 @@ bool System::handleSafetyZoneRisk(const Vehicle& vehicle,
     CommandType cmdType,
     const std::string& riskLevel) {
     // 检查是否为无人车
-    if (!controllableVehicles_.isControllable(vehicle.id)) {
+    if (!controllableVehicles_.isManagedVehicle(vehicle.id)) {
         return false;
     }
 
@@ -787,21 +787,23 @@ bool System::handleSafetyZoneRisk(const Vehicle& vehicle,
         cmd.latitude = target->geo.latitude;
         cmd.longitude = target->geo.longitude;
 
-        // 计算相对距离
-        double rel_dx = target->position.x - vehicle.position.x;
-        double rel_dy = target->position.y - vehicle.position.y;
-
-        // 计算相对速度
-        cmd.relativeSpeed = std::sqrt(
-            (target->position.x - vehicle.position.x) * (target->position.x - vehicle.position.x) +
-            (target->position.y - vehicle.position.y) * (target->position.y - vehicle.position.y));
-        cmd.relativeMotionX = rel_dx;
-        cmd.relativeMotionY = rel_dy;
+        // 统一语义：relativeSpeed/relativeMotion 为相对速度向量（m/s）
+        const double h_t = target->heading * M_PI / 180.0;
+        const double h_v = vehicle.heading * M_PI / 180.0;
+        const double t_vx = target->speed * std::sin(h_t);
+        const double t_vy = target->speed * std::cos(h_t);
+        const double v_vx = vehicle.speed * std::sin(h_v);
+        const double v_vy = vehicle.speed * std::cos(h_v);
+        const double rel_vx = t_vx - v_vx;
+        const double rel_vy = t_vy - v_vy;
+        cmd.relativeSpeed = std::sqrt(rel_vx * rel_vx + rel_vy * rel_vy);
+        cmd.relativeMotionX = rel_vx;
+        cmd.relativeMotionY = rel_vy;
         cmd.minDistance = distance;
 
         Logger::debug("安全区冲突目标信息: id=", target->id,
-            ", 相对距离=(", rel_dx, ",", rel_dy, ")",
-            ", 相对速度=", cmd.relativeSpeed,
+            ", 相对速度向量=(", cmd.relativeMotionX, ",", cmd.relativeMotionY, ")",
+            ", 相对速度=", cmd.relativeSpeed, "m/s",
             ", 最小距离=", distance);
     }
 

src/detector/CollisionDetector.cpp

@@ -2,6 +2,7 @@
 
 #include "config/SystemConfig.h"
 #include "utils/Logger.h"
+#include <algorithm>
 #include <cmath>
 #include <unordered_set>
 
@@ -41,14 +42,8 @@ void CollisionDetector::updateTraffic(const std::vector<Aircraft>& aircraft,
 
         // 插入四叉树
         try {
-            Logger::debug("尝试插入车辆: id=", updatedVehicle.id,
-                ", position=(", updatedVehicle.position.x, ",",
-                updatedVehicle.position.y, ")", ", type=",
-                updatedVehicle.isControllable ? "UNMANNED"
-                : "SPECIAL");
             vehicleTree_.insert(updatedVehicle);
             ++validVehicles;
-            Logger::debug("成功插入车辆: id=", updatedVehicle.id);
         } catch (const std::exception& e) {
             Logger::error("无法插入车辆到四叉树: id=", updatedVehicle.id,
                 ", error=", e.what());
@@ -60,12 +55,6 @@ void CollisionDetector::updateTraffic(const std::vector<Aircraft>& aircraft,
     Logger::debug("四叉树边界: min=(", bounds.x, ",", bounds.y, "), max=(",
         bounds.x + bounds.width, ",", bounds.y + bounds.height, ")");
     auto allVehicles = vehicleTree_.queryRange(bounds);
-    for (const auto& vehicle : allVehicles) {
-        Logger::debug(
-            "查询到车辆: id=", vehicle.id, ", position=(", vehicle.position.x,
-            ",", vehicle.position.y, ")",
-            ", type=", vehicle.isControllable ? "UNMANNED" : "SPECIAL");
-    }
     Logger::debug("交通数据更新完成: 有效车辆数量=", validVehicles,
         ", 四叉树中的车辆数量=", allVehicles.size());
 }
@@ -80,11 +69,19 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
     // 获取所有车辆
     auto allVehicles = vehicleTree_.queryRange(vehicleTree_.getBounds());
 
-    // 可控车辆（无人车）
-    std::vector<Vehicle> unmannedVehicles;
+    // 前端注册表内的受管车辆（WUREN + TEQIN）
+    std::vector<Vehicle> managedVehicles;
+    managedVehicles.reserve(allVehicles.size());
     for (const auto& vehicle : allVehicles) {
-        if (vehicle.isControllable) {
-            unmannedVehicles.push_back(vehicle);
+        if (controllableVehicles_) {
+            if (controllableVehicles_->isManagedVehicle(vehicle.vehicleNo)) {
+                managedVehicles.push_back(vehicle);
+            }
+        } else {
+            // 兼容：若未提供注册表，则仅按可控标记处理
+            if (vehicle.isControllable) {
+                managedVehicles.push_back(vehicle);
+            }
         }
     }
 
@@ -92,16 +89,15 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
     std::unordered_set<std::pair<std::string, std::string>, CollisionPairHash>
         currentCollisions;
 
-    // 检测可控车辆与航空器的碰撞
+    // 检测受管车辆与航空器的碰撞
     for (const auto& aircraft : aircraftData_) {
-        for (const auto& vehicle : unmannedVehicles) {
+        for (const auto& vehicle : managedVehicles) {
             // 获取冲突记录键
             auto collisionKey = getCollisionKey(aircraft.id, vehicle.id);
 
             // 获取区域配置
             const auto& areaConfig = getCollisionParams(aircraft.position);
-            double safeDistance = areaConfig.warning_zone_radius.aircraft +
-                areaConfig.warning_zone_radius.unmanned;
+            double safeDistance = areaConfig.getThresholds(aircraft.type, vehicle.type).warning;
 
             // 检查是否存在未解除的冲突记录
             auto it = collisionRecords_.find(collisionKey);
@@ -127,11 +123,6 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
             double vy = av.vy - vv.vy;
             double relativeSpeed = std::sqrt(vx * vx + vy * vy);
 
-            // 根据相对运动方向调整相对速度符号
-            if (collisionResult.timeToCollision > 0) {
-                relativeSpeed = -relativeSpeed;
-            }
-
             // 评估风险等级
             auto [level, zoneType] =
                 evaluateRisk(collisionResult, aircraft.position, aircraft.type,
@@ -173,7 +164,7 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
                 risks.push_back({ aircraft.id,
                                  vehicle.id,
                                  level,
-                                 collisionResult.minDistance,
+                                 collisionResult.distance,
                                  collisionResult.minDistance,
                                  relativeSpeed,
                                  {vx, vy},
@@ -199,7 +190,7 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
                     risks.push_back({ aircraft.id,
                                      vehicle.id,
                                      it->second.maxLevel,
-                                     collisionResult.minDistance,
+                                     collisionResult.distance,
                                      collisionResult.minDistance,
                                      relativeSpeed,
                                      {vx, vy},
@@ -345,6 +336,32 @@ CollisionResult CollisionDetector::checkCollision(const MovingObject& obj1,
     double dy = obj2.position.y - obj1.position.y;
     double current_distance = std::sqrt(dx * dx + dy * dy);
 
+    // 高度阈值过滤：飞机高于阈值时，不参与地面二维碰撞判断
+    if (areaConfig.height_threshold > 0.0) {
+        if (obj1.isAircraft()) {
+            const auto& ac = static_cast<const Aircraft&>(obj1);
+            if (std::isfinite(ac.altitude) && ac.altitude > areaConfig.height_threshold) {
+                CollisionResult result;
+                result.willCollide = false;
+                result.minDistance = current_distance;
+                result.distance = current_distance;
+                result.timeToMinDistance = 0.0;
+                return result;
+            }
+        }
+        if (obj2.isAircraft()) {
+            const auto& ac = static_cast<const Aircraft&>(obj2);
+            if (std::isfinite(ac.altitude) && ac.altitude > areaConfig.height_threshold) {
+                CollisionResult result;
+                result.willCollide = false;
+                result.minDistance = current_distance;
+                result.distance = current_distance;
+                result.timeToMinDistance = 0.0;
+                return result;
+            }
+        }
+    }
+
     // 如果是静止的无人车，使用基础速度
     double speed1_calc = obj1.speed;
     double speed2_calc = obj2.speed;
@@ -378,9 +395,9 @@ CollisionResult CollisionDetector::checkCollision(const MovingObject& obj1,
     }
 
     // 否则进行碰撞检测
-    return predictCircleBasedCollision(obj1.position, radius1, obj1.speed,
+    return predictCircleBasedCollision(obj1.position, radius1, speed1_calc,
         obj1.heading, obj2.position, radius2,
-        obj2.speed, obj2.heading, timeWindow);
+        speed2_calc, obj2.heading, timeWindow);
 }
 
 CollisionResult CollisionDetector::predictCircleBasedCollision(
@@ -390,249 +407,30 @@ CollisionResult CollisionDetector::predictCircleBasedCollision(
 
     CollisionResult result;
 
-    // 计算安全距离（两个圆的半径之和）
-    double safe_distance = radius1 + radius2;
+    const double safe_distance = radius1 + radius2;
+    const double dx = pos2.x - pos1.x;
+    const double dy = pos2.y - pos1.y;
+    const double current_distance = std::sqrt(dx * dx + dy * dy);
 
-    // 1. 计算当前距离
-    double dx = pos2.x - pos1.x;
-    double dy = pos2.y - pos1.y;
-    double current_distance = std::sqrt(dx * dx + dy * dy);
-
-    // 初始化最小距离和当前距离
-    result.minDistance = current_distance;
     result.distance = current_distance;
+    result.minDistance = current_distance;
     result.timeToMinDistance = 0.0;
 
-    // 2. 计算速度分量
-    double vx1 = speed1 * std::sin((90.0 - heading1) * M_PI / 180.0);
-    double vy1 = speed1 * std::cos((90.0 - heading1) * M_PI / 180.0);
-    double vx2 = speed2 * std::sin((90.0 - heading2) * M_PI / 180.0);
-    double vy2 = speed2 * std::cos((90.0 - heading2) * M_PI / 180.0);
-
-    // 3. 计算相对运动
-    double rel_vx = vx2 - vx1;
-    double rel_vy = vy2 - vy1;
-    double rel_speed = std::sqrt(rel_vx * rel_vx + rel_vy * rel_vy);
-
-    // 计算相对运动方向与连线方向的夹角
-    double motion_angle = std::atan2(rel_vy, rel_vx);
-    double connection_angle = std::atan2(dy, dx);
-    double angle = std::abs(motion_angle - connection_angle);
-    if (angle > M_PI) {
-        angle = 2 * M_PI - angle; // 确保夹角不超过180度
-    }
-
-    // 4. 确定碰撞类型
+    // 碰撞类型仅用于标记，不影响连续时间的圆形碰撞求解
     if (speed1 < 0.1 && speed2 < 0.1) {
         result.type = CollisionType::STATIC;
-        if (current_distance <= safe_distance) {
-            result.willCollide = true;
-            result.timeToCollision = 0.0;
-            result.minDistance = current_distance;
-            result.timeToMinDistance = 0.0;
-            result.collisionPoint = { (pos1.x + pos2.x) / 2.0,
-                                     (pos1.y + pos2.y) / 2.0 };
-            result.object1State = CollisionObjectState(pos1, speed1, heading1);
-            result.object2State = CollisionObjectState(pos2, speed2, heading2);
-        }
-        return result;
-    }
-
-    double angle_diff = getAngleDifference(heading1, heading2);
-    if (angle_diff > 150.0) {
-        result.type = CollisionType::HEAD_ON;
-        // 对于相向运动，直接计算碰撞时间
-        double closing_speed = speed1 + speed2; // 相对接近速度是两个速度之和
-        if (closing_speed > 0.1) {              // 避免除以零
-            double collision_distance = current_distance - safe_distance;
-            if (collision_distance <= 0) {
-                result.willCollide = true;
-                result.timeToCollision = 0.0;
-                result.minDistance = current_distance;
-                result.timeToMinDistance = 0.0;
-                result.collisionPoint = { (pos1.x + pos2.x) / 2.0,
-                                         (pos1.y + pos2.y) / 2.0 };
-                result.object1State =
-                    CollisionObjectState(pos1, speed1, heading1);
-                result.object2State =
-                    CollisionObjectState(pos2, speed2, heading2);
-            } else {
-                result.timeToCollision = collision_distance / closing_speed;
-                result.willCollide = result.timeToCollision <= timeWindow;
-
-                if (result.willCollide) {
-                    result.minDistance = safe_distance;
-                    result.timeToMinDistance = result.timeToCollision;
-
-                    // 计算碰撞时两个物体的实际位置
-                    double move_distance1 = speed1 * result.timeToCollision;
-                    double move_distance2 = speed2 * result.timeToCollision;
-
-                    // 计算物体1的碰撞位置（向前移动到碰撞点前方）
-                    Vector2D collision1 = {
-                        pos1.x +
-                            move_distance1 * std::sin(heading1 * M_PI / 180.0),
-                        pos1.y +
-                            move_distance1 * std::cos(heading1 * M_PI / 180.0) };
-
-                    // 计算物体2的碰撞位置（也是向前移动）
-                    Vector2D collision2 = {
-                        pos2.x +
-                            move_distance2 * std::sin(heading2 * M_PI / 180.0),
-                        pos2.y +
-                            move_distance2 * std::cos(heading2 * M_PI / 180.0) };
-
-                    // 碰撞点两个物体的中点
-                    result.collisionPoint = {
-                        (collision1.x + collision2.x) / 2.0,
-                        (collision1.y + collision2.y) / 2.0 };
-
-                    result.object1State =
-                        CollisionObjectState(collision1, speed1, heading1);
-                    result.object2State =
-                        CollisionObjectState(collision2, speed2, heading2);
-                }
-            }
-            return result;
-        }
-    } else if (angle_diff > 15.0 &&
-        angle_diff < 165.0) { // 放宽交叉路径的角度范围
-        result.type = CollisionType::CROSSING;
-
-        Logger::debug("交叉路径检测: angle_diff=", angle_diff, ", pos1=(",
-            pos1.x, ",", pos1.y, ")", ", pos2=(", pos2.x, ",", pos2.y,
-            ")", ", v1=(", vx1, ",", vy1, ")", ", v2=(", vx2, ",",
-            vy2, ")", ", current_distance=", current_distance,
-            ", safe_distance=", safe_distance);
-
-        // 计算行列式 (v1 x v2)
-        double det = vx1 * (-vy2) - (-vx2) * vy1;
-
-        // 计算 det1 = (P2-P1) x (-v2)
-        double det1 = dx * (-vy2) - (-vx2) * dy;
-
-        // 计算 det2 = v1 x (P2-P1)
-        double det2 = vx1 * dy - dx * vy1;
-
-        // 计算时间
-        double t1 = det1 / det; // 物体1的时间
-        double t2 = det2 / det; // 物体2的时间
-
-        Logger::debug("交叉路径检测: det1=", det1, ", det2=", det2,
-            ", det=", det, ", t1=", t1, ", t2=", t2);
-
-        // 计算相交点
-        Vector2D intersection = { pos1.x + vx1 * t1, pos1.y + vy1 * t1 };
-
-        Logger::debug("交叉路径检测: intersection=(", intersection.x, ",",
-            intersection.y, ")", ", t1=", t1, ", t2=", t2);
-
-        // 检查时间条件
-        double first_arrival_time = std::min(t1, t2);
-        double first_arrival_speed = (t1 < t2) ? speed1 : speed2;
-        double time_threshold = safe_distance / first_arrival_speed;
-        bool valid_time =
-            (t1 >= 0 && t2 >= 0 && std::max(t1, t2) <= timeWindow &&
-                std::abs(t1 - t2) <= time_threshold);
-
-        // 如果满足所有条件，则预测为碰撞
-        if (valid_time) {
-            // 计算实际碰撞点（在交点之前）
-            double collision_distance = safe_distance;
-            double collision_time =
-                first_arrival_time - collision_distance / first_arrival_speed;
-
-            if (collision_time < 0) {
-                collision_time = 0;
-            }
-
-            // 根据碰撞时间计算两个物体的位置
-            Vector2D pos1_at_collision = { pos1.x + vx1 * collision_time,
-                                          pos1.y + vy1 * collision_time };
-
-            Vector2D pos2_at_collision = { pos2.x + vx2 * collision_time,
-                                          pos2.y + vy2 * collision_time };
-
-            // 碰撞点在两个物体位置的中点
-            result.collisionPoint = {
-                (pos1_at_collision.x + pos2_at_collision.x) / 2.0,
-                (pos1_at_collision.y + pos2_at_collision.y) / 2.0 };
-
-            Logger::debug("碰撞时间计算: first_arrival_time=",
-                first_arrival_time, ", safe_distance=", safe_distance,
-                ", collision_distance=", collision_distance,
-                ", collision_time=", collision_time);
-
-            result.willCollide = true;
-            result.timeToCollision = collision_time;
-            result.timeToMinDistance = collision_time;
-            result.minDistance = safe_distance;
-
-            // 更新物体状态
-            result.object1State =
-                CollisionObjectState(pos1_at_collision, speed1, heading1);
-            result.object2State =
-                CollisionObjectState(pos2_at_collision, speed2, heading2);
-
-            Logger::debug("交叉路径碰撞: collision_time=", collision_time,
-                ", collision1=(", pos1_at_collision.x, ",",
-                pos1_at_collision.y, ")", ", collision2=(",
-                pos2_at_collision.x, ",", pos2_at_collision.y, ")",
-                ", collision_point=(", result.collisionPoint.x, ",",
-                result.collisionPoint.y, ")");
-
-            return result;
-        }
     } else {
-        result.type = CollisionType::PARALLEL;
-        // 平行运动判断：
-        // 1. 航向差很小（小于30度）
-        // 2. 横向距离大于安全距离
-        // 3. 相对速度很小（说明速度接近）
-        if (angle_diff < 30.0) {
-            // 计算垂直于运动方向的向距离
-            // 航向角转换为学坐标系中的旋转角度（逆时针为正）
-            double rotation_angle = (90.0 - heading1) * M_PI / 180.0;
-
-            // 使用标准的二维坐标旋转式
-            double dx_rotated =
-                dx * std::cos(rotation_angle) - dy * std::sin(rotation_angle);
-            double dy_rotated =
-                dx * std::sin(rotation_angle) + dy * std::cos(rotation_angle);
-
-            // 横向距离就是旋转后的y坐标的绝对值
-            double lateral_distance = std::abs(dy_rotated);
-
-            Logger::debug("平行运动检测: angle_diff=", angle_diff,
-                ", heading1=", heading1,
-                ", rotation_angle=", rotation_angle * 180.0 / M_PI,
-                ", dx=", dx, ", dy=", dy, ", dx_rotated=", dx_rotated,
-                ", dy_rotated=", dy_rotated,
-                ", lateral_distance=", lateral_distance,
-                ", safe_distance=", safe_distance,
-                ", rel_speed=", rel_speed);
-
-            // 如果横向距离大于等于安全距离，且相对速度很小，则不会碰撞
-            if (lateral_distance >= safe_distance && rel_speed < 1.0) {
-                result.willCollide = false;
-                return result;
-            }
+        double angle_diff = getAngleDifference(heading1, heading2);
+        if (angle_diff > 150.0) {
+            result.type = CollisionType::HEAD_ON;
+        } else if (angle_diff > 15.0 && angle_diff < 165.0) {
+            result.type = CollisionType::CROSSING;
+        } else {
+            result.type = CollisionType::PARALLEL;
         }
     }
 
-    // 5. 在时间窗口内预测碰撞
-    const int STEPS = 120;          // 增加采样点数以提高精度
-    double dt = timeWindow / STEPS; // 时间步长
-
-    // 新计算速度分量（修正计算方式）
-    double vx1_sample =
-        speed1 * std::cos((90.0 - heading1) * M_PI / 180.0); // x 方向
-    double vy1_sample =
-        speed1 * std::sin((90.0 - heading1) * M_PI / 180.0); // y 方向
-    double vx2_sample = speed2 * std::cos((90.0 - heading2) * M_PI / 180.0);
-    double vy2_sample = speed2 * std::sin((90.0 - heading2) * M_PI / 180.0);
-
-    // 如果当前已经碰撞
+    // 当前已经重叠
     if (current_distance <= safe_distance) {
         result.willCollide = true;
         result.timeToCollision = 0.0;
@@ -645,74 +443,70 @@ CollisionResult CollisionDetector::predictCircleBasedCollision(
         return result;
     }
 
-    // 在时间窗口内采样检查
-    double prev_distance = current_distance;
-    Logger::debug("开始时间窗口采样: STEPS=", STEPS, ", dt=", dt,
-        ", timeWindow=", timeWindow,
-        ", safe_distance=", safe_distance,
-        ", current_distance=", current_distance);
-    for (int i = 1; i <= STEPS; ++i) {
-        double t = i * dt;
+    // 速度分量（航向：正北0度，顺时针增加；x=东，y=北）
+    const double h1 = heading1 * M_PI / 180.0;
+    const double h2 = heading2 * M_PI / 180.0;
+    const Vector2D v1{ speed1 * std::sin(h1), speed1 * std::cos(h1) };
+    const Vector2D v2{ speed2 * std::sin(h2), speed2 * std::cos(h2) };
+    const Vector2D r0{ dx, dy };
+    const Vector2D v{ v2.x - v1.x, v2.y - v1.y };
 
-        // 计算t时的位置（使用修正后的速度分量）
-        Vector2D future1 = { pos1.x + vx1_sample * t, pos1.y + vy1_sample * t };
+    const double a = v.x * v.x + v.y * v.y;
+    const double rv = r0.x * v.x + r0.y * v.y;
 
-        Vector2D future2 = { pos2.x + vx2_sample * t, pos2.y + vy2_sample * t };
-
-        // 计算t时的距离
-        double dx_t = future2.x - future1.x;
-        double dy_t = future2.y - future1.y;
-        double distance =
-            std::sqrt(dx_t * dx_t + dy_t * dy_t); // 统一使用实际距离
-
-        // 更新小距离
-        if (distance < result.minDistance) {
-            result.minDistance = distance;
-            result.timeToMinDistance = t;
-            // Logger::debug(
-            //     "更新最小距离: min_distance=", distance,
-            //     ", time_to_min=", t
-            // );
-        }
-
-        // 检查是否会碰撞
-        if (distance <= safe_distance) {
-            result.willCollide = true;
-            // 使用当前距离和安全距离做插值，提高精确度
-            double progress =
-                (distance - safe_distance) / (prev_distance - safe_distance);
-            double t_interp = t - dt * progress;
-            result.timeToCollision = t_interp;
-
-            Logger::debug("找到碰撞: t=", t, ", distance=", distance,
-                ", prev_distance=", prev_distance, ", dt=", dt,
-                ", t_interp=", t_interp, ", future1=(", future1.x,
-                ",", future1.y, ")", ", future2=(", future2.x, ",",
-                future2.y, ")");
-
-            // 使用插值时间计算更精确的碰撞点
-            Vector2D interp1 = { pos1.x + vx1_sample * t_interp,
-                                pos1.y + vy1_sample * t_interp };
-            Vector2D interp2 = { pos2.x + vx2_sample * t_interp,
-                                pos2.y + vy2_sample * t_interp };
-            result.collisionPoint = { (interp1.x + interp2.x) / 2.0,
-                                     (interp1.y + interp2.y) / 2.0 };
-            result.object1State =
-                CollisionObjectState(interp1, speed1, heading1);
-            result.object2State =
-                CollisionObjectState(interp2, speed2, heading2);
-            break;
-        }
-
-        prev_distance = distance;
-
-        // 如果相对速度很小，且距离增加，可以提前退出
-        if (rel_speed < 0.1 && i > 1) {
-            if (distance > prev_distance) {
-                break;
-            }
-        }
+    // 相对速度近似为0：距离不会变化
+    if (a < 1e-9) {
+        result.object1State = CollisionObjectState(pos1, speed1, heading1);
+        result.object2State = CollisionObjectState(pos2, speed2, heading2);
+        return result;
     }
 
+    // 计算时间窗口内的最小距离
+    double t_min = -rv / a;
+    if (t_min < 0.0) t_min = 0.0;
+    if (t_min > timeWindow) t_min = timeWindow;
+    const Vector2D r_min{ r0.x + v.x * t_min, r0.y + v.y * t_min };
+    const double d_min = std::sqrt(r_min.x * r_min.x + r_min.y * r_min.y);
+    result.minDistance = d_min;
+    result.timeToMinDistance = t_min;
+    result.object1State = CollisionObjectState({ pos1.x + v1.x * t_min, pos1.y + v1.y * t_min }, speed1, heading1);
+    result.object2State = CollisionObjectState({ pos2.x + v2.x * t_min, pos2.y + v2.y * t_min }, speed2, heading2);
+
+    // 计算首次进入安全距离的时间（连续时间解析解）
+    // |r0 + v t|^2 = safe^2 => a t^2 + 2(rv) t + (|r0|^2 - safe^2)=0
+    const double b = 2.0 * rv;
+    const double c = (r0.x * r0.x + r0.y * r0.y) - safe_distance * safe_distance;
+    const double disc = b * b - 4.0 * a * c;
+    if (disc < 0.0) {
+        return result;
+    }
+
+    const double sqrt_disc = std::sqrt(disc);
+    const double inv_2a = 1.0 / (2.0 * a);
+    double t_hit = (-b - sqrt_disc) * inv_2a;
+    double t_exit = (-b + sqrt_disc) * inv_2a;
+
+    if (t_exit < 0.0) {
+        return result;
+    }
+    if (t_hit < 0.0) {
+        t_hit = 0.0;
+    }
+    if (t_hit > timeWindow) {
+        return result;
+    }
+
+    result.willCollide = true;
+    result.timeToCollision = t_hit;
+    result.timeToMinDistance = std::min(result.timeToMinDistance, t_hit);
+    result.minDistance = std::min(result.minDistance, safe_distance);
+
+    const Vector2D p1_hit{ pos1.x + v1.x * t_hit, pos1.y + v1.y * t_hit };
+    const Vector2D p2_hit{ pos2.x + v2.x * t_hit, pos2.y + v2.y * t_hit };
+    result.collisionPoint = { (p1_hit.x + p2_hit.x) / 2.0,
+                             (p1_hit.y + p2_hit.y) / 2.0 };
+    result.object1State = CollisionObjectState(p1_hit, speed1, heading1);
+    result.object2State = CollisionObjectState(p2_hit, speed2, heading2);
+
     return result;
 }

src/detector/SafetyZone.cpp

@@ -84,14 +84,6 @@ bool SafetyZone::isObjectInZone(const MovingObject& object) const {
         ? (object.isAircraft() ? aircraftRadius_ : specialVehicleRadius_)
         : currentRadius_;
 
-    Logger::debug("检查目标 ", object.id, " 是否在安全区内: ",
-        "dx=", dx, ", dy=", dy, ", distance=", distance,
-        ", checkRadius=", checkRadius,
-        ", type=", type_ == SafetyZoneType::NONE ? "NONE" :
-        (type_ == SafetyZoneType::AIRCRAFT ? "AIRCRAFT" : "VEHICLE"),
-        ", center=(", center_.x, ",", center_.y, ")",
-        ", object=(", object.position.x, ",", object.position.y, ")");
-
     return distance <= checkRadius;
 }
 

src/detector/TrafficLightDetector.cpp

@@ -20,12 +20,18 @@ void TrafficLightDetector::processSignal(const TrafficLightSignal& signal,
         return;
     }
 
+    Logger::info("收到红绿灯信号: trafficLightId=", signal.trafficLightId,
+        ", intersection=", intersection->id,
+        ", ns_status=", static_cast<int>(signal.ns_status),
+        ", ew_status=", static_cast<int>(signal.ew_status),
+        ", timestamp=", signal.timestamp);
+
     // 保存当前处理的信号
     current_signal_ = signal;
 
     // 检查每个车辆
     for (const auto& vehicle : vehicles) {
-        if (controllable_vehicles_.isControllable(vehicle.vehicleNo)) {
+        if (controllable_vehicles_.isManagedVehicle(vehicle.vehicleNo)) {
             // 根据信号灯状态发送指令
             // FIXME: 临时修改 - 仅根据南北向状态发送指令，未考虑车辆方向和东西向状态。
             // 需要根据车辆行驶方向判断应该参考 ns_status 还是 ew_status。
@@ -66,7 +72,7 @@ void TrafficLightDetector::sendSignalCommand(const std::string& vehicleNo, Signa
 
     controllable_vehicles_.sendCommand(vehicleNo, cmd);
     system_.broadcastVehicleCommand(cmd);
-    Logger::debug("发送信号灯指令到车辆: ", vehicleNo,
+    Logger::info("发送信号灯指令到车辆: ", vehicleNo,
         " 路口: ", cmd.intersectionId,
         " 状态: ", state == SignalState::RED ? "红灯" : state == SignalState::GREEN ? "绿灯" : "黄灯");
 }

src/network/HTTPClient.cpp

@@ -1,8 +1,17 @@
 #include "HTTPClient.h"
 #include "utils/Logger.h"
+#include <chrono>
 #include <sstream>
+#include <string>
 
 namespace {
+    std::string truncateForLog(const std::string& s, std::size_t max_len = 2000) {
+        if (s.size() <= max_len) {
+            return s;
+        }
+        return s.substr(0, max_len) + "...(truncated, len=" + std::to_string(s.size()) + ")";
+    }
+
     std::string getSignalStateString(SignalState state) {
         switch (state) {
         case SignalState::RED:
@@ -92,16 +101,18 @@ bool HTTPClient::sendCommand(const std::string& host, int port, const std::strin
         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;
-    }
+    // 后端当前校验要求该字段始终存在；对非 ALERT/WARNING 场景默认填 0
+    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();
 
+    Logger::info("HTTPClient sendCommand request: url=", url.str(), " body=", truncateForLog(request_body));
+    const auto start = std::chrono::steady_clock::now();
+
     // 设置CURL选项
     curl_easy_setopt(curl_, CURLOPT_URL, url.str().c_str());
     curl_easy_setopt(curl_, CURLOPT_POST, 1L);
@@ -118,8 +129,14 @@ bool HTTPClient::sendCommand(const std::string& host, int port, const std::strin
     CURLcode res = curl_easy_perform(curl_);
     curl_slist_free_all(headers);
 
+    const auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
+        std::chrono::steady_clock::now() - start).count();
+
     if (res != CURLE_OK) {
-        Logger::error("Failed to send command: ", curl_easy_strerror(res));
+        Logger::error("HTTPClient sendCommand failed: ", curl_easy_strerror(res),
+            " url=", url.str(),
+            " elapsed_ms=", elapsed_ms,
+            " response=", truncateForLog(response_buffer_));
         return false;
     }
 
@@ -127,6 +144,11 @@ bool HTTPClient::sendCommand(const std::string& host, int port, const std::strin
     long http_code = 0;
     curl_easy_getinfo(curl_, CURLINFO_RESPONSE_CODE, &http_code);
 
+    Logger::info("HTTPClient sendCommand response: http_code=", http_code,
+        " elapsed_ms=", elapsed_ms,
+        " url=", url.str(),
+        " body=", truncateForLog(response_buffer_));
+
     if (http_code == 200) {
         try {
             // 解析响应
@@ -135,7 +157,9 @@ bool HTTPClient::sendCommand(const std::string& host, int port, const std::strin
             std::string msg = response["msg"].get<std::string>();
 
             if (code == 200) {
-                Logger::debug("Command sent successfully: ", request_body);
+                Logger::info("Command accepted by server: code=", code, " msg=", msg,
+                    " url=", url.str(),
+                    " transId=", transId.str());
                 return true;
             } else {
                 Logger::error("Command failed with code: ", code, " message: ", msg);

src/network/HTTPDataSource.cpp

@@ -328,12 +328,11 @@ bool HTTPDataSource::sendUnmannedVehicleCommand(const std::string& vehicle_id, c
         {"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;
-    }
+    // 后端当前校验要求该字段始终存在；对非 ALERT/WARNING 场景默认填 0
+    request["relativeSpeed"] = command.relativeSpeed;
+    request["relativeMotionX"] = command.relativeMotionX;
+    request["relativeMotionY"] = command.relativeMotionY;
+    request["minDistance"] = command.minDistance;
 
     std::string response;
     if (!sendRequest(url, &unmanned_vehicle_auth_, response, HttpMethod::POST, request.dump())) {

src/network/WebSocketServer.cpp

@@ -68,7 +68,7 @@ namespace network {
                 }
             }
 
-            Logger::debug("广播消息完成: ", successCount, " 个成功, ", failCount, " 个失败, 当前共 ", sessions_.size(), " 个连接");
+            // 降噪：广播结果统计过于频繁，保留必要的错误日志即可
         };
 
         // 异步投递：避免 broadcast 阻塞调用方线程（例如碰撞处理主循环）

src/vehicle/ControllableVehicles.cpp

@@ -32,13 +32,20 @@ void ControllableVehicles::updateRegistry(const std::vector<VehicleRegistryEntry
             } else {
                 controllable_vehicle_ids_.erase(e.vehicleID);
             }
+
+            if (e.vehicleType == "WUREN" || e.vehicleType == "TEQIN") {
+                managed_vehicle_ids_.insert(e.vehicleID);
+            } else {
+                managed_vehicle_ids_.erase(e.vehicleID);
+            }
             applied++;
         }
     }
 
     Logger::info("Vehicle registry updated. applied=", applied,
         " total=", vehicle_type_by_id_.size(),
-        " controllable(WUREN)=", controllable_vehicle_ids_.size());
+        " controllable(WUREN)=", controllable_vehicle_ids_.size(),
+        " managed(WUREN+TEQIN)=", managed_vehicle_ids_.size());
 }
 
 std::optional<std::string> ControllableVehicles::getVehicleType(const std::string& vehicleID) const {
@@ -60,9 +67,14 @@ bool ControllableVehicles::isControllable(const std::string& vehicleNo) const {
     return controllable_vehicle_ids_.find(vehicleNo) != controllable_vehicle_ids_.end();
 }
 
+bool ControllableVehicles::isManagedVehicle(const std::string& vehicleNo) const {
+    std::shared_lock lock(mutex_);
+    return managed_vehicle_ids_.find(vehicleNo) != managed_vehicle_ids_.end();
+}
+
 bool ControllableVehicles::sendCommand(const std::string& vehicleNo, const VehicleCommand& command) {
-    if (!isControllable(vehicleNo)) {
-        Logger::debug("Skip sendCommand: vehicle not controllable: ", vehicleNo);
+    if (!isManagedVehicle(vehicleNo)) {
+        Logger::debug("Skip sendCommand: vehicle not managed(WUREN/TEQIN): ", vehicleNo);
         return false;
     }
 

src/vehicle/ControllableVehicles.h

@@ -25,6 +25,7 @@ private:
     mutable std::shared_mutex mutex_;
     std::unordered_map<std::string, std::string> vehicle_type_by_id_;
     std::unordered_set<std::string> controllable_vehicle_ids_; // vehicleType == WUREN
+    std::unordered_set<std::string> managed_vehicle_ids_;      // vehicleType in {WUREN, TEQIN}
 
     std::unique_ptr<HTTPClient> http_client_;
 
@@ -39,5 +40,6 @@ public:
     std::unordered_set<std::string> getControllableVehicleIdsSnapshot() const;
 
     bool isControllable(const std::string& vehicleNo) const;
+    bool isManagedVehicle(const std::string& vehicleNo) const;
     bool sendCommand(const std::string& vehicleNo, const VehicleCommand& command);
 }; 

命令.md

@@ -6,6 +6,7 @@ rm -rf build
      export LD_LIBRARY_PATH=/opt/rh/devtoolset-9/root/usr/lib64:$LD_LIBRARY_PATH
 
      /opt/cmake/bin/cmake -S . -B build -DCMAKE_BUILD_TYPE=Release \
+       -DLOCAL_NLOHMANN_JSON_SOURCE_DIR=$PWD/json \
        -DLINK_STATIC_DEPS=ON \
        -DFULL_STATIC=OFF \
        -DBoost_NO_SYSTEM_PATHS=ON \