diff --git a/CMakeLists.txt b/CMakeLists.txt index 70bc398..b516ed3 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/src/collector/DataCollector.cpp b/src/collector/DataCollector.cpp index e2549a2..c666500 100644 --- a/src/collector/DataCollector.cpp +++ b/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 lock(cacheMutex_); trafficLightCache_ = std::move(newSignals); // 更新时间戳 diff --git a/src/config/AirportBounds.cpp b/src/config/AirportBounds.cpp index 14b4514..63b76ba 100644 --- a/src/config/AirportBounds.cpp +++ b/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 { // 按优先级检查位置所在区域 diff --git a/src/config/AirportBounds.h b/src/config/AirportBounds.h index d03e6eb..663175d 100644 --- a/src/config/AirportBounds.h +++ b/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 areaBounds_; // 各区域边界 diff --git a/src/core/System.cpp b/src/core/System.cpp index 5c378b4..9d4879d 100644 --- a/src/core/System.cpp +++ b/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( 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& 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& 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(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); } diff --git a/src/detector/CollisionDetector.cpp b/src/detector/CollisionDetector.cpp index a25887b..def0358 100644 --- a/src/detector/CollisionDetector.cpp +++ b/src/detector/CollisionDetector.cpp @@ -2,6 +2,7 @@ #include "config/SystemConfig.h" #include "utils/Logger.h" +#include #include #include @@ -41,14 +42,8 @@ void CollisionDetector::updateTraffic(const std::vector& 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, 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 CollisionDetector::detectCollisions() { // 获取所有车辆 auto allVehicles = vehicleTree_.queryRange(vehicleTree_.getBounds()); - // 可控车辆(无人车) - std::vector unmannedVehicles; + // 前端注册表内的受管车辆(WUREN + TEQIN) + std::vector 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 CollisionDetector::detectCollisions() { std::unordered_set, 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 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 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 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(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(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; } \ No newline at end of file diff --git a/src/detector/SafetyZone.cpp b/src/detector/SafetyZone.cpp index 620172c..687152a 100644 --- a/src/detector/SafetyZone.cpp +++ b/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; } diff --git a/src/detector/TrafficLightDetector.cpp b/src/detector/TrafficLightDetector.cpp index 84e4f5a..872aa53 100644 --- a/src/detector/TrafficLightDetector.cpp +++ b/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(signal.ns_status), + ", ew_status=", static_cast(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 ? "绿灯" : "黄灯"); } \ No newline at end of file diff --git a/src/network/HTTPClient.cpp b/src/network/HTTPClient.cpp index bf8ad03..af54a52 100644 --- a/src/network/HTTPClient.cpp +++ b/src/network/HTTPClient.cpp @@ -1,8 +1,17 @@ #include "HTTPClient.h" #include "utils/Logger.h" +#include #include +#include 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::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(); 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); diff --git a/src/network/HTTPDataSource.cpp b/src/network/HTTPDataSource.cpp index 0f16b47..fb325b0 100644 --- a/src/network/HTTPDataSource.cpp +++ b/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())) { diff --git a/src/network/WebSocketServer.cpp b/src/network/WebSocketServer.cpp index ed1388e..7367a60 100644 --- a/src/network/WebSocketServer.cpp +++ b/src/network/WebSocketServer.cpp @@ -68,7 +68,7 @@ namespace network { } } - Logger::debug("广播消息完成: ", successCount, " 个成功, ", failCount, " 个失败, 当前共 ", sessions_.size(), " 个连接"); + // 降噪:广播结果统计过于频繁,保留必要的错误日志即可 }; // 异步投递:避免 broadcast 阻塞调用方线程(例如碰撞处理主循环) diff --git a/src/vehicle/ControllableVehicles.cpp b/src/vehicle/ControllableVehicles.cpp index 9d08d10..8545e8e 100644 --- a/src/vehicle/ControllableVehicles.cpp +++ b/src/vehicle/ControllableVehicles.cpp @@ -32,13 +32,20 @@ void ControllableVehicles::updateRegistry(const std::vector vehicle_type_by_id_; std::unordered_set controllable_vehicle_ids_; // vehicleType == WUREN + std::unordered_set managed_vehicle_ids_; // vehicleType in {WUREN, TEQIN} std::unique_ptr http_client_; @@ -39,5 +40,6 @@ public: std::unordered_set 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); }; \ No newline at end of file diff --git a/命令.md b/命令.md index 298d08c..272d327 100644 --- a/命令.md +++ b/命令.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 \