diff --git a/src/detector/CollisionDetector.cpp b/src/detector/CollisionDetector.cpp index 044ab94..e605d8f 100644 --- a/src/detector/CollisionDetector.cpp +++ b/src/detector/CollisionDetector.cpp @@ -455,7 +455,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( }; // 由于有碰撞半径，实际碰撞会提前发生 - // 对于交叉路径，两车需要各自移动 safe_distance/√2 的距离才会相遇 + // 对于叉路径，两车需要各自移动 safe_distance/√2 的距离才会相遇 double offset_distance = safe_distance / std::sqrt(2.0); double offset_time = offset_distance / speed1; // 两车速度相同，用任意一个都以 double collision_time = t_cross - offset_time; @@ -484,7 +484,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( pos2.y + vy2 * collision_time }; - // 碰撞点在两车连线的中点 + // 碰撞点在两车连线中点 result.collisionPoint = { (collision1.x + collision2.x) / 2.0, (collision1.y + collision2.y) / 2.0 @@ -513,8 +513,8 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( // 2. 横向距离大于安全距离 // 3. 相对速度很小（说明速度接近） if (angle_diff < 30.0) { - // 计算垂直于运动方向的横向距离 - // 将航向角转换为学坐标系中的旋转角度（逆时针为正） + // 计算垂直于运动方向的向距离 + // 航向角转换为学坐标系中的旋转角度（逆时针为正） double rotation_angle = (90.0 - heading1) * M_PI / 180.0; // 使用标准的二维坐标旋转公式 @@ -549,6 +549,12 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( const int STEPS = 120; // 增加采样点数以提高精度 double dt = timeWindow / STEPS; // 时间步长 + // 重新计算速度分量（修正计算方式） + double vx1_sample = speed1 * std::cos((90.0 - heading1) * M_PI / 180.0); // x 方向 + double vy1_sample = speed1 * std::sin((90.0 - heading1) * M_PI / 180.0); // y 方向 + double vx2_sample = speed2 * std::cos((90.0 - heading2) * M_PI / 180.0); + double vy2_sample = speed2 * std::sin((90.0 - heading2) * M_PI / 180.0); + // 如果当前已经碰撞 if (current_distance <= safe_distance) { result.willCollide = true; @@ -576,21 +582,21 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( for (int i = 1; i <= STEPS; ++i) { double t = i * dt; - // 计算t时的位置 + // 计算t时的位置（使用修正后的速度分量） Vector2D future1 = { - pos1.x + vx1 * t, - pos1.y + vy1 * t + pos1.x + vx1_sample * t, + pos1.y + vy1_sample * t }; Vector2D future2 = { - pos2.x + vx2 * t, - pos2.y + vy2 * t + pos2.x + vx2_sample * t, + pos2.y + vy2_sample * t }; - // 计算t时刻的距离 + // 计算t时的距离 double dx_t = future2.x - future1.x; double dy_t = future2.y - future1.y; - double distance = std::sqrt(dx_t*dx_t + dy_t*dy_t); + double distance = std::sqrt(dx_t*dx_t + dy_t*dy_t); // 统一使用实际距离 Logger::debug( "采样点状态: step=", i, @@ -613,8 +619,9 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( // 检查是否会碰撞 if (distance <= safe_distance) { result.willCollide = true; - // 使用线性插值计算更精确的碰撞时间 - double t_interp = t - dt + (dt * (safe_distance - distance) / (prev_distance - distance)); + // 使用当前距离和安全距离做插值，提高精确度 + double progress = (distance - safe_distance) / (prev_distance - safe_distance); + double t_interp = t - dt * progress; result.timeToCollision = t_interp; Logger::debug( @@ -629,12 +636,12 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( // 使用插值时间计算更精确的碰撞点 Vector2D interp1 = { - pos1.x + vx1 * t_interp, - pos1.y + vy1 * t_interp + pos1.x + vx1_sample * t_interp, + pos1.y + vy1_sample * t_interp }; Vector2D interp2 = { - pos2.x + vx2 * t_interp, - pos2.y + vy2 * t_interp + pos2.x + vx2_sample * t_interp, + pos2.y + vy2_sample * t_interp }; result.collisionPoint = { (interp1.x + interp2.x) / 2.0, @@ -647,7 +654,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision( prev_distance = distance; - // 如果相对速度很小，且距离增加，可以提前退出 + // 如果相对速度很小，且距��增加，可以提前退出 if (rel_speed < 0.1 && i > 1) { if (distance > prev_distance) { break; diff --git a/tests/BasicCollisionTest.cpp b/tests/BasicCollisionTest.cpp index e4ae782..b1c4037 100644 --- a/tests/BasicCollisionTest.cpp +++ b/tests/BasicCollisionTest.cpp @@ -43,6 +43,8 @@ public: class BasicCollisionTest : public ::testing::Test { protected: void SetUp() override { + Logger::setLogLevel(LogLevel::DEBUG); // 设置日志级别为 DEBUG + airportBounds_ = std::make_unique(); mockControllableVehicles_ = std::make_unique(); @@ -77,7 +79,7 @@ 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) << "应该识别为静态碰撞"; @@ -160,7 +162,7 @@ TEST_F(BasicCollisionTest, ParallelMotion) { auto result = detector_->checkCollision(v1, v2, 30.0); EXPECT_FALSE(result.willCollide) << "平行运动且距离大于碰撞半径的物体不应该检测为碰撞"; - EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行��动"; + EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行运动"; // 增加更多验证 EXPECT_DOUBLE_EQ(result.minDistance, 50.0) << "最小距离应该是初始距离50米"; @@ -252,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"; @@ -282,7 +284,7 @@ TEST_F(BasicCollisionTest, DivergentMotion) { obj1.vehicleNo = "V1"; obj1.position = {150, 100}; obj1.speed = 10; - obj1.heading = 90; // ��右运动 + obj1.heading = 90; // 右运动 obj1.type = MovingObjectType::UNMANNED; Vehicle obj2; @@ -303,4 +305,59 @@ TEST_F(BasicCollisionTest, DivergentMotion) { // 由于两车都以相同速度向右运动，最小距离应该保持不变 EXPECT_NEAR(result.timeToMinDistance, 0.0, 0.1); +} + +// 6. 追尾场景测试 +TEST_F(BasicCollisionTest, TailgatingMotion) { + // 创建两个同向运动的物体，后车速度大于前车 + Vehicle v1; // 前车 + v1.vehicleNo = "V1"; + v1.position = {60.0, 100.0}; // 前车在前方60米处 + v1.speed = 10.0; // 前车速度10m/s + v1.heading = 90.0; // 向东运动 + v1.type = MovingObjectType::UNMANNED; + + Vehicle v2; // 后车 + v2.vehicleNo = "V2"; + v2.position = {0.0, 100.0}; // 后车在原点 + v2.speed = 15.0; // 后车速度15m/s + v2.heading = 90.0; // 向东运动 + v2.type = MovingObjectType::UNMANNED; + + auto result = detector_->checkCollision(v1, v2, 30.0); + + // 验证碰撞类型 + EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行运动"; + + // 验证会发生碰撞 + EXPECT_TRUE(result.willCollide) << "后车速度大于前车，应该预测到碰撞"; + + // 验证碰撞时间（初始距离60米，相对速度5m/s，安全距离50米，需要缩短10米，所以碰撞时间应该是2秒） + EXPECT_NEAR(result.timeToCollision, 2.0, 0.1) << "碰撞时间应该接近2秒"; + + // 验证最小距离（应该是安全距离） + EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是安全距离50米"; + + // 验证最小距离时间（应该等于碰撞时间） + EXPECT_NEAR(result.timeToMinDistance, 2.0, 0.1) << "最小距离时间应该等于碰撞时间"; + + // 验证碰撞点（在两车碰撞时的中点） + // 前车：初始位置 60 + 10 * 2 = 80 + // 后车：初始位置 0 + 15 * 2 = 30 + // 碰撞点应该在 (80 + 30) / 2 = 55 + EXPECT_NEAR(result.collisionPoint.x, 55.0, 0.1) << "碰撞点x坐标应该在55米处"; + EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点y坐标应该保持在100米"; + + // 验证碰撞时刻的物体状态 + // 前车位置：60 + 10 * 2 = 80 + EXPECT_NEAR(result.object1State.position.x, 80.0, 0.1); + EXPECT_NEAR(result.object1State.position.y, 100.0, 0.1); + EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0); + EXPECT_DOUBLE_EQ(result.object1State.heading, 90.0); + + // 后车位置：0 + 15 * 2 = 30 + EXPECT_NEAR(result.object2State.position.x, 30.0, 0.1); + 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); } \ No newline at end of file diff --git a/tests/CollisionDetectorTest.cpp b/tests/CollisionDetectorTest.cpp index 7ff262c..880073b 100644 --- a/tests/CollisionDetectorTest.cpp +++ b/tests/CollisionDetectorTest.cpp @@ -7,28 +7,6 @@ #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 { public: @@ -263,7 +241,7 @@ TEST_F(CollisionDetectorTest, StationaryVehiclesCollision) { v2.speed = 5.0; // 开始移动 v2.heading = 90.0; // 向东行驶（远离v1） collisionResult = detector_->checkCollision(v1, v2, 30.0); - EXPECT_FALSE(collisionResult.willCollide) << "移动车辆远离静止车辆且距离���于安全距离时不应该检测到碰撞"; + EXPECT_FALSE(collisionResult.willCollide) << "移动车辆远离静止车辆且距离于安全距离时不应该检测到碰撞"; } // 测试同向运动的碰撞检测 @@ -290,29 +268,14 @@ TEST_F(CollisionDetectorTest, TailgatingCollision) { // 测试2：同向不同速（v2速度更快，会追上v1） v2.speed = 15.0; - Logger::debug( - "同向追尾测试: ", - "v1=(", v1.position.x, ",", v1.position.y, "), speed=", v1.speed, - ", v2=(", v2.position.x, ",", v2.position.y, "), speed=", v2.speed, - ", 相对速度=", v2.speed - v1.speed, - ", 初始距离=", std::sqrt( - std::pow(v2.position.x - v1.position.x, 2) + - std::pow(v2.position.y - v1.position.y, 2) - ) - ); collisionResult = detector_->checkCollision(v1, v2, 30.0); - Logger::debug( - "碰撞检测结果: willCollide=", collisionResult.willCollide, - ", timeToCollision=", collisionResult.timeToCollision, - ", minDistance=", collisionResult.minDistance, - ", type=", static_cast(collisionResult.type) - ); EXPECT_TRUE(collisionResult.willCollide) << "同向但速度较快的车辆追上前车时应该检测到碰撞"; // 测试3：同向不同速但距离较远 - v2.position = {0.0, 100.0}; // 增加到100米距离 + v2.position = {0.0, 100.0}; // 在原点 + v2.speed = 11.0; // 减小速度差，从 15m/s 改为 11m/s collisionResult = detector_->checkCollision(v1, v2, 30.0); - EXPECT_FALSE(collisionResult.willCollide) << "距离较远时不应该检测到碰撞"; + EXPECT_FALSE(collisionResult.willCollide) << "相对速度小且距离较远时不应该检测到碰撞"; } // 测试航空器与静止车辆的碰撞检测 @@ -336,8 +299,8 @@ TEST_F(CollisionDetectorTest, AircraftStationaryVehicleCollision) { auto collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0); EXPECT_TRUE(collisionResult.willCollide) << "航空器接近静止车辆时应该检测到碰撞"; - // 测试2：静止车辆在航空器航向偏离处 - vehicle.position = {200.0, 170.0}; // 在航空器前方偏北，距离约100米（大于安全距离75米） + // 测试2���静止车辆在航空器航向偏离处 + vehicle.position = {200.0, 200.0}; // 在航空器前方偏北，距离约100米（大于安全距离75米） collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0); EXPECT_FALSE(collisionResult.willCollide) << "航空器与不在航向上的静止车辆不应该检测到碰撞";