From af458a0b169756dccdf7474e0b65165d0bdbb957 Mon Sep 17 00:00:00 2001 From: Tian jianyong <11429339@qq.com> Date: Fri, 13 Dec 2024 16:31:51 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86=20AABB=20=E7=AE=97?= =?UTF-8?q?=E6=B3=95=E7=9A=84=E7=A2=B0=E6=92=9E=E6=A3=80=E6=B5=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 1 - config/intersections.json | 2 +- config/system_config.json | 4 +- include/core/System.h | 76 --- src/core/System.cpp | 188 -------- src/core/System.h | 5 - src/detector/CollisionDetector.cpp | 76 ++- src/detector/CollisionDetector.h | 57 ++- src/detector/CollisionTypes.h | 37 ++ src/detector/RectangleCollision.h | 141 ++++++ src/detector/SimpleCollisionDetector.cpp | 202 -------- src/detector/SimpleCollisionDetector.h | 72 --- tests/CollisionDetectorTest.cpp | 562 +++++++++-------------- tools/map_websocket.html | 108 ++++- tools/mock_server.py | 224 +++++---- 15 files changed, 722 insertions(+), 1033 deletions(-) delete mode 100644 include/core/System.h create mode 100644 src/detector/CollisionTypes.h create mode 100644 src/detector/RectangleCollision.h delete mode 100644 src/detector/SimpleCollisionDetector.cpp delete mode 100644 src/detector/SimpleCollisionDetector.h diff --git a/CMakeLists.txt b/CMakeLists.txt index ba73f21..b5e7676 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,7 +49,6 @@ set(LIB_SOURCES src/collector/DataCollector.cpp src/core/System.cpp src/detector/CollisionDetector.cpp - src/detector/SimpleCollisionDetector.cpp src/network/HTTPDataSource.cpp src/network/WebSocketServer.cpp src/network/HTTPClient.cpp diff --git a/config/intersections.json b/config/intersections.json index 356e6ed..e924674 100644 --- a/config/intersections.json +++ b/config/intersections.json @@ -17,7 +17,7 @@ "name": "无人车与飞机交叉路口", "trafficLightId": "TL002", "position": { - "longitude": 120.089003, + "longitude": 120.090003, "latitude": 36.361999, "altitude": 12.5 }, diff --git a/config/system_config.json b/config/system_config.json index 4976dc7..4650c3e 100644 --- a/config/system_config.json +++ b/config/system_config.json @@ -36,8 +36,8 @@ "update_interval_ms": 200, "prediction": { "time_window": 20.0, - "vehicle_size": 5.0, - "aircraft_size": 25.0 + "vehicle_size": 20.0, + "aircraft_size": 60.0 } }, "logging": { diff --git a/include/core/System.h b/include/core/System.h deleted file mode 100644 index 1695048..0000000 --- a/include/core/System.h +++ /dev/null @@ -1,76 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include "config/SystemConfig.h" -#include "config/IntersectionConfig.h" -#include "network/WebSocketServer.h" -#include "types/VehicleCommand.h" -#include "types/MovingObject.h" - -class AirportBounds; -class ControllableVehicles; -class CollisionDetector; -class TrafficLightDetector; -class DataCollector; -class MovingObject; -struct CollisionRisk; -struct TrafficLightSignal; - -namespace network { - struct TimeoutWarningMessage; -} - -class System { -public: - System(); - virtual ~System(); - - bool initialize(); - void start(); - void stop(); - - static System* instance() { return instance_; } - static void signalHandler(int signal); - - const SystemConfig& getSystemConfig() const { return system_config_; } - const IntersectionConfig& getIntersectionConfig() const { return intersection_config_; } - - // 广播相关函数 - virtual void broadcastTimeoutWarning(const network::TimeoutWarningMessage& warning); - void broadcastPositionUpdate(const MovingObject& obj); - void broadcastCollisionWarning(const CollisionRisk& risk); - void broadcastVehicleCommand(const VehicleCommand& cmd); - void broadcastTrafficLightStatus(const TrafficLightSignal& signal); - -private: - static System* instance_; - - void processLoop(); - - std::atomic running_{false}; - std::thread processThread_; - std::thread ws_thread_; - - // 系统组件 - std::unique_ptr airportBounds_; - std::unique_ptr controllableVehicles_; - std::unique_ptr collisionDetector_; - std::unique_ptr trafficLightDetector_; - std::unique_ptr dataCollector_; - std::unique_ptr ws_server_; - - // 配置 - SystemConfig system_config_; - IntersectionConfig intersection_config_; - - // 记录车辆停止的时间 - std::unordered_map vehicleStopTimes_; - - // 记录上一次有风险的可控车辆列表 - std::unordered_set lastVehiclesWithRisk_; -}; \ No newline at end of file diff --git a/src/core/System.cpp b/src/core/System.cpp index 525962b..699f4d0 100644 --- a/src/core/System.cpp +++ b/src/core/System.cpp @@ -73,11 +73,6 @@ bool System::initialize() { system_config.warning.warning_interval_ms, system_config.warning.log_interval_ms }; - - // 初始化简单冲突检测器 - // simpleCollisionDetector_ = std::make_unique( - // intersection_config_, *controllableVehicles_); - // Logger::info("简单冲突检测器初始化完成"); return dataCollector_->initialize(dataSourceConfig, warnConfig); } @@ -268,19 +263,6 @@ void System::processLoop() { } processCollisions(collisions); - // 简单冲突检测 - // simpleCollisionDetector_->updateTraffic(aircraft, vehicles); - // auto simpleCollisions = simpleCollisionDetector_->detectCollisions(); - - // if(!simpleCollisions.empty()){ - // Logger::debug("检测到 ", simpleCollisions.size(), "个碰撞风险"); - // for (const auto& risk : simpleCollisions) { - // Logger::debug("碰撞风险详情: id1=", risk.id1, - // ", id2=", risk.id2, - // ", 距离=", risk.distance, "m, 风险等级=", static_cast(risk.level), - // ", 区域类型=", static_cast(risk.isIntersection)); - // } - // processCollisions(simpleCollisions); } else if (!lastVehiclesWithRisk_.empty()) { // 当前没有任何风险，但上次有风险车辆，需要处理恢复指令 Logger::debug("当前无碰撞风险，检查是否需要发送恢复指令"); @@ -361,141 +343,6 @@ void System::processLoop() { } } -void System::processCollisions(const std::vector& risks) { - // 记录当前有风险的可控车辆 - std::unordered_set currentVehiclesWithRisk; - const auto& config = SystemConfig::instance().collision_detection; - - Logger::debug("开始处理碰撞风险，当前风险数量: ", risks.size()); - - // 处理当前的碰撞风险 - for (const auto& risk : risks) { - // 处理 id1 和 id2 中的可控车辆 - bool id1_controllable = controllableVehicles_->isControllable(risk.id1); - bool id2_controllable = controllableVehicles_->isControllable(risk.id2); - - // 如果两个都不是可控车辆，跳过 - if (!id1_controllable && !id2_controllable) { - continue; - } - - // 根据风险等级处理 - auto processVehicle = [&](const std::string& vehicleId, const std::string& otherId) { - switch (risk.level) { - case SimpleRiskLevel::CRITICAL: { - // 危险区域：立即发送告警指令 - VehicleCommand cmd; - cmd.vehicleId = vehicleId; - cmd.type = CommandType::ALERT; - cmd.reason = otherId.substr(0, 2) == "AC" ? - CommandReason::AIRCRAFT_CROSSING : - CommandReason::SPECIAL_VEHICLE; - cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); - - // 设置目标位置（冲突对象的位置） - const MovingObject* target = findVehicle(otherId); - if (target) { - cmd.latitude = target->geo.latitude; - cmd.longitude = target->geo.longitude; - cmd.relativeSpeed = risk.relativeSpeed; - } - - broadcastVehicleCommand(cmd); - controllableVehicles_->sendCommand(vehicleId, cmd); - Logger::warning("发送告警指令到车辆: ", vehicleId, - " 当前距离: ", risk.distance, "m", - " 相对速度: ", risk.relativeSpeed, "m/s"); - break; - } - case SimpleRiskLevel::WARNING: { - // 预警区域：发送预警指令 - VehicleCommand cmd; - cmd.vehicleId = vehicleId; - cmd.type = CommandType::WARNING; - cmd.reason = otherId.substr(0, 2) == "AC" ? - CommandReason::AIRCRAFT_CROSSING : - CommandReason::SPECIAL_VEHICLE; - cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); - - // 设置目标位置 - const MovingObject* target = findVehicle(otherId); - if (target) { - cmd.latitude = target->geo.latitude; - cmd.longitude = target->geo.longitude; - cmd.relativeSpeed = risk.relativeSpeed; - } - - broadcastVehicleCommand(cmd); - controllableVehicles_->sendCommand(vehicleId, cmd); - Logger::info("发送预警指令到车辆: ", vehicleId, - " 当前距离: ", risk.distance, "m", - " 相对速度: ", risk.relativeSpeed, "m/s"); - break; - } - default: - break; - } - }; - - // 为每个可控车辆处理风险 - if (id1_controllable) { - currentVehiclesWithRisk.insert(risk.id1); - Logger::debug("添加当前有风险的可控车辆: ", risk.id1, - ", 当前风险车辆数量: ", currentVehiclesWithRisk.size()); - processVehicle(risk.id1, risk.id2); - } - if (id2_controllable) { - currentVehiclesWithRisk.insert(risk.id2); - Logger::debug("添加当前有风险的可控车辆: ", risk.id2, - ", 当前风险车辆数量: ", currentVehiclesWithRisk.size()); - processVehicle(risk.id2, risk.id1); - } - - // 广播碰撞预警消息 - broadcastCollisionWarning(risk); - } - - Logger::debug("当前有风险的可控车辆数量: ", currentVehiclesWithRisk.size()); - Logger::debug("上一次有风险的可控车辆数量: ", lastVehiclesWithRisk_.size()); - - // 对之前有风险但现在没有风险的车辆发送恢复指令 - for (const auto& vehicleId : lastVehiclesWithRisk_) { - Logger::debug("检查车辆是否需要恢复: ", vehicleId); - if (currentVehiclesWithRisk.find(vehicleId) == currentVehiclesWithRisk.end()) { - Logger::debug("车辆 ", vehicleId, " 当前没有风险，准备发送恢复指令"); - VehicleCommand cmd; - cmd.vehicleId = vehicleId; - cmd.type = CommandType::RESUME; - cmd.reason = CommandReason::RESUME_TRAFFIC; - cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count(); - - broadcastVehicleCommand(cmd); - controllableVehicles_->sendCommand(vehicleId, cmd); - Logger::info("发送恢复指令到车辆: ", vehicleId); - } else { - Logger::debug("车辆 ", vehicleId, " 仍然有风险，不发送恢复指令"); - } - } - - // 更新上一次有风险的车辆列表 - Logger::debug("更新风险车辆列表: 原数量=", lastVehiclesWithRisk_.size(), - ", 新数量=", currentVehiclesWithRisk.size(), - ", 原列表={", [&]() { - std::string s; - for (const auto& id : lastVehiclesWithRisk_) { - s += id + ","; - } - return s; - }(), "}, 新列表={", [&]() { - std::string s; - for (const auto& id : currentVehiclesWithRisk) { - s += id + ","; - } - return s; - }(), "}"); - lastVehiclesWithRisk_ = std::move(currentVehiclesWithRisk); -} - void System::processCollisions(const std::vector& risks) { // 记录当前有风险的可控车辆 std::unordered_set currentVehiclesWithRisk; @@ -716,17 +563,6 @@ std::string getRiskLevelString(RiskLevel level) { } } -std::string getRiskLevelString(SimpleRiskLevel level) { - switch (level) { - case SimpleRiskLevel::CRITICAL: - return "CRITICAL"; - case SimpleRiskLevel::WARNING: - return "WARNING"; - default: - return "NONE"; - } -} - void System::broadcastCollisionWarning(const CollisionRisk& risk) { if (!ws_server_) { return; @@ -751,30 +587,6 @@ void System::broadcastCollisionWarning(const CollisionRisk& risk) { " level=", getRiskLevelString(risk.level)); } -void System::broadcastCollisionWarning(const SimpleCollisionRisk& risk) { - if (!ws_server_) { - return; - } - - // 构造冲突预警消息 - nlohmann::json j = { - {"type", "collision_warning"}, - {"id1", risk.id1}, - {"id2", risk.id2}, - {"distance", risk.distance}, - {"relativeSpeed", risk.relativeSpeed}, - {"warningLevel", getRiskLevelString(risk.level)}, - {"timestamp", 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)); -} - void System::broadcastTimeoutWarning(const network::TimeoutWarningMessage& warning) { if (ws_server_) { ws_server_->broadcast(warning.toJson().dump()); diff --git a/src/core/System.h b/src/core/System.h index 8b768b0..046a9f7 100644 --- a/src/core/System.h +++ b/src/core/System.h @@ -14,7 +14,6 @@ #include "network/MessageTypes.h" #include "config/SystemConfig.h" #include "config/IntersectionConfig.h" -#include "detector/SimpleCollisionDetector.h" // 前向声明 class DataCollector; @@ -35,7 +34,6 @@ public: virtual void broadcastTimeoutWarning(const network::TimeoutWarningMessage& warning); void broadcastPositionUpdate(const MovingObject& obj); void broadcastCollisionWarning(const CollisionRisk& risk); - void broadcastCollisionWarning(const SimpleCollisionRisk& risk); void broadcastVehicleCommand(const VehicleCommand& cmd); void broadcastTrafficLightStatus(const TrafficLightSignal& signal); @@ -45,7 +43,6 @@ public: private: void processLoop(); void processCollisions(const std::vector& collisions); - void processCollisions(const std::vector& risks); std::atomic running_{false}; std::thread processThread_; @@ -73,6 +70,4 @@ private: // 辅助函数 const MovingObject* findVehicle(const std::string& vehicleId) const; - - std::unique_ptr simpleCollisionDetector_; }; \ No newline at end of file diff --git a/src/detector/CollisionDetector.cpp b/src/detector/CollisionDetector.cpp index b6687b1..b9c4beb 100644 --- a/src/detector/CollisionDetector.cpp +++ b/src/detector/CollisionDetector.cpp @@ -1,4 +1,5 @@ -#include "detector/CollisionDetector.h" +#include "CollisionDetector.h" + #include "utils/Logger.h" #include "config/SystemConfig.h" #include @@ -354,9 +355,7 @@ bool CollisionDetector::checkCollisionRisk( ", 当前距离=", currentDistance, "m, 相对速度=", relativeSpeed, "m/s, 相对运动=", relativeMotion, - ", 预测碰撞=", prediction.willCollide ? "是" : "否", - ", 警戒距离=", alertDistance, - "m, 有风险=", hasCollisionRisk ? "是" : "否" + ", 正在远离=", prediction.isMovingAway ? "是" : "否" ); } @@ -413,8 +412,8 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision( result.timeToCollision = std::numeric_limits::infinity(); result.minDistance = std::numeric_limits::infinity(); - // 计算安全距离 - double safeDistance = (size1 + size2) * 1.5; + // 计算安全距离等于两个物体尺寸之和，加上 20m 的缓冲区 + double safeDistance = size1 + size2 + 20.0; // 计算速度分量 MovementVector mv1(speed1, heading1); @@ -558,4 +557,69 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision( } return result; +} + +// 基于矩形的航空器和车辆碰撞检测 +bool CollisionDetector::checkRectangleBasedAircraftVehicleCollision( + const Aircraft& aircraft, + const Vehicle& vehicle, + double timeWindow) const { + + // 获取系统配置 + const auto& config = SystemConfig::instance(); + + // 计算速度分量 + MovementVector mv1(aircraft.speed, aircraft.heading); + MovementVector mv2(vehicle.speed, vehicle.heading); + + // 创建航空器和车辆的正方形表示 + collision::Rectangle aircraftRect( + aircraft.position, // 中心点 + config.collision_detection.prediction.aircraft_size * 2.0, // 边长 + Vector2D{mv1.vx, mv1.vy}, // 速度向量 + aircraft.heading // 朝向 + ); + + collision::Rectangle vehicleRect( + vehicle.position, // 中心点 + config.collision_detection.prediction.vehicle_size * 2.0, // 边长 + Vector2D{mv2.vx, mv2.vy}, // 速度向量 + vehicle.heading // 朝向 + ); + + // 检测碰撞 + return aircraftRect.checkCollision(vehicleRect, timeWindow); +} + +// 基于矩形的车辆间碰撞检测 +bool CollisionDetector::checkRectangleBasedVehicleCollision( + const Vehicle& v1, + const Vehicle& v2, + double timeWindow) const { + + // 获取系统配置 + const auto& config = SystemConfig::instance(); + double vehicleSize = config.collision_detection.prediction.vehicle_size; + + // 计算速度分量 + MovementVector mv1(v1.speed, v1.heading); + MovementVector mv2(v2.speed, v2.heading); + + // 创建两个车辆的正方形表示 + collision::Rectangle rect1( + v1.position, // 中心点 + vehicleSize * 2.0, // 边长 + Vector2D{mv1.vx, mv1.vy}, // 速度向量 + v1.heading // 朝向 + ); + + collision::Rectangle rect2( + v2.position, // 中心点 + vehicleSize * 2.0, // 边长 + Vector2D{mv2.vx, mv2.vy}, // 速度向量 + v2.heading // 朝向 + ); + + // 检测碰撞 + return rect1.checkCollision(rect2, timeWindow); } \ No newline at end of file diff --git a/src/detector/CollisionDetector.h b/src/detector/CollisionDetector.h index b83c051..b7596cb 100644 --- a/src/detector/CollisionDetector.h +++ b/src/detector/CollisionDetector.h @@ -6,10 +6,9 @@ #include "config/AirportBounds.h" #include "vehicle/ControllableVehicles.h" #include "config/SystemConfig.h" +#include "RectangleCollision.h" +#include "utils/Logger.h" #include -#include -#include -#include // 碰撞风险等级 enum class RiskLevel { @@ -50,21 +49,35 @@ public: CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles); // 更新交通数据 - void updateTraffic(const std::vector& aircraft, + void updateTraffic(const std::vector& aircraft, const std::vector& vehicles); // 检测所有可能的碰撞 std::vector detectCollisions(); - + + // 基于矩形的航空器和车辆碰撞检测 + bool checkRectangleBasedAircraftVehicleCollision( + const Aircraft& aircraft, + const Vehicle& vehicle, + double timeWindow = 30.0 + ) const; + + // 基于矩形的车辆间碰撞检测 + bool checkRectangleBasedVehicleCollision( + const Vehicle& v1, + const Vehicle& v2, + double timeWindow = 30.0 + ) const; + // 基于轨迹预测的碰撞检测 CollisionPrediction predictTrajectoryCollision( - const Vector2D& pos1, double speed1, double heading1, // 物体1的位置、速度、航向 - const Vector2D& pos2, double speed2, double heading2, // 物体2的位置、速度、航向 - double size1, // 物体1的尺寸 - double size2, // 物体2的尺寸 - double timeWindow = 30.0 // 预测时间窗口(默认30秒) + const Vector2D& pos1, double speed1, double heading1, + const Vector2D& pos2, double speed2, double heading2, + double size1, + double size2, + double timeWindow = 30.0 ) const; - + private: const AirportBounds& airportBounds_; QuadTree vehicleTree_; @@ -90,7 +103,7 @@ private: // 计算风险等级 RiskLevel calculateRiskLevel(double distance, const Vector2D& position, - bool isAircraft1, bool isAircraft2) const; + bool isAircraft1, bool isAircraft2) const; // 确定预警区域类型 WarningZoneType determineWarningZone(double distance, double threshold) const; @@ -120,6 +133,26 @@ private: "°, vx=", vx, "m/s, vy=", vy, "m/s"); } }; + + // 创建正方形 + collision::Rectangle createRectangle( + const Vector2D& position, + double size, + double heading, + double speed + ) const { + // 计算速度分量 + MovementVector mv(speed, heading); + Logger::debug("速度分量计算: speed=" + std::to_string(speed) + "m/s, heading=" + + std::to_string(heading) + "°, vx=" + std::to_string(mv.vx) + + "m/s, vy=" + std::to_string(mv.vy) + "m/s"); + + return collision::Rectangle{ + position, // 中心点 + size, // 正方形边长 + {mv.vx, mv.vy} // 速度向量 + }; + } }; #endif // AIRPORT_DETECTOR_COLLISION_DETECTOR_H \ No newline at end of file diff --git a/src/detector/CollisionTypes.h b/src/detector/CollisionTypes.h new file mode 100644 index 0000000..dc403cb --- /dev/null +++ b/src/detector/CollisionTypes.h @@ -0,0 +1,37 @@ +// Copyright 2024 Tianji Robotics. All rights reserved. + +#ifndef AIRPORT_DETECTOR_COLLISION_TYPES_H +#define AIRPORT_DETECTOR_COLLISION_TYPES_H + +#include "types/BasicTypes.h" + +namespace collision { + +// 碰撞类型 +enum class CollisionType { + HEAD_ON, // 相向碰撞 + CROSSING, // 交叉碰撞 + STATIC, // 静态碰撞 + PARALLEL // 平行运动 +}; + +// 碰撞检测结果 +struct CollisionResult { + bool willCollide; // 是否会碰撞 + double timeToCollision; // 碰撞时间 + Vector2D collisionPoint; // 碰撞点 + double minDistance; // 最小距离 + CollisionType type; // 碰撞类型 + + // 构造函数 + CollisionResult() : + willCollide(false), + timeToCollision(std::numeric_limits::infinity()), + collisionPoint({0, 0}), + minDistance(std::numeric_limits::infinity()), + type(CollisionType::STATIC) {} +}; + +} // namespace collision + +#endif // AIRPORT_DETECTOR_COLLISION_TYPES_H \ No newline at end of file diff --git a/src/detector/RectangleCollision.h b/src/detector/RectangleCollision.h new file mode 100644 index 0000000..c473d8c --- /dev/null +++ b/src/detector/RectangleCollision.h @@ -0,0 +1,141 @@ +#ifndef AIRPORT_DETECTOR_RECTANGLE_COLLISION_H +#define AIRPORT_DETECTOR_RECTANGLE_COLLISION_H + +#include "types/BasicTypes.h" +#include +#include +#include + +namespace collision { + +// 碰撞检测结果 +struct RectangleCollisionResult { + bool willCollide = false; // 是否会发生碰撞 + double timeToCollision = 0.0; // 到碰撞的时间 + Vector2D collisionPoint = {0, 0}; // 碰撞点 + double minDistance = 0.0; // 最小距离 +}; + +// 正方形表示 +struct Rectangle { + Vector2D center; // 中心点 + double size; // 边长 + Vector2D velocity; // 速度向量 + double heading; // 朝向（角度） + + // 构造函数 + Rectangle(const Vector2D& c, double s, const Vector2D& v, double h = 0.0) + : center(c), size(s), velocity(v), heading(h) { +#ifdef TESTING + std::cout << "正方形构造: 中心点=(" << center.x << "," << + center.y << "), 边长=" << size << + ", 速度=(" << velocity.x << "," << + velocity.y << ")" << std::endl; +#endif + } + + // 默认构造函数 + Rectangle() : center({0, 0}), size(0), velocity({0, 0}), heading(0) {} + + // 获取正方形的AABB（轴对齐包围盒） + // 正方形不需要考虑朝向，因为旋转后的正方形AABB保持不变 + void getAABB(double& x_min, double& x_max, double& y_min, double& y_max) const { + double half_size = size / 2.0; + x_min = center.x - half_size; + x_max = center.x + half_size; + y_min = center.y - half_size; + y_max = center.y + half_size; + } + + // 预测t时刻的位置 + Rectangle predictPosition(double t) const { + Rectangle future = *this; + future.center.x += velocity.x * t; + future.center.y += velocity.y * t; + return future; + } + + // 检测与另一个正方形是否碰撞 + bool checkCollision(const Rectangle& other, double timeWindow = 1.0) const { + // 1. 首先检查当前是否碰撞 + double x1_min, x1_max, y1_min, y1_max; + double x2_min, x2_max, y2_min, y2_max; + + this->getAABB(x1_min, x1_max, y1_min, y1_max); + other.getAABB(x2_min, x2_max, y2_min, y2_max); + + // 检查当前是否重叠 + if (x1_max >= x2_min && x2_max >= x1_min && + y1_max >= y2_min && y2_max >= y1_min) { +#ifdef TESTING + std::cout << "检测到当���碰撞" << std::endl; +#endif + return true; + } + + // 2. 检查未来一段时间内是否会碰撞 + // 计算相对速度 + double rel_vx = velocity.x - other.velocity.x; + double rel_vy = velocity.y - other.velocity.y; + double rel_speed = std::sqrt(rel_vx*rel_vx + rel_vy*rel_vy); + + // 如果相对速度接近0，使用当前距离判断 + if (rel_speed < 0.1) { + double dx = center.x - other.center.x; + double dy = center.y - other.center.y; + double distance = std::sqrt(dx*dx + dy*dy); + double safe_distance = size/2.0 + other.size/2.0; // 两个正方形从中心到边缘的距离之和 + + if (distance <= safe_distance) { +#ifdef TESTING + std::cout << "静止状态检测到碰撞，距离=" << distance << "米" << std::endl; +#endif + return true; + } + return false; + } + + // 3. 在时间窗口内采样检查 + const int STEPS = 20; + double dt = timeWindow / STEPS; + + for (int i = 1; i <= STEPS; ++i) { + double t = i * dt; + Rectangle future1 = predictPosition(t); + Rectangle future2 = other.predictPosition(t); + + future1.getAABB(x1_min, x1_max, y1_min, y1_max); + future2.getAABB(x2_min, x2_max, y2_min, y2_max); + + // 检查是否重叠 + if (x1_max >= x2_min && x2_max >= x1_min && + y1_max >= y2_min && y2_max >= y1_min) { +#ifdef TESTING + std::cout << "预测到碰撞，时间=" << t << "秒" << std::endl; +#endif + return true; + } + + // 计算中心点距离 + double dx = (future1.center.x - future2.center.x); + double dy = (future1.center.y - future2.center.y); + double distance = std::sqrt(dx*dx + dy*dy); + double safe_distance = size/2.0 + other.size/2.0; // 两个正方形从中心到边缘的距离之和 + + if (distance <= safe_distance) { +#ifdef TESTING + std::cout << "预测到危险接近，时间=" << t + << "秒，距离=" << distance + << "米，安全距离=" << safe_distance << "米" << std::endl; +#endif + return true; + } + } + + return false; + } +}; + +} // namespace collision + +#endif // AIRPORT_DETECTOR_RECTANGLE_COLLISION_H \ No newline at end of file diff --git a/src/detector/SimpleCollisionDetector.cpp b/src/detector/SimpleCollisionDetector.cpp deleted file mode 100644 index 1c373a3..0000000 --- a/src/detector/SimpleCollisionDetector.cpp +++ /dev/null @@ -1,202 +0,0 @@ -#include "detector/SimpleCollisionDetector.h" -#include "utils/Logger.h" -#include - -SimpleCollisionDetector::SimpleCollisionDetector( - const IntersectionConfig& intersectionConfig, - const ControllableVehicles& controllableVehicles) - : intersection_config_(intersectionConfig) - , controllable_vehicles_(controllableVehicles) { - Logger::info("简单冲突检测器初始化完成"); -} - -void SimpleCollisionDetector::updateTraffic( - const std::vector& aircraft, - const std::vector& vehicles) { - aircraft_data_ = aircraft; - vehicle_data_ = vehicles; - Logger::debug("更新交通数据: 航空器=", aircraft.size(), " 车辆=", vehicles.size()); -} - -std::vector SimpleCollisionDetector::detectCollisions() { - std::vector risks; - - // 检查可控车辆与飞机的冲突 - for (const auto& aircraft : aircraft_data_) { - for (const auto& vehicle : vehicle_data_) { - if (controllable_vehicles_.isControllable(vehicle.vehicleNo)) { - auto risk = checkAircraftVehicleCollision(aircraft, vehicle); - if (risk.level != SimpleRiskLevel::NONE) { - risks.push_back(risk); - } - } - } - } - - // 检查可控车辆之间的冲突 - for (size_t i = 0; i < vehicle_data_.size(); ++i) { - if (!controllable_vehicles_.isControllable(vehicle_data_[i].vehicleNo)) { - continue; - } - - for (size_t j = i + 1; j < vehicle_data_.size(); ++j) { - auto risk = checkVehicleCollision(vehicle_data_[i], vehicle_data_[j]); - if (risk.level != SimpleRiskLevel::NONE) { - risks.push_back(risk); - } - } - } - - return risks; -} - -SimpleCollisionRisk SimpleCollisionDetector::checkAircraftVehicleCollision( - const Aircraft& aircraft, const Vehicle& vehicle) { - - SimpleCollisionRisk risk; - risk.id1 = aircraft.flightNo; - risk.id2 = vehicle.vehicleNo; - risk.level = SimpleRiskLevel::NONE; - - // 计算当前距离 - risk.distance = calculateDistance(aircraft.position, vehicle.position); - - // 检查是否在路口附近 - if (!isNearIntersection(vehicle.position, risk.intersectionId)) { - return risk; // 不在路口，无风险 - } - - risk.isIntersection = true; - - // 计算到路口的距离 - double aircraftDist = calculateDistanceToIntersection(aircraft.position, risk.intersectionId); - double vehicleDist = calculateDistanceToIntersection(vehicle.position, risk.intersectionId); - - // 计算相对速度（取较大值） - risk.relativeSpeed = std::max(aircraft.speed, vehicle.speed); - if (risk.relativeSpeed < MIN_SPEED) { - risk.relativeSpeed = MIN_SPEED; - } - - // 计算预计碰撞时间（取较大值） - risk.timeToCollision = std::max(aircraftDist, vehicleDist) / risk.relativeSpeed; - - // 安全距离 = 路口宽度 + 飞机安全缓冲 - double safeDistance = INTERSECTION_WIDTH + AIRCRAFT_BUFFER; - - // 判断风险等级 - if (risk.distance < safeDistance || risk.timeToCollision < MIN_TIME_WINDOW) { - risk.level = SimpleRiskLevel::CRITICAL; - } else if (risk.distance < safeDistance * 1.5 || risk.timeToCollision < WARNING_TIME_WINDOW) { - risk.level = SimpleRiskLevel::WARNING; - } - - return risk; -} - -SimpleCollisionRisk SimpleCollisionDetector::checkVehicleCollision( - const Vehicle& v1, const Vehicle& v2) { - - SimpleCollisionRisk risk; - risk.id1 = v1.vehicleNo; - risk.id2 = v2.vehicleNo; - risk.level = SimpleRiskLevel::NONE; - - // 计算当前距离 - risk.distance = calculateDistance(v1.position, v2.position); - - // 检查是否在路口附近 - if (isNearIntersection(v1.position, risk.intersectionId) || - isNearIntersection(v2.position, risk.intersectionId)) { - // 路口场景 - risk.isIntersection = true; - - double v1Dist = calculateDistanceToIntersection(v1.position, risk.intersectionId); - double v2Dist = calculateDistanceToIntersection(v2.position, risk.intersectionId); - - risk.relativeSpeed = std::max(v1.speed, v2.speed); - if (risk.relativeSpeed < MIN_SPEED) { - risk.relativeSpeed = MIN_SPEED; - } - - risk.timeToCollision = std::max(v1Dist, v2Dist) / risk.relativeSpeed; - - // 路口安全距离 - double safeDistance = INTERSECTION_WIDTH + VEHICLE_BUFFER; - - if (risk.distance < safeDistance || risk.timeToCollision < MIN_TIME_WINDOW) { - risk.level = SimpleRiskLevel::CRITICAL; - } else if (risk.distance < safeDistance * 1.5 || risk.timeToCollision < WARNING_TIME_WINDOW) { - risk.level = SimpleRiskLevel::WARNING; - } - } else { - // 非路口场景（对向或同向） - risk.isIntersection = false; - - // 计算相对速度 - risk.relativeSpeed = std::abs(v1.speed - v2.speed); // 同向 - if (std::abs(v1.heading - v2.heading) > 150) { // 对向 - risk.relativeSpeed = v1.speed + v2.speed; - } - - if (risk.relativeSpeed < MIN_SPEED) { - risk.relativeSpeed = MIN_SPEED; - } - - risk.timeToCollision = risk.distance / risk.relativeSpeed; - - // 非路口安全距离 - double safeDistance = VEHICLE_BUFFER * 2; - - if (risk.distance < safeDistance || risk.timeToCollision < MIN_TIME_WINDOW) { - risk.level = SimpleRiskLevel::CRITICAL; - } else if (risk.distance < safeDistance * 1.5 || risk.timeToCollision < WARNING_TIME_WINDOW) { - risk.level = SimpleRiskLevel::WARNING; - } - } - - return risk; -} - -double SimpleCollisionDetector::calculateDistance(const Vector2D& pos1, const Vector2D& pos2) { - double dx = pos1.x - pos2.x; - double dy = pos1.y - pos2.y; - return std::sqrt(dx*dx + dy*dy); -} - -double SimpleCollisionDetector::calculateDistanceToIntersection( - const Vector2D& position, const std::string& intersectionId) { - - const auto* intersection = intersection_config_.findById(intersectionId); - if (!intersection) { - return std::numeric_limits::max(); - } - - // 将 IntersectionPosition 转换为 Vector2D - Vector2D intersectionPos; - intersectionPos.x = intersection->position.longitude; - intersectionPos.y = intersection->position.latitude; - - return calculateDistance(position, intersectionPos); -} - -bool SimpleCollisionDetector::isNearIntersection( - const Vector2D& position, std::string& intersectionId) { - - const double CHECK_DISTANCE = INTERSECTION_WIDTH * 5; // 检查范围是路口宽度的2倍 - - for (const auto& intersection : intersection_config_.getIntersections()) { - // 将 IntersectionPosition 转换为 Vector2D - Vector2D intersectionPos; - intersectionPos.x = intersection.position.longitude; - intersectionPos.y = intersection.position.latitude; - - double distance = calculateDistance(position, intersectionPos); - if (distance < CHECK_DISTANCE) { - intersectionId = intersection.id; - return true; - } - } - - return false; -} \ No newline at end of file diff --git a/src/detector/SimpleCollisionDetector.h b/src/detector/SimpleCollisionDetector.h deleted file mode 100644 index 48f8a2f..0000000 --- a/src/detector/SimpleCollisionDetector.h +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef AIRPORT_DETECTOR_SIMPLE_COLLISION_DETECTOR_H -#define AIRPORT_DETECTOR_SIMPLE_COLLISION_DETECTOR_H - -#include "types/BasicTypes.h" -#include "config/IntersectionConfig.h" -#include "vehicle/ControllableVehicles.h" -#include - -// 简单冲突检测器的风险等级 -enum class SimpleRiskLevel { - NONE = 0, // 无风险 - WARNING = 1, // 预警 - CRITICAL = 2 // 告警 -}; - -// 简单冲突检测器的风险信息 -struct SimpleCollisionRisk { - std::string id1, id2; // 冲突物体的ID - SimpleRiskLevel level; // 风险等级 - double distance; // 当前距离 - double timeToCollision; // 预计碰撞时间 - double relativeSpeed; // 相对速度 - bool isIntersection; // 是否在路口 - std::string intersectionId; // 路口ID（如果在路口） -}; - -class SimpleCollisionDetector { -public: - SimpleCollisionDetector(const IntersectionConfig& intersectionConfig, - const ControllableVehicles& controllableVehicles); - - // 更新交通数据 - void updateTraffic(const std::vector& aircraft, - const std::vector& vehicles); - - // 检测所有可能的冲突 - std::vector detectCollisions(); - -private: - // 检查车辆与飞机的冲突 - SimpleCollisionRisk checkAircraftVehicleCollision( - const Aircraft& aircraft, const Vehicle& vehicle); - - // 检查车辆与车辆的冲突 - SimpleCollisionRisk checkVehicleCollision( - const Vehicle& v1, const Vehicle& v2); - - // 计算到路口的距离 - double calculateDistanceToIntersection( - const Vector2D& position, const std::string& intersectionId); - - // 计算两点间距离 - double calculateDistance(const Vector2D& pos1, const Vector2D& pos2); - - // 判断是否在路口附近 - bool isNearIntersection(const Vector2D& position, std::string& intersectionId); - - // 配置参数 - static constexpr double INTERSECTION_WIDTH = 30.0; // 路口宽度 - static constexpr double AIRCRAFT_BUFFER = 20.0; // 飞机安全缓冲 - static constexpr double VEHICLE_BUFFER = 10.0; // 车辆安全缓冲 - static constexpr double MIN_TIME_WINDOW = 3.0; // 最小时间窗口（秒） - static constexpr double WARNING_TIME_WINDOW = 5.0; // 预警时间窗口（秒） - static constexpr double MIN_SPEED = 0.5; // 最小速度（米/秒） - - const IntersectionConfig& intersection_config_; - const ControllableVehicles& controllable_vehicles_; - std::vector aircraft_data_; - std::vector vehicle_data_; -}; - -#endif // AIRPORT_DETECTOR_SIMPLE_COLLISION_DETECTOR_H \ No newline at end of file diff --git a/tests/CollisionDetectorTest.cpp b/tests/CollisionDetectorTest.cpp index 712ee01..ccd2e2a 100644 --- a/tests/CollisionDetectorTest.cpp +++ b/tests/CollisionDetectorTest.cpp @@ -5,6 +5,29 @@ #include #include "utils/Logger.h" #include +#include + +// 在所有测试开始前初始化日志 +class GlobalTestEnvironment : public ::testing::Environment { +public: + void SetUp() override { + // 确保使用 DEBUG 级别 + Logger::initialize("logs/test.log", LogLevel::DEBUG); + Logger::debug("=== 测试开始 ==="); + } + + void TearDown() override { + Logger::debug("=== 测试结束 ==="); + Logger::initialize("", LogLevel::INFO); // 关闭日志文件 + } +}; + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + // 添加全局测试环境 + ::testing::AddGlobalTestEnvironment(new GlobalTestEnvironment); + return RUN_ALL_TESTS(); +} // Mock ControllableVehicles 类 class MockControllableVehicles : public ControllableVehicles { @@ -42,6 +65,9 @@ public: class CollisionDetectorTest : public ::testing::Test { protected: void SetUp() override { + // 打印当前工作目录 + std::cout << "Current working directory: " << std::filesystem::current_path() << std::endl; + // 创建 Mock 对象 airportBounds_ = std::make_unique(); mockControllableVehicles_ = std::make_unique(); @@ -50,6 +76,10 @@ protected: detector_ = std::make_unique(*airportBounds_, *mockControllableVehicles_); } + void TearDown() override { + Logger::initialize("", LogLevel::INFO); + } + std::unique_ptr airportBounds_; std::unique_ptr mockControllableVehicles_; std::unique_ptr detector_; @@ -62,7 +92,7 @@ TEST_F(CollisionDetectorTest, DetectControllableVehicleAircraftCollision) { .WillRepeatedly(testing::Return(true)); Logger::info("Set mock expectation: VEH001 is controllable"); - // 设置���试数据 + // 设置测试数据 Aircraft aircraft; aircraft.flightNo = "TEST001"; aircraft.position = {100, 100}; @@ -73,7 +103,7 @@ TEST_F(CollisionDetectorTest, DetectControllableVehicleAircraftCollision) { Vehicle vehicle; vehicle.vehicleNo = "VEH001"; - vehicle.position = {120, 100}; // 距离航空器20米 + vehicle.position = {120, 100}; // 离航空器20米 vehicle.speed = 5; vehicle.heading = 270; Logger::info("Created vehicle: vehicleNo=", vehicle.vehicleNo, @@ -358,357 +388,193 @@ TEST_F(CollisionDetectorTest, PerformanceTest) { EXPECT_LT(duration.count(), 100000); // 期望处理时间小于100ms } -// 测试轨迹预测的准确性 -TEST_F(CollisionDetectorTest, TrajectoryPredictionAccuracy) { - // 设置 Mock 期望 - EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_)) - .WillRepeatedly(testing::Return(true)); +// 修改矩形碰撞检测测试 +TEST_F(CollisionDetectorTest, RectangleCollisionParallelMotion) { + // 设置系统配置 + SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0; + + // 创建两个平行运动的车辆 + Vehicle v1; + v1.vehicleNo = "VEH001"; + v1.position = {100.0, 100.0}; + v1.speed = 10.0; + v1.heading = 90.0; // 向东行驶 + + Vehicle v2; + v2.vehicleNo = "VEH002"; + v2.position = {100.0, 107.0}; // 在v1北侧7米处 + v2.speed = 10.0; + v2.heading = 90.0; // 向东行驶 + + // 检测碰撞 + bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); + + // 由于两车间距小于安全距离，应该检测到碰撞 + EXPECT_TRUE(hasCollision) << "平行运动的车辆应该检测到碰撞"; +} - // 测试场景1：正面相遇碰撞 - { - Vector2D pos1{100, 100}; // 第一个物体的位置 - double speed1 = 10; // 第一个物体的速度 (m/s) - double heading1 = 90; // 第一个物体的航向（正东，90度） - - Vector2D pos2{300, 100}; // 第二个物体的位置 - double speed2 = 10; // 第二个物体的速度 (m/s) - double heading2 = 270; // 第二个物体的航向（正西，270度） - - double size1 = 5; // 第一个物体的尺寸 - double size2 = 5; // 第二个物体的尺寸 - double timeWindow = 30; // 预测时间窗口 - - auto prediction = detector_->predictTrajectoryCollision( - pos1, speed1, heading1, - pos2, speed2, heading2, - size1, size2, timeWindow - ); - - // 验证结果：首先验证是否检测到碰撞 - EXPECT_TRUE(prediction.willCollide) << "应该检测到碰撞"; - if (prediction.willCollide) { - // 验证碰撞点位置（允许1米的误差） - EXPECT_NEAR(prediction.collisionPoint.x, 200.0, 1.0) << "碰撞点x坐标应该在中点附近"; - EXPECT_NEAR(prediction.collisionPoint.y, 100.0, 1.0) << "碰撞点y坐标应该保持不变"; - } - } +TEST_F(CollisionDetectorTest, RectangleCollisionCrossingPaths) { + // 设置系统配置 + SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0; - // 测试场景2：平行路径，不会碰撞 - { - Vector2D pos1{100, 100}; - double speed1 = 10; - double heading1 = 90; // 正东（90度） - - Vector2D pos2{100, 120}; // 平行路径，距20米 - double speed2 = 10; - double heading2 = 90; // 正东（90度） - - double size1 = 5; - double size2 = 5; - double timeWindow = 30; - - auto prediction = detector_->predictTrajectoryCollision( - pos1, speed1, heading1, - pos2, speed2, heading2, - size1, size2, timeWindow - ); - - // 验证结果：不应该检测到碰撞 - EXPECT_FALSE(prediction.willCollide) << "平行路径不应该碰撞"; - // 验证最小距离（允许1米的误差） - EXPECT_NEAR(prediction.minDistance, 20.0, 1.0) << "平行路径应该保持20米的距离"; - } + // 创建两个交叉路径的车辆 + Vehicle v1; + v1.vehicleNo = "VEH001"; + v1.position = {0.0, 100.0}; + v1.speed = 10.0; + v1.heading = 90.0; // 向东行驶 - // 测试场景3：垂直相交路径 - { - Vector2D pos1{0, 100}; // 第一个物体从左开始 - double speed1 = 10; - double heading1 = 90; // 正东（90度） - - Vector2D pos2{200, 300}; // 第二个物体从上开始 - double speed2 = 10; - double heading2 = 180; // 正南（180度） - - double size1 = 5; - double size2 = 5; - double timeWindow = 30; // 30秒的预测窗口 - - Logger::info("垂直路径测试参数:"); - Logger::info(" 物体1: 位置=(" + std::to_string(pos1.x) + ", " + std::to_string(pos1.y) + - "), 速度=" + std::to_string(speed1) + ", 航向=" + std::to_string(heading1)); - Logger::info(" 物体2: 位置=(" + std::to_string(pos2.x) + ", " + std::to_string(pos2.y) + - "), 速度=" + std::to_string(speed2) + ", 航向=" + std::to_string(heading2)); - - // 计算速度分量用于调试 - double heading1Rad = heading1 * M_PI / 180.0; - double heading2Rad = heading2 * M_PI / 180.0; - double v1x = speed1 * std::sin(heading1Rad); // 东向为x轴正方向 - double v1y = speed1 * std::cos(heading1Rad); // 北向为y轴正方向 - double v2x = speed2 * std::sin(heading2Rad); - double v2y = speed2 * std::cos(heading2Rad); - Logger::info("速度分量:"); - Logger::info(" 物体1: vx=" + std::to_string(v1x) + ", vy=" + std::to_string(v1y)); - Logger::info(" 物体2: vx=" + std::to_string(v2x) + ", vy=" + std::to_string(v2y)); - - auto prediction = detector_->predictTrajectoryCollision( - pos1, speed1, heading1, - pos2, speed2, heading2, - size1, size2, timeWindow - ); - - Logger::info("预测结果:"); - Logger::info(" willCollide=" + std::to_string(prediction.willCollide)); - Logger::info(" minDistance=" + std::to_string(prediction.minDistance)); - if (prediction.willCollide) { - Logger::info(" 碰撞点=(" + std::to_string(prediction.collisionPoint.x) + ", " + - std::to_string(prediction.collisionPoint.y) + ")"); - Logger::info(" 碰撞时间=" + std::to_string(prediction.timeToCollision)); - - // 计算理论碰撞点 - // 物体1：x = 0 + 10t, y = 100 - // 物体2：x = 200, y = 300 - 10t - // 碰撞时：x1 = x2 = 200, y1 = y2 - double collisionTime = 20.0; // t = 200/10 = 20秒 - double expectedX = 200.0; // x = 0 + 10*20 = 200 - double expectedY = 100.0; // y = 100 (物体1的y保持不变) - - // 验证碰撞点位置（允许5米的误差，考虑到物体尺寸） - EXPECT_NEAR(prediction.collisionPoint.x, expectedX, 5.0) - << "碰撞点x坐标应该在交叉点附近"; - EXPECT_NEAR(prediction.collisionPoint.y, expectedY, 5.0) - << "碰撞点y坐标应该在交叉点附近"; - - // 验证碰撞时间（允许1秒的误差） - EXPECT_NEAR(prediction.timeToCollision, collisionTime, 1.0) - << "碰撞时间应该接近20秒"; - } - } + Vehicle v2; + v2.vehicleNo = "VEH002"; + v2.position = {100.0, 200.0}; + v2.speed = 10.0; + v2.heading = 180.0; // 向南行驶 - // 测试场景4：静止物体 - { - Vector2D pos1{0, 100}; // 第一个物体从远处开始 - double speed1 = 10; - double heading1 = 90; // 正东（90度） - - Vector2D pos2{200, 100}; // 静止物体在目标位置 - double speed2 = 0; // 静止物体 - double heading2 = 0; // 航向无关紧要，因为是静止的 - - double size1 = 5; - double size2 = 5; - double timeWindow = 30; // 30秒的预测窗口 - - Logger::info("静止物体测试参数:"); - Logger::info(" 物体1: 位置=(" + std::to_string(pos1.x) + ", " + std::to_string(pos1.y) + - "), 速度=" + std::to_string(speed1) + ", 航向=" + std::to_string(heading1)); - Logger::info(" 物体2: 位置=(" + std::to_string(pos2.x) + ", " + std::to_string(pos2.y) + - "), 速度=" + std::to_string(speed2) + ", 航向=" + std::to_string(heading2)); - - // 计算速度分量用于调试 - double heading1Rad = heading1 * M_PI / 180.0; - double heading2Rad = heading2 * M_PI / 180.0; - double v1x = speed1 * std::sin(heading1Rad); // 东向为x轴正方向 - double v1y = speed1 * std::cos(heading1Rad); // 北向为y轴正方向 - double v2x = speed2 * std::sin(heading2Rad); - double v2y = speed2 * std::cos(heading2Rad); - Logger::info("速度分量:"); - Logger::info(" 物体1: vx=" + std::to_string(v1x) + ", vy=" + std::to_string(v1y)); - Logger::info(" 物体2: vx=" + std::to_string(v2x) + ", vy=" + std::to_string(v2y)); - - auto prediction = detector_->predictTrajectoryCollision( - pos1, speed1, heading1, - pos2, speed2, heading2, - size1, size2, timeWindow - ); - - Logger::info("预测结果:"); - Logger::info(" willCollide=" + std::to_string(prediction.willCollide)); - Logger::info(" minDistance=" + std::to_string(prediction.minDistance)); - if (prediction.willCollide) { - Logger::info(" 碰撞点=(" + std::to_string(prediction.collisionPoint.x) + ", " + - std::to_string(prediction.collisionPoint.y) + ")"); - Logger::info(" 碰撞时间=" + std::to_string(prediction.timeToCollision)); - - // 计算理论碰撞点 - // 考虑物体尺寸：当两个物体中心点距离等于它们尺寸之和时就发生碰撞 - double safeDistance = size1 + size2; // 10米 - double expectedX = pos2.x - safeDistance; // 200 - 10 = 190米 - double expectedY = pos2.y; // 100米 - double collisionTime = expectedX / speed1; // 19秒 - - // 验证碰撞点位置（允许5米的误差，考虑到数值计算的精度） - EXPECT_NEAR(prediction.collisionPoint.x, expectedX, 5.0) - << "碰撞点应该在考虑物体尺寸后的位置"; - EXPECT_NEAR(prediction.collisionPoint.y, expectedY, 5.0) - << "碰撞点y坐标应该保持不变"; - - // 验证碰撞时间（允许1秒的误差） - EXPECT_NEAR(prediction.timeToCollision, collisionTime, 1.0) - << "碰撞时间应该接近19秒"; - } - } + // 检测碰撞 + bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); - // 测试场景5：超出时间窗口的碰撞 - { - Vector2D pos1{100, 100}; - double speed1 = 10; - double heading1 = 90; // 向东 - - Vector2D pos2{400, 100}; - double speed2 = 0; - double heading2 = 0; // 静止物体的航向可以是任意值 - - double size1 = 5; - double size2 = 5; - double timeWindow = 20; // 20秒的时间窗口（不足以到达碰撞点） - - auto prediction = detector_->predictTrajectoryCollision( - pos1, speed1, heading1, - pos2, speed2, heading2, - size1, size2, timeWindow - ); - - // 验证结果：不应该检测到碰撞（因为超出时间窗口） - EXPECT_FALSE(prediction.willCollide) << "超出时间窗口应该报告碰撞"; - EXPECT_GT(prediction.minDistance, 0) << "最小距离该大于0"; - } + // 两车将在(100, 100)处相遇，应该检测到碰撞 + EXPECT_TRUE(hasCollision) << "交叉路径的车辆应该检测到碰撞"; +} + +TEST_F(CollisionDetectorTest, RectangleCollisionAircraftVehicle) { + // 设置系统配置 + SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0; + SystemConfig::instance().collision_detection.prediction.aircraft_size = 10.0; - // 测试场景6：飞机和车辆垂直相交 - { - // 飞机尺寸：波音737-800，翼展35.8米，长度39.5米 - double aircraftSize = 40.0; // 使用较大的值以确保安全 - - // 车辆尺寸：标准摆渡车，长度12米，宽度2.5米 - double vehicleSize = 12.0; - - // 飞机从西向东，车辆从南向北 - Vector2D aircraftPos{0, 200}; // 飞机初始位置 - double aircraftSpeed = 12; // 飞机速度 - double aircraftHeading = 90; // 向东飞行 - - Vector2D vehiclePos{200, 100}; // 修改车辆初始位置，使其能与飞机相遇 - double vehicleSpeed = 6; // 车辆速度 - double vehicleHeading = 0; // 向北行驶 - - double timeWindow = 60; // 60秒的预测窗口，考虑到较低的速度 - - Logger::info("飞机和车辆垂直相交测试参数:"); - Logger::info(" 飞机: 位置=(" + std::to_string(aircraftPos.x) + ", " + std::to_string(aircraftPos.y) + - "), 速度=" + std::to_string(aircraftSpeed) + ", 航向=" + std::to_string(aircraftHeading)); - Logger::info(" 车辆: 位置=(" + std::to_string(vehiclePos.x) + ", " + std::to_string(vehiclePos.y) + - "), 速度=" + std::to_string(vehicleSpeed) + ", 航向=" + std::to_string(vehicleHeading)); - - // 计算速度分量 - double aircraftHeadingRad = aircraftHeading * M_PI / 180.0; - double vehicleHeadingRad = vehicleHeading * M_PI / 180.0; - double vax = aircraftSpeed * std::sin(aircraftHeadingRad); // 东向为x轴正方向 - double vay = aircraftSpeed * std::cos(aircraftHeadingRad); // 北向为y轴正方向 - double vvx = vehicleSpeed * std::sin(vehicleHeadingRad); - double vvy = vehicleSpeed * std::cos(vehicleHeadingRad); - - Logger::info("速度分量:"); - Logger::info(" 飞机: vx=" + std::to_string(vax) + ", vy=" + std::to_string(vay)); - Logger::info(" 车辆: vx=" + std::to_string(vvx) + ", vy=" + std::to_string(vvy)); - - // 计算相对运动方程的系数 - double dx = aircraftPos.x - vehiclePos.x; - double dy = aircraftPos.y - vehiclePos.y; - double dvx = vax - vvx; - double dvy = vay - vvy; - double safeDistance = aircraftSize + vehicleSize; - double a = dvx * dvx + dvy * dvy; - double b = 2.0 * (dx * dvx + dy * dvy); - double c = dx * dx + dy * dy - safeDistance * safeDistance; - - Logger::info("二次方程系数:"); - Logger::info(" 相对位置: dx=" + std::to_string(dx) + ", dy=" + std::to_string(dy)); - Logger::info(" 相对速度: dvx=" + std::to_string(dvx) + ", dvy=" + std::to_string(dvy)); - Logger::info(" 安全距离=" + std::to_string(safeDistance)); - Logger::info(" a=" + std::to_string(a) + ", b=" + std::to_string(b) + ", c=" + std::to_string(c)); - - auto prediction = detector_->predictTrajectoryCollision( - aircraftPos, aircraftSpeed, aircraftHeading, - vehiclePos, vehicleSpeed, vehicleHeading, - aircraftSize, vehicleSize, timeWindow - ); - - Logger::info("预测结果:"); - Logger::info(" willCollide=" + std::to_string(prediction.willCollide)); - Logger::info(" minDistance=" + std::to_string(prediction.minDistance)); - if (prediction.willCollide) { - Logger::info(" 碰撞点=(" + std::to_string(prediction.collisionPoint.x) + ", " + - std::to_string(prediction.collisionPoint.y) + ")"); - Logger::info(" 碰撞时间=" + std::to_string(prediction.timeToCollision)); - } - - // 验证结果 - EXPECT_TRUE(prediction.willCollide) << "应该检测到飞机和车的潜在碰撞"; - if (prediction.willCollide) { - // 考虑安全距离的影响，碰撞点会在实际交叉点之前 - // 飞机从(0,200)出发，速度12m/s，12.79秒到达x=164.22 - // 车辆从(200,100)出发，速度6m/s，12.79秒到达y=194.63 - EXPECT_NEAR(prediction.collisionPoint.x, 164.22, 5.0) << "碰撞点x坐标应该考虑安全距离的影响"; - EXPECT_NEAR(prediction.collisionPoint.y, 194.63, 5.0) << "碰撞点y坐标应该考虑安全距离的影响"; - - // 碰撞时间应该是12.79秒左右 - double expectedTime = 12.79; // 考虑安全距离后的预期碰撞时间 - EXPECT_NEAR(prediction.timeToCollision, expectedTime, 2.0) << "碰撞时间应该接近预期值"; - } - } + // 创建一个航空器和一个车辆 + Aircraft aircraft; + aircraft.flightNo = "TEST001"; + aircraft.position = {100.0, 100.0}; + aircraft.speed = 15.0; + aircraft.heading = 90.0; // 向东行驶 - // 测试场景7：斜向相交路径 - { - // 第一个物体从西南向东北运动 - Vector2D pos1{0, 0}; - double speed1 = 10; - double heading1 = 45; // 东北方向（45度） - - // 第二个物体从东南向西北运动 - Vector2D pos2{200, 0}; - double speed2 = 10; - double heading2 = 315; // 西北方向（315度） - - double size1 = 5; - double size2 = 5; - double timeWindow = 30; - - Logger::info("斜向相交路径测试参数:"); - Logger::info(" 物体1: 位置=(" + std::to_string(pos1.x) + ", " + std::to_string(pos1.y) + - "), 速度=" + std::to_string(speed1) + ", 航向=" + std::to_string(heading1)); - Logger::info(" 物体2: 位置=(" + std::to_string(pos2.x) + ", " + std::to_string(pos2.y) + - "), 速度=" + std::to_string(speed2) + ", 航向=" + std::to_string(heading2)); - - // 计算速度分量 - double heading1Rad = heading1 * M_PI / 180.0; - double heading2Rad = heading2 * M_PI / 180.0; - double v1x = speed1 * std::sin(heading1Rad); // 东向为x轴正方向 - double v1y = speed1 * std::cos(heading1Rad); // 北向为y轴正方向 - double v2x = speed2 * std::sin(heading2Rad); - double v2y = speed2 * std::cos(heading2Rad); - - Logger::info("速度分量:"); - Logger::info(" 物体1: vx=" + std::to_string(v1x) + ", vy=" + std::to_string(v1y)); - Logger::info(" 物体2: vx=" + std::to_string(v2x) + ", vy=" + std::to_string(v2y)); - - auto prediction = detector_->predictTrajectoryCollision( - pos1, speed1, heading1, - pos2, speed2, heading2, - size1, size2, timeWindow - ); - - // 验证结果 - EXPECT_TRUE(prediction.willCollide) << "斜向相交路径应该检测到碰撞"; - if (prediction.willCollide) { - // 考虑安全距离（两个物体各5米），碰撞检测会在实际交叉点之前触发 - // 由于两个物体速度相同且对称，碰撞点应该在中点附近，但会略低于理想位置 - EXPECT_NEAR(prediction.collisionPoint.x, 100.0, 5.0) << "碰撞点x坐标应该在预期位置"; - EXPECT_NEAR(prediction.collisionPoint.y, 95.0, 5.0) << "碰撞点y坐标应该考虑安全距离的影响"; - - // 到达碰撞点的时间 - // 物体1需要移动：√((100-0)² + (95-0)²) ≈ 137.8米 - // 速度为10m/s，所以时间应该约为13.78秒 - double expectedTime = 13.78; - EXPECT_NEAR(prediction.timeToCollision, expectedTime, 2.0) << "碰撞时间应该接近预期值"; - } - } + Vehicle vehicle; + vehicle.vehicleNo = "VEH001"; + vehicle.position = {120.0, 105.0}; // 在航空器前方偏北5米处 + vehicle.speed = 8.0; + vehicle.heading = 270.0; // 向西行驶（与航空器相向而行） + + std::cout << "\n=== 矩形碰撞检测测试（航空器-车辆） ===" << std::endl; + std::cout << "配置信息：" << std::endl; + std::cout << "- 航空器尺寸: " << SystemConfig::instance().collision_detection.prediction.aircraft_size << "米" << std::endl; + std::cout << "- 车辆尺寸: " << SystemConfig::instance().collision_detection.prediction.vehicle_size << "米" << std::endl; + + std::cout << "\n航空器信息：" << std::endl; + std::cout << "- 编号: " << aircraft.flightNo << std::endl; + std::cout << "- 位置: (" << aircraft.position.x << ", " << aircraft.position.y << ")" << std::endl; + std::cout << "- 速度: " << aircraft.speed << "米/秒" << std::endl; + std::cout << "- 朝向: " << aircraft.heading << "度" << std::endl; + + std::cout << "\n车辆信息：" << std::endl; + std::cout << "- 编号: " << vehicle.vehicleNo << std::endl; + std::cout << "- 位置: (" << vehicle.position.x << ", " << vehicle.position.y << ")" << std::endl; + std::cout << "- 速度: " << vehicle.speed << "米/秒" << std::endl; + std::cout << "- 朝向: " << vehicle.heading << "度" << std::endl; + + // 计算实际距离 + double dx = vehicle.position.x - aircraft.position.x; + double dy = vehicle.position.y - aircraft.position.y; + double distance = std::sqrt(dx*dx + dy*dy); + std::cout << "\n实际距离: " << distance << "米" << std::endl; + + // 计算相对速度 + double relativeSpeed = aircraft.speed + vehicle.speed; // 相向而行，速度相加 + std::cout << "相对速度: " << relativeSpeed << "米/秒" << std::endl; + + // 预计碰撞时间 + double timeToCollision = distance / relativeSpeed; + std::cout << "预计碰撞时间: " << timeToCollision << "秒" << std::endl; + + // 检测碰撞 + bool hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle); + std::cout << "\n碰撞检测结果: " << (hasCollision ? "是" : "否") << std::endl; + + // 由于距离较近且相向而行，应该检测到碰撞 + EXPECT_TRUE(hasCollision) << "相向而行且距离较近的航空器和车辆应该检测到碰撞"; + + // 测试边界情况：增加距离 + vehicle.position.x = 150.0; // 增加到50米距离 + hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle); + EXPECT_FALSE(hasCollision) << "距离较远时不应该检测到碰撞"; + + // 测试边界情况：静止状态 + vehicle.position.x = 110.0; // 距离缩短到10米 + vehicle.speed = 0.0; + aircraft.speed = 0.0; + hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle); + EXPECT_TRUE(hasCollision) << "静止状态下距离较近时应该检测到碰撞"; + + // 测试边界情况：垂直路径 + vehicle.position = {100.0, 120.0}; // 在航空器正北方 + vehicle.speed = 10.0; + vehicle.heading = 180.0; // 向南行驶 + aircraft.speed = 10.0; + aircraft.heading = 90.0; // 向东行驶 + hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle); + EXPECT_TRUE(hasCollision) << "垂直路径且距离较近时应该检测到碰撞"; +} + +TEST_F(CollisionDetectorTest, RectangleCollisionWithRotation) { + // 设置系统配置 + SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0; + + // 创建两个有一定角度的车辆 + Vehicle v1; + v1.vehicleNo = "VEH001"; + v1.position = {100.0, 100.0}; + v1.speed = 10.0; + v1.heading = 45.0; // 向东北行驶 + + Vehicle v2; + v2.vehicleNo = "VEH002"; + v2.position = {105.0, 105.0}; // 距离缩短到约7.07米 + v2.speed = 10.0; + v2.heading = 315.0; // 向西北行驶 + + // 检测碰撞 + bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); + + // 由于两车轨迹交叉且距离较近，应该检测到碰撞 + EXPECT_TRUE(hasCollision) << "不同角度的车辆相交时应该检测到碰撞"; + + // 将v2移动到更远的位置 + v2.position = {150.0, 150.0}; + hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); + + // 距离较远时不应该检测到碰撞 + EXPECT_FALSE(hasCollision) << "距离较远时不应该检测到碰撞"; +} + +TEST_F(CollisionDetectorTest, RectangleCollisionEdgeCases) { + // 设置系统配置 + SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0; + + Vehicle v1; + v1.vehicleNo = "VEH001"; + v1.position = {100.0, 100.0}; + v1.speed = 0.0; // 静止 + v1.heading = 90.0; + + Vehicle v2; + v2.vehicleNo = "VEH002"; + + // 测试1：两个静止车辆，刚好接触 + v2.position = {110.0, 100.0}; // 距离为10米（两车长度和） + v2.speed = 0.0; + v2.heading = 90.0; + + bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); + EXPECT_TRUE(hasCollision) << "两个静止且刚好接触的车辆应该检测到碰撞"; + + // 测试2：两个静止车辆，距离略大于安全距离 + v2.position = {111.0, 100.0}; // 距离为11米 + hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); + EXPECT_FALSE(hasCollision) << "两个静止且距离大于安全距离的车辆不应该检测到碰撞"; + + // 测试3：极低速度 + v2.position = {120.0, 100.0}; + v2.speed = 0.1; // 极低速度 + hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2); + EXPECT_FALSE(hasCollision) << "距离足够且速度极低的车辆不应该检测到碰撞"; } \ No newline at end of file diff --git a/tools/map_websocket.html b/tools/map_websocket.html index bfd79e8..9e16a98 100644 --- a/tools/map_websocket.html +++ b/tools/map_websocket.html @@ -37,8 +37,8 @@ .warning { color: #f90; } .command { color: #800080; } .vehicle-icon { - width: 10px; - height: 10px; + width: 20px; + height: 20px; background-color: black; clip-path: polygon(0% 0%, 100% 0%, 100% 100%, 0% 100%); border: 2px solid white; @@ -51,8 +51,8 @@ border: 2px solid white; } .special-vehicle-icon { - width: 10px; - height: 10px; + width: 20px; + height: 20px; background-color: orange; clip-path: polygon(0% 0%, 100% 0%, 100% 100%, 0% 100%); border: 2px solid white; @@ -65,10 +65,10 @@ border: 2px solid white; } .traffic-light { - width: 10px; - height: 10px; + width: 12px; + height: 12px; border-radius: 50%; - border: 1px solid white; + border: 2px solid white; z-index: 1000; } .traffic-light-red { @@ -85,6 +85,21 @@ text-align: center; white-space: nowrap; } + .command-text { + position: absolute; + top: 0; + left: 0; + width: 100%; + height: 100%; + display: flex; + justify-content: center; + align-items: center; + color: white; + font-size: 10px; + font-weight: bold; + pointer-events: none; + z-index: 1000; + } @@ -117,7 +132,7 @@ }; const EAST_INTERSECTION = { latitude: 36.361999, - longitude: 120.089003 + longitude: 120.090003 }; // 存储所有标记 @@ -126,22 +141,38 @@ const intersections = new Map(); // 创建自定义图标 - function createIcon(className) { + function createIcon(className, command = '') { let size; if (className.includes('aircraft')) { size = [50, 50]; // 50米正方形 } else if (className.includes('vehicle')) { - size = [10, 10]; // 10米正方形 + size = [20, 20]; // 10米正方形 } else if (className.includes('traffic-light')) { - size = [10, 10]; // 10像素的红绿灯 + size = [12, 12]; // 10像素的红绿灯 } else { size = [20, 20]; // 其他图标保持原样 } + // 如果有指令，创建带指令的图标 + if (command && className === 'vehicle-icon') { + const html = ` +
+
+
${command}
+
`; + return L.divIcon({ + html: html, + className: '', + iconSize: size, + iconAnchor: [size[0]/2, size[1]/2] + }); + } + + // 没有指令时，创建普通图标 return L.divIcon({ className: className, iconSize: size, - iconAnchor: [size[0]/2, size[1]/2] // 设置图标锚点为中心 + iconAnchor: [size[0]/2, size[1]/2] }); } @@ -207,6 +238,48 @@ } } + function updateVehicleCommand(vehicleId, commandType) { + console.log('更新车辆指令:', vehicleId, commandType); + + // 只处理无人车 + if (!vehicleId.startsWith('QN')) { + return; + } + + // 如果是 SIGNAL 指令，不更新显示 + if (commandType === 'SIGNAL') { + console.log('忽略 SIGNAL 指令'); + return; + } + + // 获取指令字母 + let commandText = ''; + switch(commandType) { + case 'ALERT': + commandText = 'A'; + break; + case 'WARNING': + commandText = 'W'; + break; + case 'RESUME': + commandText = 'R'; + break; + default: + commandText = ''; + } + + console.log('指令文本:', commandText); + + // 更新图标 + const marker = markers.get(vehicleId); + if (marker && commandText) { + console.log('设置新图标:', vehicleId, commandText); + marker.setIcon(createIcon('vehicle-icon', commandText)); + } else if (marker) { + marker.setIcon(createIcon('vehicle-icon')); + } + } + function connect() { if (ws) { log('已经连接，请先断开', 'error'); @@ -232,11 +305,6 @@ ws.onmessage = (event) => { try { const data = JSON.parse(event.data); - const formattedData = JSON.stringify(data, null, 2); - - // 根据消息类型使用不同的样式和处理方式 - let type = 'info'; - let message = '收到消息:\n' + formattedData; switch (data.type) { case 'position_update': @@ -251,6 +319,8 @@ break; case 'vehicle_command': type = 'command'; + console.log('收到指令消息:', data); // 调试日志 + updateVehicleCommand(data.vehicleId, data.commandType); // 为控制指令添加中文描述 const commandTypes = { 'SIGNAL': '信号灯指令', @@ -275,6 +345,8 @@ break; } + const formattedData = JSON.stringify(data, null, 2); + let message = '收到消息:\n' + formattedData; log(message, type); } catch (e) { log('收到消息: ' + event.data, 'info'); @@ -326,7 +398,7 @@ // 添加距离标签（以米为单位） const distance = Math.abs(Math.round(i / step) * 50); - if (distance > 0) { // 只显示非零���离 + if (distance > 0) { // 只显示非零距离 const label = L.divIcon({ className: 'distance-label', html: distance + 'm', diff --git a/tools/mock_server.py b/tools/mock_server.py index 2dcfa84..e3dfee4 100644 --- a/tools/mock_server.py +++ b/tools/mock_server.py @@ -36,11 +36,15 @@ DIST_50M = 50 # 两个路口的位置 WEST_INTERSECTION = {"longitude": 120.086003, "latitude": 36.361999} -EAST_INTERSECTION = {"longitude": 120.089003, "latitude": 36.361999} +EAST_INTERSECTION = {"longitude": 120.090003, "latitude": 36.361999} # 位置更新间隔（秒） UPDATE_INTERVAL = 1.0 +# 飞机和车辆尺寸（半径 米） +AIRCRAFT_SIZE_M = 30.0 +VEHICLE_SIZE_M = 10.0 + # 航空器数据 aircraft_data = [ { @@ -120,9 +124,9 @@ class VehicleState: if self.current_command == "ALERT": return command_type == "RESUME" - # RED 指令不能被 RESUME 解除 + # RED 指令可以被 GREEN 或更高优先级指令覆盖 if self.current_command == "RED": - return new_priority >= current_priority + return command_type == "GREEN" or new_priority > current_priority # WARNING 指令可以被 RESUME 解除 if self.current_command == "WARNING": @@ -150,19 +154,27 @@ class VehicleState: self.target_lat = target_lat self.target_lon = target_lon - # 如果是红绿灯状态，只更新状态不改变当前指令 + # 如果是红绿灯状态，更新状态和指令 if command_type in ["RED", "GREEN"]: self.traffic_light_state = command_type if command_type == "RED": self.current_command = command_type self.command_priority = priority_map[command_type] self.is_running = False + print(f"车辆 {self.vehicle_no} 收到红灯指令，停止运行") else: # GREEN - # 如果没有其他阻塞性指令，则清除当前指令 - if self.current_command not in ["ALERT", "WARNING", "RED"]: - self.current_command = None - self.command_priority = 0 + # 清除 RED 指令 + if self.current_command == "RED": + print(f"车辆 {self.vehicle_no} 收到绿灯指令，清除红灯指令") + + # 设置为绿灯指令 + self.current_command = command_type + self.command_priority = priority_map[command_type] + + # 如果没有其他阻塞性指令，允许移动 + if self.current_command not in ["ALERT", "WARNING"]: self.is_running = True + print(f"车辆 {self.vehicle_no} 收到绿灯指令，恢复运行") else: # 其他指令正常更新 self.current_command = command_type @@ -172,12 +184,16 @@ class VehicleState: def can_move(self): """检查车辆是否可以移动""" - # 如果是红灯，不能移动 - if self.traffic_light_state == "RED": + # 如果有告警指令，不能移动 + if self.current_command == "ALERT": return False - # 如果有告警或预警指令，不能移动 - if self.current_command in ["ALERT", "WARNING"]: + # 如果有预警指令，不能移动 + if self.current_command == "WARNING": + return False + + # 如果是红灯且当前指令是 RED，不能移动 + if self.traffic_light_state == "RED" and self.current_command == "RED": return False # 其他情况可以移动 @@ -256,9 +272,9 @@ def handle_vehicle_command(): "status": "ok", "message": "Special vehicle only responds to traffic light signals" }) - # 更新红绿灯状态 - vehicle_state.traffic_light_state = command_type - print(f"特勤车 {vehicle_id} 更新红绿灯状态: {command_type}") + # 更新红绿灯状态和指令状态 + vehicle_state.update_command(command_type, target_lat, target_lon) + print(f"特勤车 {vehicle_id} 更新状态: command={command_type}, traffic_light={vehicle_state.traffic_light_state}") return jsonify({ "status": "ok", "message": "Traffic light state updated" @@ -273,9 +289,9 @@ def handle_vehicle_command(): print(f"指令优先级过低: vehicle={vehicle_id}, current_priority={vehicle_state.command_priority}, " f"command={command_type}") return jsonify({ - "status": "error", + "status": "warning", "message": "Command priority too low" - }), 400 + }) # 处理不同类型的指令 if command_type in ["ALERT", "WARNING"]: @@ -322,7 +338,27 @@ def handle_vehicle_command(): vehicle_state.brake_mode = None vehicle_state.target_lat = None vehicle_state.target_lon = None - + + # 更新车辆运行状态 + vehicle_state.is_running = vehicle_state.can_move() + if vehicle_state.is_running: + print(f"车辆 {vehicle_id} 恢复运行") + current_vehicle["speed"] = DEFAULT_VEHICLE_SPEED + + # 记录状态变化但不更新指令 + vehicle_state.command_reason = reason + vehicle_state.last_command_time = time.time() + + print(f"Vehicle {vehicle_id} state updated: running={vehicle_state.is_running}, " + f"command={vehicle_state.current_command}, traffic_light={vehicle_state.traffic_light_state}, " + f"reason={reason}, priority={vehicle_state.command_priority}, " + f"target_speed={vehicle_state.target_speed}, brake_mode={vehicle_state.brake_mode}") + + return jsonify({ + "status": "ok", + "message": "Command executed successfully" + }) + # 更新车辆状态 vehicle_state.update_command(command_type, target_lat, target_lon) vehicle_state.command_reason = reason @@ -382,37 +418,30 @@ def get_front_traffic_light(vehicle, distance_to_west, distance_to_east): # QN001 的路线：西路口北侧 -> 东 -> 北 if vehicle["phase"] == 0: # 南北移动 - if vehicle["direction"] == -1: # 向南 + # 在西路口以南时，向北移动需要判断西路口红绿灯 + if vehicle["direction"] == 1 and vehicle["latitude"] < WEST_INTERSECTION["latitude"]: return traffic_light_data[0], distance_to_west # 西路口红绿灯 - else: # 向北 + # 在西路口以北时，向南移动需要判断西路口红绿灯 + elif vehicle["direction"] == -1 and vehicle["latitude"] > WEST_INTERSECTION["latitude"]: return traffic_light_data[0], distance_to_west # 西路口红绿灯 else: # 东西移动 - if vehicle["direction"] == 1: # 向东 - return None, float('inf') # 已过西路口，无红绿灯 - else: # 向西 + # 在西路口以西时，向东移动需要判断西路口红绿灯 + if vehicle["direction"] == 1 and vehicle["longitude"] < WEST_INTERSECTION["longitude"]: return traffic_light_data[0], distance_to_west # 西路口红绿灯 - elif vehicle["vehicleNo"] == "QN002": - if vehicle["direction"] == 1: # 向东 - return traffic_light_data[1], distance_to_east # 东路口红绿灯 - else: # 向西 - return traffic_light_data[1], distance_to_east # 东路口红绿灯 - elif vehicle["vehicleNo"] == "TQ001": - # 特勤车：先东西后南北 - if "phase" not in vehicle: - vehicle["phase"] = 0 - - if vehicle["phase"] == 0: # 东西移动 - if vehicle["direction"] == 1: # 向东 - return None, float('inf') # 已过西路口，无红绿灯 - else: # 向西 + # 在西路口以东时，向西移动需要判断西路口红绿灯 + elif vehicle["direction"] == -1 and vehicle["longitude"] > WEST_INTERSECTION["longitude"]: return traffic_light_data[0], distance_to_west # 西路口红绿灯 - else: # 南北移动 - if vehicle["direction"] == 1: # 向北 - return traffic_light_data[0], distance_to_west # 西路口红绿灯 - else: # 向南 - return None, float('inf') # 已过西路口，无红绿灯 - return None, float('inf') # 默认返回无红绿灯 + elif vehicle["vehicleNo"] == "QN002": + # 在东路口以西时，向东移动需要判断东路口红绿灯 + if vehicle["direction"] == 1 and vehicle["longitude"] < EAST_INTERSECTION["longitude"]: + return traffic_light_data[1], distance_to_east # 东路口红绿灯 + # 在东路口以东时，向西移动需要判断东路口红绿灯 + elif vehicle["direction"] == -1 and vehicle["longitude"] > EAST_INTERSECTION["longitude"]: + return traffic_light_data[1], distance_to_east # 东路口红绿灯 + + # 其他情况，表示车辆已经通过路口或不需要判断红绿灯 + return None, float('inf') def update_vehicle_position(vehicle, elapsed_time): """更新车辆位置""" @@ -426,23 +455,36 @@ def update_vehicle_position(vehicle, elapsed_time): vehicle["phase"] = 0 print(f"初始化车辆 {vehicle['vehicleNo']} 的 phase 属性为 0") + # 获取前方红绿灯状态和距离 + traffic_light, distance = get_front_traffic_light(vehicle, + calculate_distance_to_intersection(vehicle, WEST_INTERSECTION), + calculate_distance_to_intersection(vehicle, EAST_INTERSECTION)) + # 特勤车只响应红绿灯 if vehicle["vehicleNo"].startswith("TQ"): - # 只有红灯时才停止 - if vehicle_state.traffic_light_state == "RED": + # 红灯且距离停止线小于等于 50 米时停车 + if traffic_light and traffic_light["state"] == 0 and distance <= DIST_50M: vehicle["speed"] = 0 - print(f"特勤车 {vehicle['vehicleNo']} 遇红灯停止") + print(f"特勤车 {vehicle['vehicleNo']} 遇红灯在停止线前停车，距路口 {distance:.1f}米") return # 其他情况正常行驶 vehicle["speed"] = DEFAULT_VEHICLE_SPEED print(f"特勤车 {vehicle['vehicleNo']} 正常行驶，速度={vehicle['speed']}km/h") else: # 普通车辆的移动逻辑 - if not vehicle_state.can_move(): - vehicle["speed"] = 0 - print(f"车辆 {vehicle['vehicleNo']} 不能移动: command={vehicle_state.current_command}, " - f"traffic_light={vehicle_state.traffic_light_state}") - return + # 先判断是否在红灯影响范围内（50米） + if traffic_light and traffic_light["state"] == 0 and distance <= DIST_50M: + # 在红灯影响范围内，检查是否需要停车 + if not vehicle_state.can_move(): + vehicle["speed"] = 0 + print(f"车辆 {vehicle['vehicleNo']} 遇红灯在停止线前停车，距路口 {distance:.1f}米") + return + else: + # 不在红灯影响范围内，只检查其他指令 + if vehicle_state.current_command in ["ALERT", "WARNING"]: + vehicle["speed"] = 0 + print(f"车辆 {vehicle['vehicleNo']} 不能移动: command={vehicle_state.current_command}") + return # 可以移动，设置正常速度 vehicle["speed"] = DEFAULT_VEHICLE_SPEED @@ -457,6 +499,7 @@ def update_vehicle_position(vehicle, elapsed_time): # 计算经纬度变化 dlat, dlon = meters_to_degrees(distance, vehicle["latitude"]) + # 更新位置 if vehicle["vehicleNo"].startswith("QN"): # 无人车的路径更新逻辑 if vehicle["vehicleNo"] == "QN001": @@ -547,24 +590,6 @@ def update_aircraft_position(aircraft, elapsed_time): aircraft["direction"] = 1 # 向北 aircraft["latitude"] = new_lat -def update_traffic_light_for_aircraft(): - """根据航空器位置更新东路口红绿灯状态""" - if not aircraft_data: - return - - aircraft = aircraft_data[0] - # 计算到东路口的距离（米） - lat_diff = abs(aircraft["latitude"] - EAST_INTERSECTION["latitude"]) * 111319.9 - - # 更新TL002（东路口）的状态 - for light in traffic_light_data: - if light["id"] == "TL002": - if lat_diff <= 50.0: # 距离小于50米时为红灯 - light["state"] = 1 # 红灯 - else: - light["state"] = 1 # 绿灯 - break - # 定义路口信息 INTERSECTIONS = { "TL001": { @@ -624,22 +649,38 @@ def get_flight_positions(): return jsonify(aircraft_data) +def switch_traffic_light_state(): + """统一处理红绿灯状态切换""" + global last_light_switch_time + current_time = time.time() + + # 西路口红绿灯每15秒切换一次 + elapsed_since_switch = current_time - last_light_switch_time + if elapsed_since_switch >= 15: + traffic_light_data[0]["state"] = 1 if traffic_light_data[0]["state"] == 0 else 0 + last_light_switch_time = current_time + print(f"西路口红绿灯状态切换为: {'绿灯' if traffic_light_data[0]['state'] == 1 else '红灯'}") + + # 更新东路口红绿灯（根据航空器位置） + if aircraft_data: + aircraft = aircraft_data[0] + lat_diff = abs(aircraft["latitude"] - EAST_INTERSECTION["latitude"]) * 111319.9 + + old_state = traffic_light_data[1]["state"] + traffic_light_data[1]["state"] = 1 if lat_diff > (AIRCRAFT_SIZE_M + DIST_50M) else 0 + + if old_state != traffic_light_data[1]["state"]: + print(f"东路口红绿灯状态切换为: {'绿灯' if traffic_light_data[1]['state'] == 1 else '红灯'}") + @app.route('/api/getCurrentVehiclePositions') def get_vehicle_positions(): - global last_vehicle_update_time, last_light_switch_time + global last_vehicle_update_time current_time = time.time() elapsed_time = current_time - last_vehicle_update_time try: - # 更新红绿灯状态 - elapsed_since_switch = current_time - last_light_switch_time - if elapsed_since_switch >= 1500: # 每15秒切换一次 - traffic_light_data[0]["state"] = 1 if traffic_light_data[0]["state"] == 0 else 1 - last_light_switch_time = current_time - print(f"西路口红绿灯状态切换为: {'绿灯' if traffic_light_data[0]['state'] == 1 else '红灯'}") - - # 根据航空器位置更新东路口红绿灯 - update_traffic_light_for_aircraft() + # 统一处理红绿灯状态切换 + switch_traffic_light_state() # 只在达到更新间隔时更新位置 if elapsed_time >= UPDATE_INTERVAL: @@ -656,35 +697,14 @@ def get_vehicle_positions(): @app.route('/api/getTrafficLightSignals') def get_traffic_light_signals(): - global last_light_switch_time current_time = time.time() # 更新时间戳 for light in traffic_light_data: light["timestamp"] = int(current_time * 1000) - # TL001（西路口）状态每15秒切换一次 - elapsed_since_switch = current_time - last_light_switch_time - if elapsed_since_switch >= 1500: # 每15秒切换一次 - traffic_light_data[0]["state"] = 1 if traffic_light_data[0]["state"] == 0 else 1 - last_light_switch_time = current_time - print(f"西路口红绿灯状态切换为: {'绿灯' if traffic_light_data[0]['state'] == 1 else '红灯'}") - - # 根据航空器位置更新TL002（东路口） - if aircraft_data: - aircraft = aircraft_data[0] - # 计算到东路口的距离（米） - lat_diff = abs(aircraft["latitude"] - EAST_INTERSECTION["latitude"]) * 111319.9 - - # 更新TL002（东路口）的状态 - old_state = traffic_light_data[1]["state"] - if lat_diff <= 50.0: # 距离小于50米时为红灯 - traffic_light_data[1]["state"] = 1 # 红灯 - else: - traffic_light_data[1]["state"] = 1 # 绿灯 - - if old_state != traffic_light_data[1]["state"]: - print(f"东路口红绿灯状态切换为: {'绿灯' if traffic_light_data[1]['state'] == 1 else '红灯'}") + # 统一处理红绿灯状态切换 + switch_traffic_light_state() return jsonify(traffic_light_data)