714 lines
29 KiB
C++
714 lines
29 KiB
C++
#include "detector/CollisionDetector.h"
|
||
#include "vehicle/ControllableVehicles.h"
|
||
#include "config/AirportBounds.h"
|
||
#include <gtest/gtest.h>
|
||
#include <gmock/gmock.h>
|
||
#include "utils/Logger.h"
|
||
#include <chrono>
|
||
|
||
// Mock ControllableVehicles 类
|
||
class MockControllableVehicles : public ControllableVehicles {
|
||
public:
|
||
MockControllableVehicles() : ControllableVehicles("") {} // 使用空字符串，避免加载实际配置
|
||
MOCK_METHOD(bool, isControllable, (const std::string& vehicleNo), (const));
|
||
};
|
||
|
||
// Mock AirportBounds 类
|
||
class MockAirportBounds : public AirportBounds {
|
||
public:
|
||
MockAirportBounds() : AirportBounds("") {
|
||
// 设置更大的测试边界，以包含所有测试数据
|
||
airportBounds_ = Bounds(0, 0, 4000, 2000); // 4000x2000 的测试区域
|
||
Logger::info("MockAirportBounds initialized with bounds: ",
|
||
"x=", airportBounds_.x, ", y=", airportBounds_.y,
|
||
", width=", airportBounds_.width, ", height=", airportBounds_.height);
|
||
}
|
||
|
||
// 覆盖原有方法，返回测试用的配置
|
||
AreaType getAreaType(const Vector2D& position) const override {
|
||
return AreaType::RUNWAY; // 简化测试，总是返回跑道区域
|
||
}
|
||
|
||
const AreaConfig& getAreaConfig(AreaType type) const override {
|
||
static const AreaConfig config{50.0, 100.0, 15.0};
|
||
return config;
|
||
}
|
||
|
||
const Bounds& getAirportBounds() const override {
|
||
return airportBounds_;
|
||
}
|
||
};
|
||
|
||
class CollisionDetectorTest : public ::testing::Test {
|
||
protected:
|
||
void SetUp() override {
|
||
// 创建 Mock 对象
|
||
airportBounds_ = std::make_unique<MockAirportBounds>();
|
||
mockControllableVehicles_ = std::make_unique<MockControllableVehicles>();
|
||
|
||
// 创建冲突检测器
|
||
detector_ = std::make_unique<CollisionDetector>(*airportBounds_, *mockControllableVehicles_);
|
||
}
|
||
|
||
std::unique_ptr<MockAirportBounds> airportBounds_;
|
||
std::unique_ptr<MockControllableVehicles> mockControllableVehicles_;
|
||
std::unique_ptr<CollisionDetector> detector_;
|
||
};
|
||
|
||
// 测试可控车辆与航空器的冲突检测
|
||
TEST_F(CollisionDetectorTest, DetectControllableVehicleAircraftCollision) {
|
||
// 设置 Mock 期望 - 在创建数据之前设置
|
||
EXPECT_CALL(*mockControllableVehicles_, isControllable("VEH001"))
|
||
.WillRepeatedly(testing::Return(true));
|
||
Logger::info("Set mock expectation: VEH001 is controllable");
|
||
|
||
// 设置<E8AEBE><E7BDAE><EFBFBD>试数据
|
||
Aircraft aircraft;
|
||
aircraft.flightNo = "TEST001";
|
||
aircraft.position = {100, 100};
|
||
aircraft.speed = 10;
|
||
aircraft.heading = 90;
|
||
Logger::info("Created aircraft: flightNo=", aircraft.flightNo,
|
||
", position=(", aircraft.position.x, ", ", aircraft.position.y, ")");
|
||
|
||
Vehicle vehicle;
|
||
vehicle.vehicleNo = "VEH001";
|
||
vehicle.position = {120, 100}; // 距离航空器20米
|
||
vehicle.speed = 5;
|
||
vehicle.heading = 270;
|
||
Logger::info("Created vehicle: vehicleNo=", vehicle.vehicleNo,
|
||
", position=(", vehicle.position.x, ", ", vehicle.position.y, ")");
|
||
|
||
// 更新交通数据
|
||
detector_->updateTraffic({aircraft}, {vehicle});
|
||
Logger::info("Updated traffic data");
|
||
|
||
// 执行冲突检测
|
||
auto risks = detector_->detectCollisions();
|
||
|
||
// 验证结果
|
||
ASSERT_EQ(risks.size(), 1); // 应该检测到一个冲突风险
|
||
if (!risks.empty()) {
|
||
const auto& risk = risks[0];
|
||
EXPECT_EQ(risk.id1, "TEST001"); // 航空器ID
|
||
EXPECT_EQ(risk.id2, "VEH001"); // 车辆ID
|
||
EXPECT_EQ(risk.distance, 20); // 距离应该是20米
|
||
EXPECT_EQ(risk.level, RiskLevel::CRITICAL); // 20米距离应该是严重风险
|
||
EXPECT_GT(risk.relativeSpeed, 0); // 相对速度应该大于0
|
||
}
|
||
}
|
||
|
||
// 测试可控车辆与其他车辆的冲突检测
|
||
TEST_F(CollisionDetectorTest, DetectControllableVehicleOtherVehicleCollision) {
|
||
// 设置 Mock 期望
|
||
EXPECT_CALL(*mockControllableVehicles_, isControllable("VEH001"))
|
||
.WillRepeatedly(testing::Return(true));
|
||
EXPECT_CALL(*mockControllableVehicles_, isControllable("VEH002"))
|
||
.WillRepeatedly(testing::Return(false));
|
||
|
||
// 设置测试数据
|
||
Vehicle controlVehicle;
|
||
controlVehicle.vehicleNo = "VEH001";
|
||
controlVehicle.position = {100, 100};
|
||
controlVehicle.speed = 5;
|
||
controlVehicle.heading = 90;
|
||
|
||
Vehicle otherVehicle;
|
||
otherVehicle.vehicleNo = "VEH002";
|
||
otherVehicle.position = {120, 100}; // 距离可控车辆20米
|
||
otherVehicle.speed = 5;
|
||
otherVehicle.heading = 270;
|
||
|
||
// 更新交通数据
|
||
detector_->updateTraffic({}, {controlVehicle, otherVehicle});
|
||
|
||
// 执行冲突检测
|
||
auto risks = detector_->detectCollisions();
|
||
|
||
// 验证结果
|
||
ASSERT_EQ(risks.size(), 1); // 应该检测到一个冲突风险
|
||
if (!risks.empty()) {
|
||
EXPECT_EQ(risks[0].id1, "VEH001"); // 可控车辆ID
|
||
EXPECT_EQ(risks[0].id2, "VEH002"); // 其他车辆ID
|
||
EXPECT_EQ(risks[0].distance, 20); // 距离应该是20米
|
||
EXPECT_EQ(risks[0].level, RiskLevel::CRITICAL); // 20米距离应该是告警
|
||
}
|
||
}
|
||
|
||
// 测试非可控车辆的冲突检测（不应该产生冲突风险）
|
||
TEST_F(CollisionDetectorTest, NonControllableVehicleCollision) {
|
||
// 设置测试数据
|
||
Vehicle vehicle1;
|
||
vehicle1.vehicleNo = "VEH001";
|
||
vehicle1.position = {100, 100};
|
||
vehicle1.speed = 5;
|
||
vehicle1.heading = 90;
|
||
|
||
Vehicle vehicle2;
|
||
vehicle2.vehicleNo = "VEH002";
|
||
vehicle2.position = {120, 100}; // 距离20米
|
||
vehicle2.speed = 5;
|
||
vehicle2.heading = 270;
|
||
|
||
// 设置 Mock 期望
|
||
EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_))
|
||
.WillRepeatedly(testing::Return(false));
|
||
|
||
// 更新交通数据
|
||
detector_->updateTraffic({}, {vehicle1, vehicle2});
|
||
|
||
// 执行冲突检测
|
||
auto risks = detector_->detectCollisions();
|
||
|
||
// 验证结果
|
||
EXPECT_EQ(risks.size(), 0); // 非可控车辆之间的冲突不应该被检测
|
||
}
|
||
|
||
// 测试多个可控车辆之间的冲突检测
|
||
TEST_F(CollisionDetectorTest, MultipleControllableVehiclesCollision) {
|
||
// 设置 Mock 期望 - 所有车辆都是可控的
|
||
ON_CALL(*mockControllableVehicles_, isControllable(testing::_))
|
||
.WillByDefault(testing::Return(true));
|
||
EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_))
|
||
.Times(testing::AtLeast(3)); // 至少调用3次，因为有3辆车
|
||
Logger::info("Set mock expectation: all vehicles are controllable");
|
||
|
||
// 设置测试数据
|
||
std::vector<Vehicle> vehicles;
|
||
|
||
// VEH001 在 (100, 100)
|
||
Vehicle v1;
|
||
v1.vehicleNo = "VEH001";
|
||
v1.position = {100.0, 100.0};
|
||
v1.speed = 5;
|
||
v1.heading = 90;
|
||
vehicles.push_back(v1);
|
||
|
||
// VEH002 在 (120, 100)，与 VEH001 相距 20 米
|
||
Vehicle v2;
|
||
v2.vehicleNo = "VEH002";
|
||
v2.position = {120.0, 100.0};
|
||
v2.speed = 5;
|
||
v2.heading = 90;
|
||
vehicles.push_back(v2);
|
||
|
||
// VEH003 在 (200, 100)，与其他车辆距离超过阈值
|
||
Vehicle v3;
|
||
v3.vehicleNo = "VEH003";
|
||
v3.position = {200.0, 100.0};
|
||
v3.speed = 5;
|
||
v3.heading = 90;
|
||
vehicles.push_back(v3);
|
||
|
||
// 更新交通数据
|
||
detector_->updateTraffic({}, vehicles);
|
||
|
||
// 执行冲突检测
|
||
auto risks = detector_->detectCollisions();
|
||
|
||
// 验证结果
|
||
EXPECT_EQ(risks.size(), 1); // 应该只检测到1个冲突风险（VEH001和VEH002之间）
|
||
|
||
if (risks.size() == 1) {
|
||
// 验证冲突风险的详细信息
|
||
EXPECT_TRUE((risks[0].id1 == "VEH001" && risks[0].id2 == "VEH002") ||
|
||
(risks[0].id1 == "VEH002" && risks[0].id2 == "VEH001"));
|
||
EXPECT_EQ(risks[0].distance, 20.0);
|
||
EXPECT_EQ(risks[0].level, RiskLevel::CRITICAL); // 20米应该是告警级别
|
||
}
|
||
}
|
||
|
||
// 性能测试：模拟真实机场场景
|
||
TEST_F(CollisionDetectorTest, PerformanceTest) {
|
||
// 设置 Mock 期望 - 默认车辆不可控
|
||
EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_))
|
||
.WillRepeatedly(testing::Return(false));
|
||
|
||
// 设置3辆可控车辆
|
||
std::vector<std::string> controlVehicleNos = {
|
||
"VEH001", // 滑行道上的可控车辆
|
||
"VEH002", // 停机位的可控车辆
|
||
"VEH003" // 服务区的可控车辆
|
||
};
|
||
|
||
for (const auto& vehicleNo : controlVehicleNos) {
|
||
EXPECT_CALL(*mockControllableVehicles_, isControllable(vehicleNo))
|
||
.WillRepeatedly(testing::Return(true));
|
||
}
|
||
Logger::info("Set mock expectations for controllable vehicles");
|
||
|
||
// 创建测试数据
|
||
std::vector<Aircraft> aircraft;
|
||
std::vector<Vehicle> vehicles;
|
||
|
||
// 跑道区域：5架航空器
|
||
for (int i = 0; i < 5; ++i) {
|
||
Aircraft a;
|
||
a.flightNo = "RW" + std::to_string(i + 1);
|
||
a.position = {1800.0 + i * 500, 30.0}; // 跑道上等间距分布
|
||
a.speed = 30; // 较快速度
|
||
a.heading = 90;
|
||
aircraft.push_back(a);
|
||
}
|
||
|
||
// 滑行道：5架航空器
|
||
for (int i = 0; i < 5; ++i) {
|
||
Aircraft a;
|
||
a.flightNo = "TW" + std::to_string(i + 1);
|
||
a.position = {1800.0 + i * 500, 90.0}; // 滑行道上等间距分布
|
||
a.speed = 10; // 中等速度
|
||
a.heading = 90;
|
||
aircraft.push_back(a);
|
||
}
|
||
|
||
// 停机位区域：100架航空器，180辆车辆
|
||
for (int i = 0; i < 100; ++i) {
|
||
Aircraft a;
|
||
a.flightNo = "GT" + std::to_string(i + 1);
|
||
a.position = {
|
||
750.0 + (i % 10) * 150, // 10列
|
||
500.0 + (i / 10) * 100 // 10行
|
||
};
|
||
a.speed = 0; // 静止
|
||
a.heading = 180;
|
||
aircraft.push_back(a);
|
||
}
|
||
|
||
for (int i = 0; i < 180; ++i) {
|
||
Vehicle v;
|
||
v.vehicleNo = "GV" + std::to_string(i + 1);
|
||
v.position = {
|
||
750.0 + (i % 12) * 125, // 12列
|
||
500.0 + (i / 12) * 83 // 15行
|
||
};
|
||
v.speed = 5; // 低速
|
||
v.heading = (i % 4) * 90; // 4个方向
|
||
vehicles.push_back(v);
|
||
}
|
||
|
||
// 服务区：40架航空器，120辆车辆
|
||
for (int i = 0; i < 40; ++i) {
|
||
Aircraft a;
|
||
a.flightNo = "SA" + std::to_string(i + 1);
|
||
a.position = {
|
||
1000.0 + (i % 8) * 250, // 8列
|
||
500.0 + (i / 8) * 200 // 5行
|
||
};
|
||
a.speed = 0; // 静止
|
||
a.heading = 180;
|
||
aircraft.push_back(a);
|
||
}
|
||
|
||
for (int i = 0; i < 120; ++i) {
|
||
Vehicle v;
|
||
v.vehicleNo = "SV" + std::to_string(i + 1);
|
||
v.position = {
|
||
1000.0 + (i % 10) * 200, // 10列
|
||
500.0 + (i / 10) * 100 // 12行
|
||
};
|
||
v.speed = 8; // 中等速度
|
||
v.heading = (i % 8) * 45; // 8个方向
|
||
vehicles.push_back(v);
|
||
}
|
||
|
||
// 添加3辆可控车辆
|
||
// 1. 滑行道上的可控车辆
|
||
Vehicle taxiwayVehicle;
|
||
taxiwayVehicle.vehicleNo = "VEH001";
|
||
taxiwayVehicle.position = {1800.0, 90.0}; // 在滑行道上
|
||
taxiwayVehicle.speed = 10;
|
||
taxiwayVehicle.heading = 90;
|
||
vehicles.push_back(taxiwayVehicle);
|
||
|
||
// 2. 停机位的可控车辆
|
||
Vehicle gateVehicle;
|
||
gateVehicle.vehicleNo = "VEH002";
|
||
gateVehicle.position = {750.0, 500.0}; // 在停机位区域
|
||
gateVehicle.speed = 5;
|
||
gateVehicle.heading = 180;
|
||
vehicles.push_back(gateVehicle);
|
||
|
||
// 3. 服务区的可控车辆
|
||
Vehicle serviceVehicle;
|
||
serviceVehicle.vehicleNo = "VEH003";
|
||
serviceVehicle.position = {1000.0, 500.0}; // 在服务区
|
||
serviceVehicle.speed = 8;
|
||
serviceVehicle.heading = 270;
|
||
vehicles.push_back(serviceVehicle);
|
||
|
||
// 更新交通数据
|
||
detector_->updateTraffic(aircraft, vehicles);
|
||
Logger::info("Updated traffic data with ", aircraft.size(), " aircraft and ",
|
||
vehicles.size(), " vehicles (including 3 controllable vehicles)");
|
||
|
||
// 执行冲突检测并记录时间
|
||
auto start = std::chrono::high_resolution_clock::now();
|
||
|
||
auto risks = detector_->detectCollisions();
|
||
|
||
auto end = std::chrono::high_resolution_clock::now();
|
||
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
|
||
Logger::info("Collision detection completed in ", duration.count(), " microseconds");
|
||
Logger::info("Found ", risks.size(), " risks");
|
||
|
||
// 验证结果
|
||
ASSERT_GT(risks.size(), 0); // 应该检测到一些冲突风险
|
||
|
||
// 验证性能要求
|
||
EXPECT_LT(duration.count(), 100000); // 期望处理时间小于100ms
|
||
}
|
||
|
||
// 测试轨迹预测的准确性
|
||
TEST_F(CollisionDetectorTest, TrajectoryPredictionAccuracy) {
|
||
// 设置 Mock 期望
|
||
EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_))
|
||
.WillRepeatedly(testing::Return(true));
|
||
|
||
// 测试场景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坐标应该保持不变";
|
||
}
|
||
}
|
||
|
||
// 测试场景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米的距离";
|
||
}
|
||
|
||
// 测试场景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秒";
|
||
}
|
||
}
|
||
|
||
// 测试场景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秒";
|
||
}
|
||
}
|
||
|
||
// 测试场景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";
|
||
}
|
||
|
||
// 测试场景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) << "碰撞时间应该接近预期值";
|
||
}
|
||
}
|
||
|
||
// 测试场景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) << "碰撞时间应该接近预期值";
|
||
}
|
||
}
|
||
}
|