From 90b372f659516312626c27acccb16ce8ba83ab24 Mon Sep 17 00:00:00 2001 From: Tian jianyong <11429339@qq.com> Date: Thu, 6 Feb 2025 16:39:43 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86=20CPP=20=E6=96=87?= =?UTF-8?q?=E4=BB=B6=E7=9A=84=E6=A0=BC=E5=BC=8F=E5=8C=96=EF=BC=8C=E4=BD=BF?= =?UTF-8?q?=E7=94=A8=20K&R=20=E4=BB=A3=E7=A0=81=E9=A3=8E=E6=A0=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-format | 14 + .cursorrules | 21 +- CMakeLists.txt | 1 + HTTPClient.cpp | 12 - src/collector/DataCollector.cpp | 254 +++++----- src/collector/DataSourceConfig.cpp | 77 ++- src/config/AirportBounds.cpp | 96 ++-- src/config/IntersectionConfig.cpp | 24 +- src/config/SystemConfig.cpp | 10 +- src/core/System.cpp | 440 ++++++++-------- src/detector/CollisionDetector.cpp | 692 ++++++++++++-------------- src/detector/SafetyZone.cpp | 50 +- src/detector/TrafficLightDetector.cpp | 41 +- src/network/HTTPClient.cpp | 10 +- src/network/HTTPDataSource.cpp | 293 ++++++----- src/network/WebSocketServer.cpp | 233 +++++---- src/vehicle/ControllableVehicles.cpp | 6 +- tests/AirportBoundsTest.cpp | 104 ++-- 18 files changed, 1188 insertions(+), 1190 deletions(-) create mode 100644 .clang-format delete mode 100644 HTTPClient.cpp diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..1ab6e90 --- /dev/null +++ b/.clang-format @@ -0,0 +1,14 @@ +# 精简版 K&R 风格配置 +BasedOnStyle: LLVM +BreakBeforeBraces: Linux +IndentWidth: 4 +AllowShortFunctionsOnASingleLine: Empty + +# 显式禁用所有大括号换行 +BraceWrapping: + AfterClass: false + AfterStruct: false + AfterUnion: false + AfterFunction: false + AfterNamespace: false + AfterControlStatement: Never \ No newline at end of file diff --git a/.cursorrules b/.cursorrules index 2c17181..bb6293d 100644 --- a/.cursorrules +++ b/.cursorrules @@ -1,14 +1,5 @@ 请遵循： - 最小化修改代码，不要修改任何与解决当前问题无关的代码（包括注释），一定不能删除任何其他的代码 -- 使用 C++20 标准 -- 使用 CMake 3.14 及以上版本 -- 使用 nlohmann_json 3.11.3 版本 -- 使用 FetchContent 管理第三方库 -- 使用 Google Test 进行单元测试 -- 使用 Google Mock 进行单元测试 -- 使用 Google Benchmark 进行性能测试 -- 在头文件中定义函数，在源文件中实现函数 -- 在头文件中定义类，在源文件中实现类 - 使用源代码中的格式化方式和注释风格 - 日志的字符串格式采用拼接方式，不要使用{}占位符 @@ -23,7 +14,6 @@ 2. 列表格式 - 无序列表统一使用 - - - 有序列表使用 1. 2. 3. - 列表项前后空一行 - 列表嵌套使用两个空格缩进 @@ -44,18 +34,11 @@ - 格式统一一致 请遵循如下 C++ 规范： -- 使用 C++20 标准 +- 使用 C++17 标准 - 使用 CMake 3.14 及以上版本 - 使用 nlohmann_json 3.11.3 版本 - 使用 FetchContent 管理第三方库 - 使用 Google Test 进行单元测试 - 使用 Google Mock 进行单元测试 - 使用 Google Benchmark 进行性能测试 -- 在头文件中定义函数，在源文件中实现函数 - -修改代码原则: -- 先询问代码的用途 -- 理解代码的功能和作用 -- 确认是否可以修改或删除 -- 如果不确定就直接问我 -- 不要随便修改原有代码中的中文注释 \ No newline at end of file +- 在头文件中定义函数，在源文件中实现函数 \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 08a9ec2..913cea4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,6 +81,7 @@ endif() # 源文件列表 set(LIB_SOURCES src/collector/DataCollector.cpp + src/collector/DataSourceConfig.cpp src/core/System.cpp src/detector/CollisionDetector.cpp src/detector/SafetyZone.cpp diff --git a/HTTPClient.cpp b/HTTPClient.cpp deleted file mode 100644 index 721017f..0000000 --- a/HTTPClient.cpp +++ /dev/null @@ -1,12 +0,0 @@ -std::string getSignalStateString(SignalState state) { - switch (state) { - case SignalState::RED: - return "RED"; - case SignalState::YELLOW: - return "YELLOW"; - case SignalState::GREEN: - return "GREEN"; - default: - return "UNKNOWN"; - } -} \ No newline at end of file diff --git a/src/collector/DataCollector.cpp b/src/collector/DataCollector.cpp index 11b0a91..451dd01 100644 --- a/src/collector/DataCollector.cpp +++ b/src/collector/DataCollector.cpp @@ -2,23 +2,23 @@ #include "network/HTTPDataSource.h" #include "utils/Logger.h" #include -#include #include +#include #include -DataCollector::DataCollector(const AirportBounds& bounds) - : last_position_fetch_(std::chrono::steady_clock::now()) - , last_unmanned_fetch_(std::chrono::steady_clock::now()) - , last_traffic_light_fetch_(std::chrono::steady_clock::now()) - , last_warning_time_(std::chrono::steady_clock::now()) - , airportBounds_(bounds) { -} +DataCollector::DataCollector(const AirportBounds& bounds) + : last_position_fetch_(std::chrono::steady_clock::now()), + last_unmanned_fetch_(std::chrono::steady_clock::now()), + last_traffic_light_fetch_(std::chrono::steady_clock::now()), + last_warning_time_(std::chrono::steady_clock::now()), + airportBounds_(bounds) {} DataCollector::~DataCollector() { stop(); } -bool DataCollector::initialize(const DataSourceConfig& dataSourceConfig, const WarnConfig& warnConfig) { +bool DataCollector::initialize(const DataSourceConfig& dataSourceConfig, + const WarnConfig& warnConfig) { if (!dataSource_) { dataSourceConfig_ = dataSourceConfig; dataSource_ = std::make_shared(dataSourceConfig_); @@ -35,24 +35,24 @@ void DataCollector::start() { if (running_) { return; } - + if (!dataSource_) { Logger::error("No data source available"); return; } - + running_ = true; last_position_fetch_ = std::chrono::steady_clock::now(); last_unmanned_fetch_ = std::chrono::steady_clock::now(); last_traffic_light_fetch_ = std::chrono::steady_clock::now(); - + // 尝试连接，但即使失败也继续运行 if (!dataSource_->connect()) { Logger::error("Failed to connect to data source"); // 立即检查超时，这样如果一开始就连不上，会马上发出告警 checkTimeout(); } - + // 启动三个独立的采集线程 positionThread_ = std::thread(&DataCollector::positionLoop, this); unmannedThread_ = std::thread(&DataCollector::unmannedLoop, this); @@ -63,9 +63,9 @@ void DataCollector::stop() { if (!running_) { return; } - + running_ = false; - + // 等待所有线程结束 if (positionThread_.joinable()) { positionThread_.join(); @@ -76,32 +76,37 @@ void DataCollector::stop() { if (trafficLightThread_.joinable()) { trafficLightThread_.join(); } - + if (dataSource_) { dataSource_->disconnect(); } } void DataCollector::positionLoop() { - Logger::debug("位置数据采集循环启动，刷新间隔: ", dataSourceConfig_.position.refresh_interval_ms, "ms"); + Logger::debug("位置数据采集循环启动，刷新间隔: ", + dataSourceConfig_.position.refresh_interval_ms, "ms"); auto next_collection_time = std::chrono::steady_clock::now(); - + while (running_) { bool success = fetchPositionData(); if (!success) { Logger::warning("位置数据采集失败"); checkTimeout(); } - + // 计算下一次采集时间 - next_collection_time += std::chrono::milliseconds(dataSourceConfig_.position.refresh_interval_ms); + next_collection_time += std::chrono::milliseconds( + dataSourceConfig_.position.refresh_interval_ms); auto now = std::chrono::steady_clock::now(); - + if (next_collection_time > now) { - auto wait_time = std::chrono::duration_cast( - next_collection_time - now).count(); + auto wait_time = + std::chrono::duration_cast( + next_collection_time - now) + .count(); if (wait_time > 0) { - std::this_thread::sleep_for(std::chrono::milliseconds(wait_time)); + std::this_thread::sleep_for( + std::chrono::milliseconds(wait_time)); } } else { // 如果已经超过了下一次采集时间，立即重置 @@ -111,25 +116,30 @@ void DataCollector::positionLoop() { } void DataCollector::unmannedLoop() { - Logger::debug("无人车状态采集循环启动，刷新间隔: ", dataSourceConfig_.vehicle.refresh_interval_ms, "ms"); + Logger::debug("无人车状态采集循环启动，刷新间隔: ", + dataSourceConfig_.vehicle.refresh_interval_ms, "ms"); auto next_collection_time = std::chrono::steady_clock::now(); - + while (running_) { bool success = fetchUnmannedVehicleData(); if (!success) { Logger::warning("无人车状态采集失败"); checkTimeout(); } - + // 计算下一次采集时间 - next_collection_time += std::chrono::milliseconds(dataSourceConfig_.vehicle.refresh_interval_ms); + next_collection_time += std::chrono::milliseconds( + dataSourceConfig_.vehicle.refresh_interval_ms); auto now = std::chrono::steady_clock::now(); - + if (next_collection_time > now) { - auto wait_time = std::chrono::duration_cast( - next_collection_time - now).count(); + auto wait_time = + std::chrono::duration_cast( + next_collection_time - now) + .count(); if (wait_time > 0) { - std::this_thread::sleep_for(std::chrono::milliseconds(wait_time)); + std::this_thread::sleep_for( + std::chrono::milliseconds(wait_time)); } } else { // 如果已经超过了下一次采集时间，立即重置 @@ -139,25 +149,30 @@ void DataCollector::unmannedLoop() { } void DataCollector::trafficLightLoop() { - Logger::debug("红绿灯数据采集循环启动，刷新间隔: ", dataSourceConfig_.traffic_light.refresh_interval_ms, "ms"); + Logger::debug("红绿灯数据采集循环启动，刷新间隔: ", + dataSourceConfig_.traffic_light.refresh_interval_ms, "ms"); auto next_collection_time = std::chrono::steady_clock::now(); - + while (running_) { bool success = fetchTrafficLightData(); if (!success) { Logger::warning("红绿灯数据采集失败"); checkTimeout(); } - + // 计算下一次采集时间 - next_collection_time += std::chrono::milliseconds(dataSourceConfig_.traffic_light.refresh_interval_ms); + next_collection_time += std::chrono::milliseconds( + dataSourceConfig_.traffic_light.refresh_interval_ms); auto now = std::chrono::steady_clock::now(); - + if (next_collection_time > now) { - auto wait_time = std::chrono::duration_cast( - next_collection_time - now).count(); + auto wait_time = + std::chrono::duration_cast( + next_collection_time - now) + .count(); if (wait_time > 0) { - std::this_thread::sleep_for(std::chrono::milliseconds(wait_time)); + std::this_thread::sleep_for( + std::chrono::milliseconds(wait_time)); } } else { // 如果已经超过了下一次采集时间，立即重置 @@ -168,24 +183,30 @@ void DataCollector::trafficLightLoop() { void DataCollector::checkTimeout() { auto now = std::chrono::steady_clock::now(); - + // 检查位置数据超时 - auto position_elapsed = std::chrono::duration_cast( - now - last_position_fetch_).count(); + auto position_elapsed = + std::chrono::duration_cast( + now - last_position_fetch_) + .count(); if (position_elapsed > dataSourceConfig_.position.timeout_ms) { sendTimeoutWarning("position", position_elapsed); } - + // 检查无人车数据超时 - auto unmanned_elapsed = std::chrono::duration_cast( - now - last_unmanned_fetch_).count(); + auto unmanned_elapsed = + std::chrono::duration_cast( + now - last_unmanned_fetch_) + .count(); if (unmanned_elapsed > dataSourceConfig_.vehicle.timeout_ms) { sendTimeoutWarning("unmanned", unmanned_elapsed); } - + // 检查红绿灯数据超时 - auto traffic_light_elapsed = std::chrono::duration_cast( - now - last_traffic_light_fetch_).count(); + auto traffic_light_elapsed = + std::chrono::duration_cast( + now - last_traffic_light_fetch_) + .count(); if (traffic_light_elapsed > dataSourceConfig_.traffic_light.timeout_ms) { sendTimeoutWarning("traffic_light", traffic_light_elapsed); } @@ -193,38 +214,46 @@ void DataCollector::checkTimeout() { void DataCollector::resetTimeout(const std::string& source) { auto now = std::chrono::steady_clock::now(); - + if (source == "position") { auto elapsed = std::chrono::duration_cast( - now - last_position_fetch_).count(); + now - last_position_fetch_) + .count(); if (elapsed > dataSourceConfig_.position.timeout_ms) { - Logger::info("Position data source connection restored after ", elapsed, "ms timeout"); + Logger::info("Position data source connection restored after ", + elapsed, "ms timeout"); } last_position_fetch_ = now; - } - else if (source == "unmanned") { + } else if (source == "unmanned") { auto elapsed = std::chrono::duration_cast( - now - last_unmanned_fetch_).count(); + now - last_unmanned_fetch_) + .count(); if (elapsed > dataSourceConfig_.vehicle.timeout_ms) { - Logger::info("Unmanned vehicle data source connection restored after ", elapsed, "ms timeout"); + Logger::info( + "Unmanned vehicle data source connection restored after ", + elapsed, "ms timeout"); } last_unmanned_fetch_ = now; - } - else if (source == "traffic_light") { + } else if (source == "traffic_light") { auto elapsed = std::chrono::duration_cast( - now - last_traffic_light_fetch_).count(); + now - last_traffic_light_fetch_) + .count(); if (elapsed > dataSourceConfig_.traffic_light.timeout_ms) { - Logger::info("Traffic light data source connection restored after ", elapsed, "ms timeout"); + Logger::info("Traffic light data source connection restored after ", + elapsed, "ms timeout"); } last_traffic_light_fetch_ = now; } } -void DataCollector::sendTimeoutWarning(const std::string& source, int64_t elapsed_ms) { +void DataCollector::sendTimeoutWarning(const std::string& source, + int64_t elapsed_ms) { auto now = std::chrono::steady_clock::now(); - auto warning_elapsed = std::chrono::duration_cast( - now - last_warning_time_).count(); - + auto warning_elapsed = + std::chrono::duration_cast( + now - last_warning_time_) + .count(); + if (warning_elapsed >= warnConfig_.warning_interval_ms) { if (!system_) { Logger::debug("System not set, skipping timeout warning"); @@ -234,71 +263,68 @@ void DataCollector::sendTimeoutWarning(const std::string& source, int64_t elapse network::TimeoutWarningMessage msg; msg.source = source; msg.elapsed_ms = elapsed_ms; - + // 根据数据源类型设置超时阈值 int64_t timeout_ms = 0; if (source == "position") { timeout_ms = dataSourceConfig_.position.timeout_ms; - } - else if (source == "unmanned") { + } else if (source == "unmanned") { timeout_ms = dataSourceConfig_.vehicle.timeout_ms; - } - else if (source == "traffic_light") { + } else if (source == "traffic_light") { timeout_ms = dataSourceConfig_.traffic_light.timeout_ms; } - + msg.is_read_timeout = (elapsed_ms > timeout_ms); - + system_->broadcastTimeoutWarning(msg); last_warning_time_ = now; } } -void DataCollector::filterPositionData(std::vector& aircraft, std::vector& vehicles) { +void DataCollector::filterPositionData(std::vector& aircraft, + std::vector& vehicles) { size_t originalAircraftCount = aircraft.size(); size_t originalVehicleCount = vehicles.size(); - - // 过滤航空器数据 - auto aircraftIt = std::remove_if(aircraft.begin(), aircraft.end(), - [this](const Aircraft& a) { + + // 过滤掉不在机场区域内的航空器数据 + auto aircraftIt = std::remove_if( + aircraft.begin(), aircraft.end(), [this](const Aircraft& a) { return !airportBounds_.isPointInBounds(a.position); }); aircraft.erase(aircraftIt, aircraft.end()); - - // 过滤车辆数据 - auto vehicleIt = std::remove_if(vehicles.begin(), vehicles.end(), - [this](const Vehicle& v) { + + // 过滤掉不在机场区域内的车辆数据 + auto vehicleIt = std::remove_if( + vehicles.begin(), vehicles.end(), [this](const Vehicle& v) { return !airportBounds_.isPointInBounds(v.position); }); vehicles.erase(vehicleIt, vehicles.end()); - + // 记录过滤结果 size_t filteredAircraftCount = originalAircraftCount - aircraft.size(); size_t filteredVehicleCount = originalVehicleCount - vehicles.size(); - + if (filteredAircraftCount > 0 || filteredVehicleCount > 0) { - Logger::info( - "数据过滤结果: 过滤掉 ", filteredAircraftCount, " 架航空器, ", - filteredVehicleCount, " 辆车辆" - ); + Logger::info("数据过滤结果: 过滤掉 ", filteredAircraftCount, + " 架航空器, ", filteredVehicleCount, " 辆车辆"); } } bool DataCollector::fetchPositionData() { std::vector newAircraft; std::vector newVehicles; - + if (!dataSource_) { Logger::error("No data source available"); return false; } - - bool success = dataSource_->fetchPositionAircraftData(newAircraft) && - dataSource_->fetchPositionVehicleData(newVehicles); - + + bool success = dataSource_->fetchPositionAircraftData(newAircraft) && + dataSource_->fetchPositionVehicleData(newVehicles); + if (success) { std::lock_guard lock(cacheMutex_); - + // 更新运动信息 for (auto& a : newAircraft) { auto it = lastAircraftPositions_.find(a.id); @@ -312,7 +338,7 @@ bool DataCollector::fetchPositionData() { // 更新时间戳 lastAircraftTimestamp_ = a.timestamp; } - + // 更新运动信息 for (auto& v : newVehicles) { auto it = lastVehiclePositions_.find(v.id); @@ -326,30 +352,32 @@ bool DataCollector::fetchPositionData() { // 更新时间戳 lastVehicleTimestamp_ = v.timestamp; } - + // 转换坐标系 for (auto& a : newAircraft) { // 1. 先转换为世界坐标 - a.position = CoordinateConverter::instance().toLocalXY(a.geo.latitude, a.geo.longitude); + a.position = CoordinateConverter::instance().toLocalXY( + a.geo.latitude, a.geo.longitude); // 2. 再转换为机场坐标 a.position = airportBounds_.toAirportCoordinate(a.position); } - + for (auto& v : newVehicles) { // 1. 先转换为世界坐标 - v.position = CoordinateConverter::instance().toLocalXY(v.geo.latitude, v.geo.longitude); + v.position = CoordinateConverter::instance().toLocalXY( + v.geo.latitude, v.geo.longitude); // 2. 再转换为机场坐标 v.position = airportBounds_.toAirportCoordinate(v.position); } - + // 过滤数据 filterPositionData(newAircraft, newVehicles); - + aircraftCache_ = std::move(newAircraft); vehicleCache_ = std::move(newVehicles); resetTimeout("position"); } - + return success; } @@ -362,15 +390,17 @@ bool DataCollector::fetchUnmannedVehicleData() { bool any_success = false; bool any_failure = false; auto& vehicles = ControllableVehicles::getInstance().getVehicles(); - + for (const auto& vehicle : vehicles) { if (vehicle.type == "UNMANNED") { std::string status; - if (dataSource_->fetchUnmannedVehicleStatus(vehicle.vehicleNo, status)) { + if (dataSource_->fetchUnmannedVehicleStatus(vehicle.vehicleNo, + status)) { any_success = true; } else { any_failure = true; - Logger::error("获取无人车状态失败, vehicleNo: " + vehicle.vehicleNo); + Logger::error("获取无人车状态失败, vehicleNo: " + + vehicle.vehicleNo); } } } @@ -380,21 +410,21 @@ bool DataCollector::fetchUnmannedVehicleData() { // 如果有部分成功，就返回 true return true; } - + // 只有全部失败才返回 false return !any_failure; } bool DataCollector::fetchTrafficLightData() { std::vector newSignals; - + if (!dataSource_) { Logger::error("No data source available"); return false; } - + bool success = dataSource_->fetchTrafficLightSignals(newSignals); - + if (success) { std::lock_guard lock(cacheMutex_); trafficLightCache_ = std::move(newSignals); @@ -404,7 +434,7 @@ bool DataCollector::fetchTrafficLightData() { } resetTimeout("traffic_light"); } - + return success; } @@ -413,7 +443,8 @@ std::vector DataCollector::getTrafficLightSignals() { return trafficLightCache_; } -bool DataCollector::fetchTrafficLightSignals(std::vector& signals) { +bool DataCollector::fetchTrafficLightSignals( + std::vector& signals) { if (!dataSource_) { return false; } @@ -430,10 +461,11 @@ std::vector DataCollector::getVehicleData() { return vehicleCache_; } -bool DataCollector::fetchUnmannedVehicleStatus(const std::string& vehicle_id, std::string& status) { +bool DataCollector::fetchUnmannedVehicleStatus(const std::string& vehicle_id, + std::string& status) { if (!dataSource_) { Logger::error("No data source available"); return false; } return dataSource_->fetchUnmannedVehicleStatus(vehicle_id, status); -} \ No newline at end of file +} \ No newline at end of file diff --git a/src/collector/DataSourceConfig.cpp b/src/collector/DataSourceConfig.cpp index 19a389a..aa6fd74 100644 --- a/src/collector/DataSourceConfig.cpp +++ b/src/collector/DataSourceConfig.cpp @@ -8,40 +8,65 @@ DataSourceConfig DataSourceConfig::fromSystemConfig(const SystemConfig& config) // 转换位置数据源配置 dataSourceConfig.position.host = config.data_source.position.host; dataSourceConfig.position.port = config.data_source.position.port; - dataSourceConfig.position.aircraft_path = config.data_source.position.aircraft_path; - dataSourceConfig.position.vehicle_path = config.data_source.position.vehicle_path; - dataSourceConfig.position.refresh_interval_ms = config.data_source.position.refresh_interval_ms; - dataSourceConfig.position.timeout_ms = config.data_source.position.timeout_ms; - dataSourceConfig.position.read_timeout_ms = config.data_source.position.read_timeout_ms; - dataSourceConfig.position.auth.username = config.data_source.position.auth.username; - dataSourceConfig.position.auth.password = config.data_source.position.auth.password; - dataSourceConfig.position.auth.auth_path = config.data_source.position.auth.auth_path; - dataSourceConfig.position.auth.auth_required = config.data_source.position.auth.auth_required; + dataSourceConfig.position.aircraft_path = + config.data_source.position.aircraft_path; + dataSourceConfig.position.vehicle_path = + config.data_source.position.vehicle_path; + dataSourceConfig.position.refresh_interval_ms = + config.data_source.position.refresh_interval_ms; + dataSourceConfig.position.timeout_ms = + config.data_source.position.timeout_ms; + dataSourceConfig.position.read_timeout_ms = + config.data_source.position.read_timeout_ms; + dataSourceConfig.position.auth.username = + config.data_source.position.auth.username; + dataSourceConfig.position.auth.password = + config.data_source.position.auth.password; + dataSourceConfig.position.auth.auth_path = + config.data_source.position.auth.auth_path; + dataSourceConfig.position.auth.auth_required = + config.data_source.position.auth.auth_required; // 转换无人车数据源配置 dataSourceConfig.vehicle.host = config.data_source.vehicle.host; dataSourceConfig.vehicle.port = config.data_source.vehicle.port; - dataSourceConfig.vehicle.status_path = config.data_source.vehicle.status_path; - dataSourceConfig.vehicle.command_path = config.data_source.vehicle.command_path; - dataSourceConfig.vehicle.refresh_interval_ms = config.data_source.vehicle.refresh_interval_ms; + dataSourceConfig.vehicle.status_path = + config.data_source.vehicle.status_path; + dataSourceConfig.vehicle.command_path = + config.data_source.vehicle.command_path; + dataSourceConfig.vehicle.refresh_interval_ms = + config.data_source.vehicle.refresh_interval_ms; dataSourceConfig.vehicle.timeout_ms = config.data_source.vehicle.timeout_ms; - dataSourceConfig.vehicle.read_timeout_ms = config.data_source.vehicle.read_timeout_ms; - dataSourceConfig.vehicle.auth.username = config.data_source.vehicle.auth.username; - dataSourceConfig.vehicle.auth.password = config.data_source.vehicle.auth.password; - dataSourceConfig.vehicle.auth.auth_path = config.data_source.vehicle.auth.auth_path; - dataSourceConfig.vehicle.auth.auth_required = config.data_source.vehicle.auth.auth_required; + dataSourceConfig.vehicle.read_timeout_ms = + config.data_source.vehicle.read_timeout_ms; + dataSourceConfig.vehicle.auth.username = + config.data_source.vehicle.auth.username; + dataSourceConfig.vehicle.auth.password = + config.data_source.vehicle.auth.password; + dataSourceConfig.vehicle.auth.auth_path = + config.data_source.vehicle.auth.auth_path; + dataSourceConfig.vehicle.auth.auth_required = + config.data_source.vehicle.auth.auth_required; // 转换红绿灯数据源配置 dataSourceConfig.traffic_light.host = config.data_source.traffic_light.host; dataSourceConfig.traffic_light.port = config.data_source.traffic_light.port; - dataSourceConfig.traffic_light.signal_path = config.data_source.traffic_light.signal_path; - dataSourceConfig.traffic_light.refresh_interval_ms = config.data_source.traffic_light.refresh_interval_ms; - dataSourceConfig.traffic_light.timeout_ms = config.data_source.traffic_light.timeout_ms; - dataSourceConfig.traffic_light.read_timeout_ms = config.data_source.traffic_light.read_timeout_ms; - dataSourceConfig.traffic_light.auth.username = config.data_source.traffic_light.auth.username; - dataSourceConfig.traffic_light.auth.password = config.data_source.traffic_light.auth.password; - dataSourceConfig.traffic_light.auth.auth_path = config.data_source.traffic_light.auth.auth_path; - dataSourceConfig.traffic_light.auth.auth_required = config.data_source.traffic_light.auth.auth_required; + dataSourceConfig.traffic_light.signal_path = + config.data_source.traffic_light.signal_path; + dataSourceConfig.traffic_light.refresh_interval_ms = + config.data_source.traffic_light.refresh_interval_ms; + dataSourceConfig.traffic_light.timeout_ms = + config.data_source.traffic_light.timeout_ms; + dataSourceConfig.traffic_light.read_timeout_ms = + config.data_source.traffic_light.read_timeout_ms; + dataSourceConfig.traffic_light.auth.username = + config.data_source.traffic_light.auth.username; + dataSourceConfig.traffic_light.auth.password = + config.data_source.traffic_light.auth.password; + dataSourceConfig.traffic_light.auth.auth_path = + config.data_source.traffic_light.auth.auth_path; + dataSourceConfig.traffic_light.auth.auth_required = + config.data_source.traffic_light.auth.auth_required; return dataSourceConfig; -} \ No newline at end of file +} \ No newline at end of file diff --git a/src/config/AirportBounds.cpp b/src/config/AirportBounds.cpp index 0e9e625..2e45a1e 100644 --- a/src/config/AirportBounds.cpp +++ b/src/config/AirportBounds.cpp @@ -1,9 +1,9 @@ #include "AirportBounds.h" -#include -#include -#include #include "utils/Logger.h" #include +#include +#include +#include using json = nlohmann::json; @@ -18,7 +18,8 @@ void AirportBounds::loadConfig(const std::string& configFile) { // 检查文件是否存在 if (!std::filesystem::exists(configFile)) { Logger::error("配置文件不存在: ", configFile); - Logger::error("当前工作目录: ", std::filesystem::current_path().string()); + Logger::error("当前工作目录: ", + std::filesystem::current_path().string()); throw std::runtime_error("配置文件不存在: " + configFile); } @@ -40,9 +41,10 @@ void AirportBounds::loadConfig(const std::string& configFile) { auto& airport = j["airport"]; if (airport.contains("rotation_angle")) { // 将角度转换为弧度 - rotationAngle_ = airport["rotation_angle"].get() * M_PI / 180.0; + rotationAngle_ = + airport["rotation_angle"].get() * M_PI / 180.0; } - + if (airport.contains("reference_point")) { referencePoint_.x = airport["reference_point"]["x"].get(); referencePoint_.y = airport["reference_point"]["y"].get(); @@ -50,12 +52,10 @@ void AirportBounds::loadConfig(const std::string& configFile) { // 加载机场边界 auto& airportBounds = j["airport"]["bounds"]; - airportBounds_ = { - airportBounds["x"].get(), - airportBounds["y"].get(), - airportBounds["width"].get(), - airportBounds["height"].get() - }; + airportBounds_ = { airportBounds["x"].get(), + airportBounds["y"].get(), + airportBounds["width"].get(), + airportBounds["height"].get() }; // 检查 areas 字段 if (!j.contains("areas")) { @@ -65,12 +65,18 @@ void AirportBounds::loadConfig(const std::string& configFile) { // 加载各区域配置 for (const auto& [key, value] : j["areas"].items()) { AreaType type; - if (key == "runway") type = AreaType::RUNWAY; - else if (key == "taxiway") type = AreaType::TAXIWAY; - else if (key == "gate") type = AreaType::GATE; - else if (key == "service") type = AreaType::SERVICE; - else if (key == "test_zone") type = AreaType::TEST_ZONE; - else continue; + if (key == "runway") + type = AreaType::RUNWAY; + else if (key == "taxiway") + type = AreaType::TAXIWAY; + else if (key == "gate") + type = AreaType::GATE; + else if (key == "service") + type = AreaType::SERVICE; + else if (key == "test_zone") + type = AreaType::TEST_ZONE; + else + continue; // 检查必要的字段 if (!value.contains("bounds") || !value.contains("config")) { @@ -80,11 +86,8 @@ void AirportBounds::loadConfig(const std::string& configFile) { // 加载区域边界 auto& bounds = value["bounds"]; areaBounds_[type] = { - bounds["x"].get(), - bounds["y"].get(), - bounds["width"].get(), - bounds["height"].get() - }; + bounds["x"].get(), bounds["y"].get(), + bounds["width"].get(), bounds["height"].get() }; // 加载区域配置 auto& config = value["config"]; @@ -95,34 +98,26 @@ void AirportBounds::loadConfig(const std::string& configFile) { RadiusConfig collision_config = { collision_radius["aircraft"].get(), collision_radius["special"].get(), - collision_radius["unmanned"].get() - }; + collision_radius["unmanned"].get() }; RadiusConfig warning_config = { warning_zone_radius["aircraft"].get(), warning_zone_radius["special"].get(), - warning_zone_radius["unmanned"].get() - }; + warning_zone_radius["unmanned"].get() }; RadiusConfig alert_config = { alert_zone_radius["aircraft"].get(), alert_zone_radius["special"].get(), - alert_zone_radius["unmanned"].get() - }; + alert_zone_radius["unmanned"].get() }; - areaConfigs_[type] = { - collision_config, - config["height_threshold"].get(), - warning_config, - alert_config - }; + areaConfigs_[type] = { collision_config, + config["height_threshold"].get(), + warning_config, alert_config }; } - } - catch (const json::exception& e) { + } catch (const json::exception& e) { Logger::error("JSON解析错误: ", e.what()); throw; - } - catch (const std::exception& e) { + } catch (const std::exception& e) { Logger::error("加载配置文件失败: ", e.what()); throw; } @@ -132,29 +127,27 @@ Vector2D AirportBounds::toAirportCoordinate(const Vector2D& point) const { // 1. 计算相对于参考点的偏移 double dx = point.x - referencePoint_.x; double dy = point.y - referencePoint_.y; - + // 2. 应用旋转变换 double cos_angle = std::cos(rotationAngle_); double sin_angle = std::sin(rotationAngle_); - + // 3. 返回机场坐标系中的坐标（逆时针旋转） - Vector2D result{ - dx * cos_angle - dy * sin_angle, - dx * sin_angle + dy * cos_angle - }; - + Vector2D result{ dx * cos_angle - dy * sin_angle, + dx * sin_angle + dy * cos_angle }; + return result; } AreaType AirportBounds::getAreaType(const Vector2D& position) const { - + // 按优先级检查位置所在区域 for (const auto& [type, bounds] : areaBounds_) { if (bounds.contains(position)) { return type; } } - + // 默认返回测试区 return AreaType::TEST_ZONE; } @@ -173,13 +166,14 @@ bool AirportBounds::isPointInBounds(const Vector2D& position) const { return result; } -bool AirportBounds::isPointInArea(const Vector2D& position, AreaType areaType) const { +bool AirportBounds::isPointInArea(const Vector2D& position, + AreaType areaType) const { // 获取区域定义 auto it = areaBounds_.find(areaType); if (it == areaBounds_.end()) { return false; } - + // 在机场坐标系中判断是否在区域内 return it->second.contains(position); -} \ No newline at end of file +} \ No newline at end of file diff --git a/src/config/IntersectionConfig.cpp b/src/config/IntersectionConfig.cpp index 6617a85..f869465 100644 --- a/src/config/IntersectionConfig.cpp +++ b/src/config/IntersectionConfig.cpp @@ -7,7 +7,7 @@ IntersectionConfig IntersectionConfig::load(const std::string& configFile) { IntersectionConfig config; - + std::ifstream file(configFile); if (!file.is_open()) { throw std::runtime_error("Failed to open intersection config file: " + configFile); @@ -22,7 +22,7 @@ IntersectionConfig IntersectionConfig::load(const std::string& configFile) { info.id = item["id"].get(); info.name = item["name"].get(); info.trafficLightId = item["trafficLightId"].get(); - + // 加载位置信息，检查是否为 null if (item["position"]["longitude"].is_null() || item["position"]["latitude"].is_null() || @@ -32,7 +32,7 @@ IntersectionConfig IntersectionConfig::load(const std::string& configFile) { info.position.longitude = item["position"]["longitude"].get(); info.position.latitude = item["position"]["latitude"].get(); info.position.altitude = item["position"]["altitude"].get(); - + // 加载路口宽度和安全区配置，检查是否为 null if (item["width"].is_null() || item["safetyZone"]["aircraftRadius"].is_null() || @@ -44,20 +44,20 @@ IntersectionConfig IntersectionConfig::load(const std::string& configFile) { 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, - ", width=", info.width, - ", aircraftRadius=", info.safetyZone.aircraftRadius, - ", vehicleRadius=", info.safetyZone.vehicleRadius); + + Logger::debug("Loaded intersection: id=", info.id, + ", name=", info.name, + ", trafficLightId=", info.trafficLightId, + ", width=", info.width, + ", aircraftRadius=", info.safetyZone.aircraftRadius, + ", vehicleRadius=", info.safetyZone.vehicleRadius); } Logger::info("Loaded ", config.intersections_.size(), " intersections"); } catch (const std::exception& e) { throw std::runtime_error("Failed to parse intersection config: " + std::string(e.what())); } - + return config; } @@ -75,4 +75,4 @@ const Intersection* IntersectionConfig::findById(const std::string& intersection return info.id == intersectionId; }); return iter != intersections_.end() ? &(*iter) : nullptr; -} \ No newline at end of file +} \ No newline at end of file diff --git a/src/config/SystemConfig.cpp b/src/config/SystemConfig.cpp index 3a2e951..6f2a9aa 100644 --- a/src/config/SystemConfig.cpp +++ b/src/config/SystemConfig.cpp @@ -8,14 +8,14 @@ void SystemConfig::load(const std::string& filename) { if (!file.is_open()) { throw std::runtime_error("Failed to open config file: " + filename); } - + nlohmann::json j; file >> j; nlohmann::from_json(j, *this); - + Logger::info("Loaded system configuration from ", filename); - } - catch (const std::exception& e) { - throw std::runtime_error("Failed to load config: " + std::string(e.what())); + } catch (const std::exception& e) { + throw std::runtime_error("Failed to load config: " + + std::string(e.what())); } } \ No newline at end of file diff --git a/src/core/System.cpp b/src/core/System.cpp index 1e483b0..aaa2245 100644 --- a/src/core/System.cpp +++ b/src/core/System.cpp @@ -9,12 +9,12 @@ System* System::instance_ = nullptr; -System::System() +System::System() : controllableVehicles_(ControllableVehicles::getInstance()) { instance_ = this; std::signal(SIGINT, signalHandler); std::signal(SIGTERM, signalHandler); - + // 使用单例模式获取配置实例 auto& config = SystemConfig::instance(); } @@ -40,8 +40,7 @@ bool System::initialize() { // 初始化全局坐标转换器 CoordinateConverter::instance().setReferencePoint( system_config.airport.reference_point.latitude, - system_config.airport.reference_point.longitude - ); + system_config.airport.reference_point.longitude); // 加载路口配置 intersection_config_ = IntersectionConfig::load("config/intersections.json"); @@ -49,33 +48,29 @@ bool System::initialize() { // 初始化 WebSocket 服务器 ws_server_ = std::make_unique(system_config.websocket.port); - ws_thread_ = std::thread([this]() { - ws_server_->start(); - }); - + ws_thread_ = std::thread([this]() { ws_server_->start(); }); + // 加载机场区域配置 airportBounds_ = std::make_unique("config/airport_bounds.json"); - + // 初始化冲突检测器 collisionDetector_ = std::make_unique(*airportBounds_, controllableVehicles_); // 初始化红绿灯检测器 trafficLightDetector_ = std::make_unique(intersection_config_, controllableVehicles_, *this); - + // 初始化安全区 initializeSafetyZones(); // 加载告警配置 WarnConfig warnConfig{ system_config.warning.warning_interval_ms, - system_config.warning.log_interval_ms - }; + system_config.warning.log_interval_ms }; // 创建数据采集器并初始化 dataCollector_ = std::make_unique(*airportBounds_); return dataCollector_->initialize(system_config.data_source, warnConfig); - } - catch (const std::exception& e) { + } catch (const std::exception& e) { Logger::error("Failed to initialize system: ", e.what()); return false; } @@ -84,30 +79,29 @@ bool System::initialize() { void System::initializeSafetyZones() { // 清空现有的安全区 safetyZones_.clear(); - + // 获取所有路口配置 const auto& intersections = intersection_config_.getIntersections(); - + // 为每个路口创建安全区 for (const auto& intersection : intersections) { // 获取路口中心点的地理坐标 Vector2D center = CoordinateConverter::instance().toLocalXY( intersection.position.latitude, - intersection.position.longitude - ); - + intersection.position.longitude); + // 创建安全区 auto safetyZone = std::make_unique( center, - intersection.safetyZone.aircraftRadius, // 飞机安全区半径 - intersection.safetyZone.vehicleRadius // 特勤车安全区半径 + intersection.safetyZone.aircraftRadius, // 飞机安全区半径 + intersection.safetyZone.vehicleRadius // 特勤车安全区半径 ); - + Logger::debug("创建路口安全区: id=", intersection.id, - ", center=(", center.x, ",", center.y, ")", - ", aircraftRadius=", intersection.safetyZone.aircraftRadius, - ", vehicleRadius=", intersection.safetyZone.vehicleRadius); - + ", center=(", center.x, ",", center.y, ")", + ", aircraftRadius=", intersection.safetyZone.aircraftRadius, + ", vehicleRadius=", intersection.safetyZone.vehicleRadius); + // 保存安全区 safetyZones_[intersection.id] = std::move(safetyZone); } @@ -117,7 +111,7 @@ void System::start() { if (running_) { return; } - + running_ = true; dataCollector_->start(); processThread_ = std::thread(&System::processLoop, this); @@ -128,27 +122,27 @@ void System::stop() { if (!running_) { return; } - + running_ = false; - + // 停止 WebSocket 服务器 if (ws_server_) { ws_server_->stop(); } - + // 等待线程结束 if (ws_thread_.joinable()) { ws_thread_.join(); } - + if (processThread_.joinable()) { processThread_.join(); } - + if (dataCollector_) { dataCollector_->stop(); } - + Logger::info("System stopped"); } @@ -156,32 +150,32 @@ void System::processLoop() { while (running_) { try { auto now = std::chrono::steady_clock::now(); - + // 获取最新数据和时间戳 std::tie(latest_aircraft_, latest_vehicles_, latest_traffic_lights_) = dataCollector_->getLatestData(); auto [aircraft_ts, vehicle_ts, traffic_light_ts] = dataCollector_->getLastUpdateTimestamps(); - + // 检查数据更新 bool has_new_data = false; - + if (aircraft_ts > last_aircraft_timestamp) { has_new_data = true; last_aircraft_timestamp = aircraft_ts; Logger::debug("航空器数据已更新，新时间戳: ", aircraft_ts); } - + if (vehicle_ts > last_vehicle_timestamp) { has_new_data = true; last_vehicle_timestamp = vehicle_ts; Logger::debug("车辆数据已更新，新时间戳: ", vehicle_ts); } - + if (traffic_light_ts > last_traffic_light_timestamp) { has_new_data = true; last_traffic_light_timestamp = traffic_light_ts; Logger::debug("红绿灯数据已更新，新时间戳: ", traffic_light_ts); } - + if (has_new_data) { // 使用成员变量的引用构建 objects 向量 std::vector objects; @@ -198,45 +192,46 @@ void System::processLoop() { } // 创建当前周期的风险管理器 - std::unordered_map vehicleMaxRiskLevels; // 统一记录所有车辆的最高风险等级 - std::vector detectedRisks; // 统一收集所有检测到的风险 - + std::unordered_map vehicleMaxRiskLevels; // 统一记录所有车辆的最高风险等级 + std::vector detectedRisks; // 统一收集所有检测到的风险 + // 检查安全区更新 auto safety_zone_elapsed = std::chrono::duration_cast( - now - last_safety_zone_update).count(); - + now - last_safety_zone_update) + .count(); + if (safety_zone_elapsed >= SystemConfig::instance().collision_detection.update_interval_ms) { updateSafetyZoneStates(objects); Logger::debug("开始检查安全区冲突..."); - + // 记录安全区风险检测前的风险数量 size_t beforeSafetyZone = detectedRisks.size(); checkUnmannedVehicleSafetyZones(latest_vehicles_, objects, vehicleMaxRiskLevels, detectedRisks); Logger::debug("安全区检测新增风险数量: ", detectedRisks.size() - beforeSafetyZone); - + last_safety_zone_update = now; } - + // 检查和处理常规碰撞风险 collisionDetector_->updateTraffic(latest_aircraft_, latest_vehicles_); auto collisionRisks = collisionDetector_->detectCollisions(); - + Logger::debug("碰撞检测器检测到风险数量: ", collisionRisks.size()); for (const auto& risk : collisionRisks) { Logger::debug("碰撞风险: id1=", risk.id1, - ", id2=", risk.id2, - ", level=", static_cast(risk.level), - ", distance=", risk.distance); + ", id2=", risk.id2, + ", level=", static_cast(risk.level), + ", distance=", risk.distance); } - + // 合并风险 size_t beforeMerge = detectedRisks.size(); detectedRisks.insert(detectedRisks.end(), collisionRisks.begin(), collisionRisks.end()); Logger::debug("合并后新增风险数量: ", detectedRisks.size() - beforeMerge); - + // 统一处理所有风险 processCollisions(detectedRisks, vehicleMaxRiskLevels); - + // 广播位置更新 for (const auto& ac : latest_aircraft_) { broadcastPositionUpdate(ac); @@ -244,30 +239,29 @@ void System::processLoop() { for (const auto& veh : latest_vehicles_) { broadcastPositionUpdate(veh); } - + // 处理红绿灯信号 for (const auto& signal : latest_traffic_lights_) { broadcastTrafficLightStatus(signal); trafficLightDetector_->processSignal(signal, latest_vehicles_); } } - + // 等待下一次处理 std::this_thread::sleep_for(std::chrono::milliseconds( SystemConfig::instance().collision_detection.update_interval_ms)); - } - catch (const std::exception& e) { + } catch (const std::exception& e) { Logger::error("处理循环发生错误: ", e.what()); } } } void System::checkUnmannedVehicleSafetyZones( - const std::vector& vehicles, + const std::vector& vehicles, const std::vector& objects, std::unordered_map& vehicleMaxRiskLevels, std::vector& detectedRisks) { - + // 遍历所有无人车 for (const auto& vehicle : vehicles) { // 遍历所有路口安全区 @@ -275,13 +269,13 @@ void System::checkUnmannedVehicleSafetyZones( if (zone->getState() == 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(); - + RiskLevel riskLevel = RiskLevel::NONE; if (distance <= zoneRadius) { riskLevel = RiskLevel::CRITICAL; @@ -296,8 +290,8 @@ void System::checkUnmannedVehicleSafetyZones( if (obj->id == vehicle.id) { continue; } - - if ((obj->isAircraft() || obj->isSpecialVehicle()) && + + if ((obj->isAircraft() || obj->isSpecialVehicle()) && zone->isObjectInZone(*obj)) { // 创建风险对象 CollisionRisk risk; @@ -307,27 +301,27 @@ void System::checkUnmannedVehicleSafetyZones( 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) - ); + 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 - }; - + obj->position.y - vehicle.position.y }; + detectedRisks.push_back(risk); - + // 更新风险级别 auto& currentRisk = vehicleMaxRiskLevels[vehicle.id]; currentRisk = std::max(currentRisk, riskLevel); - + Logger::debug("安全区风险: id1=", risk.id1, - ", id2=", risk.id2, - ", level=", static_cast(risk.level), - ", distance=", risk.distance); - + ", id2=", risk.id2, + ", level=", static_cast(risk.level), + ", distance=", risk.distance); + break; } } @@ -339,37 +333,36 @@ void System::checkUnmannedVehicleSafetyZones( void System::processCollisions( const std::vector& detectedRisks, std::unordered_map& vehicleMaxRiskLevels) { - + // 记录当前有风险的可控车辆 std::unordered_set currentVehiclesWithRisk; - + // 修改日志输出，显示更多信息 Logger::debug("开始处理碰撞风险，风险数量: ", detectedRisks.size()); for (const auto& risk : detectedRisks) { Logger::debug("待处理风险: id1=", risk.id1, - ", id2=", risk.id2, - ", level=", static_cast(risk.level), - ", distance=", risk.distance, - ", minDistance=", risk.minDistance); + ", id2=", risk.id2, + ", level=", static_cast(risk.level), + ", distance=", risk.distance, + ", minDistance=", risk.minDistance); } - + // 处理当前的碰撞风险 for (const auto& risk : detectedRisks) { // 处理 id1 和 id2 中的可控车辆 bool id1_controllable = controllableVehicles_.isControllable(risk.id1); bool id2_controllable = controllableVehicles_.isControllable(risk.id2); - + auto processVehicle = [&](const std::string& vehicleId, const std::string& otherId) { // 发送指令 VehicleCommand cmd; cmd.vehicleId = vehicleId; cmd.type = risk.level == RiskLevel::CRITICAL ? CommandType::ALERT : CommandType::WARNING; - cmd.reason = otherId.substr(0, 2) == "AC" ? - CommandReason::AIRCRAFT_CROSSING : - CommandReason::SPECIAL_VEHICLE; + cmd.reason = otherId.substr(0, 2) == "AC" ? CommandReason::AIRCRAFT_CROSSING : CommandReason::SPECIAL_VEHICLE; cmd.timestamp = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()).count(); - + std::chrono::system_clock::now().time_since_epoch()) + .count(); + // 设置目标位置 const MovingObject* target = findVehicle(otherId); if (target) { @@ -380,26 +373,26 @@ void System::processCollisions( cmd.relativeMotionY = risk.relativeMotion.y; cmd.minDistance = risk.minDistance; } - + Logger::debug("准备发送指令: 车辆=", vehicleId, - ", 类型=", cmd.type == CommandType::ALERT ? "ALERT" : "WARNING", - ", 原因=", cmd.reason == CommandReason::AIRCRAFT_CROSSING ? "AIRCRAFT_CROSSING" : "SPECIAL_VEHICLE", - ", 距离=", risk.distance, - ", 风险等级=", static_cast(risk.level)); - + ", 类型=", cmd.type == CommandType::ALERT ? "ALERT" : "WARNING", + ", 原因=", cmd.reason == CommandReason::AIRCRAFT_CROSSING ? "AIRCRAFT_CROSSING" : "SPECIAL_VEHICLE", + ", 距离=", risk.distance, + ", 风险等级=", static_cast(risk.level)); + broadcastVehicleCommand(cmd); controllableVehicles_.sendCommand(vehicleId, cmd); - + // 更新风险记录 currentVehiclesWithRisk.insert(vehicleId); vehicleMaxRiskLevels[vehicleId] = risk.level; - + Logger::info("碰撞风险: 车辆=", vehicleId, - ", 目标=", otherId, - ", 距离=", risk.distance, "m", - ", 最小距离=", risk.minDistance, "m", - ", 相对速度=", risk.relativeSpeed, "m/s"); - }; + ", 目标=", otherId, + ", 距离=", risk.distance, "m", + ", 最小距离=", risk.minDistance, "m", + ", 相对速度=", risk.relativeSpeed, "m/s"); + }; // 为每个可控车辆处理风险 if (id1_controllable) { @@ -408,11 +401,11 @@ void System::processCollisions( if (id2_controllable) { processVehicle(risk.id2, risk.id1); } - + // 广播碰撞预警消息 broadcastCollisionWarning(risk); } - + // 处理恢复指令 for (const auto& vehicleId : lastVehiclesWithRisk_) { if (currentVehiclesWithRisk.find(vehicleId) == currentVehiclesWithRisk.end()) { @@ -423,15 +416,16 @@ void System::processCollisions( cmd.type = CommandType::RESUME; cmd.reason = CommandReason::RESUME_TRAFFIC; cmd.timestamp = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()).count(); - + std::chrono::system_clock::now().time_since_epoch()) + .count(); + broadcastVehicleCommand(cmd); controllableVehicles_.sendCommand(vehicleId, cmd); Logger::info("发送恢复指令到车辆: ", vehicleId); } } } - + // 更新风险车辆列表 lastVehiclesWithRisk_ = std::move(currentVehiclesWithRisk); } @@ -440,7 +434,7 @@ void System::broadcastPositionUpdate(const MovingObject& obj) { if (!ws_server_) { return; } - + network::PositionUpdateMessage msg; msg.objectId = obj.id; msg.objectType = (typeid(obj) == typeid(Aircraft)) ? "aircraft" : "vehicle"; @@ -449,67 +443,64 @@ void System::broadcastPositionUpdate(const MovingObject& obj) { msg.latitude = obj.geo.latitude; msg.heading = obj.heading; msg.speed = obj.speed; - + nlohmann::json j = { {"type", "position_update"}, {"object_id", msg.objectId}, {"object_type", msg.objectType}, {"timestamp", msg.timestamp}, - {"position", { - {"longitude", msg.longitude}, - {"latitude", msg.latitude} - }}, + {"position", {{"longitude", msg.longitude}, {"latitude", msg.latitude}}}, {"heading", msg.heading}, - {"speed", msg.speed} - }; - + {"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); + " type=", msg.objectType, + " pos=(", msg.longitude, ",", msg.latitude, ")", + " heading=", msg.heading, + " speed=", msg.speed, + " timestamp=", msg.timestamp); } void System::broadcastTrafficLightStatus(const TrafficLightSignal& signal) { if (!ws_server_) { return; } - + // 查找路口信息 const Intersection* intersection = intersection_config_.findByTrafficLightId(signal.trafficLightId); - + nlohmann::json j = { {"type", "traffic_light_status"}, {"id", signal.trafficLightId}, {"status", static_cast(signal.status)}, - {"timestamp", signal.timestamp} - }; - + {"timestamp", signal.timestamp} }; + // 添加路口信息 if (intersection) { j["intersection"] = intersection->id; j["position"] = { {"longitude", intersection->position.longitude}, - {"latitude", intersection->position.latitude} - }; + {"latitude", intersection->position.latitude} }; } - + ws_server_->broadcast(j.dump()); - Logger::debug("广播红绿灯状态: id=", signal.trafficLightId, - " status=", static_cast(signal.status), - " intersection=", intersection ? intersection->id : "", - " position=(", intersection ? intersection->position.longitude : 0.0, ",", - intersection ? intersection->position.latitude : 0.0, ")", - " timestamp=", signal.timestamp); + Logger::debug("广播红绿灯状态: id=", signal.trafficLightId, + " status=", static_cast(signal.status), + " intersection=", intersection ? intersection->id : "", + " position=(", intersection ? intersection->position.longitude : 0.0, ",", + intersection ? intersection->position.latitude : 0.0, ")", + " timestamp=", signal.timestamp); } std::string getRiskLevelString(RiskLevel level) { switch (level) { - case RiskLevel::CRITICAL: return "critical"; - case RiskLevel::WARNING: return "warning"; - default: return "none"; + case RiskLevel::CRITICAL: + return "critical"; + case RiskLevel::WARNING: + return "warning"; + default: + return "none"; } } @@ -517,7 +508,7 @@ void System::broadcastCollisionWarning(const CollisionRisk& risk) { if (!ws_server_) { return; } - + // 构造冲突预警消息 nlohmann::json j = { {"type", "collision_warning"}, @@ -527,28 +518,28 @@ void System::broadcastCollisionWarning(const CollisionRisk& risk) { {"relativeSpeed", risk.relativeSpeed}, {"warningLevel", getRiskLevelString(risk.level)}, {"timestamp", std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()).count()} - }; - + std::chrono::system_clock::now().time_since_epoch()) + .count()} }; + // 广播冲突预警消息 ws_server_->broadcast(j.dump()); Logger::debug("广播冲突预警: id1=", risk.id1, " id2=", risk.id2, - " distance=", risk.distance, "m", - " relativeSpeed=", risk.relativeSpeed, "m/s", - " level=", getRiskLevelString(risk.level)); + " distance=", risk.distance, "m", + " relativeSpeed=", risk.relativeSpeed, "m/s", + " level=", getRiskLevelString(risk.level)); } void System::broadcastTimeoutWarning(const network::TimeoutWarningMessage& warning) { if (ws_server_) { ws_server_->broadcast(warning.toJson().dump()); - + // 根据超时时间记录不同级的日志 - if (warning.elapsed_ms > 30000) { // 30秒以上 + if (warning.elapsed_ms > 30000) { // 30秒以上 Logger::error("Severe timeout: ", warning.source, " - ", - warning.elapsed_ms, "ms without response"); + warning.elapsed_ms, "ms without response"); } else { Logger::warning("Connection timeout: ", warning.source, " - ", - warning.elapsed_ms, "ms without response"); + warning.elapsed_ms, "ms without response"); } } } @@ -557,34 +548,46 @@ void System::broadcastVehicleCommand(const VehicleCommand& cmd) { if (!ws_server_) { return; } - + nlohmann::json j = { {"type", "vehicle_command"}, {"vehicleId", cmd.vehicleId}, {"commandType", [&]() { - switch (cmd.type) { - case CommandType::SIGNAL: return "SIGNAL"; - case CommandType::ALERT: return "ALERT"; - case CommandType::WARNING: return "WARNING"; - case CommandType::RESUME: return "RESUME"; - case CommandType::PARKING: return "PARKING"; - default: return "UNKNOWN"; - } - }()}, + switch (cmd.type) { + case CommandType::SIGNAL: + return "SIGNAL"; + case CommandType::ALERT: + return "ALERT"; + case CommandType::WARNING: + return "WARNING"; + case CommandType::RESUME: + return "RESUME"; + case CommandType::PARKING: + return "PARKING"; + default: + return "UNKNOWN"; + } + }()}, {"reason", [&]() { - switch (cmd.reason) { - case CommandReason::TRAFFIC_LIGHT: return "TRAFFIC_LIGHT"; - case CommandReason::AIRCRAFT_CROSSING: return "AIRCRAFT_CROSSING"; - case CommandReason::SPECIAL_VEHICLE: return "SPECIAL_VEHICLE"; - case CommandReason::AIRCRAFT_PUSH: return "AIRCRAFT_PUSH"; - case CommandReason::RESUME_TRAFFIC: return "RESUME_TRAFFIC"; - case CommandReason::PARKING_SIDE: return "PARKING_SIDE"; - default: return "UNKNOWN"; - } - }()}, - {"timestamp", cmd.timestamp} - }; - + switch (cmd.reason) { + case CommandReason::TRAFFIC_LIGHT: + return "TRAFFIC_LIGHT"; + case CommandReason::AIRCRAFT_CROSSING: + return "AIRCRAFT_CROSSING"; + case CommandReason::SPECIAL_VEHICLE: + return "SPECIAL_VEHICLE"; + case CommandReason::AIRCRAFT_PUSH: + return "AIRCRAFT_PUSH"; + case CommandReason::RESUME_TRAFFIC: + return "RESUME_TRAFFIC"; + case CommandReason::PARKING_SIDE: + return "PARKING_SIDE"; + default: + return "UNKNOWN"; + } + }()}, + {"timestamp", cmd.timestamp} }; + // 添加可选字段 if (cmd.type == CommandType::SIGNAL) { j["signalState"] = cmd.signalState == SignalState::RED ? "RED" : "GREEN"; @@ -596,7 +599,7 @@ void System::broadcastVehicleCommand(const VehicleCommand& cmd) { j["targetLatitude"] = cmd.latitude; j["targetLongitude"] = cmd.longitude; } - + ws_server_->broadcast(j.dump()); } @@ -608,21 +611,21 @@ const MovingObject* System::findVehicle(const std::string& vehicleId) const { // 获取数据引用（注意：DataCollector 内部会加锁） const auto& aircrafts = dataCollector_->getAircraftRef(); const auto& vehicles = dataCollector_->getVehicleRef(); - + // 在航空器中查找 for (const auto& aircraft : aircrafts) { if (aircraft.flightNo == vehicleId) { return &aircraft; } } - + // 在车辆中查找 for (const auto& vehicle : vehicles) { if (vehicle.vehicleNo == vehicleId) { return &vehicle; } } - + return nullptr; } @@ -630,18 +633,17 @@ void System::updateSafetyZoneStates(const std::vector& objects) { // 重置所有安全区状态为未激活，同时重置类型 for (auto& [id, zone] : safetyZones_) { zone->setState(SafetyZoneState::INACTIVE); - zone->resetType(); // 重置安全区类型 + 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()); - + 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); @@ -650,32 +652,32 @@ 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, ")"); - + Logger::debug("检查安全区入侵: id=", obj.id, + ", type=", obj.isAircraft() ? "飞机" : (obj.isSpecialVehicle() ? "特勤车" : "其他"), + ", position=(", obj.position.x, ",", obj.position.y, ")"); + // 检查每个安全区 for (auto& [id, zone] : safetyZones_) { // 先检查是否在安全区内 if (zone->isObjectInZone(obj)) { // 如果在区内，尝试激活安全区 if (zone->tryActivate(obj)) { - Logger::debug("目标 ", obj.id, " 进入路口 ", id, " 安全区, 类型: ", - zone->getType() == SafetyZoneType::AIRCRAFT ? "飞机" : "特勤车", - ", 半径: ", zone->getCurrentRadius(), - ", 中心点=(", zone->getCenter().x, ",", zone->getCenter().y, ")"); + Logger::debug("目标 ", obj.id, " 进入路口 ", id, " 安全区, 类型: ", + zone->getType() == SafetyZoneType::AIRCRAFT ? "飞机" : "特勤车", + ", 半径: ", zone->getCurrentRadius(), + ", 中心点=(", zone->getCenter().x, ",", zone->getCenter().y, ")"); } } } } -bool System::handleSafetyZoneRisk(const Vehicle& vehicle, - const SafetyZone* zone, - const std::vector& objects, - double distance, - const std::string& intersectionId, - CommandType cmdType, - const std::string& riskLevel) { +bool System::handleSafetyZoneRisk(const Vehicle& vehicle, + const 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; @@ -684,46 +686,44 @@ bool System::handleSafetyZoneRisk(const Vehicle& vehicle, VehicleCommand cmd; cmd.vehicleId = vehicle.id; cmd.type = cmdType; - cmd.reason = zone->getType() == SafetyZoneType::AIRCRAFT ? - CommandReason::AIRCRAFT_CROSSING : - CommandReason::SPECIAL_VEHICLE; + cmd.reason = zone->getType() == SafetyZoneType::AIRCRAFT ? CommandReason::AIRCRAFT_CROSSING : CommandReason::SPECIAL_VEHICLE; cmd.timestamp = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()).count(); - + std::chrono::system_clock::now().time_since_epoch()) + .count(); + // 查找触发安全区的目标物体 const MovingObject* target = nullptr; for (const auto& obj : objects) { - if ((obj->isAircraft() || obj->isSpecialVehicle()) && + 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) - ); + (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); + ", 相对距离=(", rel_dx, ",", rel_dy, ")", + ", 相对速度=", cmd.relativeSpeed, + ", 最小距离=", distance); } - + broadcastVehicleCommand(cmd); controllableVehicles_.sendCommand(vehicle.id, cmd); @@ -733,10 +733,10 @@ bool System::handleSafetyZoneRisk(const Vehicle& vehicle, risk.level = cmdType == CommandType::ALERT ? RiskLevel::CRITICAL : RiskLevel::WARNING; risk.distance = distance; risk.relativeSpeed = cmd.relativeSpeed; - risk.relativeMotion = {cmd.relativeMotionX, cmd.relativeMotionY}; + risk.relativeMotion = { cmd.relativeMotionX, cmd.relativeMotionY }; risk.zoneType = WarningZoneType::WARNING; broadcastCollisionWarning(risk); - + return true; } \ No newline at end of file diff --git a/src/detector/CollisionDetector.cpp b/src/detector/CollisionDetector.cpp index d06cf41..f938cb5 100644 --- a/src/detector/CollisionDetector.cpp +++ b/src/detector/CollisionDetector.cpp @@ -1,43 +1,42 @@ #include "CollisionDetector.h" -#include "utils/Logger.h" #include "config/SystemConfig.h" +#include "utils/Logger.h" #include #include CollisionDetector::CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles) - : airportBounds_(bounds) - , vehicleTree_(bounds.getAirportBounds(), 8) // 使用机场总边界初始化四叉树 - , controllableVehicles_(&controllableVehicles) { - + : airportBounds_(bounds), + vehicleTree_(bounds.getAirportBounds(), 8), // 使用机场总边界初始化四叉树 + controllableVehicles_(&controllableVehicles) { // 记录初始化信息 const auto& airportBounds = bounds.getAirportBounds(); } void CollisionDetector::updateTraffic(const std::vector& aircraft, - const std::vector& vehicles) { + const std::vector& vehicles) { Logger::debug("更新交通数据: 航空器数量=", aircraft.size(), - ", 车辆数量=", vehicles.size()); - + ", 车辆数量=", vehicles.size()); + // 更新航空器数据 aircraftData_ = aircraft; - + // 更新车辆四叉树 vehicleTree_.clear(); size_t validVehicles = 0; - + for (const auto& vehicle : vehicles) { // 首先检查车辆是否在机场边界内 if (!airportBounds_.isPointInBounds(vehicle.position)) { - Logger::error("车辆在机场边界外: id=", vehicle.id, - ", position=(", vehicle.position.x, - ",", vehicle.position.y, ")"); - continue; // 跳过边界外的车辆 + Logger::error("车辆在机场边界外: id=", vehicle.id, ", position=(", + vehicle.position.x, ",", vehicle.position.y, ")"); + continue; // 跳过边界外的车辆 } - + // 根据配置设置车辆类型 Vehicle updatedVehicle = vehicle; - const auto* config = controllableVehicles_->findVehicle(vehicle.vehicleNo); + const auto* config = + controllableVehicles_->findVehicle(vehicle.vehicleNo); if (config) { if (config->type == "UNMANNED") { updatedVehicle.type = MovingObjectType::UNMANNED; @@ -50,43 +49,48 @@ void CollisionDetector::updateTraffic(const std::vector& aircraft, updatedVehicle.type = MovingObjectType::SPECIAL; updatedVehicle.isControllable = false; } - + // 插入四叉树 try { Logger::debug("尝试插入车辆: id=", updatedVehicle.id, - ", position=(", updatedVehicle.position.x, ",", updatedVehicle.position.y, ")", - ", type=", updatedVehicle.isControllable ? "UNMANNED" : "SPECIAL"); + ", 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()); + Logger::error("无法插入车辆到四叉树: id=", updatedVehicle.id, + ", error=", e.what()); } } - + // 记录更新后的状态 auto bounds = vehicleTree_.getBounds(); - Logger::debug("四叉树边界: min=(", bounds.x, ",", bounds.y, - "), max=(", bounds.x + bounds.width, ",", bounds.y + bounds.height, ")"); + 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( + "查询到车辆: id=", vehicle.id, ", position=(", vehicle.position.x, + ",", vehicle.position.y, ")", + ", type=", vehicle.isControllable ? "UNMANNED" : "SPECIAL"); } Logger::debug("交通数据更新完成: 有效车辆数量=", validVehicles, - ", 四叉树中的车辆数量=", allVehicles.size()); + ", 四叉树中的车辆数量=", allVehicles.size()); } std::vector CollisionDetector::detectCollisions() { std::vector risks; - + // 获取配置参数 - const auto& predictionConfig = SystemConfig::instance().collision_detection.prediction; - + const auto& predictionConfig = + SystemConfig::instance().collision_detection.prediction; + // 获取所有车辆 auto allVehicles = vehicleTree_.queryRange(vehicleTree_.getBounds()); - + // 可控车辆（无人车） std::vector unmannedVehicles; for (const auto& vehicle : allVehicles) { @@ -94,137 +98,137 @@ std::vector CollisionDetector::detectCollisions() { unmannedVehicles.push_back(vehicle); } } - + // 记录当前检测到的冲突对 - std::unordered_set, CollisionPairHash> currentCollisions; - + 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; - + 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); - + 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, ")"); + ", 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); - + auto collisionResult = + checkCollision(aircraft, vehicle, predictionConfig.time_window); + // 计算相对运动 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); - + 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 - ); - + 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; // 更新冲突点 + record.collisionPoint = + collisionResult.collisionPoint; // 更新冲突点 Logger::debug("更新冲突记录: obj1=", aircraft.id, - ", obj2=", vehicle.id, - ", maxLevel=", static_cast(record.maxLevel), - ", collisionPoint=(", record.collisionPoint.x, - ",", record.collisionPoint.y, ")"); + ", 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 - }; + 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, ")"); + ", obj2=", vehicle.id, + ", level=", static_cast(level), + ", collisionPoint=(", + collisionResult.collisionPoint.x, ",", + collisionResult.collisionPoint.y, ")"); } - + // 添加到当前冲突集合 currentCollisions.insert(collisionKey); - + // 添加到风险列表 - 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)); + 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)) { + if (checkCollisionResolved(aircraft, vehicle, it->second, + safeDistance)) { it->second.resolved = true; Logger::debug("冲突已解除: obj1=", aircraft.id, - ", obj2=", vehicle.id); + ", obj2=", vehicle.id); } else { // 虽然当前没有检测到风险，但由于未满足解除条件，继续保持原有风险等级 currentCollisions.insert(collisionKey); - risks.push_back({ - aircraft.id, - vehicle.id, - it->second.maxLevel, - collisionResult.minDistance, - collisionResult.minDistance, - relativeSpeed, - {vx, vy}, - zoneType, - collisionResult.timeToCollision, - it->second.collisionPoint - }); - Logger::debug("保持原有风险等级: obj1=", aircraft.id, - ", obj2=", vehicle.id, - ", maxLevel=", static_cast(it->second.maxLevel), - ", minDistance=", collisionResult.minDistance, "m", - ", timeToCollision=", collisionResult.timeToCollision, "s"); + risks.push_back({ aircraft.id, + vehicle.id, + it->second.maxLevel, + collisionResult.minDistance, + collisionResult.minDistance, + relativeSpeed, + {vx, vy}, + zoneType, + collisionResult.timeToCollision, + it->second.collisionPoint }); + Logger::debug( + "保持原有风险等级: obj1=", aircraft.id, + ", obj2=", vehicle.id, + ", maxLevel=", static_cast(it->second.maxLevel), + ", minDistance=", collisionResult.minDistance, "m", + ", timeToCollision=", collisionResult.timeToCollision, + "s"); } } } } - + // 清理已解除的冲突记录 for (auto it = collisionRecords_.begin(); it != collisionRecords_.end();) { if (it->second.resolved) { @@ -233,70 +237,71 @@ std::vector CollisionDetector::detectCollisions() { ++it; } } - + return risks; } bool CollisionDetector::checkCollisionResolved(const MovingObject& obj1, - const MovingObject& obj2, - const CollisionRecord& record, - double safeDistance) const { + 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 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 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)); // 修正航向角 + 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); + 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"); + Logger::debug("航空器已通过交叉点，航空器距离=", dist1, + "m, 安全距离=", safeDistance, "m"); return dist1 >= safeDistance; } else { // 航空器还未通过交叉点，需要两者都在安全距离外且预计不会碰撞 - auto collisionResult = checkCollision(obj1, obj2, 30.0); // 使用30秒的预测窗口 + 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 ? "是" : "否"); - + + Logger::debug("航空器未通过交叉点，航空器距离=", dist1, + "m, 无人车距离=", dist2, "m, 安全距离=", safeDistance, + "m, 预测碰撞=", willCollide ? "是" : "否"); + return !willCollide && bothOutside; } } // 统一的风险评估函数 std::pair CollisionDetector::evaluateRisk( - const CollisionResult& collisionResult, - const Vector2D& position, - MovingObjectType type1, - MovingObjectType type2) const { - + const CollisionResult& collisionResult, const Vector2D& position, + MovingObjectType type1, MovingObjectType type2) const { + // 取区域配置 const auto& areaConfig = getCollisionParams(position); auto thresholds = areaConfig.getThresholds(type1, type2); - + RiskLevel level = RiskLevel::NONE; WarningZoneType zoneType = WarningZoneType::NONE; - + // 如果预测到碰撞，根据当前距离判断风险等级 if (collisionResult.willCollide) { // 当前距离小于警报距离 -> 危险等级 @@ -310,10 +315,9 @@ std::pair CollisionDetector::evaluateRisk( zoneType = WarningZoneType::WARNING; } } - + // 修改日志，添加物体类型信息 - Logger::debug( - "风险评估: 物体1类型=", static_cast(type1), + Logger::debug("风险评估: 物体1类型=", static_cast(type1), ", 物体2类型=", static_cast(type2), ", 当前距离=", collisionResult.distance, "m, 预测最小距离=", collisionResult.minDistance, @@ -321,55 +325,60 @@ std::pair CollisionDetector::evaluateRisk( "m, 警报阈值=", thresholds.critical, "m, 预测碰撞=", collisionResult.willCollide ? "是" : "否", ", 风险等级=", static_cast(level), - ", 区域类型=", static_cast(zoneType) - ); - - return {level, zoneType}; + ", 区域类型=", static_cast(zoneType)); + + return { level, zoneType }; } // 统一的碰撞检测函数 -CollisionResult CollisionDetector::checkCollision( - const MovingObject& obj1, +CollisionResult CollisionDetector::checkCollision(const MovingObject& obj1, const MovingObject& obj2, double timeWindow) const { - + // 添加物体信息日志 - Logger::debug( - "检查碰撞: obj1[", obj1.id, "]=(", obj1.position.x, ",", obj1.position.y, - "), obj2[", obj2.id, "]=(", obj2.position.x, ",", obj2.position.y, ")" - ); - + Logger::debug("检查碰撞: obj1[", obj1.id, "]=(", obj1.position.x, ",", + obj1.position.y, "), obj2[", obj2.id, "]=(", obj2.position.x, + ",", obj2.position.y, ")"); + // 获取区域配置 const auto& areaConfig = getCollisionParams(obj1.position); - + // 获取各自的碰撞半径和警告半径 double radius1 = areaConfig.collision_radius.getRadius(obj1.type); double radius2 = areaConfig.collision_radius.getRadius(obj2.type); - double warning_radius1 = areaConfig.warning_zone_radius.getRadius(obj1.type); - double warning_radius2 = areaConfig.warning_zone_radius.getRadius(obj2.type); - + double warning_radius1 = + areaConfig.warning_zone_radius.getRadius(obj1.type); + double warning_radius2 = + areaConfig.warning_zone_radius.getRadius(obj2.type); + // 计算当前距离 double dx = obj2.position.x - obj1.position.x; double dy = obj2.position.y - obj1.position.y; - double current_distance = std::sqrt(dx*dx + dy*dy); - + double current_distance = std::sqrt(dx * dx + dy * dy); + // 如果是静止的无人车，使用基础速度 double speed1_calc = obj1.speed; double speed2_calc = obj2.speed; - + if (obj1.type == MovingObjectType::UNMANNED && speed1_calc < 0.1) { - speed1_calc = SystemConfig::instance().collision_detection.prediction.min_unmanned_speed; // 使用配置的最低速度 + speed1_calc = SystemConfig::instance() + .collision_detection.prediction + .min_unmanned_speed; // 使用配置的最低速度 } if (obj2.type == MovingObjectType::UNMANNED && speed2_calc < 0.1) { - speed2_calc = SystemConfig::instance().collision_detection.prediction.min_unmanned_speed; // 使用配置的最低速度 + speed2_calc = SystemConfig::instance() + .collision_detection.prediction + .min_unmanned_speed; // 使用配置的最低速度 } // 计算速度分量 - double vx1 = speed1_calc * std::cos((90.0 - obj1.heading) * M_PI / 180.0); // x 方向 - double vy1 = speed1_calc * std::sin((90.0 - obj1.heading) * M_PI / 180.0); // y 方向 + double vx1 = + speed1_calc * std::cos((90.0 - obj1.heading) * M_PI / 180.0); // x 方向 + double vy1 = + speed1_calc * std::sin((90.0 - obj1.heading) * M_PI / 180.0); // y 方向 double vx2 = speed2_calc * std::cos((90.0 - obj2.heading) * M_PI / 180.0); double vy2 = speed2_calc * std::sin((90.0 - obj2.heading) * M_PI / 180.0); - + // 如果当前距离大于警告区域，直接返回不会碰撞 if (current_distance > (warning_radius1 + warning_radius2)) { CollisionResult result; @@ -378,54 +387,52 @@ CollisionResult CollisionDetector::checkCollision( result.distance = current_distance; return result; } - + // 否则进行碰撞检测 - return predictCircleBasedCollision( - obj1.position, radius1, obj1.speed, obj1.heading, - obj2.position, radius2, obj2.speed, obj2.heading, - timeWindow - ); + return predictCircleBasedCollision(obj1.position, radius1, obj1.speed, + obj1.heading, obj2.position, radius2, + obj2.speed, obj2.heading, timeWindow); } CollisionResult CollisionDetector::predictCircleBasedCollision( const Vector2D& pos1, double radius1, double speed1, double heading1, const Vector2D& pos2, double radius2, double speed2, double heading2, double timeWindow) const { - + CollisionResult result; - + // 计算安全距离（两个圆的半径之和） double safe_distance = radius1 + radius2; - + // 1. 计算当前距离 double dx = pos2.x - pos1.x; double dy = pos2.y - pos1.y; - double current_distance = std::sqrt(dx*dx + dy*dy); - + double current_distance = std::sqrt(dx * dx + dy * dy); + // 初始化最小距离和当前距离 result.minDistance = current_distance; result.distance = 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 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度 + angle = 2 * M_PI - angle; // 确保夹角不超过180度 } - + // 4. 确定碰撞类型 if (speed1 < 0.1 && speed2 < 0.1) { result.type = CollisionType::STATIC; @@ -434,82 +441,80 @@ CollisionResult CollisionDetector::predictCircleBasedCollision( 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.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 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); + 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) - }; - + 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) - }; - + 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); + (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) { // 放宽交叉路径的角度范围 + } 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 - ); + + 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; @@ -521,90 +526,72 @@ CollisionResult CollisionDetector::predictCircleBasedCollision( double det2 = vx1 * dy - dx * vy1; // 计算时间 - double t1 = det1 / det; // 物体1的时间 - double t2 = det2 / det; // 物体2的时间 + double t1 = det1 / det; // 物体1的时间 + double t2 = det2 / det; // 物体2的时间 - Logger::debug( - "交叉路径检测: det1=", det1, - ", det2=", det2, - ", det=", det, - ", t1=", t1, - ", t2=", t2 - ); + Logger::debug("交叉路径检测: det1=", det1, ", det2=", det2, + ", det=", det, ", t1=", t1, ", t2=", t2); // 计算相交点 - Vector2D intersection = { - pos1.x + vx1 * t1, - pos1.y + vy1 * t1 - }; + Vector2D intersection = { pos1.x + vx1 * t1, pos1.y + vy1 * t1 }; - Logger::debug( - "交叉路径检测: intersection=(", intersection.x, ",", intersection.y, ")", - ", t1=", t1, - ", t2=", t2 - ); + 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); + 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; - + 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 - }; - + 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 - }; + (pos1_at_collision.y + pos2_at_collision.y) / 2.0 }; - Logger::debug( - "碰撞时间计算: first_arrival_time=", first_arrival_time, - ", safe_distance=", safe_distance, + Logger::debug("碰撞时间计算: first_arrival_time=", + first_arrival_time, ", safe_distance=", safe_distance, ", collision_distance=", collision_distance, - ", collision_time=", collision_time - ); + ", 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, ")" - ); - + // 更新物体状态 + 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 { @@ -617,27 +604,25 @@ 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); - + 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, + + Logger::debug("平行运动检测: angle_diff=", angle_diff, ", heading1=", heading1, ", rotation_angle=", rotation_angle * 180.0 / M_PI, - ", dx=", dx, - ", dy=", dy, - ", dx_rotated=", dx_rotated, + ", dx=", dx, ", dy=", dy, ", dx_rotated=", dx_rotated, ", dy_rotated=", dy_rotated, ", lateral_distance=", lateral_distance, ", safe_distance=", safe_distance, - ", rel_speed=", rel_speed - ); - + ", rel_speed=", rel_speed); + // 如果横向距离大于等于安全距离，且相对速度很小，则不会碰撞 if (lateral_distance >= safe_distance && rel_speed < 1.0) { result.willCollide = false; @@ -645,60 +630,52 @@ CollisionResult CollisionDetector::predictCircleBasedCollision( } } } - + // 5. 在时间窗口内预测碰撞 - const int STEPS = 120; // 增加采样点数以提高精度 - double dt = timeWindow / STEPS; // 时间步长 - + 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 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; result.minDistance = current_distance; result.timeToMinDistance = 0.0; - result.collisionPoint = { - (pos1.x + pos2.x) / 2.0, - (pos1.y + pos2.y) / 2.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 prev_distance = current_distance; - Logger::debug( - "开始时间窗口采样: STEPS=", STEPS, - ", dt=", dt, + Logger::debug("开始时间窗口采样: STEPS=", STEPS, ", dt=", dt, ", timeWindow=", timeWindow, ", safe_distance=", safe_distance, - ", current_distance=", current_distance - ); + ", current_distance=", current_distance); for (int i = 1; i <= STEPS; ++i) { double t = i * dt; - + // 计算t时的位置（使用修正后的速度分量） - Vector2D future1 = { - pos1.x + vx1_sample * t, - pos1.y + vy1_sample * t - }; - - Vector2D future2 = { - pos2.x + vx2_sample * t, - pos2.y + vy2_sample * t - }; - + Vector2D future1 = { pos1.x + vx1_sample * t, pos1.y + vy1_sample * t }; + + 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); // 统一使用实际距离 - + double distance = + std::sqrt(dx_t * dx_t + dy_t * dy_t); // 统一使用实际距离 + // 更新小距离 if (distance < result.minDistance) { result.minDistance = distance; @@ -708,45 +685,38 @@ CollisionResult CollisionDetector::predictCircleBasedCollision( // ", time_to_min=", t // ); } - + // 检查是否会碰撞 if (distance <= safe_distance) { result.willCollide = true; // 使用当前距离和安全距离做插值，提高精确度 - double progress = (distance - safe_distance) / (prev_distance - safe_distance); + 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, ")" - ); - + + 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); + 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) { @@ -754,6 +724,6 @@ CollisionResult CollisionDetector::predictCircleBasedCollision( } } } - + return result; -} \ No newline at end of file +} \ No newline at end of file diff --git a/src/detector/SafetyZone.cpp b/src/detector/SafetyZone.cpp index 0cb4372..620172c 100644 --- a/src/detector/SafetyZone.cpp +++ b/src/detector/SafetyZone.cpp @@ -10,10 +10,10 @@ SafetyZone::SafetyZone(const Vector2D& center, double aircraftRadius, double spe , currentRadius_(0.0) , state_(SafetyZoneState::INACTIVE) , type_(SafetyZoneType::NONE) { - - Logger::debug("创建安全区: center=(", center_.x, ",", center_.y, - "), aircraftRadius=", aircraftRadius_, - ", specialVehicleRadius=", specialVehicleRadius_); + + Logger::debug("创建安全区: center=(", center_.x, ",", center_.y, + "), aircraftRadius=", aircraftRadius_, + ", specialVehicleRadius=", specialVehicleRadius_); } bool SafetyZone::isInZone(const Vector2D& point) const { @@ -21,12 +21,12 @@ bool SafetyZone::isInZone(const Vector2D& point) const { 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_; } @@ -56,20 +56,20 @@ bool SafetyZone::isTypeMatched(const MovingObject& object) const { if (type_ == SafetyZoneType::NONE) { return true; } - + // 如果是无人车，返回 true if (object.isUnmannedVehicle()) { return true; } - + // 如果安全区类型已设置，检查是否匹配 if ((type_ == SafetyZoneType::AIRCRAFT && !object.isAircraft()) || (type_ == SafetyZoneType::VEHICLE && !object.isSpecialVehicle())) { - Logger::debug("目标 ", object.id, " 类型与安全区类型不匹配，当前安全区类型: ", - type_ == SafetyZoneType::AIRCRAFT ? "飞机" : "特勤车"); + Logger::debug("目标 ", object.id, " 类型与安全区类型不匹配，当前安全区类型: ", + type_ == SafetyZoneType::AIRCRAFT ? "飞机" : "特勤车"); return false; } - + return true; } @@ -78,20 +78,20 @@ bool SafetyZone::isObjectInZone(const MovingObject& object) const { double dx = object.position.x - center_.x; double dy = object.position.y - center_.y; double distance = std::sqrt(dx * dx + dy * dy); - + // 如果安全区类型未设置，使用对应的临时半径进行检查 - double checkRadius = (type_ == SafetyZoneType::NONE) + double checkRadius = (type_ == SafetyZoneType::NONE) ? (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, ")"); - + "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; } @@ -101,25 +101,25 @@ bool SafetyZone::tryActivate(const MovingObject& object) { Logger::debug("目标 ", object.id, " 是无人车，不激活安全区"); return false; } - + // 如果类型已设置且不匹配，不激活 if (!isTypeMatched(object)) { return false; } - + // 检查是否在区内 bool inZone = isObjectInZone(object); if (!inZone) { return false; } - + // 如果安全区类型未设置，设置新的类型和半径 if (type_ == SafetyZoneType::NONE) { type_ = object.isAircraft() ? SafetyZoneType::AIRCRAFT : SafetyZoneType::VEHICLE; currentRadius_ = object.isAircraft() ? aircraftRadius_ : specialVehicleRadius_; Logger::debug("安全区设置为", object.isAircraft() ? "飞机" : "特勤车", "类型，半径: ", currentRadius_); } - + // 激活安全区 state_ = SafetyZoneState::ACTIVE; return true; diff --git a/src/detector/TrafficLightDetector.cpp b/src/detector/TrafficLightDetector.cpp index 62ddf4b..b6ff766 100644 --- a/src/detector/TrafficLightDetector.cpp +++ b/src/detector/TrafficLightDetector.cpp @@ -5,15 +5,14 @@ #include TrafficLightDetector::TrafficLightDetector(const IntersectionConfig& intersectionConfig, - ControllableVehicles& controllableVehicles, - System& system) + ControllableVehicles& controllableVehicles, + System& system) : intersection_config_(intersectionConfig) , controllable_vehicles_(controllableVehicles) - , system_(system) { -} + , system_(system) {} -void TrafficLightDetector::processSignal(const TrafficLightSignal& signal, - const std::vector& vehicles) { +void TrafficLightDetector::processSignal(const TrafficLightSignal& signal, + const std::vector& vehicles) { // 根据红绿灯ID查找对应的路口 const Intersection* intersection = intersection_config_.findByTrafficLightId(signal.trafficLightId); if (!intersection) { @@ -29,18 +28,18 @@ void TrafficLightDetector::processSignal(const TrafficLightSignal& signal, if (controllable_vehicles_.isControllable(vehicle.vehicleNo)) { // 根据信号灯状态发送指令 switch (signal.status) { - case SignalStatus::RED: - sendSignalCommand(vehicle.vehicleNo, SignalState::RED); - break; - case SignalStatus::GREEN: - sendSignalCommand(vehicle.vehicleNo, SignalState::GREEN); - break; - case SignalStatus::YELLOW: - sendSignalCommand(vehicle.vehicleNo, SignalState::YELLOW); - break; - default: - Logger::warning("未知的信号灯状态"); - break; + case SignalStatus::RED: + sendSignalCommand(vehicle.vehicleNo, SignalState::RED); + break; + case SignalStatus::GREEN: + sendSignalCommand(vehicle.vehicleNo, SignalState::GREEN); + break; + case SignalStatus::YELLOW: + sendSignalCommand(vehicle.vehicleNo, SignalState::YELLOW); + break; + default: + Logger::warning("未知的信号灯状态"); + break; } } } @@ -65,7 +64,7 @@ void TrafficLightDetector::sendSignalCommand(const std::string& vehicleNo, Signa controllable_vehicles_.sendCommand(vehicleNo, cmd); system_.broadcastVehicleCommand(cmd); - Logger::debug("发送信号灯指令到车辆: ", vehicleNo, - " 路口: ", cmd.intersectionId, - " 状态: ", state == SignalState::RED ? "红灯" : state == SignalState::GREEN ? "绿灯" : "黄灯"); + Logger::debug("发送信号灯指令到车辆: ", 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 e236a1e..a1408f0 100644 --- a/src/network/HTTPClient.cpp +++ b/src/network/HTTPClient.cpp @@ -3,8 +3,8 @@ #include namespace { -std::string getSignalStateString(SignalState state) { - switch (state) { + std::string getSignalStateString(SignalState state) { + switch (state) { case SignalState::RED: return "RED"; case SignalState::YELLOW: @@ -13,8 +13,8 @@ std::string getSignalStateString(SignalState state) { return "GREEN"; default: return "UNKNOWN"; + } } -} } // anonymous namespace HTTPClient::HTTPClient() { @@ -116,14 +116,14 @@ bool HTTPClient::sendCommand(const std::string& ip, int port, const VehicleComma // 检查响应状态码 long http_code = 0; curl_easy_getinfo(curl_, CURLINFO_RESPONSE_CODE, &http_code); - + if (http_code == 200) { try { // 解析响应 nlohmann::json response = nlohmann::json::parse(response_buffer_); int code = response["code"].get(); std::string msg = response["msg"].get(); - + if (code == 200) { Logger::debug("Command sent successfully: ", request_body); return true; diff --git a/src/network/HTTPDataSource.cpp b/src/network/HTTPDataSource.cpp index c36785f..7582369 100644 --- a/src/network/HTTPDataSource.cpp +++ b/src/network/HTTPDataSource.cpp @@ -59,37 +59,36 @@ bool HTTPDataSource::tryReconnect() { if (is_reconnecting_) { return false; } - + is_reconnecting_ = true; - + try { auto now = std::chrono::steady_clock::now(); auto elapsed = std::chrono::duration_cast( now - last_connect_attempt_).count(); - + if (elapsed < config_.position.timeout_ms) { is_reconnecting_ = false; return false; } - + last_connect_attempt_ = now; - + for (int retry = 0; retry < MAX_RETRIES; ++retry) { if (connect()) { is_reconnecting_ = false; return true; } - + if (retry < MAX_RETRIES - 1) { - std::this_thread::sleep_for(std::chrono::milliseconds(config_.position.timeout_ms/3)); + std::this_thread::sleep_for(std::chrono::milliseconds(config_.position.timeout_ms / 3)); } } - + Logger::error("Failed to reconnect after ", MAX_RETRIES, " attempts"); is_reconnecting_ = false; return false; - } - catch (const std::exception& e) { + } catch (const std::exception& e) { Logger::error("Error during reconnection: ", e.what()); is_reconnecting_ = false; return false; @@ -100,11 +99,11 @@ bool HTTPDataSource::checkConnectionHealth() { auto now = std::chrono::steady_clock::now(); auto elapsed = std::chrono::duration_cast( now - last_health_check_).count(); - + if (elapsed < HEALTH_CHECK_INTERVAL) { return true; } - + last_health_check_ = now; return curl_ != nullptr; } @@ -113,17 +112,17 @@ bool HTTPDataSource::ensureConnected() { if (isAvailable() && checkConnectionHealth()) { return true; } - + return tryReconnect(); } bool HTTPDataSource::sendRequest(const std::string& url, const AuthState* auth_state, - std::string& response, HttpMethod method, const std::string& body) { + std::string& response, HttpMethod method, const std::string& body) { if (!curl_) { Logger::error("CURL not initialized"); return false; } - + curl_easy_reset(curl_); curl_easy_setopt(curl_, CURLOPT_URL, url.c_str()); curl_easy_setopt(curl_, CURLOPT_WRITEFUNCTION, WriteCallback); @@ -131,7 +130,7 @@ bool HTTPDataSource::sendRequest(const std::string& url, const AuthState* auth_s curl_easy_setopt(curl_, CURLOPT_TIMEOUT, config_.position.timeout_ms / 1000); curl_easy_setopt(curl_, CURLOPT_CONNECTTIMEOUT, config_.position.timeout_ms / 1000); curl_easy_setopt(curl_, CURLOPT_NOSIGNAL, 1L); - + if (method == HttpMethod::GET) { curl_easy_setopt(curl_, CURLOPT_HTTPGET, 1L); curl_easy_setopt(curl_, CURLOPT_POST, 0L); @@ -144,64 +143,64 @@ bool HTTPDataSource::sendRequest(const std::string& url, const AuthState* auth_s curl_easy_setopt(curl_, CURLOPT_POSTFIELDS, ""); } } - + struct curl_slist* headers = nullptr; headers = curl_slist_append(headers, "Accept: application/json"); headers = curl_slist_append(headers, "Cache-Control: no-cache"); headers = curl_slist_append(headers, "Expect:"); - + if (method == HttpMethod::POST) { headers = curl_slist_append(headers, "Content-Type: application/json"); } - + if (auth_state && auth_state->is_authenticated && !auth_state->token.empty()) { std::string auth_header = "Authorization: " + auth_state->token; headers = curl_slist_append(headers, auth_header.c_str()); } - + curl_easy_setopt(curl_, CURLOPT_HTTPHEADER, headers); - + response.clear(); CURLcode res = curl_easy_perform(curl_); curl_slist_free_all(headers); - + if (res != CURLE_OK) { Logger::error("请求失败: ", curl_easy_strerror(res)); return false; } - + long http_code = 0; curl_easy_getinfo(curl_, CURLINFO_RESPONSE_CODE, &http_code); - + if (http_code != 200) { Logger::error("HTTP 请求失败，状态码: ", http_code); return false; } - + return true; } bool HTTPDataSource::fetchPositionAircraftData(std::vector& aircraft) { std::lock_guard lock(mutex_); - + if (!ensureConnected()) { Logger::error("连接失败"); return false; } - - if (!ensureAuthenticated(config_.position.auth, - config_.position.host, - config_.position.port, - position_auth_, - DataSourceType::POSITION)) { + + if (!ensureAuthenticated(config_.position.auth, + config_.position.host, + config_.position.port, + position_auth_, + DataSourceType::POSITION)) { Logger::error("认证失败"); return false; } - - std::string url = "http://" + config_.position.host + ":" + - std::to_string(config_.position.port) + - config_.position.aircraft_path; - + + std::string url = "http://" + config_.position.host + ":" + + std::to_string(config_.position.port) + + config_.position.aircraft_path; + std::string response; if (!sendRequest(url, &position_auth_, response, HttpMethod::GET)) { Logger::error("请求失败"); @@ -213,23 +212,23 @@ bool HTTPDataSource::fetchPositionAircraftData(std::vector& aircraft) bool HTTPDataSource::fetchPositionVehicleData(std::vector& vehicles) { std::lock_guard lock(mutex_); - + if (!ensureConnected()) { return false; } - - if (!ensureAuthenticated(config_.position.auth, - config_.position.host, - config_.position.port, - position_auth_, - DataSourceType::POSITION)) { + + if (!ensureAuthenticated(config_.position.auth, + config_.position.host, + config_.position.port, + position_auth_, + DataSourceType::POSITION)) { return false; } - - std::string url = "http://" + config_.position.host + ":" + - std::to_string(config_.position.port) + - config_.position.vehicle_path; - + + std::string url = "http://" + config_.position.host + ":" + + std::to_string(config_.position.port) + + config_.position.vehicle_path; + std::string response; if (!sendRequest(url, &position_auth_, response, HttpMethod::GET)) { return false; @@ -240,23 +239,23 @@ bool HTTPDataSource::fetchPositionVehicleData(std::vector& vehicles) { bool HTTPDataSource::fetchTrafficLightSignals(std::vector& signals) { std::lock_guard lock(mutex_); - + if (!ensureConnected()) { return false; } - - if (!ensureAuthenticated(config_.traffic_light.auth, - config_.traffic_light.host, - config_.traffic_light.port, - traffic_light_auth_, - DataSourceType::TRAFFIC_LIGHT)) { + + if (!ensureAuthenticated(config_.traffic_light.auth, + config_.traffic_light.host, + config_.traffic_light.port, + traffic_light_auth_, + DataSourceType::TRAFFIC_LIGHT)) { return false; } - - std::string url = "http://" + config_.traffic_light.host + ":" + - std::to_string(config_.traffic_light.port) + - config_.traffic_light.signal_path; - + + std::string url = "http://" + config_.traffic_light.host + ":" + + std::to_string(config_.traffic_light.port) + + config_.traffic_light.signal_path; + std::string response; if (!sendRequest(url, &traffic_light_auth_, response, HttpMethod::GET)) { return false; @@ -267,39 +266,39 @@ bool HTTPDataSource::fetchTrafficLightSignals(std::vector& s std::string getSignalStateString(SignalState state) { switch (state) { - case SignalState::RED: - return "RED"; - case SignalState::YELLOW: - return "YELLOW"; - case SignalState::GREEN: - return "GREEN"; - default: - return "GREEN"; // 默认为绿灯 + case SignalState::RED: + return "RED"; + case SignalState::YELLOW: + return "YELLOW"; + case SignalState::GREEN: + return "GREEN"; + default: + return "GREEN"; // 默认为绿灯 } } bool HTTPDataSource::sendUnmannedVehicleCommand(const std::string& vehicle_id, const VehicleCommand& command) { std::lock_guard lock(mutex_); - + if (!ensureConnected()) { return false; } - - if (!ensureAuthenticated(config_.vehicle.auth, - config_.vehicle.host, - config_.vehicle.port, - unmanned_vehicle_auth_, - DataSourceType::UNMANNED)) { + + if (!ensureAuthenticated(config_.vehicle.auth, + config_.vehicle.host, + config_.vehicle.port, + unmanned_vehicle_auth_, + DataSourceType::UNMANNED)) { return false; } - - std::string url = "http://" + config_.vehicle.host + ":" + - std::to_string(config_.vehicle.port) + - config_.vehicle.command_path; - + + std::string url = "http://" + config_.vehicle.host + ":" + + std::to_string(config_.vehicle.port) + + config_.vehicle.command_path; + std::stringstream transId; transId << std::hex << std::chrono::system_clock::now().time_since_epoch().count(); - + nlohmann::json request = { {"transId", transId.str()}, {"timestamp", command.timestamp}, @@ -335,27 +334,26 @@ bool HTTPDataSource::sendUnmannedVehicleCommand(const std::string& vehicle_id, c request["relativeMotionY"] = command.relativeMotionY; request["minDistance"] = command.minDistance; } - + std::string response; if (!sendRequest(url, &unmanned_vehicle_auth_, response, HttpMethod::POST, request.dump())) { return false; } - + try { nlohmann::json j = nlohmann::json::parse(response); if (!j.contains("code") || !j.contains("msg")) { Logger::error("Invalid command response format"); return false; } - + if (j["code"].get() != 200) { Logger::error("Command failed: ", j["msg"].get()); return false; } - + return true; - } - catch (const std::exception& e) { + } catch (const std::exception& e) { Logger::error("Failed to parse command response: ", e.what()); return false; } @@ -363,73 +361,72 @@ bool HTTPDataSource::sendUnmannedVehicleCommand(const std::string& vehicle_id, c bool HTTPDataSource::fetchUnmannedVehicleStatus(const std::string& vehicle_id, std::string& status) { std::lock_guard lock(mutex_); - + if (!ensureConnected()) { return false; } - - if (!ensureAuthenticated(config_.vehicle.auth, - config_.vehicle.host, - config_.vehicle.port, - unmanned_vehicle_auth_, - DataSourceType::UNMANNED)) { + + if (!ensureAuthenticated(config_.vehicle.auth, + config_.vehicle.host, + config_.vehicle.port, + unmanned_vehicle_auth_, + DataSourceType::UNMANNED)) { return false; } - - std::string url = "http://" + config_.vehicle.host + ":" + - std::to_string(config_.vehicle.port) + - config_.vehicle.status_path + "?vehicleId=" + vehicle_id; - + + std::string url = "http://" + config_.vehicle.host + ":" + + std::to_string(config_.vehicle.port) + + config_.vehicle.status_path + "?vehicleId=" + vehicle_id; + std::string response; if (!sendRequest(url, &unmanned_vehicle_auth_, response, HttpMethod::GET)) { return false; } - + try { nlohmann::json j = nlohmann::json::parse(response); if (!j.contains("status") || !j.contains("data")) { Logger::error("Invalid vehicle status response format"); return false; } - + if (j["status"].get() != 200) { Logger::error("Failed to get vehicle status"); return false; } - + status = j["data"].dump(); return true; - } - catch (const std::exception& e) { + } catch (const std::exception& e) { Logger::error("Failed to parse vehicle status response: ", e.what()); return false; } } -bool HTTPDataSource::authenticatePosition(const AuthConfig& auth_config, +bool HTTPDataSource::authenticatePosition(const AuthConfig& auth_config, const std::string& host, int port, AuthState& auth_state) { if (!auth_config.auth_required) { auth_state.is_authenticated = true; return true; } - + std::lock_guard lock(auth_mutex_); - + if (!curl_) { Logger::error("CURL not initialized"); return false; } - - std::string url = "http://" + host + ":" + std::to_string(port) + - auth_config.auth_path + "?username=" + auth_config.username + - "&password=" + auth_config.password; - + + std::string url = "http://" + host + ":" + std::to_string(port) + + auth_config.auth_path + "?username=" + auth_config.username + + "&password=" + auth_config.password; + std::string response; if (!sendRequest(url, nullptr, response, HttpMethod::POST, "")) { Logger::error("认证请求失败"); return false; } - + try { nlohmann::json j = nlohmann::json::parse(response); if (!j.contains("status") || !j.contains("msg") || !j.contains("data")) { @@ -449,46 +446,45 @@ bool HTTPDataSource::authenticatePosition(const AuthConfig& auth_config, auth_state.token = token; auth_state.is_authenticated = true; auth_state.last_auth_time = std::chrono::steady_clock::now(); - + return true; - } - catch (const std::exception& e) { + } catch (const std::exception& e) { Logger::error("解析认证响应失败: ", e.what(), ", 响应内容: ", response); return false; } } -bool HTTPDataSource::authenticateUnmanned(const AuthConfig& auth_config, +bool HTTPDataSource::authenticateUnmanned(const AuthConfig& auth_config, const std::string& host, int port, AuthState& auth_state) { auth_state.is_authenticated = true; return true; } -bool HTTPDataSource::authenticateTrafficLight(const AuthConfig& auth_config, +bool HTTPDataSource::authenticateTrafficLight(const AuthConfig& auth_config, const std::string& host, int port, AuthState& auth_state) { auth_state.is_authenticated = true; return true; } bool HTTPDataSource::ensureAuthenticated(const AuthConfig& auth_config, const std::string& host, - int port, AuthState& auth_state, DataSourceType type) { + int port, AuthState& auth_state, DataSourceType type) { if (!auth_config.auth_required) { return true; } - + if (!auth_state.is_authenticated) { - switch(type) { - case DataSourceType::POSITION: - return authenticatePosition(auth_config, host, port, auth_state); - case DataSourceType::UNMANNED: - return authenticateUnmanned(auth_config, host, port, auth_state); - case DataSourceType::TRAFFIC_LIGHT: - return authenticateTrafficLight(auth_config, host, port, auth_state); - default: - return false; + switch (type) { + case DataSourceType::POSITION: + return authenticatePosition(auth_config, host, port, auth_state); + case DataSourceType::UNMANNED: + return authenticateUnmanned(auth_config, host, port, auth_state); + case DataSourceType::TRAFFIC_LIGHT: + return authenticateTrafficLight(auth_config, host, port, auth_state); + default: + return false; } } - + return true; } @@ -499,12 +495,12 @@ bool HTTPDataSource::parsePositionAircraftResponse(const std::string& response, Logger::error("Invalid aircraft response format"); return false; } - + if (j["status"].get() != 200) { Logger::error("Failed to get aircraft data: ", j["msg"].get()); return false; } - + const auto& data = j["data"]; aircraft.clear(); for (const auto& item : data) { @@ -514,7 +510,7 @@ bool HTTPDataSource::parsePositionAircraftResponse(const std::string& response, ac.geo.longitude = item["longitude"].get(); ac.geo.latitude = item["latitude"].get(); ac.timestamp = item["time"].get(); - + if (item.contains("altitude")) { ac.altitude = item["altitude"].get(); } @@ -524,8 +520,7 @@ bool HTTPDataSource::parsePositionAircraftResponse(const std::string& response, aircraft.push_back(ac); } return true; - } - catch (const std::exception& e) { + } catch (const std::exception& e) { Logger::error("Failed to parse aircraft response: ", e.what()); return false; } @@ -538,12 +533,12 @@ bool HTTPDataSource::parsePositionVehicleResponse(const std::string& response, s Logger::error("Invalid vehicle response format"); return false; } - + if (j["status"].get() != 200) { Logger::error("Failed to get vehicle data: ", j["msg"].get()); return false; } - + const auto& data = j["data"]; vehicles.clear(); for (const auto& item : data) { @@ -553,7 +548,7 @@ bool HTTPDataSource::parsePositionVehicleResponse(const std::string& response, s vehicle.geo.longitude = item["longitude"].get(); vehicle.geo.latitude = item["latitude"].get(); vehicle.timestamp = item["time"].get(); - + if (item.contains("direction")) { vehicle.heading = item["direction"].get(); } @@ -563,8 +558,7 @@ bool HTTPDataSource::parsePositionVehicleResponse(const std::string& response, s vehicles.push_back(vehicle); } return true; - } - catch (const std::exception& e) { + } catch (const std::exception& e) { Logger::error("Failed to parse vehicle response: ", e.what()); return false; } @@ -577,12 +571,12 @@ bool HTTPDataSource::parseTrafficLightResponse(const std::string& response, std: Logger::error("Invalid traffic light response format"); return false; } - + if (j["status"].get() != 200) { Logger::error("Failed to get traffic light data: ", j["msg"].get()); return false; } - + const auto& data = j["data"]; signals.clear(); for (const auto& item : data) { @@ -591,12 +585,12 @@ bool HTTPDataSource::parseTrafficLightResponse(const std::string& response, std: signal.status = [&]() { int state = item["state"].get(); switch (state) { - case 0: return SignalStatus::RED; - case 1: return SignalStatus::GREEN; - case 2: return SignalStatus::YELLOW; - default: return SignalStatus::UNKNOWN; + case 0: return SignalStatus::RED; + case 1: return SignalStatus::GREEN; + case 2: return SignalStatus::YELLOW; + default: return SignalStatus::UNKNOWN; } - }(); + }(); signal.timestamp = item["timestamp"].get(); signal.intersectionId = item["intersection"].get(); signal.latitude = item["position"]["latitude"].get(); @@ -604,8 +598,7 @@ bool HTTPDataSource::parseTrafficLightResponse(const std::string& response, std: signals.push_back(signal); } return true; - } - catch (const std::exception& e) { + } catch (const std::exception& e) { Logger::error("Failed to parse traffic light response: ", e.what()); return false; } diff --git a/src/network/WebSocketServer.cpp b/src/network/WebSocketServer.cpp index dd18d23..a4a26aa 100644 --- a/src/network/WebSocketServer.cpp +++ b/src/network/WebSocketServer.cpp @@ -7,136 +7,135 @@ namespace network { -WebSocketServer::WebSocketServer(uint16_t port) - : acceptor_(ioc_, {boost::asio::ip::tcp::v4(), port}) { -} + WebSocketServer::WebSocketServer(uint16_t port) + : acceptor_(ioc_, { boost::asio::ip::tcp::v4(), port }) {} -WebSocketServer::~WebSocketServer() { - // 关闭所有连接 - std::lock_guard lock(sessions_mutex_); - for (auto& session : sessions_) { - try { - session.lock()->close(boost::beast::websocket::close_code::normal); - } catch (...) { - // 忽略关闭时的错误 - } - } - sessions_.clear(); - - // 停止 io_context - ioc_.stop(); -} - -void WebSocketServer::start() { - Logger::info("WebSocket 服务器启动，监听端口: ", acceptor_.local_endpoint().port()); - handleAccept(); - ioc_.run(); -} - -void WebSocketServer::broadcast(const std::string& message) { - std::lock_guard lock(sessions_mutex_); - auto it = sessions_.begin(); - int successCount = 0; - int failCount = 0; - - while (it != sessions_.end()) { - if (auto session = it->lock()) { // 获取 shared_ptr + WebSocketServer::~WebSocketServer() { + // 关闭所有连接 + std::lock_guard lock(sessions_mutex_); + for (auto& session : sessions_) { try { - session->write(boost::asio::buffer(message)); - ++successCount; - ++it; + session.lock()->close(boost::beast::websocket::close_code::normal); } catch (...) { - Logger::warning("广播消息到客户端失败"); - ++failCount; - it = sessions_.erase(it); // 移除失败的会话 + // 忽略关闭时的错误 } - } else { - Logger::debug("移除失效的会话"); - it = sessions_.erase(it); // 移除已失效的会话 } + sessions_.clear(); + + // 停止 io_context + ioc_.stop(); } - - Logger::debug("广播消息完成: ", successCount, " 个成功, ", failCount, " 个失败, 当前共 ", sessions_.size(), " 个连接"); -} -void WebSocketServer::handleAccept() { - acceptor_.async_accept( - [this](boost::system::error_code ec, boost::asio::ip::tcp::socket socket) { - if (!ec) { - Logger::info("接受新的 WebSocket 连接: ", socket.remote_endpoint().address().to_string()); - - // 创建新的 WebSocket 会话 - auto ws = std::make_shared>(std::move(socket)); - - // 异步完成 WebSocket 握手 - ws->async_accept( - [this, ws](boost::system::error_code ec) { - if (!ec) { - // 握手成功，保存会话并开始读取消息 - { - std::lock_guard lock(sessions_mutex_); - sessions_.push_back(ws); // 存储 weak_ptr - Logger::info("WebSocket 握手成功，当前连接数: ", sessions_.size()); - } - doRead(ws); - } else { - Logger::error("WebSocket 握手失败: ", ec.message()); - } - }); + void WebSocketServer::start() { + Logger::info("WebSocket 服务器启动，监听端口: ", acceptor_.local_endpoint().port()); + handleAccept(); + ioc_.run(); + } + + void WebSocketServer::broadcast(const std::string& message) { + std::lock_guard lock(sessions_mutex_); + auto it = sessions_.begin(); + int successCount = 0; + int failCount = 0; + + while (it != sessions_.end()) { + if (auto session = it->lock()) { // 获取 shared_ptr + try { + session->write(boost::asio::buffer(message)); + ++successCount; + ++it; + } catch (...) { + Logger::warning("广播消息到客户端失败"); + ++failCount; + it = sessions_.erase(it); // 移除失败的会话 + } } else { - Logger::error("接受 WebSocket 连接失败: ", ec.message()); + Logger::debug("移除失效的会话"); + it = sessions_.erase(it); // 移除已失效的会话 } - - // 继续接受新的连接 - handleAccept(); - }); -} + } -void WebSocketServer::doRead(std::shared_ptr> ws) { - // 为每个会话创建一个专用的缓冲区 - auto buffer = std::make_shared(); - - // 异步读取消息 - ws->async_read( - *buffer, - [this, ws, buffer = buffer](boost::system::error_code ec, std::size_t bytes_transferred) { - if (!ec) { - // 成功读取消息，继续读取下一条 - buffer->consume(buffer->size()); // 清空缓冲区 - doRead(ws); - } else { - // 发生错误或连接关闭，移除会话 - std::lock_guard lock(sessions_mutex_); - auto it = std::find_if(sessions_.begin(), sessions_.end(), - [ws](const std::weak_ptr>& weak) { - return !weak.expired() && weak.lock() == ws; - }); - if (it != sessions_.end()) { - sessions_.erase(it); + Logger::debug("广播消息完成: ", successCount, " 个成功, ", failCount, " 个失败, 当前共 ", sessions_.size(), " 个连接"); + } + + void WebSocketServer::handleAccept() { + acceptor_.async_accept( + [this](boost::system::error_code ec, boost::asio::ip::tcp::socket socket) { + if (!ec) { + Logger::info("接受新的 WebSocket 连接: ", socket.remote_endpoint().address().to_string()); + + // 创建新的 WebSocket 会话 + auto ws = std::make_shared>(std::move(socket)); + + // 异步完成 WebSocket 握手 + ws->async_accept( + [this, ws](boost::system::error_code ec) { + if (!ec) { + // 握手成功，保存会话并开始读取消息 + { + std::lock_guard lock(sessions_mutex_); + sessions_.push_back(ws); // 存储 weak_ptr + Logger::info("WebSocket 握手成功，当前连接数: ", sessions_.size()); + } + doRead(ws); + } else { + Logger::error("WebSocket 握手失败: ", ec.message()); + } + }); + } else { + Logger::error("接受 WebSocket 连接失败: ", ec.message()); + } + + // 继续接受新的连接 + handleAccept(); + }); + } + + void WebSocketServer::doRead(std::shared_ptr> ws) { + // 为每个会话创建一个专用的缓冲区 + auto buffer = std::make_shared(); + + // 异步读取消息 + ws->async_read( + *buffer, + [this, ws, buffer = buffer](boost::system::error_code ec, std::size_t bytes_transferred) { + if (!ec) { + // 成功读取消息，继续读取下一条 + buffer->consume(buffer->size()); // 清空缓冲区 + doRead(ws); + } else { + // 发生错误或连接关闭，移除会话 + std::lock_guard lock(sessions_mutex_); + auto it = std::find_if(sessions_.begin(), sessions_.end(), + [ws](const std::weak_ptr>& weak) { + return !weak.expired() && weak.lock() == ws; + }); + if (it != sessions_.end()) { + sessions_.erase(it); + } + } + }); + } + + void WebSocketServer::stop() { + running_ = false; + Logger::info("正在停止 WebSocket 服务器..."); + ioc_.stop(); // 停止 io_context + + // 关闭所有连接 + std::lock_guard lock(sessions_mutex_); + for (auto& session : sessions_) { + if (auto ws = session.lock()) { + try { + ws->close(boost::beast::websocket::close_code::normal); + Logger::debug("关闭 WebSocket 连接"); + } catch (...) { + Logger::warning("关闭 WebSocket 连接时发生错误"); } } - }); -} - -void WebSocketServer::stop() { - running_ = false; - Logger::info("正在停止 WebSocket 服务器..."); - ioc_.stop(); // 停止 io_context - - // 关闭所有连接 - std::lock_guard lock(sessions_mutex_); - for (auto& session : sessions_) { - if (auto ws = session.lock()) { - try { - ws->close(boost::beast::websocket::close_code::normal); - Logger::debug("关闭 WebSocket 连接"); - } catch (...) { - Logger::warning("关闭 WebSocket 连接时发生错误"); - } } + sessions_.clear(); + Logger::info("WebSocket 服务器已停止"); } - sessions_.clear(); - Logger::info("WebSocket 服务器已停止"); -} } // namespace network \ No newline at end of file diff --git a/src/vehicle/ControllableVehicles.cpp b/src/vehicle/ControllableVehicles.cpp index f1aef2f..2e1ec17 100644 --- a/src/vehicle/ControllableVehicles.cpp +++ b/src/vehicle/ControllableVehicles.cpp @@ -13,7 +13,7 @@ ControllableVehicles& ControllableVehicles::getInstance() { return *instance_; } -ControllableVehicles::ControllableVehicles(const std::string& configFile) +ControllableVehicles::ControllableVehicles(const std::string& configFile) : http_client_(std::make_unique()) { if (!configFile.empty()) { loadConfig(configFile); @@ -36,7 +36,7 @@ const ControllableVehicleConfig* ControllableVehicles::findVehicle(const std::st [&](const ControllableVehicleConfig& config) { return config.vehicleNo == vehicleNo; }); - + return iter != vehicles_.end() ? &(*iter) : nullptr; } @@ -89,7 +89,7 @@ bool ControllableVehicles::sendCommand(const std::string& vehicleNo, const Vehic try { bool success = http_client_->sendCommand(vehicle->ip, vehicle->port, command); if (success) { - Logger::info("Successfully sent command to vehicle ", vehicleNo, ": ", + Logger::info("Successfully sent command to vehicle ", vehicleNo, ": ", command.type == CommandType::SIGNAL ? "SIGNAL" : command.type == CommandType::ALERT ? "ALERT" : command.type == CommandType::WARNING ? "WARNING" : diff --git a/tests/AirportBoundsTest.cpp b/tests/AirportBoundsTest.cpp index c8a09dc..a5d5b32 100644 --- a/tests/AirportBoundsTest.cpp +++ b/tests/AirportBoundsTest.cpp @@ -29,15 +29,15 @@ protected: void SetUp() override { // 设置日志级别 Logger::setLogLevel(LogLevel::DEBUG); - + // 创建测试实例 bounds_ = std::make_unique(); - + // 设置基本测试数据 // 参考点设为原点 (0, 0) // 旋转角度设为 90 度，便于验证 bounds_->setTestData( - Vector2D{0, 0}, // 参考点 + Vector2D{ 0, 0 }, // 参考点 90.0, // 旋转角度（度） Bounds(-2000, -2000, 4000, 4000) // 4km x 4km 的区域 ); @@ -54,57 +54,57 @@ protected: // 测试坐标转换 - 参考点 TEST_F(AirportBoundsTest, ReferencePointTransformation) { // 参考点应该转换为原点 (0,0) - Vector2D result = bounds_->toAirportCoordinate(Vector2D{0, 0}); - EXPECT_TRUE(pointsAreClose(result, Vector2D{0, 0})) + Vector2D result = bounds_->toAirportCoordinate(Vector2D{ 0, 0 }); + EXPECT_TRUE(pointsAreClose(result, Vector2D{ 0, 0 })) << "参考点转换结果: (" << result.x << ", " << result.y << ")"; } // 测试坐标转换 - 基本位移（90度逆时针旋转） TEST_F(AirportBoundsTest, BasicTranslation) { // 向右移动 100 米，逆时针旋转 90 度后，应该在 y 轴正方向 - Vector2D result = bounds_->toAirportCoordinate(Vector2D{100, 0}); + Vector2D result = bounds_->toAirportCoordinate(Vector2D{ 100, 0 }); EXPECT_NEAR(result.x, 0, 0.001); EXPECT_NEAR(result.y, 100, 0.001) - << "向右100米的点逆时针旋转90度后应该在 (0, 100)，实际在: (" + << "向右100米的点逆时针旋转90度后应该在 (0, 100)，实际在: (" << result.x << ", " << result.y << ")"; - + // 向上移动 100 米，逆时针旋转 90 度后，应该在 x 轴负方向 - result = bounds_->toAirportCoordinate(Vector2D{0, 100}); + result = bounds_->toAirportCoordinate(Vector2D{ 0, 100 }); EXPECT_NEAR(result.x, -100, 0.001); EXPECT_NEAR(result.y, 0, 0.001) - << "向上100米的点逆时针旋转90度后应该在 (-100, 0)，实际在: (" + << "向上100米的点逆时针旋转90度后应该在 (-100, 0)，实际在: (" << result.x << ", " << result.y << ")"; } // 测试坐标转换 - 复合变换（90度逆时针旋转） TEST_F(AirportBoundsTest, CompositeTransformation) { // 向右上方移动 (100, 100)，逆时针旋转 90 度 - Vector2D result = bounds_->toAirportCoordinate(Vector2D{100, 100}); + Vector2D result = bounds_->toAirportCoordinate(Vector2D{ 100, 100 }); EXPECT_NEAR(result.x, -100, 0.001); EXPECT_NEAR(result.y, 100, 0.001) - << "点(100,100)逆时针旋转90度后应该在 (-100, 100)，实际在: (" + << "点(100,100)逆时针旋转90度后应该在 (-100, 100)，实际在: (" << result.x << ", " << result.y << ")"; } // 测试边界检查 - 边界内的点 TEST_F(AirportBoundsTest, PointInBounds) { // 测试参考点（一定在边界内） - EXPECT_TRUE(bounds_->isPointInBounds(Vector2D{0, 0})) + EXPECT_TRUE(bounds_->isPointInBounds(Vector2D{ 0, 0 })) << "参考点应该在边界内"; - + // 测试边界内的点 - EXPECT_TRUE(bounds_->isPointInBounds(Vector2D{1000, 1000})) + EXPECT_TRUE(bounds_->isPointInBounds(Vector2D{ 1000, 1000 })) << "距离参考点1km的点应该在边界内"; } // 测试边界检查 - 边界外的点 TEST_F(AirportBoundsTest, PointOutOfBounds) { // 测试边界外的点 - EXPECT_FALSE(bounds_->isPointInBounds(Vector2D{3000, 3000})) + EXPECT_FALSE(bounds_->isPointInBounds(Vector2D{ 3000, 3000 })) << "距离参考点3km的点应该在边界外"; - + // 测试另一个边界外的点 - EXPECT_FALSE(bounds_->isPointInBounds(Vector2D{-2500, -2500})) + EXPECT_FALSE(bounds_->isPointInBounds(Vector2D{ -2500, -2500 })) << "距离参考点2.5km的点应该在边界外"; } @@ -112,24 +112,24 @@ TEST_F(AirportBoundsTest, PointOutOfBounds) { TEST_F(AirportBoundsTest, DifferentRotations) { // 重新设置为45度逆时针旋转 bounds_->setTestData( - Vector2D{0, 0}, // 参考点 + Vector2D{ 0, 0 }, // 参考点 45.0, // 旋转角度（度） Bounds(-2000, -2000, 4000, 4000) ); - + // 向右移动 100 米，逆时针旋转 45 度 - Vector2D result = bounds_->toAirportCoordinate(Vector2D{100, 0}); + Vector2D result = bounds_->toAirportCoordinate(Vector2D{ 100, 0 }); double sqrt2_2 = std::sqrt(2.0) / 2.0; // cos(45°) = sin(45°) = √2/2 EXPECT_NEAR(result.x, 100 * sqrt2_2, 0.001); EXPECT_NEAR(result.y, 100 * sqrt2_2, 0.001) - << "向右100米的点逆时针旋转45度后应该在 (70.71, 70.71)，实际在: (" + << "向右100米的点逆时针旋转45度后应该在 (70.71, 70.71)，实际在: (" << result.x << ", " << result.y << ")"; - + // 向上移动 100 米，逆时针旋转 45 度 - result = bounds_->toAirportCoordinate(Vector2D{0, 100}); + result = bounds_->toAirportCoordinate(Vector2D{ 0, 100 }); EXPECT_NEAR(result.x, -100 * sqrt2_2, 0.001); EXPECT_NEAR(result.y, 100 * sqrt2_2, 0.001) - << "向上100米的点逆时针旋转45度后应该在 (-70.71, 70.71)，实际在: (" + << "向上100米的点逆时针旋转45度后应该在 (-70.71, 70.71)，实际在: (" << result.x << ", " << result.y << ")"; } @@ -137,33 +137,33 @@ TEST_F(AirportBoundsTest, DifferentRotations) { TEST_F(AirportBoundsTest, RotationTransformation) { // 重新设置为30度逆时针旋转 bounds_->setTestData( - Vector2D{0, 0}, // 参考点 + Vector2D{ 0, 0 }, // 参考点 30.0, // 旋转角度（度） Bounds(-2000, -2000, 4000, 4000) ); - + // 在世界坐标系中向右移动1000米，逆时针旋转 30 度 - Vector2D result = bounds_->toAirportCoordinate(Vector2D{1000, 0}); - double cos30 = std::cos(M_PI/6); // cos(30°) ≈ 0.866025 - double sin30 = std::sin(M_PI/6); // sin(30°) ≈ 0.5 - + Vector2D result = bounds_->toAirportCoordinate(Vector2D{ 1000, 0 }); + double cos30 = std::cos(M_PI / 6); // cos(30°) ≈ 0.866025 + double sin30 = std::sin(M_PI / 6); // sin(30°) ≈ 0.5 + // 在机场坐标系中的期望结果： // x = 1000 * cos(30°) ≈ 866.025 // y = 1000 * sin(30°) ≈ 500 EXPECT_NEAR(result.x, 1000 * cos30, 0.1); EXPECT_NEAR(result.y, 1000 * sin30, 0.1) - << "向右1000米的点逆时针旋转30度后应该在 (866.025, 500)，实际在: (" + << "向右1000米的点逆时针旋转30度后应该在 (866.025, 500)，实际在: (" << result.x << ", " << result.y << ")"; - + // 在世界坐标系中向上移动1000米，逆时针旋转 30 度 - result = bounds_->toAirportCoordinate(Vector2D{0, 1000}); - + result = bounds_->toAirportCoordinate(Vector2D{ 0, 1000 }); + // 在机场坐标系中的期望结果： // x = -1000 * sin(30°) ≈ -500 // y = 1000 * cos(30°) ≈ 866.025 EXPECT_NEAR(result.x, -1000 * sin30, 0.1); EXPECT_NEAR(result.y, 1000 * cos30, 0.1) - << "向上1000米的点逆时针旋转30度后应该在 (-500, 866.025)，实际在: (" + << "向上1000米的点逆时针旋转30度后应该在 (-500, 866.025)，实际在: (" << result.x << ", " << result.y << ")"; } @@ -171,27 +171,27 @@ TEST_F(AirportBoundsTest, RotationTransformation) { TEST_F(AirportBoundsTest, AreaCheck) { // 重新设置为0度旋转，便于测试 bounds_->setTestData( - Vector2D{0, 0}, // 参考点 + Vector2D{ 0, 0 }, // 参考点 0.0, // 旋转角度（度） Bounds(-2000, -2000, 4000, 4000) ); - + // 设置测试区域 bounds_->areaBounds_[AreaType::RUNWAY] = Bounds(-1000, -100, 2000, 200); bounds_->areaBounds_[AreaType::TAXIWAY] = Bounds(-800, -300, 1600, 600); - + // 测试跑道上的点 - Vector2D runwayPoint{500, 0}; // 跑道中心偏右500米 + Vector2D runwayPoint{ 500, 0 }; // 跑道中心偏右500米 EXPECT_TRUE(bounds_->isPointInArea(runwayPoint, AreaType::RUNWAY)) << "跑道上的点应该在跑道区域内"; - + // 测试滑行道上的点 - Vector2D taxiwayPoint{0, 200}; // 滑行道中心向上200米 + Vector2D taxiwayPoint{ 0, 200 }; // 滑行道中心向上200米 EXPECT_TRUE(bounds_->isPointInArea(taxiwayPoint, AreaType::TAXIWAY)) << "滑行道上的点应该在滑行道区域内"; - + // 测试区域外的点 - Vector2D outPoint{3000, 3000}; // 远离机场的点 + Vector2D outPoint{ 3000, 3000 }; // 远离机场的点 EXPECT_FALSE(bounds_->isPointInArea(outPoint, AreaType::RUNWAY)) << "远离机场的点不应该在任何区域内"; EXPECT_FALSE(bounds_->isPointInArea(outPoint, AreaType::TAXIWAY)) @@ -202,12 +202,12 @@ TEST_F(AirportBoundsTest, AreaCheck) { TEST_F(AirportBoundsTest, ConfigLoading) { // 使用项目中的配置文件 AirportBounds bounds("config/airport_bounds.json"); - + // 验证配置是否正确加载 const auto& airportBounds = bounds.getAirportBounds(); EXPECT_TRUE(airportBounds.width > 0 && airportBounds.height > 0) << "机场边界的宽度和高度应该为正值"; - + // 验证区域配置 const auto& runwayBounds = bounds.getAreaBounds(AreaType::RUNWAY); EXPECT_TRUE(runwayBounds.width > 0 && runwayBounds.height > 0) @@ -220,7 +220,7 @@ TEST_F(AirportBoundsTest, ConfigLoading) { TEST_F(AirportBoundsTest, ConfigErrorHandling) { // 测试文件不存在 EXPECT_THROW(AirportBounds("nonexistent.json"), std::runtime_error); - + // 测试配置文件格式错误 EXPECT_THROW(AirportBounds("tests/data/invalid_airport_bounds.json"), std::runtime_error); } @@ -229,18 +229,18 @@ TEST_F(AirportBoundsTest, ConfigErrorHandling) { TEST_F(AirportBoundsTest, EdgeCases) { // 重新设置为0度旋转，便于测试 bounds_->setTestData( - Vector2D{0, 0}, // 参考点 + Vector2D{ 0, 0 }, // 参考点 0.0, // 旋转角度（度） Bounds(-2000, -2000, 4000, 4000) ); - + // 测试正好在边界上的点 - Vector2D edge{2000, 2000}; // 右上角边界点 + Vector2D edge{ 2000, 2000 }; // 右上角边界点 EXPECT_TRUE(bounds_->isPointInBounds(edge)) << "边界上的点应该被认为在边界内"; - + // 测试边界外的点 - Vector2D outside{2001, 2001}; // 刚好超出边界 + Vector2D outside{ 2001, 2001 }; // 刚好超出边界 EXPECT_FALSE(bounds_->isPointInBounds(outside)) << "边界外的点应该被认为在边界外"; } \ No newline at end of file