From 6ce6e36d0770621d9d4d94c53e61e1af208bb5b8 Mon Sep 17 00:00:00 2001 From: Tian jianyong <11429339@qq.com> Date: Mon, 16 Dec 2024 18:37:56 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86=E6=9C=BA=E5=9C=BA?= =?UTF-8?q?=E7=9A=84=E8=BE=B9=E7=95=8C=E5=92=8C=E8=B7=AF=E5=8F=A3=E7=9A=84?= =?UTF-8?q?=E9=85=8D=E7=BD=AE=EF=BC=8C=E4=BD=BF=E7=94=A8=E7=9C=9F=E5=AE=9E?= =?UTF-8?q?=E5=9D=90=E6=A0=87=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- config/airport_bounds.json | 6 +- config/intersections.json | 16 +- config/system_config.json | 22 +- docs/airport_bounds_description.md | 2 +- src/config/AirportBounds.cpp | 34 +- src/config/SystemConfig.cpp | 2 + src/config/SystemConfig.h | 15 +- src/core/System.cpp | 2 + src/detector/CollisionDetector.cpp | 21 +- src/detector/CollisionDetector.h | 4 +- src/spatial/CoordinateConverter.cpp | 20 +- src/types/RiskLevel.h | 7 - src/types/VehicleCommand.h | 4 +- tests/BasicCollisionTest.cpp | 55 ++- tools/map_websocket.html | 283 ++++++++++++--- tools/mock_server.py | 378 ++++++++++++------- tools/mock_server.py.bak | 538 ---------------------------- 17 files changed, 629 insertions(+), 780 deletions(-) delete mode 100644 src/types/RiskLevel.h delete mode 100644 tools/mock_server.py.bak diff --git a/config/airport_bounds.json b/config/airport_bounds.json index d0e93b7..fb00df4 100644 --- a/config/airport_bounds.json +++ b/config/airport_bounds.json @@ -122,18 +122,18 @@ "config": { "collision_radius": { "aircraft": 50.0, - "special": 50.0, + "special": 25.0, "unmanned": 25.0 }, "height_threshold": 10.0, "warning_zone_radius": { "aircraft": 100.0, - "special": 100.0, + "special": 50.0, "unmanned": 50.0 }, "alert_zone_radius": { "aircraft": 50.0, - "special": 50.0, + "special": 25.0, "unmanned": 25.0 } } diff --git a/config/intersections.json b/config/intersections.json index e924674..bdde6ba 100644 --- a/config/intersections.json +++ b/config/intersections.json @@ -1,25 +1,25 @@ { "intersections": [ { - "id": "西路口", + "id": "T2路口", "name": "无人车与特勤车交叉路口", "trafficLightId": "TL001", "position": { - "longitude": 120.086003, - "latitude": 36.361999, - "altitude": 12.5 + "longitude": 120.08502054, + "latitude": 36.35448347, + "altitude": 9.543 }, "radius": 100.0, "stopDistance": 50.0 }, { - "id": "东路口", + "id": "T6路口", "name": "无人车与飞机交叉路口", "trafficLightId": "TL002", "position": { - "longitude": 120.090003, - "latitude": 36.361999, - "altitude": 12.5 + "longitude": 120.08649105, + "latitude": 36.35074527, + "altitude": 9.778 }, "radius": 100.0, "stopDistance": 50.0 diff --git a/config/system_config.json b/config/system_config.json index 4650c3e..6733ca4 100644 --- a/config/system_config.json +++ b/config/system_config.json @@ -4,9 +4,22 @@ "iata": "TAO", "icao": "ZSQD", "reference_point": { - "latitude": 36.361999, - "longitude": 120.088003 - } + "latitude": 36.34807893, + "longitude": 120.08201044 + }, + "coordinate_points": [ + {"point": "T1", "longitude": 120.0868853, "latitude": 36.35496367}, + {"point": "T2", "longitude": 120.08502054, "latitude": 36.35448347}, + {"point": "T3", "longitude": 120.08341044, "latitude": 36.35406879}, + {"point": "T4", "longitude": 120.08558121, "latitude": 36.35305878}, + {"point": "T5", "longitude": 120.08400957, "latitude": 36.35265197}, + {"point": "T6", "longitude": 120.08649105, "latitude": 36.35074527}, + {"point": "T7", "longitude": 120.08562915, "latitude": 36.35052372}, + {"point": "T8", "longitude": 120.08676664, "latitude": 36.35004529}, + {"point": "T9", "longitude": 120.08520616, "latitude": 36.34964473}, + {"point": "T10", "longitude": 120.08710569, "latitude": 36.34917893}, + {"point": "T11", "longitude": 120.0873865, "latitude": 36.3509885} + ] }, "data_source": { "host": "localhost", @@ -37,7 +50,8 @@ "prediction": { "time_window": 20.0, "vehicle_size": 20.0, - "aircraft_size": 60.0 + "aircraft_size": 60.0, + "min_unmanned_speed": 1.0 } }, "logging": { diff --git a/docs/airport_bounds_description.md b/docs/airport_bounds_description.md index 23b903d..74e0610 100644 --- a/docs/airport_bounds_description.md +++ b/docs/airport_bounds_description.md @@ -77,7 +77,7 @@ ## 注意事项 1. 所有距离单位均为米 -2. 坐标系统使用左上角为原点 +2. 坐标系统使用左下角为原点 3. 各区域的参数需要根据实际机场情况调整 4. 碰撞检测半径应考虑车辆和航空器的实际尺寸 5. 高度阈值应考虑实际运营需求和安全裕度 diff --git a/src/config/AirportBounds.cpp b/src/config/AirportBounds.cpp index 4dba8e4..78cdd8f 100644 --- a/src/config/AirportBounds.cpp +++ b/src/config/AirportBounds.cpp @@ -75,15 +75,35 @@ void AirportBounds::loadConfig(const std::string& configFile) { // 加载区域配置 auto& config = value["config"]; + auto& collision_radius = config["collision_radius"]; + auto& warning_zone_radius = config["warning_zone_radius"]; + auto& alert_zone_radius = config["alert_zone_radius"]; + + RadiusConfig collision_config = { + collision_radius["aircraft"].get(), + collision_radius["special"].get(), + collision_radius["unmanned"].get() + }; + + RadiusConfig warning_config = { + warning_zone_radius["aircraft"].get(), + warning_zone_radius["special"].get(), + warning_zone_radius["unmanned"].get() + }; + + RadiusConfig alert_config = { + alert_zone_radius["aircraft"].get(), + alert_zone_radius["special"].get(), + alert_zone_radius["unmanned"].get() + }; + areaConfigs_[type] = { - config["vehicle_collision_radius"].get(), - config["aircraft_collision_radius"].get(), + collision_config, config["height_threshold"].get(), - config["warning_zone_radius"].get(), - config["alert_zone_radius"].get() + warning_config, + alert_config }; } - } catch (const json::exception& e) { Logger::error("JSON解析错误: ", e.what()); @@ -102,8 +122,8 @@ AreaType AirportBounds::getAreaType(const Vector2D& position) const { return type; } } - // 默认返回服务区 - return AreaType::SERVICE; + // 默认返回测试区 + return AreaType::TEST_ZONE; } const AreaConfig& AirportBounds::getAreaConfig(AreaType type) const { diff --git a/src/config/SystemConfig.cpp b/src/config/SystemConfig.cpp index 340c71c..11762c6 100644 --- a/src/config/SystemConfig.cpp +++ b/src/config/SystemConfig.cpp @@ -18,6 +18,7 @@ void SystemConfig::load(const std::string& filename) { airport.icao = j["airport"]["icao"]; airport.reference_point.latitude = j["airport"]["reference_point"]["latitude"]; airport.reference_point.longitude = j["airport"]["reference_point"]["longitude"]; + airport.coordinate_points = j["airport"]["coordinate_points"].get>(); // 加载数据源配置 data_source.host = j["data_source"]["host"]; @@ -43,6 +44,7 @@ void SystemConfig::load(const std::string& filename) { collision_detection.prediction.time_window = j["collision_detection"]["prediction"]["time_window"]; collision_detection.prediction.vehicle_size = j["collision_detection"]["prediction"]["vehicle_size"]; collision_detection.prediction.aircraft_size = j["collision_detection"]["prediction"]["aircraft_size"]; + collision_detection.prediction.min_unmanned_speed = j["collision_detection"]["prediction"]["min_unmanned_speed"]; // 加载日志配置 logging.level = j["logging"]["level"]; diff --git a/src/config/SystemConfig.h b/src/config/SystemConfig.h index 1d3c520..7e504ad 100644 --- a/src/config/SystemConfig.h +++ b/src/config/SystemConfig.h @@ -28,6 +28,15 @@ public: double latitude; double longitude; } reference_point; + + // 定义坐标点结构体 + struct CoordinatePoint { + std::string point; + double latitude; + double longitude; + }; + + std::vector coordinate_points; } airport; struct DataSource { @@ -58,6 +67,7 @@ public: double time_window; double vehicle_size; double aircraft_size; + double min_unmanned_speed; // 无人车最低速度 (m/s) } prediction; } collision_detection; @@ -83,4 +93,7 @@ public: private: SystemConfig() = default; // 私有构造函数 friend class System; // 允许 System 类访问私有成员 -}; \ No newline at end of file +}; + +// JSON 序列化支持 +NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(SystemConfig::Airport::CoordinatePoint, point, latitude, longitude) \ No newline at end of file diff --git a/src/core/System.cpp b/src/core/System.cpp index 699f4d0..0150c5d 100644 --- a/src/core/System.cpp +++ b/src/core/System.cpp @@ -382,6 +382,7 @@ void System::processCollisions(const std::vector& risks) { cmd.relativeSpeed = risk.relativeSpeed; cmd.relativeMotionX = risk.relativeMotion.x; cmd.relativeMotionY = risk.relativeMotion.y; + cmd.minDistance = risk.minDistance; } broadcastVehicleCommand(cmd); @@ -411,6 +412,7 @@ void System::processCollisions(const std::vector& risks) { cmd.relativeSpeed = risk.relativeSpeed; cmd.relativeMotionX = risk.relativeMotion.x; cmd.relativeMotionY = risk.relativeMotion.y; + cmd.minDistance = risk.minDistance; } broadcastVehicleCommand(cmd); diff --git a/src/detector/CollisionDetector.cpp b/src/detector/CollisionDetector.cpp index e605d8f..882cf6b 100644 --- a/src/detector/CollisionDetector.cpp +++ b/src/detector/CollisionDetector.cpp @@ -274,6 +274,23 @@ collision::CollisionResult CollisionDetector::checkCollision( double dy = obj2.position.y - obj1.position.y; 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; // 使用配置的最低速度 + } + if (obj2.type == MovingObjectType::UNMANNED && speed2_calc < 0.1) { + 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 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)) { collision::CollisionResult result; @@ -432,7 +449,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( ", det=", det ); - // 对于交叉路径，们可以直接计算交点时间 + // 对于交叉路径，们可以直接计算交时间 // 解方程组： // x1 + vx1 * t = x2 + vx2 * t // y1 + vy1 * t = y2 + vy2 * t @@ -654,7 +671,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( prev_distance = distance; - // 如果相对速度很小，且距��增加，可以提前退出 + // 如果相对速度很小，且距离增加，可以提前退出 if (rel_speed < 0.1 && i > 1) { if (distance > prev_distance) { break; diff --git a/src/detector/CollisionDetector.h b/src/detector/CollisionDetector.h index ef9a2b2..fdf2378 100644 --- a/src/detector/CollisionDetector.h +++ b/src/detector/CollisionDetector.h @@ -14,7 +14,7 @@ enum class RiskLevel { NONE = 0, // 无风险 WARNING = 1, // 预警：进入预警区域 - CRITICAL = 2, // 告警：进入危险区域 + CRITICAL = 2 // 告警：进入危险区域 }; // 预警区域类型 @@ -29,7 +29,7 @@ struct CollisionRisk { std::string id1, id2; // 碰撞物体的ID RiskLevel level; // 风险等级 double distance; // 当前距离 - double minDistance; // 预测的最小距离 + double minDistance; // 预测的最小距离（两个中心点之间的距离） double relativeSpeed; // 相对速度 Vector2D relativeMotion; // 相对运动向量 WarningZoneType zoneType; // 预警区域类型 diff --git a/src/spatial/CoordinateConverter.cpp b/src/spatial/CoordinateConverter.cpp index 14b7103..dcbe02e 100644 --- a/src/spatial/CoordinateConverter.cpp +++ b/src/spatial/CoordinateConverter.cpp @@ -1,15 +1,13 @@ #include "spatial/CoordinateConverter.h" +#include "config/SystemConfig.h" #include -// 青岛胶东机场参考点 -const double REF_LAT = 36.361999; // 北纬36°21'43.2" -const double REF_LON = 120.088003; // 东经120°05'16.8" - // 地球半径（米） const double EARTH_RADIUS = 6378137.0; CoordinateConverter::CoordinateConverter() noexcept { - setReferencePoint(REF_LAT, REF_LON); + const auto& refPoint = SystemConfig::instance().airport.reference_point; + setReferencePoint(refPoint.latitude, refPoint.longitude); } void CoordinateConverter::setReferencePoint(double lat, double lon) noexcept { @@ -32,21 +30,13 @@ Vector2D CoordinateConverter::toLocalXY(double lat, double lon) const noexcept { result.x = EARTH_RADIUS * cos_ref_lat_ * d_lon; // 使用预计算的余弦值 result.y = EARTH_RADIUS * d_lat; - // 将原点平移到机场西南角（2000, 1000） - result.x += 2000.0; - result.y += 1000.0; - return result; } GeoPosition CoordinateConverter::toLatLon(const Vector2D& pos) const noexcept { - // 将坐标原点平移回参考点 - double x = pos.x - 2000.0; - double y = pos.y - 1000.0; - // 计算经纬度差（弧度） - double d_lon = x / (EARTH_RADIUS * cos_ref_lat_); // 使用预计算的余弦值 - double d_lat = y / EARTH_RADIUS; + double d_lon = pos.x / (EARTH_RADIUS * cos_ref_lat_); // 使用预计算的余弦值 + double d_lat = pos.y / EARTH_RADIUS; // 计算实际经纬度（弧度） double lon_rad = d_lon + ref_lon_; diff --git a/src/types/RiskLevel.h b/src/types/RiskLevel.h deleted file mode 100644 index 5f5edf2..0000000 --- a/src/types/RiskLevel.h +++ /dev/null @@ -1,7 +0,0 @@ -#pragma once - -enum class RiskLevel { - NONE, // 无风险 - WARNING, // 预警 - CRITICAL // 告警 -}; \ No newline at end of file diff --git a/src/types/VehicleCommand.h b/src/types/VehicleCommand.h index 7945aa5..318c13c 100644 --- a/src/types/VehicleCommand.h +++ b/src/types/VehicleCommand.h @@ -39,6 +39,7 @@ struct VehicleCommand { double relativeSpeed; // 相对速度（仅当 type 为 ALERT/WARNING 时有效） double relativeMotionX; // 相对运动 X 分量（仅当 type 为 ALERT/WARNING 时有效） double relativeMotionY; // 相对运动 Y 分量（仅当 type 为 ALERT/WARNING 时有效） + double minDistance; // 最小距离（仅当 type 为 ALERT/WARNING 时有效） VehicleCommand() : type(CommandType::RESUME), @@ -49,7 +50,8 @@ struct VehicleCommand { longitude(0), relativeSpeed(0), relativeMotionX(0), - relativeMotionY(0) {} + relativeMotionY(0), + minDistance(0) {} }; #endif // AIRPORT_TYPES_VEHICLE_COMMAND_H \ No newline at end of file diff --git a/tests/BasicCollisionTest.cpp b/tests/BasicCollisionTest.cpp index b1c4037..8674f66 100644 --- a/tests/BasicCollisionTest.cpp +++ b/tests/BasicCollisionTest.cpp @@ -79,13 +79,13 @@ TEST_F(BasicCollisionTest, StaticCollision) { auto result = detector_->checkCollision(v1, v2, 30.0); - EXPECT_TRUE(result.willCollide) << "距离小于碰撞半径的静止物��应该检测为碰撞"; + EXPECT_TRUE(result.willCollide) << "距离小于碰撞半径的静止物体应该检测为碰撞"; EXPECT_DOUBLE_EQ(result.timeToCollision, 0.0) << "静止物体的碰撞时间应该为0"; EXPECT_EQ(result.type, collision::CollisionType::STATIC) << "应该识别为静态碰撞"; // 增加更多验证 EXPECT_DOUBLE_EQ(result.minDistance, 20.0) << "最小距离应该是当前距离20米"; - EXPECT_DOUBLE_EQ(result.timeToMinDistance, 0.0) << "静止物体的最小距离时间应该为0"; + EXPECT_DOUBLE_EQ(result.timeToMinDistance, 0.0) << "静止物体最小距离时间应该为0"; EXPECT_NEAR(result.collisionPoint.x, 110.0, 0.1) << "碰撞点应该在两车中点"; EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点应该在同一水平线上"; @@ -127,7 +127,7 @@ TEST_F(BasicCollisionTest, HeadOnCollision) { EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点应该在同一水平线上"; // 增加更多验证 - EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是碰撞半径之和50米"; + EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离��该是碰撞半径之和50米"; EXPECT_NEAR(result.timeToMinDistance, 2.5, 0.1) << "最小距离时间应该等于碰撞时间"; // 验证物体状态 @@ -254,7 +254,7 @@ TEST_F(BasicCollisionTest, PerpendicularCrossingPaths) { double collision_time = 1.46; // 根据实际计算得到 Vector2D collision_point = {117.68, 132.32}; // 根据实际计算得到 - EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25��，碰时间应该接近1.46秒"; + EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25米，碰时间应该接近1.46秒"; EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在117.68"; EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在132.32"; @@ -332,7 +332,7 @@ TEST_F(BasicCollisionTest, TailgatingMotion) { // 验证会发生碰撞 EXPECT_TRUE(result.willCollide) << "后车速度大于前车，应该预测到碰撞"; - // 验证碰撞时间（初始距离60米，相对速度5m/s，安全距离50米，需要缩短10米，所以碰撞时间应该是2秒） + // 验证碰撞时间（初始距离60米，相对速��5m/s，安全距离50米，需要缩短10米，所以碰撞时间应该是2秒） EXPECT_NEAR(result.timeToCollision, 2.0, 0.1) << "碰撞时间应该接近2秒"; // 验证最小距离（应该是安全距离） @@ -360,4 +360,49 @@ TEST_F(BasicCollisionTest, TailgatingMotion) { EXPECT_NEAR(result.object2State.position.y, 100.0, 0.1); EXPECT_DOUBLE_EQ(result.object2State.speed, 15.0); EXPECT_DOUBLE_EQ(result.object2State.heading, 90.0); +} + +// 6. QN002 与 AC001 交叉路径测试（实际场景） +TEST_F(BasicCollisionTest, QN002AircraftCrossing) { + // 创建 AC001 航空器，从 T7 出发 + Aircraft aircraft; + aircraft.flightNo = "AC001"; + aircraft.position = {0.0, 0.0}; // T7 点的相对坐标 + aircraft.speed = 50.0 / 3.6; // 转换为米/秒 + aircraft.heading = 45.0; // 根据 T7 到 T11 的方向计算 + aircraft.type = MovingObjectType::AIRCRAFT; + + // 创建 QN002 无人车，从 T4 到 T8 路径上的一点出发 + Vehicle qn002; + qn002.vehicleNo = "QN002"; + qn002.position = {-2.0, 120.0}; // 在 T4-T8 路径上，距离 T7 约 120 米（小于检测范围 150 米） + qn002.speed = 36.0 / 3.6; // 转换为米/秒 + qn002.heading = 135.0; // 根据 T4 到 T8 的方向计算 + qn002.type = MovingObjectType::UNMANNED; + qn002.isControllable = true; + + // 检测碰撞 + auto result = detector_->checkCollision(aircraft, qn002, 30.0); + + // 验证是否检测到碰撞 + EXPECT_TRUE(result.willCollide) << "QN002 与 AC001 的交叉路径应该检测为碰撞"; + EXPECT_EQ(result.type, collision::CollisionType::CROSSING) << "应该识别为交叉碰撞"; + + // 记录详细的测试信息 + Logger::debug("QN002-AC001 碰撞测试结果:"); + Logger::debug("碰撞类型: ", static_cast(result.type)); + Logger::debug("最小距离: ", result.minDistance, "m"); + Logger::debug("碰撞时间: ", result.timeToCollision, "s"); + Logger::debug("碰撞点: (", result.collisionPoint.x, ",", result.collisionPoint.y, ")"); + + // 验证碰撞参数 + EXPECT_GT(result.timeToCollision, 0.0) << "碰撞时间应该大于0"; + EXPECT_LT(result.timeToCollision, 30.0) << "碰撞时间应该在预测窗口内"; + EXPECT_LE(result.minDistance, 75.0) << "最小距离应该小于预警距离"; + + // 验证物体状态 + EXPECT_DOUBLE_EQ(result.object1State.speed, 50.0 / 3.6); + EXPECT_DOUBLE_EQ(result.object1State.heading, 45.0); + EXPECT_DOUBLE_EQ(result.object2State.speed, 36.0 / 3.6); + EXPECT_DOUBLE_EQ(result.object2State.heading, 135.0); } \ No newline at end of file diff --git a/tools/map_websocket.html b/tools/map_websocket.html index 9e16a98..8e9c4a8 100644 --- a/tools/map_websocket.html +++ b/tools/map_websocket.html @@ -77,6 +77,30 @@ .traffic-light-green { background-color: green; } + .countdown-label { + background: #000; + border: 1px solid #666; + border-radius: 2px; + font-family: "Digital-7", "DSEG7 Classic", Monaco, monospace; + font-weight: normal; + font-size: 12px; + text-align: center; + white-space: nowrap; + padding: 1px 2px; + line-height: 14px; + min-width: 28px; + box-shadow: 0 0 2px rgba(0,0,0,0.5); + display: flex; + align-items: center; + justify-content: center; + height: 14px; + } + .countdown-red { + color: #ff3333; + } + .countdown-green { + color: #33ff33; + } .distance-label { background: none; border: none; @@ -119,20 +143,56 @@ const messagesDiv = document.getElementById('messages'); // 初始化地图 - const map = L.map('map').setView([36.361999, 120.088503], 17); + const map = L.map('map').setView([36.35305878, 120.08558121], 17); L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', { maxZoom: 19, attribution: '© OpenStreetMap contributors' }).addTo(map); // 定义路口坐标 - const WEST_INTERSECTION = { - latitude: 36.361999, - longitude: 120.086003 + const T1_INTERSECTION = { + latitude: 36.35496367, + longitude: 120.0868853 }; - const EAST_INTERSECTION = { - latitude: 36.361999, - longitude: 120.090003 + + const T2_INTERSECTION = { + latitude: 36.35448347, + longitude: 120.08502054 + }; + + const T3_INTERSECTION = { + latitude: 36.35406879, + longitude: 120.08341044 + }; + + const T4_INTERSECTION = { + latitude: 36.35305878, + longitude: 120.08558121 + }; + + const T6_INTERSECTION = { + latitude: 36.35074527, + longitude: 120.08649105 + }; + + const T7_INTERSECTION = { + latitude: 36.35052372, + longitude: 120.08562915 + }; + + const T8_INTERSECTION = { + latitude: 36.35004529, + longitude: 120.08676664 + }; + + const T10_INTERSECTION = { + latitude: 36.34917893, + longitude: 120.08710569 + }; + + const T11_INTERSECTION = { + latitude: 36.3509885, + longitude: 120.0873865 }; // 存储所有标记 @@ -204,7 +264,7 @@ let marker = markers.get(id); if (!marker) { - // 创建新标记 + // 创建新标 marker = L.marker(position, { icon: createIcon(iconClass) }).addTo(map); @@ -221,20 +281,85 @@ } } + // 添加红绿灯状态和倒计时变量 + let lastTrafficLightState = null; + let countdownInterval = null; + let countdown = 10; // 10秒倒计时 + function updateTrafficLight(data) { const id = data.id; const position = [data.position.latitude, data.position.longitude]; const state = data.status === 0 ? 'red' : 'green'; + // 检查是否是西路口（TL001）的红绿灯状态变化 + if (id === 'TL001' && lastTrafficLightState !== state) { + lastTrafficLightState = state; + // 重置倒计时 + countdown = 10; + // 清除现有的倒计时 + if (countdownInterval) { + clearInterval(countdownInterval); + } + // 启动新的倒计时 + countdownInterval = setInterval(() => { + countdown = Math.max(0, countdown - 1); + // 更新红绿灯标签显示 + const light = trafficLights.get(id); + if (light) { + // 格式化倒计时为 0:SS 格式 + const countdownStr = `0:${countdown.toString().padStart(2, '0')}`; + const label = L.divIcon({ + className: `countdown-label countdown-${state}`, + html: countdownStr, + iconSize: [32, 16], + iconAnchor: [16, 30] + }); + // 更新或创建倒计时标签 + if (!light.countdownMarker) { + light.countdownMarker = L.marker(position, { + icon: label, + interactive: false + }).addTo(map); + } else { + light.countdownMarker.setIcon(label); + } + } + }, 1000); + } + let light = trafficLights.get(id); if (!light) { light = L.marker(position, { icon: createIcon(`traffic-light traffic-light-${state}`) }).addTo(map); - light.bindTooltip(id); + // 为西路口添加倒计时显示 + if (id === 'TL001') { + const countdownStr = `0:${countdown.toString().padStart(2, '0')}`; + const label = L.divIcon({ + className: `countdown-label countdown-${state}`, + html: countdownStr, + iconSize: [32, 16], + iconAnchor: [16, 30] + }); + light.countdownMarker = L.marker(position, { + icon: label, + interactive: false + }).addTo(map); + } trafficLights.set(id, light); } else { light.setIcon(createIcon(`traffic-light traffic-light-${state}`)); + // 更新西路口的倒计时显示 + if (id === 'TL001' && light.countdownMarker) { + const countdownStr = `0:${countdown.toString().padStart(2, '0')}`; + const label = L.divIcon({ + className: `countdown-label countdown-${state}`, + html: countdownStr, + iconSize: [32, 16], + iconAnchor: [16, 30] + }); + light.countdownMarker.setIcon(label); + } } } @@ -273,7 +398,7 @@ // 更新图标 const marker = markers.get(vehicleId); if (marker && commandText) { - console.log('设置新图标:', vehicleId, commandText); + console.log('设置新标:', vehicleId, commandText); marker.setIcon(createIcon('vehicle-icon', commandText)); } else if (marker) { marker.setIcon(createIcon('vehicle-icon')); @@ -305,6 +430,8 @@ ws.onmessage = (event) => { try { const data = JSON.parse(event.data); + let type = 'info'; // 默认类型 + let message = ''; switch (data.type) { case 'position_update': @@ -312,7 +439,9 @@ updatePosition(data); break; case 'traffic_light_status': + type = 'info'; updateTrafficLight(data); + message = `红绿灯状态更新:\n信号灯: ${data.id}\n状态: ${data.status === 0 ? '红灯' : '绿灯'}`; break; case 'collision_warning': type = 'warning'; @@ -345,8 +474,10 @@ break; } - const formattedData = JSON.stringify(data, null, 2); - let message = '收到消息:\n' + formattedData; + // 如果没有特定消息，使用格式化的数据 + if (!message) { + message = '收到消息:\n' + JSON.stringify(data, null, 2); + } log(message, type); } catch (e) { log('收到消息: ' + event.data, 'info'); @@ -363,6 +494,22 @@ return; } + // 清除倒计时 + if (countdownInterval) { + clearInterval(countdownInterval); + countdownInterval = null; + } + lastTrafficLightState = null; + countdown = 10; + + // 清除倒计时标记 + trafficLights.forEach(light => { + if (light.countdownMarker) { + map.removeLayer(light.countdownMarker); + light.countdownMarker = null; + } + }); + ws.close(); ws = null; @@ -374,39 +521,70 @@ } // 添加道路刻度标记函数 - function addRoadMarks(startPoint, endPoint, isLatitude = false) { - const start = isLatitude ? startPoint[0] : startPoint[1]; - const end = isLatitude ? endPoint[0] : endPoint[1]; - const fixed = isLatitude ? startPoint[1] : startPoint[0]; - const step = 0.0005; // 约50米 - const direction = start < end ? 1 : -1; + function addRoadMarks(startPoint, endPoint) { + // 计算两点之间的距离（米） + const lat1 = startPoint[0]; + const lon1 = startPoint[1]; + const lat2 = endPoint[0]; + const lon2 = endPoint[1]; - for (let i = 0; Math.abs(i) <= Math.abs(end - start); i += step * direction) { - const position = isLatitude ? - [start + i, fixed] : - [fixed, start + i]; + // 计算道路角度（考虑经纬度投影） + const latMid = (lat1 + lat2) / 2; // 使用中点纬度来计算经度缩放 + const lonScale = Math.cos(latMid * Math.PI / 180); // 经度缩放因子 + const dx = (lon2 - lon1) * lonScale; + const dy = lat2 - lat1; + const angle = Math.atan2(dy, dx); + + // 计算垂直于道路的方向（只在右侧显示刻度） + const perpAngle = angle + Math.PI / 2; + const markLength = 0.00005; // 保持您设置的较短刻度线长度 + const offset = 0.00004; // 向右偏移一点，避免与道路重叠 + + // 计算总距离 + const dist = Math.sqrt(dx * dx + dy * dy); + + // 每50米一个刻度 + const step = 0.0005; // 约50米 + const steps = Math.floor(dist / step); + + for (let i = 0; i <= steps; i++) { + // 计算刻度位置 + const ratio = i / steps; + // 在经纬度坐标系中正插值 + const pos = [ + lat1 + dy * ratio, + lon1 + (dx / lonScale) * ratio + ]; + + // 计算垂直偏移（考虑经纬度投影） + const offsetPos = [ + pos[0] + Math.sin(perpAngle) * offset, + pos[1] + Math.cos(perpAngle) * offset / lonScale + ]; + + // 计算刻度线终点（考虑经纬度投影） + const markEnd = [ + offsetPos[0] + Math.sin(perpAngle) * markLength, + offsetPos[1] + Math.cos(perpAngle) * markLength / lonScale + ]; // 添加刻度线 - const markLine = isLatitude ? - [[position[0], position[1] - 0.0001], [position[0], position[1] + 0.0001]] : - [[position[0] - 0.0001, position[1]], [position[0] + 0.0001, position[1]]]; - - L.polyline(markLine, { + L.polyline([offsetPos, markEnd], { color: '#666', - weight: 2 + weight: 1.5 }).addTo(map); - // 添加距离标签（以米为单位） - const distance = Math.abs(Math.round(i / step) * 50); - if (distance > 0) { // 只显示非零距离 + // 添加距离标签 + const distance = Math.round(i * 50); + if (distance > 0) { const label = L.divIcon({ className: 'distance-label', html: distance + 'm', iconSize: [40, 20], - iconAnchor: [20, 10] + iconAnchor: [-5, 10] }); - L.marker(position, { + L.marker(markEnd, { icon: label, interactive: false }).addTo(map); @@ -415,53 +593,50 @@ } // 添加道路 - // 东西向主路 + // T2 到 T10主路 const mainRoadEW = L.polyline([ - [WEST_INTERSECTION.latitude, WEST_INTERSECTION.longitude - 0.002], - [EAST_INTERSECTION.latitude, EAST_INTERSECTION.longitude + 0.002] + [T2_INTERSECTION.latitude, T2_INTERSECTION.longitude], + [T10_INTERSECTION.latitude, T10_INTERSECTION.longitude] ], { color: '#999', weight: 8 }).addTo(map); - // 西路口南北向道路 + // T1 到 T3道路 const westRoadNS = L.polyline([ - [WEST_INTERSECTION.latitude + 0.002, WEST_INTERSECTION.longitude], - [WEST_INTERSECTION.latitude - 0.002, WEST_INTERSECTION.longitude] + [T1_INTERSECTION.latitude, T1_INTERSECTION.longitude], + [T3_INTERSECTION.latitude, T3_INTERSECTION.longitude] ], { color: '#999', weight: 8 }).addTo(map); - // 东路口南北向道路 + // T7 到 T11道路 const eastRoadNS = L.polyline([ - [EAST_INTERSECTION.latitude + 0.002, EAST_INTERSECTION.longitude], - [EAST_INTERSECTION.latitude - 0.002, EAST_INTERSECTION.longitude] + [T7_INTERSECTION.latitude, T7_INTERSECTION.longitude], + [T11_INTERSECTION.latitude, T11_INTERSECTION.longitude] ], { color: '#999', weight: 8 }).addTo(map); // 添加刻度标记 - // 西路口南北向刻度 + // T1 到 T3路刻度 addRoadMarks( - [WEST_INTERSECTION.latitude - 0.002, WEST_INTERSECTION.longitude], - [WEST_INTERSECTION.latitude + 0.002, WEST_INTERSECTION.longitude], - true + [T1_INTERSECTION.latitude, T1_INTERSECTION.longitude], + [T3_INTERSECTION.latitude, T3_INTERSECTION.longitude] ); - // 东路口南北向刻度 + // T7 到 T11路刻度 addRoadMarks( - [EAST_INTERSECTION.latitude - 0.002, EAST_INTERSECTION.longitude], - [EAST_INTERSECTION.latitude + 0.002, EAST_INTERSECTION.longitude], - true + [T7_INTERSECTION.latitude, T7_INTERSECTION.longitude], + [T11_INTERSECTION.latitude, T11_INTERSECTION.longitude] ); - // 东西向刻度 + // T2 到 T10路刻度 addRoadMarks( - [WEST_INTERSECTION.latitude, WEST_INTERSECTION.longitude - 0.002], - [EAST_INTERSECTION.latitude, EAST_INTERSECTION.longitude + 0.002], - false + [T2_INTERSECTION.latitude, T2_INTERSECTION.longitude], + [T10_INTERSECTION.latitude, T10_INTERSECTION.longitude] ); diff --git a/tools/mock_server.py b/tools/mock_server.py index b14d1e9..d11041f 100644 --- a/tools/mock_server.py +++ b/tools/mock_server.py @@ -34,25 +34,64 @@ DIST_150M = 150 DIST_100M = 100 DIST_50M = 50 -# 两个路口的位置 -WEST_INTERSECTION = {"longitude": 120.086003, "latitude": 36.361999} -EAST_INTERSECTION = {"longitude": 120.090003, "latitude": 36.361999} +# 时间配置（秒） +WAIT_TIME_AFTER_RETURN = 10.0 # 返回起点后的等待时间 +UPDATE_INTERVAL = 1.0 # 位置更新间隔 +TRAFFIC_LIGHT_SWITCH_INTERVAL = 10.0 # 红绿灯切换间隔 -# 位置更新间隔（秒） -UPDATE_INTERVAL = 1.0 +# 两个路口的位置 +T2_INTERSECTION = {"longitude": 120.08502054, "latitude": 36.35448347} +T6_INTERSECTION = {"longitude": 120.08649105, "latitude": 36.35074527} + +# 坐标点 +# 无人车1起点 +POINT_T1 = {"longitude": 120.0868853, "latitude": 36.35496367} +# 冲突点 1 +POINT_T2 = {"longitude": 120.08502054, "latitude": 36.35448347} +# 特勤车终点 +POINT_T3 = {"longitude": 120.08341044, "latitude": 36.35406879} +# 无人车1终点, 无人车 2 起点，特勤车起点 +POINT_T4 = {"longitude": 120.08558121, "latitude": 36.35305878} +POINT_T5 = {"longitude": 120.08400957, "latitude": 36.35265197} +# 冲突点 2 +POINT_T6 = {"longitude": 120.08649105, "latitude": 36.35074527} +# 航空器 1终点 +POINT_T7 = {"longitude": 120.08562915, "latitude": 36.35052372} +# 无人车 2终点 +POINT_T8 = {"longitude": 120.08676664, "latitude": 36.35004529} +POINT_T9 = {"longitude": 120.08520616, "latitude": 36.34964473} +POINT_T10 = {"longitude": 120.08710569, "latitude": 36.34917893} +# 航空器 1终点 +POINT_T11 = {"longitude": 120.0873865, "latitude": 36.3509885} + +# QN002 新起点（距离 T6 路口 150 米） +POINT_T12 = {"longitude": 120.08603613, "latitude": 36.35190217} # T4 和 T6 之间，距 T6 150米 # 飞机和车辆尺寸（半径 米） AIRCRAFT_SIZE_M = 30.0 VEHICLE_SIZE_M = 10.0 -# 航空器数据 +# 计算初始方向向量 +initial_target_lat = POINT_T11["latitude"] - POINT_T7["latitude"] +initial_target_lon = POINT_T11["longitude"] - POINT_T7["longitude"] + +# 归一化方向向量 +initial_dist = math.sqrt(initial_target_lat * initial_target_lat + initial_target_lon * initial_target_lon) +if initial_dist > 0: + initial_target_lat /= initial_dist + initial_target_lon /= initial_dist + +# 修改航空器数据的初始位置和方向 aircraft_data = [ { "flightNo": "AC001", # 航空器 - "latitude": EAST_INTERSECTION["latitude"] - (DIST_150M / 111319.9), # 东路口南150米 - "longitude": EAST_INTERSECTION["longitude"], + "latitude": POINT_T7["latitude"], + "longitude": POINT_T7["longitude"], "time": int(time.time() * 1000), - "direction": 1, # 1表示向北 + "direction": { # 改用方向向量替代单一方向值 + "lat": initial_target_lat, + "lon": initial_target_lon + }, "speed": 50.0 # 滑行速度 50km/h } ] @@ -62,30 +101,43 @@ DEFAULT_VEHICLE_SPEED = 36.0 # km/h EMERGENCY_BRAKE_DECELERATION = 0.8 # 紧急制动减速度 (每次更新减速 80%) NORMAL_BRAKE_DECELERATION = 0.2 # 正常制动减速度 (每次更新减速 20%) -# 车辆数据 +# 计算 QN002 的初始方向向量 +qn002_target_lat = POINT_T8["latitude"] - POINT_T4["latitude"] +qn002_target_lon = POINT_T8["longitude"] - POINT_T4["longitude"] + +# 归一化方向向量 +qn002_dist = math.sqrt(qn002_target_lat * qn002_target_lat + qn002_target_lon * qn002_target_lon) +if qn002_dist > 0: + qn002_target_lat /= qn002_dist + qn002_target_lon /= qn002_dist + +# 修改车辆数据中 QN002 的初始位置和方向 vehicle_data = [ { - "vehicleNo": "QN001", # 无人车1（西路口） - "latitude": WEST_INTERSECTION["latitude"] + (DIST_100M / 111319.9), # 西路口北50米 - "longitude": WEST_INTERSECTION["longitude"], + "vehicleNo": "QN001", # 无人车1 + "latitude": POINT_T1["latitude"], + "longitude": POINT_T1["longitude"], "time": int(time.time() * 1000), "direction": -1, # -1表示向南 - "speed": DEFAULT_VEHICLE_SPEED, # 使用默认速度 - "phase": 0 # 0: 南北移动, 1: 东西移动 + "speed": DEFAULT_VEHICLE_SPEED, + "phase": 0 }, { - "vehicleNo": "QN002", # 无人车2（东路口） - "latitude": EAST_INTERSECTION["latitude"], - "longitude": EAST_INTERSECTION["longitude"] - (DIST_150M / (111319.9 * math.cos(math.radians(EAST_INTERSECTION["latitude"])))), # 东路口西150米 + "vehicleNo": "QN002", # 无人车2 + "latitude": POINT_T12["latitude"], # 使用新的起点 + "longitude": POINT_T12["longitude"], "time": int(time.time() * 1000), - "direction": 1, # 1表示向东 - "speed": DEFAULT_VEHICLE_SPEED, # 使用默认速度 - "phase": 0 # 初始化 phase + "direction": { # 改用方向向量 + "lat": qn002_target_lat, + "lon": qn002_target_lon + }, + "speed": DEFAULT_VEHICLE_SPEED, + "phase": 0 }, { - "vehicleNo": "TQ001", # 特勤车（西路口） - "latitude": WEST_INTERSECTION["latitude"], - "longitude": WEST_INTERSECTION["longitude"] + (DIST_50M / (111319.9 * math.cos(math.radians(WEST_INTERSECTION["latitude"])))), # 西路口东50米 + "vehicleNo": "TQ001", # 特勤车 + "latitude": POINT_T4["latitude"], + "longitude": POINT_T4["longitude"], "time": int(time.time() * 1000), "direction": -1, # -1表示向西 "speed": DEFAULT_VEHICLE_SPEED, # 使用默认速度 @@ -302,7 +354,7 @@ def handle_vehicle_command(): current_vehicle = v break - # 执行告警指令，直接停车 + # 执行告警指令直接停车 print(f"执行{'紧急' if command_type == 'ALERT' else '正常'}制动: vehicle={vehicle_id}") vehicle_state.current_command = command_type vehicle_state.command_priority = COMMAND_PRIORITIES.get(command_type, 0) @@ -400,13 +452,13 @@ def calculate_distance_to_intersection(vehicle, intersection): def get_nearest_intersection(vehicle): """获取车辆最近的路口及其红绿灯状态""" - west_dist = calculate_distance_to_intersection(vehicle, WEST_INTERSECTION) - east_dist = calculate_distance_to_intersection(vehicle, EAST_INTERSECTION) + west_dist = calculate_distance_to_intersection(vehicle, T2_INTERSECTION) + east_dist = calculate_distance_to_intersection(vehicle, T6_INTERSECTION) if west_dist <= east_dist: - return WEST_INTERSECTION, traffic_light_data[0], west_dist # TL001 + return T2_INTERSECTION, traffic_light_data[0], west_dist # TL001 else: - return EAST_INTERSECTION, traffic_light_data[1], east_dist # TL002 + return T6_INTERSECTION, traffic_light_data[1], east_dist # TL002 def get_front_traffic_light(vehicle, distance_to_west, distance_to_east): """获取车辆前方的红绿灯状态""" @@ -419,30 +471,108 @@ def get_front_traffic_light(vehicle, distance_to_west, distance_to_east): # QN001 的路线：西路口北侧 -> 东 -> 北 if vehicle["phase"] == 0: # 南北移动 # 在西路口以南时，向北移动需要判断西路口红绿灯 - if vehicle["direction"] == 1 and vehicle["latitude"] < WEST_INTERSECTION["latitude"]: + if vehicle["direction"] == 1 and vehicle["latitude"] < T2_INTERSECTION["latitude"]: return traffic_light_data[0], distance_to_west # 西路口红绿灯 # 在西路口以北时，向南移动需要判断西路口红绿灯 - elif vehicle["direction"] == -1 and vehicle["latitude"] > WEST_INTERSECTION["latitude"]: + elif vehicle["direction"] == -1 and vehicle["latitude"] > T2_INTERSECTION["latitude"]: return traffic_light_data[0], distance_to_west # 西路口红绿灯 else: # 东西移动 # 在西路口以西时，向东移动需要判断西路口红绿灯 - if vehicle["direction"] == 1 and vehicle["longitude"] < WEST_INTERSECTION["longitude"]: + if vehicle["direction"] == 1 and vehicle["longitude"] < T2_INTERSECTION["longitude"]: return traffic_light_data[0], distance_to_west # 西路口红绿灯 # 在西路口以东时，向西移动需要判断西路口红绿灯 - elif vehicle["direction"] == -1 and vehicle["longitude"] > WEST_INTERSECTION["longitude"]: + elif vehicle["direction"] == -1 and vehicle["longitude"] > T2_INTERSECTION["longitude"]: return traffic_light_data[0], distance_to_west # 西路口红绿灯 elif vehicle["vehicleNo"] == "QN002": - # 在东路口以西时，向东移动需要判断东路口红绿灯 - if vehicle["direction"] == 1 and vehicle["longitude"] < EAST_INTERSECTION["longitude"]: + # 在东路口以西时，向东移动需要判断东路红绿灯 + if vehicle["direction"] == 1 and vehicle["longitude"] < T6_INTERSECTION["longitude"]: return traffic_light_data[1], distance_to_east # 东路口红绿灯 # 在东路口以东时，向西移动需要判断东路口红绿灯 - elif vehicle["direction"] == -1 and vehicle["longitude"] > EAST_INTERSECTION["longitude"]: + elif vehicle["direction"] == -1 and vehicle["longitude"] > T6_INTERSECTION["longitude"]: return traffic_light_data[1], distance_to_east # 东路口红绿灯 - # 其他情况，表示车辆已经通过路口或不需要判断红绿灯 + # 其他情况，表示车辆已经过路口或不需要判断红绿灯 return None, float('inf') +def update_position_with_vector(obj, target_point, start_point, speed, elapsed_time, return_to_start=False): + """ + 基于向量的位置更新逻辑 + + Args: + obj: 需要更新位置的对象（航空器或车辆） + target_point: 目标点坐标 {"latitude": float, "longitude": float} + start_point: 起点坐标 {"latitude": float, "longitude": float} + speed: 移动速度（km/h） + elapsed_time: 经过的时间（秒） + return_to_start: 是否在到达目标点时返回起点 + """ + # 检查是否在等待状态 + if "wait_until" not in obj: + obj["wait_until"] = 0 + + current_time = time.time() + if current_time < obj["wait_until"]: + return False # 还在等待中，不更新位置 + + # 计算这一步要移动的距离（米）并转换为经纬度 + speed_mps = speed * 1000 / 3600 + distance = speed_mps * elapsed_time + + # 将移动距离转换为经纬度变化量 + move_dlat, move_dlon = meters_to_degrees(distance, obj["latitude"]) + + # 将15米的判断距离转换为经纬度 + check_dlat, check_dlon = meters_to_degrees(15, obj["latitude"]) + + # 计算当前位置到终点的向量 + vector_lat = target_point["latitude"] - obj["latitude"] + vector_lon = target_point["longitude"] - obj["longitude"] + + # 计算向量长度（经纬度空间） + vector_length = math.sqrt(vector_lat * vector_lat + vector_lon * vector_lon) + + # 检查是否到达终点 + if vector_length <= math.sqrt(check_dlat * check_dlat + check_dlon * check_dlon): + if return_to_start: + # 返回起点并设置等待时间 + obj["latitude"] = start_point["latitude"] + obj["longitude"] = start_point["longitude"] + obj["wait_until"] = current_time + WAIT_TIME_AFTER_RETURN # 使用常量 + print(f"对象 {obj.get('flightNo', obj.get('vehicleNo'))} 返回起点，等待{WAIT_TIME_AFTER_RETURN}秒后继续") + else: + # 否则就停在目标点 + obj["latitude"] = target_point["latitude"] + obj["longitude"] = target_point["longitude"] + return True # 表示已到达终点 + else: + # 归一化方向向量 + unit_lat = vector_lat / vector_length + unit_lon = vector_lon / vector_length + + # 计算这一步的位置变化 + dlat = move_dlat * unit_lat + dlon = move_dlon * unit_lon + + # 更新位置 + obj["latitude"] += dlat + obj["longitude"] += dlon + return False # 表示正在移动中 + +def update_aircraft_position(aircraft, elapsed_time): + """更新航空器位置""" + reached_target = update_position_with_vector( + aircraft, + POINT_T11, # 目标点 + POINT_T7, # 起点 + aircraft["speed"], + elapsed_time, + return_to_start=True # 航空器需要返回起点 + ) + + if reached_target: + print(f"航空器 {aircraft['flightNo']} 到达终点，返回起点 T7") + def update_vehicle_position(vehicle, elapsed_time): """更新车辆位置""" # 获取车辆状态 @@ -457,8 +587,8 @@ def update_vehicle_position(vehicle, elapsed_time): # 获取前方红绿灯状态和距离 traffic_light, distance = get_front_traffic_light(vehicle, - calculate_distance_to_intersection(vehicle, WEST_INTERSECTION), - calculate_distance_to_intersection(vehicle, EAST_INTERSECTION)) + calculate_distance_to_intersection(vehicle, T2_INTERSECTION), + calculate_distance_to_intersection(vehicle, T6_INTERSECTION)) # 特勤车只响应红绿灯 if vehicle["vehicleNo"].startswith("TQ"): @@ -490,119 +620,103 @@ def update_vehicle_position(vehicle, elapsed_time): vehicle["speed"] = DEFAULT_VEHICLE_SPEED print(f"车辆 {vehicle['vehicleNo']} 可以移动，速度={vehicle['speed']}km/h") - # 计算基础速度（米/秒） - base_speed_mps = vehicle["speed"] * 1000 / 3600 - - # 计算移动距离 - distance = base_speed_mps * elapsed_time - - # 计算经纬度变化 - dlat, dlon = meters_to_degrees(distance, vehicle["latitude"]) - # 更新位置 if vehicle["vehicleNo"].startswith("QN"): # 无人车的路径更新逻辑 if vehicle["vehicleNo"] == "QN001": - # 无人车1：先南北后东西往返 - if vehicle["phase"] == 0: # 南北移动 - new_lat = vehicle["latitude"] + (dlat * vehicle["direction"]) - if vehicle["direction"] == -1 and new_lat <= WEST_INTERSECTION["latitude"]: - # 到达路口，切换到东西移动 - vehicle["phase"] = 1 - vehicle["direction"] = 1 # 向东 - print(f"QN001 到达西路口，切换到东西移动，方向=东") - elif vehicle["direction"] == 1 and new_lat >= WEST_INTERSECTION["latitude"] + (DIST_50M / 111319.9): - # 返回起点 - vehicle["phase"] = 0 - vehicle["direction"] = -1 # 向南 - print(f"QN001 到达北端，切换到南北移动，方向=南") - vehicle["latitude"] = new_lat - print(f"QN001 南北移动: lat={new_lat}, direction={vehicle['direction']}") + if vehicle["phase"] == 0: # T1 -> T2 + reached_target = update_position_with_vector( + vehicle, + POINT_T2, # 目标点（西路口） + POINT_T1, # 起点 + vehicle["speed"], + elapsed_time, + return_to_start=False # 不返回起点，继续到下一个点 + ) - else: # 东西移动 - new_lon = vehicle["longitude"] + (dlon * vehicle["direction"]) - if vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_100M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): - # 到达东端，开始返回 - vehicle["direction"] = -1 # 向西 - print(f"QN001 到达东端，切换方向=西") - elif vehicle["direction"] == -1 and new_lon <= WEST_INTERSECTION["longitude"]: - # 返回路口，切换到南北移动 - vehicle["phase"] = 0 - vehicle["direction"] = 1 # 向北 - print(f"QN001 返回西路口，切换到南北移动，方向=北") - vehicle["longitude"] = new_lon - print(f"QN001 东西移动: lon={new_lon}, direction={vehicle['direction']}") + if reached_target: + vehicle["phase"] = 1 # 切换到下一阶段 + print(f"无人车 {vehicle['vehicleNo']} 到达 T2，开始前往 T4") + + elif vehicle["phase"] == 1: # T2 -> T4 + reached_target = update_position_with_vector( + vehicle, + POINT_T4, # 目标点 + POINT_T2, # 起点 + vehicle["speed"], + elapsed_time, + return_to_start=False # 不返回起点，继续到下一个点 + ) + + if reached_target: + # 到达 T4 后直接跳回起点 T1 + vehicle["latitude"] = POINT_T1["latitude"] + vehicle["longitude"] = POINT_T1["longitude"] + vehicle["phase"] = 0 # 重置到初始阶段 + print(f"无人车 {vehicle['vehicleNo']} 到达 T4，直接返回起点 T1") else: # QN002 - # 无人车2：西向往返（西路口） - new_lon = vehicle["longitude"] + (dlon * vehicle["direction"]) - if new_lon >= EAST_INTERSECTION["longitude"] + (DIST_150M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): - vehicle["direction"] = -1 # 向西 - print(f"QN002 到达东端，切换方向=西") - elif new_lon <= EAST_INTERSECTION["longitude"] - (DIST_150M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): - vehicle["direction"] = 1 # 向东 - print(f"QN002 到达西端，切换方向=东") - vehicle["longitude"] = new_lon - print(f"QN002 东西移动: lon={new_lon}, direction={vehicle['direction']}") + reached_target = update_position_with_vector( + vehicle, + POINT_T8, # 目标点 + POINT_T12, # 使用新的起点 + vehicle["speed"], + elapsed_time, + return_to_start=True # QN002 需要返回起点 + ) + + if reached_target: + print(f"无人车 {vehicle['vehicleNo']} 到达终点，返回起点 T12") elif vehicle["vehicleNo"].startswith("TQ"): # 特勤车的路径更新逻辑 - if vehicle["phase"] == 0: # 东西移动 - new_lon = vehicle["longitude"] + (dlon * vehicle["direction"]) - if vehicle["direction"] == -1 and new_lon <= WEST_INTERSECTION["longitude"]: - vehicle["phase"] = 1 # 切换到南北移动 - vehicle["direction"] = -1 # 向南 - print(f"TQ001 到达西路口，切换到南北移动，方向=南") - elif vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_50M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): - vehicle["direction"] = -1 # 向西 - print(f"TQ001 到达东端，切换方向=西") - vehicle["longitude"] = new_lon - print(f"TQ001 东西移动: lon={new_lon}, direction={vehicle['direction']}") - else: # 南北移动 - new_lat = vehicle["latitude"] + (dlat * vehicle["direction"]) - if new_lat <= WEST_INTERSECTION["latitude"] - (DIST_100M / 111319.9): # 到达南端 - vehicle["direction"] = 1 # 向北 - print(f"TQ001 到达南端，切换方向=北") - elif new_lat >= WEST_INTERSECTION["latitude"]: # 返回路口 - vehicle["phase"] = 0 # 切换回东西移动 - vehicle["direction"] = 1 # 向东 - vehicle["longitude"] = WEST_INTERSECTION["longitude"] # 重置到起始位置 - print(f"TQ001 返回西路口，切换到东西移动，方向=东") - vehicle["latitude"] = new_lat - print(f"TQ001 南北移动: lat={new_lat}, direction={vehicle['direction']}") - + if vehicle["phase"] == 0: # T4 -> T2 + reached_target = update_position_with_vector( + vehicle, + POINT_T2, # 目标点（西路口） + POINT_T4, # 起点 + vehicle["speed"], + elapsed_time, + return_to_start=False # 不返回起点，继续到下一个点 + ) + + if reached_target: + vehicle["phase"] = 1 # 切换到下一阶段 + print(f"特勤车 {vehicle['vehicleNo']} 到达 T2，开始前往 T3") + + elif vehicle["phase"] == 1: # T2 -> T3 + reached_target = update_position_with_vector( + vehicle, + POINT_T3, # 目标点（特勤车终点） + POINT_T2, # 起点 + vehicle["speed"], + elapsed_time, + return_to_start=False # 不返回起点，继续到下一个点 + ) + + if reached_target: + # 到达 T3 后直接跳回起点 T4 + vehicle["latitude"] = POINT_T4["latitude"] + vehicle["longitude"] = POINT_T4["longitude"] + vehicle["phase"] = 0 # 重置到初始阶段 + print(f"特勤车 {vehicle['vehicleNo']} 到达 T3，直接返回起点 T4") + # 新时间戳 vehicle["time"] = int(time.time() * 1000) -def update_aircraft_position(aircraft, elapsed_time): - """更新航空器位置""" - # 计算一次更新的动距离（米） - speed_mps = aircraft["speed"] * 1000 / 3600 # 速度转换为米/秒 - distance = speed_mps * elapsed_time - - # 计算经纬度变化 - dlat, dlon = meters_to_degrees(distance, aircraft["latitude"]) - - new_lat = aircraft["latitude"] + (dlat * aircraft["direction"]) - if new_lat >= EAST_INTERSECTION["latitude"] + (DIST_150M / 111319.9): - aircraft["direction"] = -1 # 向南 - elif new_lat <= EAST_INTERSECTION["latitude"] - (DIST_150M / 111319.9): - aircraft["direction"] = 1 # 向北 - aircraft["latitude"] = new_lat - # 定义路口信息 INTERSECTIONS = { "TL001": { "id": "INT001", "name": "西路口", - "latitude": WEST_INTERSECTION["latitude"], - "longitude": WEST_INTERSECTION["longitude"] + "latitude": T2_INTERSECTION["latitude"], + "longitude": T2_INTERSECTION["longitude"] }, "TL002": { "id": "INT002", "name": "东路口", - "latitude": EAST_INTERSECTION["latitude"], - "longitude": EAST_INTERSECTION["longitude"] + "latitude": T6_INTERSECTION["latitude"], + "longitude": T6_INTERSECTION["longitude"] } } @@ -656,7 +770,7 @@ def switch_traffic_light_state(): # 西路口红绿灯每15秒切换一次 elapsed_since_switch = current_time - last_light_switch_time - if elapsed_since_switch >= 15: + if elapsed_since_switch >= TRAFFIC_LIGHT_SWITCH_INTERVAL: # 使用常量 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 '红灯'}") @@ -664,7 +778,7 @@ def switch_traffic_light_state(): # 更新东路口红绿灯（根据航空器位置） if aircraft_data: aircraft = aircraft_data[0] - lat_diff = abs(aircraft["latitude"] - EAST_INTERSECTION["latitude"]) * 111319.9 + lat_diff = abs(aircraft["latitude"] - T6_INTERSECTION["latitude"]) * 111319.9 old_state = traffic_light_data[1]["state"] traffic_light_data[1]["state"] = 1 if lat_diff > DIST_50M else 0 diff --git a/tools/mock_server.py.bak b/tools/mock_server.py.bak deleted file mode 100644 index 42dc1d1..0000000 --- a/tools/mock_server.py.bak +++ /dev/null @@ -1,538 +0,0 @@ -from flask import Flask, jsonify, request -import time -import math -import random - -app = Flask(__name__) - -# 地球半径（米） -EARTH_RADIUS = 6378137.0 - -def meters_to_degrees(meters, latitude): - """ - 将米转换为度，考虑纬度的影响 - """ - # 纬度方向：1度 = 111,319.9米 - meters_per_deg_lat = 111319.9 - # 经度方向：1度 = 111,319.9 * cos(latitude)米 - meters_per_deg_lon = meters_per_deg_lat * math.cos(math.radians(latitude)) - return meters / meters_per_deg_lat, meters / meters_per_deg_lon - -# 距离配置（米） -DIST_150M = 150 -DIST_100M = 100 -DIST_50M = 50 - -# 两个路口的位置 -WEST_INTERSECTION = {"longitude": 120.088003, "latitude": 36.361999} -EAST_INTERSECTION = {"longitude": 120.089003, "latitude": 36.361999} - -# 位置更新间隔（秒） -UPDATE_INTERVAL = 1.0 - -# 航空器数据 -aircraft_data = [ - { - "flightNo": "AC001", # 航空器 - "latitude": EAST_INTERSECTION["latitude"] - (DIST_150M / 111319.9), # 东路口南150米 - "longitude": EAST_INTERSECTION["longitude"], - "time": int(time.time() * 1000), - "direction": 1, # 1表示向北 - "speed": 50.0 # 滑行速度 50km/h - } -] - -# 添加默认速度常量 -DEFAULT_VEHICLE_SPEED = 36.0 # km/h -EMERGENCY_BRAKE_DECELERATION = 0.8 # 紧急制动减速度 (每次更新减速 80%) -NORMAL_BRAKE_DECELERATION = 0.2 # 正常制动减速度 (每次更新减速 20%) - -# 车辆数据 -vehicle_data = [ - { - "vehicleNo": "QN001", # 无人车1（西路口） - "latitude": WEST_INTERSECTION["latitude"] + (DIST_50M / 111319.9), # 西路口北50米 - "longitude": WEST_INTERSECTION["longitude"], - "time": int(time.time() * 1000), - "direction": -1, # -1表示向南 - "speed": DEFAULT_VEHICLE_SPEED, # 使用默认速度 - "phase": 0 # 0: 南北移动, 1: 东西移动 - }, - { - "vehicleNo": "QN002", # 无人车2（东路口） - "latitude": EAST_INTERSECTION["latitude"], - "longitude": EAST_INTERSECTION["longitude"] - (DIST_150M / (111319.9 * math.cos(math.radians(EAST_INTERSECTION["latitude"])))), # 东路口西150米 - "time": int(time.time() * 1000), - "direction": 1, # 1表示向东 - "speed": DEFAULT_VEHICLE_SPEED # 使用默认速度 - }, - { - "vehicleNo": "TQ001", # 特勤车（西路口） - "latitude": WEST_INTERSECTION["latitude"], - "longitude": WEST_INTERSECTION["longitude"] + (DIST_50M / (111319.9 * math.cos(math.radians(WEST_INTERSECTION["latitude"])))), # 西路口东50米 - "time": int(time.time() * 1000), - "direction": -1, # -1表示向西 - "speed": DEFAULT_VEHICLE_SPEED # 使用默认速度 - } -] - -# 添加车辆状态类 -class VehicleState: - def __init__(self, vehicle_no): - self.vehicle_no = vehicle_no - self.is_running = True # 运行状态 - self.current_command = None # 当前执行的指令 - self.command_priority = 0 # 当前指令优先级 - self.target_speed = DEFAULT_VEHICLE_SPEED # 目标速度 - self.brake_mode = None # 制动模式：'emergency' 或 'normal' - - def can_be_overridden_by(self, command_type): - # 指令优先级: SIGNAL(4) > ALERT(3) > WARNING(2) > RESUME(1) - priority_map = { - "SIGNAL": 4, - "ALERT": 3, - "WARNING": 2, - "RESUME": 1 - } - new_priority = priority_map.get(command_type, 0) - - # 如果当前没有指令，或者新指令优先级更高，或者是相同类型的指令，则允许覆盖 - return (self.current_command is None or - new_priority >= self.command_priority or - command_type == self.current_command) - - def update_command(self, command_type): - priority_map = { - "SIGNAL": 4, - "ALERT": 3, - "WARNING": 2, - "RESUME": 1 - } - self.command_priority = priority_map.get(command_type, 0) - self.current_command = command_type - -# 添加车辆状态管理 -vehicle_states = { - "QN001": VehicleState("QN001"), - "QN002": VehicleState("QN002"), - "TQ001": VehicleState("TQ001") -} - -def calculate_distance_to_target(vehicle, target_lat, target_lon): - """计算车辆到目标位置的距离（米）""" - dlat = target_lat - vehicle["latitude"] - dlon = target_lon - vehicle["longitude"] - - # 使用 Haversine 公式计算距离 - R = 6371000 # 地球半径（米） - a = math.sin(dlat/2) * math.sin(dlat/2) + \ - math.cos(math.radians(vehicle["latitude"])) * math.cos(math.radians(target_lat)) * \ - math.sin(dlon/2) * math.sin(dlon/2) - c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) - return R * c - -@app.route('/vehicle/command', methods=['POST']) -def handle_vehicle_command(): - try: - data = request.json - vehicle_id = data.get("vehicleId") - command_type = data.get("type", "").upper() - reason = data.get("reason", "").upper() - - # 检查车辆是否存在 - vehicle_state = vehicle_states.get(vehicle_id) - if not vehicle_state: - return jsonify({ - "status": "error", - "message": f"Vehicle {vehicle_id} not found" - }), 404 - - # 检查是否为特勤车辆 - if vehicle_id.startswith("TQ"): - return jsonify({ - "status": "ok", - "message": "Special vehicle ignores command" - }) - - # 检查指令优先级 - if not vehicle_state.can_be_overridden_by(command_type): - return jsonify({ - "status": "error", - "message": "Command priority too low" - }), 400 - - # 获取目标位置 - target_lat = data.get("latitude", 0.0) - target_lon = data.get("longitude", 0.0) - - # 处理不同类型的指令 - if command_type == "SIGNAL": - signal_state = data.get("signalState", "").upper() - if signal_state == "RED": - # 计算到路口的距离 - distance = calculate_distance_to_target( - next(v for v in vehicles if v["vehicleNo"] == vehicle_id), - target_lat, target_lon - ) - - # 根据距离决定制动方式 - if distance <= 50: # 已经在停止线内 - vehicle_state.is_running = False - vehicle_state.target_speed = 0 - vehicle_state.brake_mode = "emergency" # 紧急停车 - elif distance <= 100: # 距离停止线100米内 - vehicle_state.is_running = False - vehicle_state.target_speed = 0 - vehicle_state.brake_mode = "normal" # 正常制动 - elif signal_state == "GREEN": - vehicle_state.is_running = True - vehicle_state.target_speed = DEFAULT_VEHICLE_SPEED - vehicle_state.brake_mode = None - - elif command_type == "ALERT": - vehicle_state.is_running = False - vehicle_state.target_speed = 0 - vehicle_state.brake_mode = "emergency" # 告警紧急制动 - - elif command_type == "WARNING": - vehicle_state.is_running = False - vehicle_state.target_speed = 0 - vehicle_state.brake_mode = "normal" # 预警正常制动 - - elif command_type == "RESUME": - vehicle_state.is_running = True - vehicle_state.target_speed = DEFAULT_VEHICLE_SPEED - vehicle_state.brake_mode = None - - # 更新车辆状态 - vehicle_state.command_type = command_type - vehicle_state.command_reason = reason - vehicle_state.command_priority = COMMAND_PRIORITIES.get(command_type, 0) - vehicle_state.last_command_time = time.time() - - print(f"Vehicle {vehicle_id} state updated: running={vehicle_state.is_running}, " - f"command={command_type}, reason={reason}, priority={vehicle_state.command_priority}, " - f"target_lat={target_lat}, target_lon={target_lon}") - - return jsonify({ - "status": "ok", - "message": "Command executed successfully" - }) - - except Exception as e: - print(f"Error handling vehicle command: {str(e)}") - return jsonify({ - "status": "error", - "message": str(e) - }), 500 - -def calculate_distance_to_intersection(vehicle, intersection): - """计算车辆到路口的距离（米）""" - vehicle_lat = vehicle["latitude"] - vehicle_lon = vehicle["longitude"] - intersection_lat = intersection["latitude"] - intersection_lon = intersection["longitude"] - - # 使用经纬度计算距离 - lat_diff = (vehicle_lat - intersection_lat) * 111319.9 - lon_diff = (vehicle_lon - intersection_lon) * (111319.9 * math.cos(math.radians(vehicle_lat))) - return math.sqrt(lat_diff * lat_diff + lon_diff * lon_diff) - -def get_nearest_intersection(vehicle): - """获取车辆最近的路口及其红绿灯状态""" - west_dist = calculate_distance_to_intersection(vehicle, WEST_INTERSECTION) - east_dist = calculate_distance_to_intersection(vehicle, EAST_INTERSECTION) - - if west_dist <= east_dist: - return WEST_INTERSECTION, traffic_light_data[0], west_dist # TL001 - else: - return EAST_INTERSECTION, traffic_light_data[1], east_dist # TL002 - -def get_front_traffic_light(vehicle, distance_to_west, distance_to_east): - """获取车辆前方的红绿灯状态""" - # 根据车辆的位置和方向判断前方路口 - if vehicle["vehicleNo"].startswith("QN"): - if vehicle["vehicleNo"] == "QN001": - # QN001 的路线：西路口北侧 -> 南 -> 东 -> 北 - if vehicle["phase"] == 0: # 南北移动 - if vehicle["direction"] == -1: # 向南 - return traffic_light_data[0], distance_to_west # 西路口红绿灯 - else: # 向北 - return None, float('inf') # 没有红绿灯 - else: # 东西移动 - if vehicle["direction"] == 1: # 向东 - return None, float('inf') # 没有红绿灯 - else: # 向西 - return traffic_light_data[0], distance_to_west # 西路口红绿灯 - else: # QN002 - # QN002 的路线：东西方向往返 - if vehicle["direction"] == 1: # 向东 - return traffic_light_data[1], distance_to_east # 东路口红绿灯 - else: # 向西 - return traffic_light_data[0], distance_to_west # 西路口红绿灯 - elif vehicle["vehicleNo"].startswith("TQ"): - # 特勤车：先东西后南北 - if vehicle["phase"] == 0: # 东西移动 - if vehicle["direction"] == 1: # 向东 - return None, float('inf') # 没有���绿灯 - else: # 向西 - return traffic_light_data[0], distance_to_west # 西路口红绿灯 - else: # 南北移动 - return None, float('inf') # 南北方向没有红绿灯 - - return None, float('inf') # 默认返回无红绿灯 - -def update_vehicle_position(vehicle, elapsed_time): - """更新车辆位置""" - # 计算到两个路口的距离 - distance_to_west = calculate_distance_to_intersection(vehicle, WEST_INTERSECTION) - distance_to_east = calculate_distance_to_intersection(vehicle, EAST_INTERSECTION) - - # 获取前方红绿灯状态 - traffic_light, distance = get_front_traffic_light(vehicle, distance_to_west, distance_to_east) - - # 计算基础速度（米/秒） - if vehicle["vehicleNo"].startswith("TQ"): - base_speed_mps = DEFAULT_VEHICLE_SPEED * 1000 / 3600 - else: - vehicle_state = vehicle_states.get(vehicle["vehicleNo"]) - if not vehicle_state or not vehicle_state.is_running: - return - base_speed_mps = vehicle["speed"] * 1000 / 3600 - - # 如果是无人车，还需要考虑制动模式 - if vehicle_state.brake_mode: - current_speed_mps = base_speed_mps - target_speed_mps = vehicle_state.target_speed * 1000 / 3600 - - if vehicle_state.brake_mode == "emergency": - base_speed_mps = current_speed_mps * (1 - EMERGENCY_BRAKE_DECELERATION) - else: # normal - base_speed_mps = current_speed_mps * (1 - NORMAL_BRAKE_DECELERATION) - - base_speed_mps = max(base_speed_mps, target_speed_mps) - - # 根据红绿灯状态调整速度 - if traffic_light and traffic_light["state"] == 0: # 红灯 - if distance <= 50: # 已经在停止线内 - speed_mps = 0 # 紧急停车 - print(f"车辆 {vehicle['vehicleNo']} 在停止线内遇红灯，紧急停车") - elif distance <= 100: # 距离停止线100米内 - # 计算减速度，使车辆在停止线前停下 - deceleration = (base_speed_mps * base_speed_mps) / (2 * (distance - 50)) - speed_mps = max(0, base_speed_mps - deceleration * elapsed_time) - print(f"车辆 {vehicle['vehicleNo']} 距红灯 {distance:.1f}米，减速至 {speed_mps * 3.6:.1f}km/h") - else: - speed_mps = base_speed_mps - else: # 绿灯或无红绿灯 - speed_mps = base_speed_mps - - # 更新车辆速度 - vehicle["speed"] = speed_mps * 3600 / 1000 # 转回 km/h - - # 计算移动距离 - distance = speed_mps * elapsed_time - - # 计算经纬度变化 - dlat, dlon = meters_to_degrees(distance, vehicle["latitude"]) - - if vehicle["vehicleNo"].startswith("QN"): - # 无人车的路径更新逻辑 - if vehicle["vehicleNo"] == "QN001": - # 无人车1：先南北后东西往返 - if "phase" not in vehicle: - vehicle["phase"] = 0 - - if vehicle["phase"] == 0: # 南北移动 - new_lat = vehicle["latitude"] + (dlat * vehicle["direction"]) - if vehicle["direction"] == -1 and new_lat <= WEST_INTERSECTION["latitude"]: - # 到达路口，切换到东西移动 - vehicle["phase"] = 1 - vehicle["direction"] = 1 # 向东 - elif vehicle["direction"] == 1 and new_lat >= WEST_INTERSECTION["latitude"] + (DIST_50M / 111319.9): - # 返回起点 - vehicle["phase"] = 0 - vehicle["direction"] = -1 # 向南 - vehicle["latitude"] = new_lat - - else: # 东西移动 - new_lon = vehicle["longitude"] + (dlon * vehicle["direction"]) - if vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_100M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): - # 到达东端，开始返回 - vehicle["direction"] = -1 # 向西 - elif vehicle["direction"] == -1 and new_lon <= WEST_INTERSECTION["longitude"]: - # 返回路口，切换到南北移动 - vehicle["phase"] = 0 - vehicle["direction"] = 1 # 向北 - vehicle["longitude"] = new_lon - - else: # QN002 - # 无人车2：东西方向往返（东路口） - new_lon = vehicle["longitude"] + (dlon * vehicle["direction"]) - if new_lon >= EAST_INTERSECTION["longitude"] + (DIST_100M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): - vehicle["direction"] = -1 # 向西 - elif new_lon <= EAST_INTERSECTION["longitude"] - (DIST_150M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): - vehicle["direction"] = 1 # 向东 - vehicle["longitude"] = new_lon - - elif vehicle["vehicleNo"].startswith("TQ"): - # 特勤车的路径更新逻辑 - if "phase" not in vehicle: - vehicle["phase"] = 0 - - if vehicle["phase"] == 0: # 东西移动 - new_lon = vehicle["longitude"] + (dlon * vehicle["direction"]) - if vehicle["direction"] == -1 and new_lon <= WEST_INTERSECTION["longitude"]: - vehicle["phase"] = 1 # 切换到南北移动 - vehicle["direction"] = -1 # 向南 - elif vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_50M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))): - vehicle["direction"] = -1 # 向西 - vehicle["longitude"] = new_lon - else: # 南北移动 - new_lat = vehicle["latitude"] + (dlat * vehicle["direction"]) - if new_lat <= WEST_INTERSECTION["latitude"] - (DIST_100M / 111319.9): # 到达南端 - vehicle["direction"] = 1 # 向北 - elif new_lat >= WEST_INTERSECTION["latitude"]: # 返回到路口 - vehicle["phase"] = 0 # 切换回东西移动 - vehicle["direction"] = 1 # 向北 - vehicle["longitude"] = WEST_INTERSECTION["longitude"] # 重置到起始位置 - vehicle["latitude"] = new_lat - - # 更新时间戳 - vehicle["time"] = int(time.time() * 1000) - -def update_aircraft_position(aircraft, elapsed_time): - """更新航空器位置""" - # 计算一次更新的移动距离（米） - speed_mps = aircraft["speed"] * 1000 / 3600 # 速度转换为米/秒 - distance = speed_mps * elapsed_time - - # 计算经纬度变化 - dlat, dlon = meters_to_degrees(distance, aircraft["latitude"]) - - new_lat = aircraft["latitude"] + (dlat * aircraft["direction"]) - if new_lat >= EAST_INTERSECTION["latitude"] + (DIST_150M / 111319.9): - aircraft["direction"] = -1 # 向南 - elif new_lat <= EAST_INTERSECTION["latitude"] - (DIST_150M / 111319.9): - 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"] = 0 # 红灯 - else: - light["state"] = 1 # 绿灯 - break - -# 定义路口信息 -INTERSECTIONS = { - "TL001": { - "id": "INT001", - "name": "西路口", - "latitude": WEST_INTERSECTION["latitude"], - "longitude": WEST_INTERSECTION["longitude"] - }, - "TL002": { - "id": "INT002", - "name": "东路口", - "latitude": EAST_INTERSECTION["latitude"], - "longitude": EAST_INTERSECTION["longitude"] - } -} - -def generate_traffic_light_data(): - """生成红绿灯数据""" - traffic_light_data = [] - - # 生成两个固定路口的红绿灯数据 - for tl_id, intersection in INTERSECTIONS.items(): - signal = { - "id": tl_id, - "state": random.randint(0, 1), # 0: RED, 1: GREEN - "timestamp": int(time.time() * 1000), - "intersectionId": intersection["id"], - "latitude": intersection["latitude"], - "longitude": intersection["longitude"] - } - traffic_light_data.append(signal) - - return traffic_light_data - -# 红绿灯���据 -traffic_light_data = generate_traffic_light_data() - -# 分别记录航空器和车辆的上次更新时间 -last_aircraft_update_time = time.time() -last_vehicle_update_time = time.time() -last_light_switch_time = time.time() - -@app.route('/api/getCurrentFlightPositions') -def get_flight_positions(): - global last_aircraft_update_time - current_time = time.time() - elapsed_time = current_time - last_aircraft_update_time - - # 只在达到更新间隔时更新位置 - if elapsed_time >= UPDATE_INTERVAL: - for aircraft in aircraft_data: - update_aircraft_position(aircraft, UPDATE_INTERVAL) - aircraft["time"] = int(current_time * 1000) - last_aircraft_update_time = current_time - - return jsonify(aircraft_data) - -@app.route('/api/getCurrentVehiclePositions') -def get_vehicle_positions(): - global last_vehicle_update_time - current_time = time.time() - elapsed_time = current_time - last_vehicle_update_time - - # 只在达到更新间隔时更新位置 - if elapsed_time >= UPDATE_INTERVAL: - for vehicle in vehicle_data: - update_vehicle_position(vehicle, UPDATE_INTERVAL) - vehicle["time"] = int(current_time * 1000) - last_vehicle_update_time = current_time - - return jsonify(vehicle_data) - -@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 >= 15: # 每15秒切换一次 - traffic_light_data[0]["state"] = 1 if traffic_light_data[0]["state"] == 0 else 0 - last_light_switch_time = current_time - - # 根据航空器位置更新TL002（东路口） - update_traffic_light_for_aircraft() - - return jsonify(traffic_light_data) - -@app.after_request -def add_cors_headers(response): - response.headers['Access-Control-Allow-Origin'] = '*' - response.headers['Access-Control-Allow-Headers'] = 'Content-Type' - response.headers['Access-Control-Allow-Methods'] = 'GET, POST, OPTIONS' - return response - -if __name__ == '__main__': - app.run(host='localhost', port=8081, debug=True) \ No newline at end of file