From d6a6377ab99e60772526b6e11f04c4f313bc4820 Mon Sep 17 00:00:00 2001 From: Tian jianyong <11429339@qq.com> Date: Wed, 18 Dec 2024 02:58:44 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=AE=89=E5=85=A8=E5=8C=BA?= =?UTF-8?q?=E5=86=B2=E7=AA=81=E6=A3=80=E6=B5=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 + config/airport_bounds.json | 18 +- config/intersections.json | 14 +- config/vehicle_control.json | 8 + src/config/IntersectionConfig.cpp | 20 +- src/config/IntersectionConfig.h | 10 +- src/core/System.cpp | 367 ++++++++++++++++++---- src/core/System.h | 30 ++ src/detector/CollisionDetector.cpp | 449 +++++++++++++++------------ src/detector/CollisionDetector.h | 31 ++ src/detector/CollisionTypes.h | 2 + src/detector/SafetyZone.cpp | 143 +++++++++ src/detector/SafetyZone.h | 64 ++++ src/network/HTTPDataSource.h | 2 +- src/types/BasicTypes.h | 22 +- src/vehicle/ControllableVehicles.cpp | 12 +- src/vehicle/ControllableVehicles.h | 1 + tests/BasicCollisionTest.cpp | 160 ++++++++-- tests/CollisionDetectorTest.cpp | 5 +- tests/SafetyZoneTest.cpp | 178 +++++++++++ tools/map_websocket.html | 14 +- tools/mock_server.py | 23 +- 22 files changed, 1258 insertions(+), 317 deletions(-) create mode 100644 src/detector/SafetyZone.cpp create mode 100644 src/detector/SafetyZone.h create mode 100644 tests/SafetyZoneTest.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9be3ac1..93cb088 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,6 +49,7 @@ set(LIB_SOURCES src/collector/DataCollector.cpp src/core/System.cpp src/detector/CollisionDetector.cpp + src/detector/SafetyZone.cpp src/network/HTTPDataSource.cpp src/network/WebSocketServer.cpp src/network/HTTPClient.cpp @@ -100,6 +101,7 @@ if(EXISTS "${CMAKE_SOURCE_DIR}/tests") tests/BasicTypesTest.cpp tests/HTTPDataSourceTest.cpp tests/DataCollectorTest.cpp + tests/SafetyZoneTest.cpp ) # 创建测试可执行文件 diff --git a/config/airport_bounds.json b/config/airport_bounds.json index fb00df4..8a6aadf 100644 --- a/config/airport_bounds.json +++ b/config/airport_bounds.json @@ -121,20 +121,20 @@ }, "config": { "collision_radius": { - "aircraft": 50.0, - "special": 25.0, - "unmanned": 25.0 + "aircraft": 30.0, + "special": 15.0, + "unmanned": 15.0 }, "height_threshold": 10.0, "warning_zone_radius": { - "aircraft": 100.0, - "special": 50.0, - "unmanned": 50.0 + "aircraft": 70.0, + "special": 30.0, + "unmanned": 30.0 }, "alert_zone_radius": { - "aircraft": 50.0, - "special": 25.0, - "unmanned": 25.0 + "aircraft": 35.0, + "special": 15.0, + "unmanned": 15.0 } } } diff --git a/config/intersections.json b/config/intersections.json index bdde6ba..c5c0ad8 100644 --- a/config/intersections.json +++ b/config/intersections.json @@ -9,8 +9,11 @@ "latitude": 36.35448347, "altitude": 9.543 }, - "radius": 100.0, - "stopDistance": 50.0 + "width": 20.0, + "safetyZone": { + "aircraftRadius": 50.0, + "vehicleRadius": 50.0 + } }, { "id": "T6路口", @@ -21,8 +24,11 @@ "latitude": 36.35074527, "altitude": 9.778 }, - "radius": 100.0, - "stopDistance": 50.0 + "width": 30.0, + "safetyZone": { + "aircraftRadius": 50.0, + "vehicleRadius": 50.0 + } } ] } \ No newline at end of file diff --git a/config/vehicle_control.json b/config/vehicle_control.json index 745192d..4ec60a2 100644 --- a/config/vehicle_control.json +++ b/config/vehicle_control.json @@ -2,11 +2,19 @@ "vehicles": [ { "vehicleNo": "QN001", + "type": "UNMANNED", "ip": "localhost", "port": 8081 }, { "vehicleNo": "QN002", + "type": "UNMANNED", + "ip": "localhost", + "port": 8081 + }, + { + "vehicleNo": "TQ001", + "type": "SPECIAL", "ip": "localhost", "port": 8081 } diff --git a/src/config/IntersectionConfig.cpp b/src/config/IntersectionConfig.cpp index a1007a3..6617a85 100644 --- a/src/config/IntersectionConfig.cpp +++ b/src/config/IntersectionConfig.cpp @@ -33,18 +33,24 @@ IntersectionConfig IntersectionConfig::load(const std::string& configFile) { info.position.latitude = item["position"]["latitude"].get(); info.position.altitude = item["position"]["altitude"].get(); - // 加载距离阈值，检查是否为 null - if (item["radius"].is_null() || item["stopDistance"].is_null()) { - throw std::runtime_error("Distance values cannot be null for intersection: " + info.id); + // 加载路口宽度和安全区配置，检查是否为 null + if (item["width"].is_null() || + item["safetyZone"]["aircraftRadius"].is_null() || + item["safetyZone"]["vehicleRadius"].is_null()) { + throw std::runtime_error("Width or safety zone values cannot be null for intersection: " + info.id); } - info.radius = item["radius"].get(); - info.stopDistance = item["stopDistance"].get(); + info.width = item["width"].get(); + info.safetyZone.aircraftRadius = item["safetyZone"]["aircraftRadius"].get(); + info.safetyZone.vehicleRadius = item["safetyZone"]["vehicleRadius"].get(); config.intersections_.push_back(info); Logger::debug("Loaded intersection: id=", info.id, - " name=", info.name, - " trafficLightId=", info.trafficLightId); + ", name=", info.name, + ", trafficLightId=", info.trafficLightId, + ", width=", info.width, + ", aircraftRadius=", info.safetyZone.aircraftRadius, + ", vehicleRadius=", info.safetyZone.vehicleRadius); } Logger::info("Loaded ", config.intersections_.size(), " intersections"); diff --git a/src/config/IntersectionConfig.h b/src/config/IntersectionConfig.h index 35ee9d6..e7295aa 100644 --- a/src/config/IntersectionConfig.h +++ b/src/config/IntersectionConfig.h @@ -10,13 +10,19 @@ struct IntersectionPosition { double altitude; }; +// 路口安全区配置 +struct SafetyZoneConfig { + double aircraftRadius; // 航空器安全区半径 + double vehicleRadius; // 特勤车安全区半径 +}; + struct Intersection { std::string id; std::string name; std::string trafficLightId; IntersectionPosition position; - double radius; // 路口影响半径，单位：米 - double stopDistance; // 建议停车距离，单位：米 + double width; // 路口宽度，单位：米 + SafetyZoneConfig safetyZone; // 安全区配置 }; class IntersectionConfig { diff --git a/src/core/System.cpp b/src/core/System.cpp index 0150c5d..84324cc 100644 --- a/src/core/System.cpp +++ b/src/core/System.cpp @@ -5,6 +5,7 @@ #include "System.h" #include "utils/Logger.h" #include "collector/DataCollector.h" +#include "spatial/CoordinateConverter.h" System* System::instance_ = nullptr; @@ -74,6 +75,9 @@ bool System::initialize() { system_config.warning.log_interval_ms }; + // 初始化安全区 + initializeSafetyZones(); + return dataCollector_->initialize(dataSourceConfig, warnConfig); } catch (const std::exception& e) { @@ -82,6 +86,45 @@ bool System::initialize() { } } +void System::initializeSafetyZones() { + // 清空现有的安全区 + safetyZones_.clear(); + + // 获取所有路口配置 + const auto& intersections = intersection_config_.getIntersections(); + + // 创建坐标转换器 + CoordinateConverter converter; + converter.setReferencePoint( + SystemConfig::instance().airport.reference_point.latitude, + SystemConfig::instance().airport.reference_point.longitude + ); + + // 为每个路口创建安全区 + for (const auto& intersection : intersections) { + // 获取路口中心点的地理坐标 + Vector2D center = converter.toLocalXY( + intersection.position.latitude, + intersection.position.longitude + ); + + // 创建安全区 + auto safetyZone = std::make_unique( + center, + intersection.safetyZone.aircraftRadius, // 飞机安全区半径 + intersection.safetyZone.vehicleRadius // 特勤车安全区半径 + ); + + Logger::debug("创建路口安全区: id=", intersection.id, + ", center=(", center.x, ",", center.y, ")", + ", aircraftRadius=", intersection.safetyZone.aircraftRadius, + ", vehicleRadius=", intersection.safetyZone.vehicleRadius); + + // 保存安全区 + safetyZones_[intersection.id] = std::move(safetyZone); + } +} + void System::start() { if (running_) { return; @@ -126,10 +169,12 @@ void System::processLoop() { auto last_vehicle_update = std::chrono::steady_clock::now(); auto last_collision_update = std::chrono::steady_clock::now(); auto last_traffic_light_update = std::chrono::steady_clock::now(); + auto last_safety_zone_update = std::chrono::steady_clock::now(); // 添加安全区更新时间 int64_t last_aircraft_timestamp = 0; int64_t last_vehicle_timestamp = 0; int64_t last_collision_timestamp = 0; int64_t last_traffic_light_timestamp = 0; + int64_t last_safety_zone_timestamp = 0; // 添加安全区时间戳 Logger::debug("数据处理循环启动"); @@ -147,6 +192,41 @@ void System::processLoop() { auto vehicles = dataCollector_->getVehicleData(); auto traffic_lights = dataCollector_->getTrafficLightSignals(); + // 合并航空器和车辆数据用于安全区检查 + std::vector objects; + for (auto& ac : aircraft) { + objects.push_back(&ac); + } + for (auto& veh : vehicles) { + objects.push_back(&veh); + //Logger::debug("车辆 ", veh.vehicleNo, " 类型: ", veh.type); + //Logger::debug("车辆 ", veh.vehicleNo, " 是否可控: ", veh.isControllable); + const auto* config = controllableVehicles_->findVehicle(veh.vehicleNo); + if (config) { + veh.type = config->type == "UNMANNED" ? MovingObjectType::UNMANNED : MovingObjectType::SPECIAL; + veh.isControllable = config->type == "UNMANNED"; + } + } + + // 检查安全区更新 + auto safety_zone_elapsed = std::chrono::duration_cast( + now - last_safety_zone_update).count(); + if (safety_zone_elapsed >= system_config.collision_detection.update_interval_ms) { + // 只有在数据更新时才进行检测 + if (last_aircraft_timestamp > last_safety_zone_timestamp || + last_vehicle_timestamp > last_safety_zone_timestamp) { + // 更新安全区状态 + updateSafetyZoneStates(objects); + Logger::debug("开始检查安全区冲突..."); + checkUnmannedVehicleSafetyZones(vehicles, objects); + last_safety_zone_timestamp = std::max(last_aircraft_timestamp, last_vehicle_timestamp); + } + last_safety_zone_update = now; + } + + // 检查无人车与安全区的冲突 + checkUnmannedVehicleSafetyZones(vehicles, objects); + // 检查航空器更新 auto aircraft_elapsed = std::chrono::duration_cast( now - last_aircraft_update).count(); @@ -193,7 +273,7 @@ void System::processLoop() { Logger::debug("距离上次更新的时间间隔: ", vehicle_elapsed, "ms"); Logger::debug("配置的更新间隔: ", system_config.websocket.position_update.vehicle_interval_ms, "ms"); Logger::debug("上次时间戳: ", last_vehicle_timestamp); - Logger::debug("收到车辆数量: ", vehicles.size()); + Logger::debug("收辆数量: ", vehicles.size()); // 如果是第一次更新（last_vehicle_timestamp == 0），则广播所有车辆位置 if (last_vehicle_timestamp == 0 && !vehicles.empty()) { @@ -227,7 +307,7 @@ void System::processLoop() { if (has_new_vehicles) { Logger::debug("发现新数据，准备广播..."); - Logger::debug("更新时间戳: ", last_vehicle_timestamp, " -> ", max_timestamp); + Logger::debug("更新间戳: ", last_vehicle_timestamp, " -> ", max_timestamp); for (const auto& veh : vehicles) { broadcastPositionUpdate(veh); } @@ -241,59 +321,59 @@ void System::processLoop() { Logger::debug("车辆位置更新检查结束 <<<<<<<<<<<<<<<"); } - // 检查冲突更新 - auto collision_elapsed = std::chrono::duration_cast( - now - last_collision_update).count(); - if (collision_elapsed >= system_config.collision_detection.update_interval_ms) { - // 只有当有新的数据时才更新冲突检测 - if (last_aircraft_timestamp > last_collision_timestamp || - last_vehicle_timestamp > last_collision_timestamp) { + // // 检查冲突更新 + // auto collision_elapsed = std::chrono::duration_cast( + // now - last_collision_update).count(); + // if (collision_elapsed >= system_config.collision_detection.update_interval_ms) { + // // 只有有新的数据时才更新冲突检测 + // if (last_aircraft_timestamp > last_collision_timestamp || + // last_vehicle_timestamp > last_collision_timestamp) { - // 更新冲突检测器 - collisionDetector_->updateTraffic(aircraft, vehicles); - auto collisions = collisionDetector_->detectCollisions(); + // // 更新冲突检测器 + // collisionDetector_->updateTraffic(aircraft, vehicles); + // auto collisions = collisionDetector_->detectCollisions(); - if (!collisions.empty()) { - Logger::debug("检测到 ", collisions.size(), "个碰撞风险"); - for (const auto& risk : collisions) { - Logger::debug("碰撞风险详情: id1=", risk.id1, - ", id2=", risk.id2, - ", 距离=", risk.distance, "m, 预测最小距离=", risk.minDistance, "m, 风险等级=", static_cast(risk.level), - ", 区域类型=", static_cast(risk.zoneType)); - } - processCollisions(collisions); + // if (!collisions.empty()) { + // Logger::debug("检测到 ", collisions.size(), "个碰撞风险"); + // for (const auto& risk : collisions) { + // Logger::debug("碰撞风险详情: id1=", risk.id1, + // ", id2=", risk.id2, + // ", 距离=", risk.distance, "m, 预测最小距离=", risk.minDistance, "m, ���险等级=", static_cast(risk.level), + // ", 区域类型=", static_cast(risk.zoneType)); + // } + // processCollisions(collisions); - } else if (!lastVehiclesWithRisk_.empty()) { - // 当前没有任何风险，但上次有风险车辆，需要处理恢复指令 - Logger::debug("当前无碰撞风险，检查是否需要发送恢复指令"); - for (const auto& vehicleId : lastVehiclesWithRisk_) { - 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(); + // } else if (!lastVehiclesWithRisk_.empty()) { + // // 当前没有任何风险，��上次有风险车辆，需要处理恢复指令 + // Logger::debug("当前无碰撞风险，检查是否需要发送恢复指令"); + // for (const auto& vehicleId : lastVehiclesWithRisk_) { + // 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); - } - // 清空风险车辆列表 - Logger::debug("清空风险车辆列表: 原数量=", lastVehiclesWithRisk_.size(), - ", 原列表={", [&]() { - std::string s; - for (const auto& id : lastVehiclesWithRisk_) { - s += id + ","; - } - return s; - }(), "}"); - lastVehiclesWithRisk_.clear(); - } + // broadcastVehicleCommand(cmd); + // controllableVehicles_->sendCommand(vehicleId, cmd); + // Logger::info("发送恢复指令到车辆: ", vehicleId); + // } + // // 清空风险车辆列表 + // Logger::debug("清空风险车辆列表: 原数量=", lastVehiclesWithRisk_.size(), + // ", 原列表={", [&]() { + // std::string s; + // for (const auto& id : lastVehiclesWithRisk_) { + // s += id + ","; + // } + // return s; + // }(), "}"); + // lastVehiclesWithRisk_.clear(); + // } - last_collision_timestamp = std::max(last_aircraft_timestamp, last_vehicle_timestamp); - } - last_collision_update = now; - } + // last_collision_timestamp = std::max(last_aircraft_timestamp, last_vehicle_timestamp); + // } + // last_collision_update = now; + // } // 处理红绿灯信号 auto traffic_light_elapsed = std::chrono::duration_cast( @@ -448,13 +528,13 @@ void System::processCollisions(const std::vector& risks) { } Logger::debug("当前有风险的可控车辆数量: ", currentVehiclesWithRisk.size()); - Logger::debug("上一次有风险的可控车辆数量: ", lastVehiclesWithRisk_.size()); + Logger::debug("上一次有风险的可车辆数量: ", lastVehiclesWithRisk_.size()); // 对之前有风险但现在没有风险的车辆发送恢复指令 for (const auto& vehicleId : lastVehiclesWithRisk_) { Logger::debug("检查车辆是否需要恢复: ", vehicleId); if (currentVehiclesWithRisk.find(vehicleId) == currentVehiclesWithRisk.end()) { - Logger::debug("车辆 ", vehicleId, " 当前没有风险，准备发送恢复指令"); + Logger::debug("车辆 ", vehicleId, " 前没有风险，准备发送恢复指令"); VehicleCommand cmd; cmd.vehicleId = vehicleId; cmd.type = CommandType::RESUME; @@ -593,7 +673,7 @@ void System::broadcastTimeoutWarning(const network::TimeoutWarningMessage& warni if (ws_server_) { ws_server_->broadcast(warning.toJson().dump()); - // 根据超时时间记录不同级别的日志 + // 根据超时时间记录不同级的日志 if (warning.elapsed_ms > 30000) { // 30秒以上 Logger::error("Severe timeout: ", warning.source, " - ", warning.elapsed_ms, "ms without response"); @@ -640,7 +720,7 @@ void System::broadcastVehicleCommand(const VehicleCommand& cmd) { j["intersectionId"] = cmd.intersectionId; } - // 添加目标位置（对于所有非 RESUME 类型的指令） + // 添���目标位置（对于所有非 RESUME 类型的指令） if (cmd.type != CommandType::RESUME) { j["targetLatitude"] = cmd.latitude; j["targetLongitude"] = cmd.longitude; @@ -673,4 +753,183 @@ const MovingObject* System::findVehicle(const std::string& vehicleId) const { } return nullptr; +} + +void System::updateSafetyZoneStates(const std::vector& objects) { + // 重置所有安全区状态为未激活，同时重置类型 + for (auto& [id, zone] : safetyZones_) { + zone->setState(collision::SafetyZoneState::INACTIVE); + zone->resetType(); // 重置安全区类型 + } + + // 检查所有移动物体 + 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); + } + } +} + +void System::checkSafetyZoneIntrusion(const MovingObject& obj) { + // 创建坐标转换器 + CoordinateConverter converter; + converter.setReferencePoint( + SystemConfig::instance().airport.reference_point.latitude, + SystemConfig::instance().airport.reference_point.longitude + ); + + // 转换目标位置到本地坐标系 + Vector2D position = converter.toLocalXY(obj.geo.latitude, obj.geo.longitude); + Logger::debug("检查安全区入侵: id=", obj.id, + ", type=", obj.isAircraft() ? "飞机" : (obj.isSpecialVehicle() ? "特勤车" : "其他"), + ", position=(", position.x, ",", position.y, ")"); + + // 检查每个安全区 + for (auto& [id, zone] : safetyZones_) { + // 检查是否在安全区内，同时会尝试设置安全区类型 + if (zone->isObjectInZone(obj)) { + zone->setState(collision::SafetyZoneState::ACTIVE); + Logger::debug("目标 ", obj.id, " 进入路口 ", id, " 安全区, 类型: ", + zone->getType() == collision::SafetyZoneType::AIRCRAFT ? "飞机" : "特勤车", + ", 半径: ", zone->getCurrentRadius(), + ", 中心点: (", zone->getCenter().x, ",", zone->getCenter().y, ")"); + } + } +} + +void System::checkUnmannedVehicleSafetyZones(const std::vector& vehicles, + const std::vector& objects) { + // 遍历所有无人车 + for (const auto& vehicle : vehicles) { + bool hasRisk = false; + std::string riskIntersectionId; + + // 遍历所有路口安全区 + for (const auto& [intersectionId, zone] : safetyZones_) { + // 如果安全区未激活，跳过 + if (zone->getState() == collision::SafetyZoneState::INACTIVE) { + continue; + } + + // 计算无人车到安全区中心的距离 + double dx = vehicle.position.x - zone->getCenter().x; + double dy = vehicle.position.y - zone->getCenter().y; + double distance = std::sqrt(dx * dx + dy * dy); + double zoneRadius = zone->getCurrentRadius(); + + // 检查是否在预警或告警范围内 + if (distance <= zoneRadius) { + // 告警范围 + Logger::warning("无人车 ", vehicle.id, " 进入路口 ", intersectionId, " 安全区告警范围"); + hasRisk = handleSafetyZoneRisk(vehicle, zone.get(), objects, distance, + intersectionId, CommandType::ALERT, "告警"); + riskIntersectionId = intersectionId; + } else if (distance <= 2 * zoneRadius) { + // 预警范围 + Logger::warning("无人车 ", vehicle.id, " 进入路口 ", intersectionId, " 安全区预警范围"); + hasRisk = handleSafetyZoneRisk(vehicle, zone.get(), objects, distance, + intersectionId, CommandType::WARNING, "预警"); + riskIntersectionId = intersectionId; + } + } + + // 检查是否需要发送恢��指令 + auto it = lastVehiclesWithSafetyZoneRisk_.find(vehicle.id); + if (!hasRisk && it != lastVehiclesWithSafetyZoneRisk_.end()) { + // 之前有风险，现在没有风险，发送恢复指令 + Logger::info("无人车 ", vehicle.id, " 离开路口 ", it->second, " 安全区"); + VehicleCommand cmd; + cmd.vehicleId = vehicle.id; + cmd.type = CommandType::RESUME; + cmd.reason = CommandReason::RESUME_TRAFFIC; + cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); + + broadcastVehicleCommand(cmd); + controllableVehicles_->sendCommand(vehicle.id, cmd); + + lastVehiclesWithSafetyZoneRisk_.erase(it); + } else if (hasRisk) { + // 更新最后一次风险记录 + lastVehiclesWithSafetyZoneRisk_[vehicle.id] = riskIntersectionId; + } + } +} + +bool System::handleSafetyZoneRisk(const Vehicle& vehicle, + const collision::SafetyZone* zone, + const std::vector& objects, + double distance, + const std::string& intersectionId, + CommandType cmdType, + const std::string& riskLevel) { + // 检查是否为无人车 + if (!controllableVehicles_->isControllable(vehicle.id)) { + return false; + } + + VehicleCommand cmd; + cmd.vehicleId = vehicle.id; + cmd.type = cmdType; + cmd.reason = zone->getType() == collision::SafetyZoneType::AIRCRAFT ? + CommandReason::AIRCRAFT_CROSSING : + CommandReason::SPECIAL_VEHICLE; + cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); + + // 查找触发安全区的目标物体 + const MovingObject* target = nullptr; + for (const auto& obj : objects) { + if ((obj->isAircraft() || obj->isSpecialVehicle()) && + zone->isObjectInZone(*obj)) { + target = obj; + break; + } + } + + // 如果找到目标物体，添加相关信息 + if (target) { + 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; + cmd.minDistance = distance; + + Logger::debug("目标物体信息: id=", target->id, + ", 相对距离=(", rel_dx, ",", rel_dy, ")", + ", 相对速度=", cmd.relativeSpeed, + ", 最小距离=", distance); + } + + broadcastVehicleCommand(cmd); + controllableVehicles_->sendCommand(vehicle.id, cmd); + + CollisionRisk risk; + risk.id1 = vehicle.id; + risk.id2 = target ? target->id : ""; + risk.level = cmdType == CommandType::ALERT ? RiskLevel::CRITICAL : RiskLevel::WARNING; + risk.distance = distance; + risk.relativeSpeed = cmd.relativeSpeed; + risk.relativeMotion = Vector2D(cmd.relativeMotionX, cmd.relativeMotionY); + risk.zoneType = WarningZoneType::WARNING; + + broadcastCollisionWarning(risk); + + return true; } \ No newline at end of file diff --git a/src/core/System.h b/src/core/System.h index 046a9f7..f6f263f 100644 --- a/src/core/System.h +++ b/src/core/System.h @@ -4,10 +4,12 @@ #include #include #include +#include #include #include "types/BasicTypes.h" #include "detector/CollisionDetector.h" #include "detector/TrafficLightDetector.h" +#include "detector/SafetyZone.h" #include "config/AirportBounds.h" #include "vehicle/ControllableVehicles.h" #include "network/WebSocketServer.h" @@ -44,6 +46,31 @@ private: void processLoop(); void processCollisions(const std::vector& collisions); + // 初始化安全区 + void initializeSafetyZones(); + + // 更新安全区状态 + void updateSafetyZoneStates(const std::vector& objects); + + // 检查目标是否进入安全区 + void checkSafetyZoneIntrusion(const MovingObject& obj); + + // 检查无人车与安全区的冲突 + void checkUnmannedVehicleSafetyZones(const std::vector& vehicles, + const std::vector& objects); + + // 处理安全区风险 + bool handleSafetyZoneRisk(const Vehicle& vehicle, + const collision::SafetyZone* zone, + const std::vector& objects, + double distance, + const std::string& intersectionId, + CommandType cmdType, + const std::string& riskLevel); + + // 记录上一次有安全区风险的无人车 + std::unordered_map lastVehiclesWithSafetyZoneRisk_; // + std::atomic running_{false}; std::thread processThread_; @@ -63,6 +90,9 @@ private: // 路口配置 IntersectionConfig intersection_config_; + // 安全区管理 + std::unordered_map> safetyZones_; + static System* instance_; // 记录上一次有风险的车辆列表 diff --git a/src/detector/CollisionDetector.cpp b/src/detector/CollisionDetector.cpp index 882cf6b..86f953f 100644 --- a/src/detector/CollisionDetector.cpp +++ b/src/detector/CollisionDetector.cpp @@ -4,6 +4,7 @@ #include "config/SystemConfig.h" #include #include +#include CollisionDetector::CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles) : airportBounds_(bounds) @@ -35,12 +36,19 @@ void CollisionDetector::updateTraffic(const std::vector& aircraft, if (vehicle.position.x >= bounds.x && vehicle.position.x <= (bounds.x + bounds.width) && vehicle.position.y >= bounds.y && vehicle.position.y <= (bounds.y + bounds.height)) { - // 根据是否可控设置车辆类型 + // 根据配置设置车辆类型 Vehicle updatedVehicle = vehicle; - if (controllableVehicles_->isControllable(vehicle.vehicleNo)) { - updatedVehicle.type = MovingObjectType::UNMANNED; - updatedVehicle.isControllable = true; + const auto* config = controllableVehicles_->findVehicle(vehicle.vehicleNo); + if (config) { + if (config->type == "UNMANNED") { + updatedVehicle.type = MovingObjectType::UNMANNED; + updatedVehicle.isControllable = true; + } else if (config->type == "SPECIAL") { + updatedVehicle.type = MovingObjectType::SPECIAL; + updatedVehicle.isControllable = false; + } } else { + // 未配置的车辆默认为特勤车 updatedVehicle.type = MovingObjectType::SPECIAL; updatedVehicle.isControllable = false; } @@ -89,130 +97,194 @@ std::vector CollisionDetector::detectCollisions() { } } + // 记录当前检测到的冲突对 + std::unordered_set, CollisionPairHash> currentCollisions; + // 检测可控车辆与航空器的碰撞 for (const auto& aircraft : aircraftData_) { for (const auto& vehicle : unmannedVehicles) { + // 获取冲突记录键 + 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; + + // 检查是否存在未解除的冲突记录 + auto it = collisionRecords_.find(collisionKey); + bool hasUnresolvedConflict = (it != collisionRecords_.end() && !it->second.resolved); + + if (hasUnresolvedConflict) { + Logger::debug("存在未解除的冲突记录: obj1=", aircraft.id, + ", obj2=", vehicle.id, + ", maxLevel=", static_cast(it->second.maxLevel), + ", collisionPoint=(", it->second.collisionPoint.x, + ",", it->second.collisionPoint.y, ")"); + } + // 检测碰撞 auto collisionResult = checkCollision( aircraft, vehicle, predictionConfig.time_window); - - if (collisionResult.willCollide) { - // 计算相对运动 - MovementVector av(aircraft.speed, aircraft.heading); - MovementVector vv(vehicle.speed, vehicle.heading); - double vx = av.vx - vv.vx; - double vy = av.vy - vv.vy; - double relativeSpeed = std::sqrt(vx*vx + vy*vy); - - // 根据相对运动方向调整相对速度符号 - if (collisionResult.timeToCollision > 0) { - // 如果预测到未来会碰撞，说明当前在接近，相对速度应为负 - relativeSpeed = -relativeSpeed; + + // 计算相对运动 + MovementVector av(aircraft.speed, aircraft.heading); + MovementVector vv(vehicle.speed, vehicle.heading); + double vx = av.vx - vv.vx; + double vy = av.vy - vv.vy; + double relativeSpeed = std::sqrt(vx*vx + vy*vy); + + // 根据相对运动方向调整相对速度符号 + if (collisionResult.timeToCollision > 0) { + relativeSpeed = -relativeSpeed; + } + + // 评估风险等级 + auto [level, zoneType] = evaluateRisk( + collisionResult, + aircraft.position, + aircraft.type, + vehicle.type + ); + + if (level != RiskLevel::NONE) { + // 记录或更新冲突信息 + auto& record = collisionRecords_[collisionKey]; + if (!record.resolved) { + // 更新已存在的冲突记录 + record.maxLevel = std::max(record.maxLevel, level); + record.collisionPoint = collisionResult.collisionPoint; // 更新冲突点 + Logger::debug("更新冲突记录: obj1=", aircraft.id, + ", obj2=", vehicle.id, + ", maxLevel=", static_cast(record.maxLevel), + ", collisionPoint=(", record.collisionPoint.x, + ",", record.collisionPoint.y, ")"); + } else { + // 创建新的冲突记录 + record = { + collisionResult.collisionPoint, // 使用当前检测到的冲突点 + std::chrono::system_clock::now().time_since_epoch().count(), + level, + false + }; + Logger::debug("创建新的冲突记录: obj1=", aircraft.id, + ", obj2=", vehicle.id, + ", level=", static_cast(level), + ", collisionPoint=(", collisionResult.collisionPoint.x, + ",", collisionResult.collisionPoint.y, ")"); } - // 评估风险等级 - auto [level, zoneType] = evaluateRisk( - collisionResult, - aircraft.position, - aircraft.type, - vehicle.type - ); + // 添加到当前冲突集合 + currentCollisions.insert(collisionKey); - if (level != RiskLevel::NONE) { + // 添加到风险列表 + risks.push_back({ + aircraft.id, + vehicle.id, + level, + collisionResult.minDistance, + collisionResult.minDistance, + relativeSpeed, + {vx, vy}, + zoneType, + collisionResult.timeToCollision, + collisionResult.collisionPoint + }); + + Logger::debug("检测到碰撞风险: obj1=", aircraft.id, + ", obj2=", vehicle.id, + ", minDistance=", collisionResult.minDistance, "m", + ", timeToCollision=", collisionResult.timeToCollision, "s", + ", level=", static_cast(level)); + } else if (hasUnresolvedConflict) { + // 检查是否满足解除条件 + if (checkCollisionResolved(aircraft, vehicle, it->second, safeDistance)) { + it->second.resolved = true; + Logger::debug("冲突已解除: obj1=", aircraft.id, + ", obj2=", vehicle.id); + } else { + // 虽然当前没有检测到风险，但由于未满足解除条件，继续保持原有风险等级 + currentCollisions.insert(collisionKey); risks.push_back({ aircraft.id, vehicle.id, - level, + it->second.maxLevel, collisionResult.minDistance, collisionResult.minDistance, relativeSpeed, {vx, vy}, zoneType, collisionResult.timeToCollision, - collisionResult.collisionPoint + it->second.collisionPoint }); - - Logger::debug("检测到碰撞风险: obj1=", aircraft.id, + Logger::debug("保持原有风险等级: obj1=", aircraft.id, ", obj2=", vehicle.id, + ", maxLevel=", static_cast(it->second.maxLevel), ", minDistance=", collisionResult.minDistance, "m", - ", timeToCollision=", collisionResult.timeToCollision, "s", - ", collisionPoint=(", collisionResult.collisionPoint.x, ",", - collisionResult.collisionPoint.y, ")", - ", level=", static_cast(level), - ", zone=", static_cast(zoneType), - ", relativeSpeed=", relativeSpeed, "m/s", - ", relativeMotion=(", vx, ",", vy, ")"); + ", timeToCollision=", collisionResult.timeToCollision, "s"); } } } } - // 检测可控车辆与所有车辆的碰撞 - for (const auto& vehicle1 : unmannedVehicles) { - for (const auto& vehicle2 : allVehicles) { - // 跳过自身 - if (vehicle1.id == vehicle2.id) { - continue; - } - - // 检测碰撞 - auto collisionResult = checkCollision( - vehicle1, vehicle2, predictionConfig.time_window); - - if (collisionResult.willCollide) { - // 计算相对运动 - MovementVector v1v(vehicle1.speed, vehicle1.heading); - MovementVector v2v(vehicle2.speed, vehicle2.heading); - double vx = v1v.vx - v2v.vx; - double vy = v1v.vy - v2v.vy; - double relativeSpeed = std::sqrt(vx*vx + vy*vy); - - // 根据相对运动方向调整相对速度符号 - if (collisionResult.timeToCollision > 0) { - // 如果预测到未来会碰撞，说明当前在接近，相对速度应为负 - relativeSpeed = -relativeSpeed; - } - - // 评估风险等级 - auto [level, zoneType] = evaluateRisk( - collisionResult, - vehicle1.position, - vehicle1.type, - vehicle2.type - ); - - if (level != RiskLevel::NONE) { - risks.push_back({ - vehicle1.id, - vehicle2.id, - level, - collisionResult.minDistance, - collisionResult.minDistance, - relativeSpeed, - {vx, vy}, - zoneType, - collisionResult.timeToCollision, - collisionResult.collisionPoint - }); - - Logger::debug("检测到碰撞风险: obj1=", vehicle1.id, - ", obj2=", vehicle2.id, - ", minDistance=", collisionResult.minDistance, "m", - ", timeToCollision=", collisionResult.timeToCollision, "s", - ", collisionPoint=(", collisionResult.collisionPoint.x, ",", - collisionResult.collisionPoint.y, ")", - ", level=", static_cast(level), - ", zone=", static_cast(zoneType), - ", relativeSpeed=", relativeSpeed, "m/s", - ", relativeMotion=(", vx, ",", vy, ")"); - } - } + // 清理已解除的冲突记录 + for (auto it = collisionRecords_.begin(); it != collisionRecords_.end();) { + if (it->second.resolved) { + it = collisionRecords_.erase(it); + } else { + ++it; } } return risks; } +bool CollisionDetector::checkCollisionResolved(const MovingObject& obj1, + const MovingObject& obj2, + const CollisionRecord& record, + double safeDistance) const { + // 计算航空器与冲突点的距离 + double dist1 = std::sqrt( + std::pow(obj1.position.x - record.collisionPoint.x, 2) + + std::pow(obj1.position.y - record.collisionPoint.y, 2) + ); + + double dist2 = std::sqrt( + std::pow(obj2.position.x - record.collisionPoint.x, 2) + + std::pow(obj2.position.y - record.collisionPoint.y, 2) + ); + + // 计算冲突点相对于航空器的位置向量与航空器航向的夹角 + double dx = record.collisionPoint.x - obj1.position.x; // 冲突点相对于航空器的向量 + double dy = record.collisionPoint.y - obj1.position.y; + double pointAngle = std::atan2(dy, dx) * 180.0 / M_PI; // 冲突点相对于航空器的角度 + double angleDiff = normalizeAngle(pointAngle - (90.0 - obj1.heading)); // 修正航向角 + + Logger::debug("检查冲突解除条件: 航空器位置=(", obj1.position.x, ",", obj1.position.y, + "), 冲突点=(", record.collisionPoint.x, ",", record.collisionPoint.y, + "), 航向=", obj1.heading, + ", 冲突点角度=", pointAngle, + ", 角度差=", angleDiff, + ", 距离=", dist1); + + // 判断航空器是否已经通过交叉点 + if (angleDiff > 90 && angleDiff < 270) { + // 航空器已经通过交叉点，只要航空器离开冲突点的距离大于安全距离就可以解除 + Logger::debug("航空器已通过交叉点，航空器距离=", dist1, "m, 安全距离=", safeDistance, "m"); + return dist1 >= safeDistance; + } else { + // 航空器还未通过交叉点，需要两者都在安全距离外且预计不会碰撞 + auto collisionResult = checkCollision(obj1, obj2, 30.0); // 使用30秒的预测窗口 + bool willCollide = collisionResult.willCollide; + bool bothOutside = (dist1 >= safeDistance && dist2 >= safeDistance); + + Logger::debug("航空器未通过交叉点，航空器距离=", dist1, "m, 无人车距离=", dist2, + "m, 安全距离=", safeDistance, "m, 预测碰撞=", willCollide ? "是" : "否"); + + return !willCollide && bothOutside; + } +} + // 统一的风险评估函数 std::pair CollisionDetector::evaluateRisk( const collision::CollisionResult& collisionResult, @@ -227,15 +299,15 @@ std::pair CollisionDetector::evaluateRisk( RiskLevel level = RiskLevel::NONE; WarningZoneType zoneType = WarningZoneType::NONE; - // 如果预测到碰撞，只根据距离判断风险等级 + // 如果预测到碰撞，根据当前距离判断风险等级 if (collisionResult.willCollide) { // 当前距离小于警报距离 -> 危险等级 - if (collisionResult.minDistance <= thresholds.critical) { + if (collisionResult.distance <= thresholds.critical) { level = RiskLevel::CRITICAL; zoneType = WarningZoneType::DANGER; } // 当前距离在预警范围内 -> 预警等级 - else if (collisionResult.minDistance <= thresholds.warning) { + else if (collisionResult.distance <= thresholds.warning) { level = RiskLevel::WARNING; zoneType = WarningZoneType::WARNING; } @@ -243,7 +315,8 @@ std::pair CollisionDetector::evaluateRisk( // 记录调试信息 Logger::debug( - "风险评估: 当前最小距离=", collisionResult.minDistance, + "风险评估: 当前距离=", collisionResult.distance, + "m, 预测最小距离=", collisionResult.minDistance, "m, 预警阈值=", thresholds.warning, "m, 警报阈值=", thresholds.critical, "m, 预测碰撞=", collisionResult.willCollide ? "是" : "否", @@ -322,8 +395,9 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( double dy = pos2.y - pos1.y; double current_distance = std::sqrt(dx*dx + dy*dy); - // 初始化最小距离 + // 初始化最小距离和当前距离 result.minDistance = current_distance; + result.distance = current_distance; result.timeToMinDistance = 0.0; // 2. 计算速度分量 @@ -405,7 +479,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( 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 @@ -426,102 +500,101 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( ", pos2=(", pos2.x, ",", pos2.y, ")", ", v1=(", vx1, ",", vy1, ")", ", v2=(", vx2, ",", vy2, ")", - ", rel_v=(", rel_vx, ",", rel_vy, ")", ", current_distance=", current_distance, ", safe_distance=", safe_distance ); - // 计算两条轨迹的最近点时间 - // 参考：https://geomalgorithms.com/a07-_distance.html - double a = vx1 * vx1 + vy1 * vy1; // v1 · v1 - double b = -(vx1 * vx2 + vy1 * vy2); // -v1 · v2 - double c = vx2 * vx2 + vy2 * vy2; // v2 · v2 - double d = vx1 * dx + vy1 * dy; // v1 · w - double e = -(vx2 * dx + vy2 * dy); // -v2 · w - double det = a * c - b * b; // 行列式 + // 计算行列式 (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( - "参数计算: a=", a, - ", b=", b, - ", c=", c, - ", d=", d, - ", e=", e, - ", det=", det + "交叉路径检测: det1=", det1, + ", det2=", det2, + ", det=", det, + ", t1=", t1, + ", t2=", t2 ); - // 对于交叉路径，们可以直接计算交时间 - // 解方程组： - // x1 + vx1 * t = x2 + vx2 * t - // y1 + vy1 * t = y2 + vy2 * t - double t_cross; - if (std::abs(vx1 - vx2) > 0.1) { - t_cross = (pos2.x - pos1.x) / (vx1 - vx2); - } else { - t_cross = (pos2.y - pos1.y) / (vy1 - vy2); - } + // 计算相交点 + Vector2D intersection = { + pos1.x + vx1 * t1, + pos1.y + vy1 * t1 + }; Logger::debug( - "交叉时间计算: t_cross=", t_cross + "交叉路径检测: intersection=(", intersection.x, ",", intersection.y, ")", + ", t1=", t1, + ", t2=", t2 ); - if (t_cross >= 0 && t_cross <= timeWindow) { - // 计算交叉点 - Vector2D cross_point = { - pos1.x + vx1 * t_cross, - pos1.y + vy1 * t_cross + // 检查时间条件 + 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; + + // 根据碰撞时间计算两个物体的位置 + 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 }; - // 由于有碰撞半径，实际碰撞会提前发生 - // 对于叉路径，两车需要各自移动 safe_distance/√2 的距离才会相遇 - double offset_distance = safe_distance / std::sqrt(2.0); - double offset_time = offset_distance / speed1; // 两车速度相同，用任意一个都以 - double collision_time = t_cross - offset_time; - Logger::debug( - "碰撞时间计算: t_cross=", t_cross, + "碰撞时间计算: first_arrival_time=", first_arrival_time, ", safe_distance=", safe_distance, - ", offset_distance=", offset_distance, - ", offset_time=", offset_time, + ", collision_distance=", collision_distance, ", collision_time=", collision_time ); - if (collision_time >= 0) { - result.willCollide = true; - result.timeToCollision = collision_time; - result.minDistance = safe_distance; - result.timeToMinDistance = collision_time; - - // 计算碰撞点（在两个物体的中间） - Vector2D collision1 = { - pos1.x + vx1 * collision_time, - pos1.y + vy1 * collision_time - }; - Vector2D collision2 = { - pos2.x + vx2 * collision_time, - pos2.y + vy2 * collision_time - }; + result.willCollide = true; + result.timeToCollision = collision_time; + result.timeToMinDistance = collision_time; + result.minDistance = safe_distance; + + // 更新物体状态 + result.object1State = collision::CollisionObjectState( + pos1_at_collision, speed1, heading1); + result.object2State = collision::CollisionObjectState( + pos2_at_collision, speed2, heading2); - // 碰撞点在两车连线中点 - result.collisionPoint = { - (collision1.x + collision2.x) / 2.0, - (collision1.y + collision2.y) / 2.0 - }; - - result.object1State = collision::CollisionObjectState( - collision1, speed1, heading1); - result.object2State = collision::CollisionObjectState( - collision2, speed2, heading2); - - Logger::debug( - "交叉路径碰撞: collision_time=", collision_time, - ", collision1=(", collision1.x, ",", collision1.y, ")", - ", collision2=(", collision2.x, ",", collision2.y, ")", - ", collision_point=(", result.collisionPoint.x, - ",", result.collisionPoint.y, ")" - ); - - return result; - } + 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 = collision::CollisionType::PARALLEL; @@ -534,7 +607,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( // 航向角转换为学坐标系中的旋转角度（逆时针为正） 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); @@ -566,7 +639,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( 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); @@ -614,23 +687,15 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( 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); // 统一使用实际距离 - - Logger::debug( - "采样点状态: step=", i, - ", t=", t, - ", distance=", distance, - ", prev_distance=", prev_distance, - ", min_distance=", result.minDistance - ); // 更新小距离 if (distance < result.minDistance) { result.minDistance = distance; result.timeToMinDistance = t; - Logger::debug( - "更新最小距离: min_distance=", distance, - ", time_to_min=", t - ); + // Logger::debug( + // "更新最小距离: min_distance=", distance, + // ", time_to_min=", t + // ); } // 检查是否会碰撞 diff --git a/src/detector/CollisionDetector.h b/src/detector/CollisionDetector.h index fdf2378..4869e93 100644 --- a/src/detector/CollisionDetector.h +++ b/src/detector/CollisionDetector.h @@ -9,6 +9,7 @@ #include "CollisionTypes.h" #include "utils/Logger.h" #include +#include // 碰撞风险等级 enum class RiskLevel { @@ -37,6 +38,21 @@ struct CollisionRisk { Vector2D collisionPoint; // 预计碰撞点 }; +// 冲突记录信息 +struct CollisionRecord { + Vector2D collisionPoint; // 冲突点位置 + int64_t detectedTime; // 检测到冲突的时间 + RiskLevel maxLevel; // 历史最高风险等级 + bool resolved; // 是否已解除 +}; + +// 冲突对象对的哈希函数 +struct CollisionPairHash { + std::size_t operator()(const std::pair& p) const { + return std::hash()(p.first + p.second); + } +}; + class CollisionDetector { public: CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles); @@ -60,6 +76,9 @@ private: std::vector aircraftData_; const ControllableVehicles* controllableVehicles_; + // 冲突记录映射表：<(id1,id2), CollisionRecord> + std::unordered_map, CollisionRecord, CollisionPairHash> collisionRecords_; + // 将角度标准化到[0, 360)范围 static double normalizeAngle(double angle) { while (angle >= 360.0) angle -= 360.0; @@ -88,6 +107,18 @@ private: MovingObjectType type1, MovingObjectType type2) const; + // 检查是否满足冲突解除条件 + bool checkCollisionResolved(const MovingObject& obj1, + const MovingObject& obj2, + const CollisionRecord& record, + double safeDistance) const; + + // 获取冲突记录的键 + std::pair getCollisionKey(const std::string& id1, + const std::string& id2) const { + return id1 < id2 ? std::make_pair(id1, id2) : std::make_pair(id2, id1); + } + // 计算相对运动 struct MovementVector { double vx, vy; diff --git a/src/detector/CollisionTypes.h b/src/detector/CollisionTypes.h index d41a0b3..5a259a5 100644 --- a/src/detector/CollisionTypes.h +++ b/src/detector/CollisionTypes.h @@ -33,6 +33,7 @@ struct CollisionResult { double timeToCollision; // 碰撞时间（从当前时刻开始的秒数） Vector2D collisionPoint; // 碰撞点位置 double minDistance; // 最小距离 + double distance; // 当前距离 double timeToMinDistance; // 到达最小距离的时间（从当前时刻开始的秒数） CollisionType type; // 碰撞类型 CollisionObjectState object1State; // 物体1在碰撞时刻的状态 @@ -44,6 +45,7 @@ struct CollisionResult { timeToCollision(std::numeric_limits::infinity()), collisionPoint({0, 0}), minDistance(std::numeric_limits::infinity()), + distance(std::numeric_limits::infinity()), timeToMinDistance(std::numeric_limits::infinity()), type(CollisionType::STATIC) {} }; diff --git a/src/detector/SafetyZone.cpp b/src/detector/SafetyZone.cpp new file mode 100644 index 0000000..4a1d004 --- /dev/null +++ b/src/detector/SafetyZone.cpp @@ -0,0 +1,143 @@ +#include "SafetyZone.h" +#include "utils/Logger.h" +#include "config/SystemConfig.h" +#include + +namespace collision { + +SafetyZone::SafetyZone(const Vector2D& center, double aircraftRadius, double specialVehicleRadius) + : center_(center) + , aircraftRadius_(aircraftRadius) + , specialVehicleRadius_(specialVehicleRadius) + , currentRadius_(0.0) + , state_(SafetyZoneState::INACTIVE) + , type_(SafetyZoneType::NONE) { + + Logger::debug("创建安全区: center=(", center_.x, ",", center_.y, + "), aircraftRadius=", aircraftRadius_, + ", specialVehicleRadius=", specialVehicleRadius_); +} + +bool SafetyZone::isInZone(const Vector2D& point) const { + // 如果未设置类型，直接返回 false + if (type_ == SafetyZoneType::NONE) { + return false; + } + + // 计算点到中心的距离 + double dx = point.x - center_.x; + double dy = point.y - center_.y; + double distance = std::sqrt(dx * dx + dy * dy); + + // 使用当前设置的安全区半径 + return distance <= currentRadius_; +} + +void SafetyZone::resetType() { + type_ = SafetyZoneType::NONE; + currentRadius_ = 0.0; + Logger::debug("重置安全区类型为 NONE"); +} + +bool SafetyZone::trySetType(const MovingObject& object) { + // 判断目标类型 + bool isAircraft = object.isAircraft(); + bool isSpecialVehicle = object.isSpecialVehicle(); + + // 如果既不是飞机也不是特勤车，返回 false + if (!isAircraft && !isSpecialVehicle) { + return false; + } + + // 如果安全区已有类型，检查是否可以改变 + if (type_ != SafetyZoneType::NONE) { + // 如果当前类型与目标类型不匹配，不允许改变 + if ((type_ == SafetyZoneType::AIRCRAFT && !isAircraft) || + (type_ == SafetyZoneType::VEHICLE && !isSpecialVehicle)) { + return false; + } + // 如果类型匹配，保持当前类型 + return true; + } + + // 设置新的类型和尺寸 + if (isAircraft) { + type_ = SafetyZoneType::AIRCRAFT; + currentRadius_ = aircraftRadius_; + state_ = SafetyZoneState::ACTIVE; // 激活安全区 + Logger::debug("安全区设置为飞机类型，半径: ", currentRadius_); + } else { + type_ = SafetyZoneType::VEHICLE; + currentRadius_ = specialVehicleRadius_; + state_ = SafetyZoneState::ACTIVE; // 激活安全区 + Logger::debug("安全区设置为特勤车类型，半径: ", currentRadius_); + } + + return true; +} + +bool SafetyZone::isObjectInZone(const MovingObject& object) const { + // 如果是无人车，直接返回 false + if (object.isUnmannedVehicle()) { + Logger::debug("目标 ", object.id, " 是无人车，不触发安全区"); + return false; + } + + // 从 SystemConfig 获取物体尺寸（半长度） + const auto& config = SystemConfig::instance().collision_detection.prediction; + double objectRadius = 0.0; + + // 获取物体半径 + if (object.isAircraft()) { + objectRadius = config.aircraft_size / 2.0; + Logger::debug("目标 ", object.id, " 是飞机，尺寸半径: ", objectRadius); + } else if (object.isSpecialVehicle()) { + objectRadius = config.vehicle_size / 2.0; + Logger::debug("目标 ", object.id, " 是特勤车，尺寸半径: ", objectRadius); + } else { + return false; + } + + // 如果安全区类型未设置，尝试设置类型 + if (type_ == SafetyZoneType::NONE) { + // 临时设置类型和半径来检查是否在区内 + SafetyZoneType tempType = object.isAircraft() ? SafetyZoneType::AIRCRAFT : SafetyZoneType::VEHICLE; + double tempRadius = object.isAircraft() ? aircraftRadius_ : specialVehicleRadius_; + + // 暂存当前值 + auto currentType = type_; + auto currentRadius = currentRadius_; + + // 临时设置类型和半径 + const_cast(this)->type_ = tempType; + const_cast(this)->currentRadius_ = tempRadius; + + // 检查是否在区内 + bool inZone = isInZone(object.position); + + // 如果在区内，保持新的类型和半径，否则恢复原值 + if (!inZone) { + const_cast(this)->type_ = currentType; + const_cast(this)->currentRadius_ = currentRadius; + } else { + // 设置状态为激活 + const_cast(this)->state_ = SafetyZoneState::ACTIVE; + Logger::debug("安全区设置为", object.isAircraft() ? "飞机" : "特勤车", "类型，半径: ", tempRadius); + } + + return inZone; + } + + // 如果类型已设置，检查是否匹配 + if ((type_ == SafetyZoneType::AIRCRAFT && !object.isAircraft()) || + (type_ == SafetyZoneType::VEHICLE && !object.isSpecialVehicle())) { + Logger::debug("目标 ", object.id, " 类型与安全区类型不匹配，当前安全区类型: ", + type_ == SafetyZoneType::AIRCRAFT ? "飞机" : "特勤车"); + return false; + } + + // 使用 isInZone 检查是否在区内 + return isInZone(object.position); +} + +} // namespace collision \ No newline at end of file diff --git a/src/detector/SafetyZone.h b/src/detector/SafetyZone.h new file mode 100644 index 0000000..0dd0ee0 --- /dev/null +++ b/src/detector/SafetyZone.h @@ -0,0 +1,64 @@ +#pragma once + +#include "types/BasicTypes.h" + +namespace collision { + +// 安全区状态 +enum class SafetyZoneState { + INACTIVE, // 未激活 + ACTIVE // 已激活 +}; + +// 安全区类型 +enum class SafetyZoneType { + NONE, // 未设置 + AIRCRAFT, // 飞机安全区 + VEHICLE // 特勤车安全区 +}; + +// 安全区定义 +class SafetyZone { +public: + // 构造函数 + SafetyZone(const Vector2D& center, double aircraftSize, double specialVehicleSize); + + // 获取状态 + SafetyZoneState getState() const { return state_; } + SafetyZoneType getType() const { return type_; } + + // 设置状态 + void setState(SafetyZoneState state) { state_ = state; } + + // 重置类型为 NONE + void resetType(); + + // 检查点是否在安全区内 + bool isInZone(const Vector2D& point) const; + + // 检查目标是否在安全区内 + bool isObjectInZone(const MovingObject& object) const; + + // 获取中心点 + const Vector2D& getCenter() const { return center_; } + + // 获取安全区尺寸配置 + double getAircraftRadius() const { return aircraftRadius_; } + double getSpecialVehicleRadius() const { return specialVehicleRadius_; } + + // 获取当前安全区半径 + double getCurrentRadius() const { return currentRadius_; } + +private: + Vector2D center_; // 安全区中心点 + double aircraftRadius_; // 飞机安全区半径 + double specialVehicleRadius_; // 特勤车安全区半径 + double currentRadius_; // 当前安全区半径 + SafetyZoneState state_; // 当前状态 + SafetyZoneType type_; // 当前类型 + + // 尝试设置安全区类型和尺寸 + bool trySetType(const MovingObject& object); +}; + +} // namespace collision \ No newline at end of file diff --git a/src/network/HTTPDataSource.h b/src/network/HTTPDataSource.h index 80c3d8e..db4f2a2 100644 --- a/src/network/HTTPDataSource.h +++ b/src/network/HTTPDataSource.h @@ -51,5 +51,5 @@ private: static size_t WriteCallback(void* contents, size_t size, size_t nmemb, void* userp); }; - #endif // AIRPORT_NETWORK_HTTP_DATA_SOURCE_H + diff --git a/src/types/BasicTypes.h b/src/types/BasicTypes.h index 37a94b3..6251125 100644 --- a/src/types/BasicTypes.h +++ b/src/types/BasicTypes.h @@ -39,11 +39,11 @@ public: virtual ~MovingObject() = default; std::string id; - GeoPosition geo; Vector2D position; + GeoPosition geo; double heading; double speed; - uint64_t timestamp; + int64_t timestamp; MovingObjectType type; // 添加类型字段 std::deque positionHistory; @@ -59,6 +59,11 @@ public: void copyHistoryFrom(const MovingObject& other); bool hasPositionChanged() const; + // 添加类型识别虚函数 + virtual bool isAircraft() const { return type == MovingObjectType::AIRCRAFT; } + virtual bool isSpecialVehicle() const { return type == MovingObjectType::SPECIAL; } + virtual bool isUnmannedVehicle() const { return type == MovingObjectType::UNMANNED; } + static constexpr size_t MAX_HISTORY = 10; }; @@ -76,12 +81,18 @@ public: static constexpr double MAX_SPEED = 100.0; // 最大速度（米/秒） static constexpr double MAX_POSITION_JUMP = 50.0; // 最大位置跳变（米） + + bool isAircraft() const override { return true; } + bool isSpecialVehicle() const override { return false; } }; // 车辆类 class Vehicle : public MovingObject { public: - Vehicle() { type = MovingObjectType::SPECIAL; } // 默认为特勤车 + Vehicle() { + type = MovingObjectType::UNMANNED; // 默认为无人车类型 + isControllable = true; // 默认为可控 + } std::string vehicleNo; bool isControllable; // true 表示无人车，false 表示特勤车 @@ -91,4 +102,9 @@ public: static constexpr double MAX_SPEED = 20.0; // 最大速度（米/秒） static constexpr double MAX_POSITION_JUMP = 10.0; // 最大位置跳变（米） + + bool isAircraft() const override { return false; } + bool isSpecialVehicle() const override { + return type == MovingObjectType::SPECIAL; // 只判断类型 + } }; \ No newline at end of file diff --git a/src/vehicle/ControllableVehicles.cpp b/src/vehicle/ControllableVehicles.cpp index 9ad8b03..bece014 100644 --- a/src/vehicle/ControllableVehicles.cpp +++ b/src/vehicle/ControllableVehicles.cpp @@ -25,8 +25,13 @@ const ControllableVehicleConfig* ControllableVehicles::findVehicle(const std::st } bool ControllableVehicles::isControllable(const std::string& vehicleNo) const { - // 只检查是否在配置文件中 - return findVehicle(vehicleNo) != nullptr; + // 查找车辆配置 + const auto* config = findVehicle(vehicleNo); + if (!config) { + return false; + } + // 只有无人车类型是可控的 + return config->type == "UNMANNED"; } void ControllableVehicles::loadConfig(const std::string& configFile) { @@ -42,10 +47,11 @@ void ControllableVehicles::loadConfig(const std::string& configFile) { for (const auto& item : jsonConfig["vehicles"]) { ControllableVehicleConfig config; config.vehicleNo = item["vehicleNo"].get(); + config.type = item["type"].get(); config.ip = item["ip"].get(); config.port = item["port"].get(); vehicles_.push_back(config); - Logger::info("Added controllable vehicle: ", config.vehicleNo); + Logger::info("Added vehicle: ", config.vehicleNo, ", type: ", config.type); } Logger::info("Loaded ", vehicles_.size(), " controllable vehicles"); diff --git a/src/vehicle/ControllableVehicles.h b/src/vehicle/ControllableVehicles.h index e872eab..cbed14c 100644 --- a/src/vehicle/ControllableVehicles.h +++ b/src/vehicle/ControllableVehicles.h @@ -8,6 +8,7 @@ struct ControllableVehicleConfig { std::string vehicleNo; // 车牌号 + std::string type; // 车辆类型（UNMANNED 或 SPECIAL） std::string ip; // IP地址 int port; // 端口号 }; diff --git a/tests/BasicCollisionTest.cpp b/tests/BasicCollisionTest.cpp index 8674f66..7f6f51e 100644 --- a/tests/BasicCollisionTest.cpp +++ b/tests/BasicCollisionTest.cpp @@ -27,6 +27,10 @@ public: config.warning_zone_radius = {100.0, 100.0, 50.0}; config.alert_zone_radius = {50.0, 50.0, 25.0}; areaConfigs_[AreaType::TEST_ZONE] = config; + + // 设置系统配置 + auto& system_config = SystemConfig::instance(); + system_config.collision_detection.prediction.time_window = 30.0; // 设置预测时间窗口为30秒 } MOCK_METHOD(AreaType, getAreaType, (const Vector2D& position), (const)); @@ -127,7 +131,7 @@ TEST_F(BasicCollisionTest, HeadOnCollision) { EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点应该在同一水平线上"; // 增加更多验证 - EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离��该是碰撞半径之和50米"; + EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离该是碰撞半径之和50米"; EXPECT_NEAR(result.timeToMinDistance, 2.5, 0.1) << "最小距离时间应该等于碰撞时间"; // 验证物体状态 @@ -174,11 +178,11 @@ TEST_F(BasicCollisionTest, ParallelMotion) { // 4. 交叉路径测试 TEST_F(BasicCollisionTest, CrossingPaths) { - // 创建两个垂直交叉运动的物体 + // 创建两个倾斜交叉运动的物体 Vehicle v1; v1.vehicleNo = "V1"; v1.position = {100.0, 100.0}; - v1.speed = 10.0; + v1.speed = 15.0; v1.heading = 75.0; // 向东北偏东方向运动 v1.type = MovingObjectType::UNMANNED; @@ -189,36 +193,43 @@ TEST_F(BasicCollisionTest, CrossingPaths) { v2.heading = 105.0; // 向东南偏东方向运动，与 v1 夹角为 30 度 v2.type = MovingObjectType::UNMANNED; + // 添加航向角度差的调试日志 + double angle_diff = std::abs(v1.heading - v2.heading); + Logger::debug("航向角度差检查:"); + Logger::debug("v1 航向: " + std::to_string(v1.heading)); + Logger::debug("v2 航向: " + std::to_string(v2.heading)); + Logger::debug("航向角度差: " + std::to_string(angle_diff)); + auto result = detector_->checkCollision(v1, v2, 30.0); EXPECT_TRUE(result.willCollide) << "交叉路径的物体应该检测为碰撞"; EXPECT_EQ(result.type, collision::CollisionType::CROSSING) << "应该识别为交叉碰撞"; // 计算碰撞时间和位置： - // v1: 速度分量 (2.59, 9.66) m/s - // v2: 速度分量 (-2.59, 9.66) m/s - // 相对速度: (-5.18, 0) m/s - // 相对速度大小: 5.18 m/s + // v1: 速度分量 (3.882, 14.489) m/s // 15 m/s * (cos(15°), sin(15°)) + // v2: 速度分量 (-2.588, 9.659) m/s // 10 m/s * (cos(75°), sin(75°)) + // 相对速度: (-6.47, -4.83) m/s + // 相对速度大小: 8.06 m/s // 碰撞点在两车轨迹交点前的安全距离处 - double collision_time = 6.12; // 根据实际计算得到 - Vector2D collision_point = {125.0, 184.15}; // 根据实际计算得到 + double collision_time = 2.6; // 根据实际计算得到 + Vector2D collision_point = {126.68, 156.39}; // 两车位置的中点 - EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25米，撞时间应该接近6.12秒"; - EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在125.0"; - EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在184.15"; + EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25米，碰撞时间应该接近2.6秒"; + EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在126.68"; + EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在156.39"; // 增加更多验证 EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是碰撞半径之和50米"; EXPECT_NEAR(result.timeToMinDistance, collision_time, 0.1) << "最小距离时间应该等于碰撞时间"; // 验证物体状态 - EXPECT_NEAR(result.object1State.position.x, 115.85, 0.1); - EXPECT_NEAR(result.object1State.position.y, 159.15, 0.1); - EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0); + EXPECT_NEAR(result.object1State.position.x, 110.09, 0.1); + EXPECT_NEAR(result.object1State.position.y, 137.67, 0.1); + EXPECT_DOUBLE_EQ(result.object1State.speed, 15.0); // 更新为正确的速度 EXPECT_DOUBLE_EQ(result.object1State.heading, 75.0); - EXPECT_NEAR(result.object2State.position.x, 134.15, 0.1); - EXPECT_NEAR(result.object2State.position.y, 209.15, 0.1); + EXPECT_NEAR(result.object2State.position.x, 143.27, 0.1); + EXPECT_NEAR(result.object2State.position.y, 175.11, 0.1); EXPECT_DOUBLE_EQ(result.object2State.speed, 10.0); EXPECT_DOUBLE_EQ(result.object2State.heading, 105.0); } @@ -278,7 +289,7 @@ TEST_F(BasicCollisionTest, DivergentMotion) { // 设置两个物体背向运动的场景 // 物体1在(150,100)，向右运动，航向90度 // 物体2在(200,100)，向右运动，航向90度 - // 两个物体都在远离对方，不应该发生碰撞 + // 两物体都在远离对方，不应该发生碰撞 Vehicle obj1; obj1.vehicleNo = "V1"; @@ -313,7 +324,7 @@ TEST_F(BasicCollisionTest, TailgatingMotion) { Vehicle v1; // 前车 v1.vehicleNo = "V1"; v1.position = {60.0, 100.0}; // 前车在前方60米处 - v1.speed = 10.0; // 前车速度10m/s + v1.speed = 10.0; // 前车���度10m/s v1.heading = 90.0; // 向东运动 v1.type = MovingObjectType::UNMANNED; @@ -332,7 +343,7 @@ TEST_F(BasicCollisionTest, TailgatingMotion) { // 验证会发生碰撞 EXPECT_TRUE(result.willCollide) << "后车速度大于前车，应该预测到碰撞"; - // 验证碰撞时间（初始距离60米，相对速��5m/s，安全距离50米，需要缩短10米，所以碰撞时间应该是2秒） + // 验证碰撞时间（初始距离60米，相对速度5m/s，安全距离50米，需要缩短10米，所以碰撞时间应该是2秒） EXPECT_NEAR(result.timeToCollision, 2.0, 0.1) << "碰撞时间应该接近2秒"; // 验证最小距离（应该是安全距离） @@ -372,7 +383,7 @@ TEST_F(BasicCollisionTest, QN002AircraftCrossing) { aircraft.heading = 45.0; // 根据 T7 到 T11 的方向计算 aircraft.type = MovingObjectType::AIRCRAFT; - // 创建 QN002 无人车，从 T4 到 T8 路径上的一点出发 + // 创建 QN002 无人车，从 T4 到 T8 路径上的一点发 Vehicle qn002; qn002.vehicleNo = "QN002"; qn002.position = {-2.0, 120.0}; // 在 T4-T8 路径上，距离 T7 约 120 米（小于检测范围 150 米） @@ -398,11 +409,116 @@ TEST_F(BasicCollisionTest, QN002AircraftCrossing) { // 验证碰撞参数 EXPECT_GT(result.timeToCollision, 0.0) << "碰撞时间应该大于0"; EXPECT_LT(result.timeToCollision, 30.0) << "碰撞时间应该在预测窗口内"; - EXPECT_LE(result.minDistance, 75.0) << "最小距离应该小于预警距离"; + EXPECT_LE(result.minDistance, 75.0) << "小距离应该小于预警距离"; // 验证物体状态 EXPECT_DOUBLE_EQ(result.object1State.speed, 50.0 / 3.6); EXPECT_DOUBLE_EQ(result.object1State.heading, 45.0); EXPECT_DOUBLE_EQ(result.object2State.speed, 36.0 / 3.6); EXPECT_DOUBLE_EQ(result.object2State.heading, 135.0); +} + +TEST_F(BasicCollisionTest, CrossingBeforeAndAfter) { + // 设置 Mock 对象的预期行为 + EXPECT_CALL(*mockControllableVehicles_, isControllable(::testing::StrEq("QN002"))) + .WillRepeatedly(::testing::Return(true)); + + // 创建一个航空器和一个无人车 + Aircraft aircraft; + aircraft.id = "AC001"; + aircraft.type = MovingObjectType::AIRCRAFT; + aircraft.speed = 10.0; // 10米/秒 + aircraft.heading = 90.0; // 向东 + + Vehicle vehicle; + vehicle.id = "QN002"; + vehicle.vehicleNo = "QN002"; + vehicle.type = MovingObjectType::UNMANNED; + vehicle.isControllable = true; + vehicle.speed = 10.0; // 10米/秒 + vehicle.heading = 0.0; // 向北 + + // 设置初始位置：航空器在交叉点西侧，无人车在南侧 + Vector2D crossPoint{300.0, 300.0}; // 交叉点坐标 + + // 1. 测试交叉前的情况 + // 航空器在交叉点西侧，无人车在南侧 + aircraft.position = {crossPoint.x - 80.0, crossPoint.y}; // 航空器在交叉点西侧80 + vehicle.position = {crossPoint.x, crossPoint.y - 80.0}; // 无人车在交叉点南侧80米 + + // 更新交通数据 + std::vector aircrafts = {aircraft}; + std::vector vehicles = {vehicle}; + detector_->updateTraffic(aircrafts, vehicles); + + // 检测碰撞（使用30秒的预测窗口） + auto risks = detector_->detectCollisions(); + + // 记录详细的测试信息 + Logger::debug("交叉前碰撞检测:"); + Logger::debug("航空器位置: (", aircraft.position.x, ",", aircraft.position.y, ")"); + Logger::debug("无人车位置: (", vehicle.position.x, ",", vehicle.position.y, ")"); + Logger::debug("交叉点位置: (", crossPoint.x, ",", crossPoint.y, ")"); + + ASSERT_FALSE(risks.empty()) << "交叉前应该检测到碰撞风险"; + EXPECT_EQ(risks[0].level, RiskLevel::WARNING) << "交叉前应该是预警级别"; + + // 2. 测试交叉过程中 + // 航空器在交叉点西侧20米，无人车在南侧20米 + aircraft.position = {crossPoint.x - 20.0, crossPoint.y}; + vehicle.position = {crossPoint.x, crossPoint.y - 20.0}; + + // 更新交通数据 + aircrafts = {aircraft}; + vehicles = {vehicle}; + detector_->updateTraffic(aircrafts, vehicles); + + // 检测碰撞 + risks = detector_->detectCollisions(); + + // 记录详细的测试信息 + Logger::debug("交叉过程中碰撞检测:"); + Logger::debug("航空器位置: (", aircraft.position.x, ",", aircraft.position.y, ")"); + Logger::debug("无人车位置: (", vehicle.position.x, ",", vehicle.position.y, ")"); + + ASSERT_FALSE(risks.empty()) << "交叉过程中应该检测到碰撞风险"; + EXPECT_EQ(risks[0].level, RiskLevel::CRITICAL) << "交叉过程中应该是告警级别"; + + // 3. 测试交叉后的情况 + // 航空器已经通过交叉点并远离安全距离，无人车还在交叉点附近 + aircraft.position = {crossPoint.x + 120.0, crossPoint.y}; // 航空器在交叉点东侧120米 + vehicle.position = {crossPoint.x, crossPoint.y - 10.0}; // 无人车还在交叉点附近 + + // 更新交通数据 + aircrafts = {aircraft}; + vehicles = {vehicle}; + detector_->updateTraffic(aircrafts, vehicles); + + // 检测碰撞 + risks = detector_->detectCollisions(); + + // 记录详细的测试信息 + Logger::debug("交叉后碰撞检测:"); + Logger::debug("航空器位置: (", aircraft.position.x, ",", aircraft.position.y, ")"); + Logger::debug("无人车位置: (", vehicle.position.x, ",", vehicle.position.y, ")"); + + EXPECT_TRUE(risks.empty()) << "航空器通过远离安全距离后，应该解除冲突"; + + // 4. 测试无人车继续运动 + // 无人车向北移动到交叉点 + vehicle.position = {crossPoint.x, crossPoint.y}; + + // 更新交通数据 + vehicles = {vehicle}; + detector_->updateTraffic(aircrafts, vehicles); + + // 检测碰撞 + risks = detector_->detectCollisions(); + + // 记录详细的测试信息 + Logger::debug("无人车继续运动碰撞检测:"); + Logger::debug("航空器位置: (", aircraft.position.x, ",", aircraft.position.y, ")"); + Logger::debug("无人车位置: (", vehicle.position.x, ",", vehicle.position.y, ")"); + + EXPECT_TRUE(risks.empty()) << "航空器已远离，无人车继续运动不应产生新的冲突"; } \ No newline at end of file diff --git a/tests/CollisionDetectorTest.cpp b/tests/CollisionDetectorTest.cpp index 880073b..ff0ffb8 100644 --- a/tests/CollisionDetectorTest.cpp +++ b/tests/CollisionDetectorTest.cpp @@ -49,6 +49,9 @@ public: class CollisionDetectorTest : public ::testing::Test { protected: void SetUp() override { + // 设置日志级别为 DEBUG + Logger::initialize("", LogLevel::DEBUG); + // 打印当前工作目录 std::cout << "Current working directory: " << std::filesystem::current_path() << std::endl; @@ -299,7 +302,7 @@ TEST_F(CollisionDetectorTest, AircraftStationaryVehicleCollision) { auto collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0); EXPECT_TRUE(collisionResult.willCollide) << "航空器接近静止车辆时应该检测到碰撞"; - // 测试2���静止车辆在航空器航向偏离处 + // 测试2：静止车辆在航空器航向偏离处 vehicle.position = {200.0, 200.0}; // 在航空器前方偏北，距离约100米（大于安全距离75米） collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0); EXPECT_FALSE(collisionResult.willCollide) << "航空器与不在航向上的静止车辆不应该检测到碰撞"; diff --git a/tests/SafetyZoneTest.cpp b/tests/SafetyZoneTest.cpp new file mode 100644 index 0000000..d8c8b46 --- /dev/null +++ b/tests/SafetyZoneTest.cpp @@ -0,0 +1,178 @@ +#include +#include "detector/SafetyZone.h" +#include "utils/Logger.h" +#include "config/SystemConfig.h" + +using namespace collision; + +class SafetyZoneTest : public ::testing::Test { +protected: + void SetUp() override { + // 确保 SystemConfig 已初始化 + SystemConfig::instance(); + // 创建一个安全区，中心点在(0,0)，飞机安全区半径50米，特勤车安全区半径30米 + safetyZone = std::make_unique(Vector2D{0, 0}, 50.0, 30.0); + } + + std::unique_ptr safetyZone; + + // 创建测试用的移动对象 + Aircraft createAircraft(const std::string& id, double x, double y) { + Aircraft obj; + obj.id = id; + obj.position = Vector2D{x, y}; + obj.geo.latitude = 0; // 这里的经纬度不重要 + obj.geo.longitude = 0; + obj.type = MovingObjectType::AIRCRAFT; // 设置类型为航空器 + return obj; + } + + Vehicle createSpecialVehicle(const std::string& id, double x, double y) { + Vehicle obj; + obj.id = id; + obj.position = Vector2D{x, y}; + obj.geo.latitude = 0; + obj.geo.longitude = 0; + obj.type = MovingObjectType::SPECIAL; // 设置类型为特勤车 + obj.isControllable = false; // 设置为非可控车辆（即特勤车） + return obj; + } + + Vehicle createUnmannedVehicle(const std::string& id, double x, double y) { + Vehicle obj; + obj.id = id; + obj.position = Vector2D{x, y}; + obj.geo.latitude = 0; + obj.geo.longitude = 0; + obj.type = MovingObjectType::UNMANNED; // 设置类型为无人车 + obj.isControllable = true; // 设置为可控车辆（即无人车） + return obj; + } + + // 获取飞机有效距离（安全区半径 + 飞机半长度） + double getAircraftEffectiveDistance() const { + const auto& config = SystemConfig::instance().collision_detection.prediction; + return safetyZone->getAircraftRadius() + config.aircraft_size / 2.0; + } + + // 获取特勤车有效距离（安全区半径 + 车辆半长度） + double getVehicleEffectiveDistance() const { + const auto& config = SystemConfig::instance().collision_detection.prediction; + return safetyZone->getSpecialVehicleRadius() + config.vehicle_size / 2.0; + } +}; + +// 测试初始状态 +TEST_F(SafetyZoneTest, InitialState) { + EXPECT_EQ(safetyZone->getState(), SafetyZoneState::INACTIVE); + EXPECT_EQ(safetyZone->getType(), SafetyZoneType::NONE); + EXPECT_EQ(safetyZone->getCurrentRadius(), 0.0); +} + +// 测试飞机进入安全区 +TEST_F(SafetyZoneTest, AircraftEntering) { + double effectiveDistance = getAircraftEffectiveDistance(); + + // 创建一个飞机，位置在安全区边缘内侧 + auto aircraft = createAircraft("AC001", effectiveDistance - 1.0, 0.0); + + // 检查飞机是否在安全区内 + EXPECT_TRUE(safetyZone->isObjectInZone(aircraft)); + EXPECT_EQ(safetyZone->getType(), SafetyZoneType::AIRCRAFT); + EXPECT_EQ(safetyZone->getCurrentRadius(), 50.0); + + // 测试飞机完全离开安全区 + aircraft = createAircraft("AC001", effectiveDistance + 1.0, 0.0); + EXPECT_FALSE(safetyZone->isObjectInZone(aircraft)); +} + +// 测试特勤车进入安全区 +TEST_F(SafetyZoneTest, SpecialVehicleEntering) { + double effectiveDistance = getVehicleEffectiveDistance(); + + // 创建一个特勤车，位置在安全区边缘内侧 + auto vehicle = createSpecialVehicle("TQ001", effectiveDistance - 1.0, 0.0); + + // 检查特勤车是否在安全区内 + EXPECT_TRUE(safetyZone->isObjectInZone(vehicle)); + EXPECT_EQ(safetyZone->getType(), SafetyZoneType::VEHICLE); + EXPECT_EQ(safetyZone->getCurrentRadius(), 30.0); + + // 测试特勤车完全离开安全区 + vehicle = createSpecialVehicle("TQ001", effectiveDistance + 1.0, 0.0); + EXPECT_FALSE(safetyZone->isObjectInZone(vehicle)); +} + +// 测试无人车进入安全区 +TEST_F(SafetyZoneTest, UnmannedVehicleEntering) { + // 创建一个无人车，位置在安全区内 + auto vehicle = createUnmannedVehicle("UV001", 10.0, 0.0); + + // 检查无人车是否在安全区内（应该返回false，因为无人车不能设置安全区类型） + EXPECT_FALSE(safetyZone->isObjectInZone(vehicle)); + EXPECT_EQ(safetyZone->getType(), SafetyZoneType::NONE); + EXPECT_EQ(safetyZone->getCurrentRadius(), 0.0); +} + +// 测试安全区类型锁定 +TEST_F(SafetyZoneTest, SafetyZoneTypeLocking) { + double aircraftEffectiveDistance = getAircraftEffectiveDistance(); + double vehicleEffectiveDistance = getVehicleEffectiveDistance(); + + // 先让飞机进入安全区 + auto aircraft = createAircraft("AC001", aircraftEffectiveDistance - 5.0, 0.0); + EXPECT_TRUE(safetyZone->isObjectInZone(aircraft)); + EXPECT_EQ(safetyZone->getType(), SafetyZoneType::AIRCRAFT); + + // 尝试让特勤车进入已经被飞机设置类型的安全区 + auto vehicle = createSpecialVehicle("TQ001", vehicleEffectiveDistance - 5.0, 0.0); + EXPECT_FALSE(safetyZone->isObjectInZone(vehicle)); + + // 确认安全区类型和尺寸没有改变 + EXPECT_EQ(safetyZone->getType(), SafetyZoneType::AIRCRAFT); + EXPECT_EQ(safetyZone->getCurrentRadius(), 50.0); +} + +// 测试边界条件 +TEST_F(SafetyZoneTest, BoundaryConditions) { + double aircraftEffectiveDistance = getAircraftEffectiveDistance(); + double vehicleEffectiveDistance = getVehicleEffectiveDistance(); + + // 测试飞机在边界上 + auto aircraft1 = createAircraft("AC001", aircraftEffectiveDistance, 0.0); + EXPECT_TRUE(safetyZone->isObjectInZone(aircraft1)); + + // 测试飞机在边界外 + auto aircraft2 = createAircraft("AC002", aircraftEffectiveDistance + 0.1, 0.0); + EXPECT_FALSE(safetyZone->isObjectInZone(aircraft2)); + + // 测试特勤车在边界上 + safetyZone = std::make_unique(Vector2D{0, 0}, 50.0, 30.0); + auto vehicle1 = createSpecialVehicle("TQ001", vehicleEffectiveDistance, 0.0); + EXPECT_TRUE(safetyZone->isObjectInZone(vehicle1)); + + // 测试特勤车在边界外 + auto vehicle2 = createSpecialVehicle("TQ002", vehicleEffectiveDistance + 0.1, 0.0); + EXPECT_FALSE(safetyZone->isObjectInZone(vehicle2)); +} + +// 测试状态重置 +TEST_F(SafetyZoneTest, StateReset) { + double effectiveDistance = getAircraftEffectiveDistance(); + + // 先让飞机进入安全区 + auto aircraft = createAircraft("AC001", effectiveDistance - 5.0, 0.0); + EXPECT_TRUE(safetyZone->isObjectInZone(aircraft)); + + // 设置状态为激活 + safetyZone->setState(SafetyZoneState::ACTIVE); + EXPECT_EQ(safetyZone->getState(), SafetyZoneState::ACTIVE); + + // 重置状态 + safetyZone->setState(SafetyZoneState::INACTIVE); + EXPECT_EQ(safetyZone->getState(), SafetyZoneState::INACTIVE); + + // 确认类型和尺寸保持不变 + EXPECT_EQ(safetyZone->getType(), SafetyZoneType::AIRCRAFT); + EXPECT_EQ(safetyZone->getCurrentRadius(), 50.0); +} \ No newline at end of file diff --git a/tools/map_websocket.html b/tools/map_websocket.html index 8e9c4a8..f9408f7 100644 --- a/tools/map_websocket.html +++ b/tools/map_websocket.html @@ -46,9 +46,21 @@ .aircraft-icon { width: 50px; height: 50px; - background-color: purple; + background-color: rgba(128, 0, 128, 0.5); clip-path: polygon(0% 0%, 100% 0%, 100% 100%, 0% 100%); border: 2px solid white; + position: relative; + } + .aircraft-icon::after { + content: ''; + position: absolute; + width: 6px; + height: 6px; + background-color: black; + border-radius: 50%; + top: 50%; + left: 50%; + transform: translate(-50%, -50%); } .special-vehicle-icon { width: 20px; diff --git a/tools/mock_server.py b/tools/mock_server.py index d11041f..97b6a9a 100644 --- a/tools/mock_server.py +++ b/tools/mock_server.py @@ -36,7 +36,7 @@ DIST_50M = 50 # 时间配置（秒） WAIT_TIME_AFTER_RETURN = 10.0 # 返回起点后的等待时间 -UPDATE_INTERVAL = 1.0 # 位置更新间隔 +UPDATE_INTERVAL = 1.0 # 位置更新间隔, 默认 1 秒 TRAFFIC_LIGHT_SWITCH_INTERVAL = 10.0 # 红绿灯切换间隔 # 两个路口的位置 @@ -92,12 +92,12 @@ aircraft_data = [ "lat": initial_target_lat, "lon": initial_target_lon }, - "speed": 50.0 # 滑行速度 50km/h + "speed": 36.0 # 滑行速度 ，默认 50km/h } ] # 添加默认速度常量 -DEFAULT_VEHICLE_SPEED = 36.0 # km/h +DEFAULT_VEHICLE_SPEED = 18.0 # km/h，默认 36km/h EMERGENCY_BRAKE_DECELERATION = 0.8 # 紧急制动减速度 (每次更新减速 80%) NORMAL_BRAKE_DECELERATION = 0.2 # 正常制动减速度 (每次更新减速 20%) @@ -470,27 +470,14 @@ def get_front_traffic_light(vehicle, distance_to_west, distance_to_east): # QN001 的路线：西路口北侧 -> 东 -> 北 if vehicle["phase"] == 0: # 南北移动 - # 在西路口以南时，向北移动需要判断西路口红绿灯 - if vehicle["direction"] == 1 and vehicle["latitude"] < T2_INTERSECTION["latitude"]: - return traffic_light_data[0], distance_to_west # 西路口红绿灯 # 在西路口以北时，向南移动需要判断西路口红绿灯 - elif vehicle["direction"] == -1 and vehicle["latitude"] > T2_INTERSECTION["latitude"]: - return traffic_light_data[0], distance_to_west # 西路口红绿灯 - else: # 东西移动 - # 在西路口以西时，向东移动需要判断西路口红绿灯 - if vehicle["direction"] == 1 and vehicle["longitude"] < T2_INTERSECTION["longitude"]: - return traffic_light_data[0], distance_to_west # 西路口红绿灯 - # 在西路口以东时，向西移动需要判断西路口红绿灯 - elif vehicle["direction"] == -1 and vehicle["longitude"] > T2_INTERSECTION["longitude"]: + if vehicle["direction"] == -1 and vehicle["latitude"] > T2_INTERSECTION["latitude"]: return traffic_light_data[0], distance_to_west # 西路口红绿灯 elif vehicle["vehicleNo"] == "QN002": # 在东路口以西时，向东移动需要判断东路红绿灯 if vehicle["direction"] == 1 and vehicle["longitude"] < T6_INTERSECTION["longitude"]: return traffic_light_data[1], distance_to_east # 东路口红绿灯 - # 在东路口以东时，向西移动需要判断东路口红绿灯 - elif vehicle["direction"] == -1 and vehicle["longitude"] > T6_INTERSECTION["longitude"]: - return traffic_light_data[1], distance_to_east # 东路口红绿灯 # 其他情况，表示车辆已经过路口或不需要判断红绿灯 return None, float('inf') @@ -781,7 +768,7 @@ def switch_traffic_light_state(): lat_diff = abs(aircraft["latitude"] - T6_INTERSECTION["latitude"]) * 111319.9 old_state = traffic_light_data[1]["state"] - traffic_light_data[1]["state"] = 1 if lat_diff > DIST_50M else 0 + traffic_light_data[1]["state"] = 1 if lat_diff > DIST_50M else 1 if old_state != traffic_light_data[1]["state"]: print(f"东路口红绿灯状态切换为: {'绿灯' if traffic_light_data[1]['state'] == 1 else '红灯'}")