Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2020-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "TestSupport.hpp"
      10                 :            : #include "ad/rss/situation/RssFormulas.hpp"
      11                 :            : 
      12                 :            : namespace ad {
      13                 :            : namespace rss {
      14                 :            : namespace situation {
      15                 :            : 
      16                 :          2 : TEST(RssFormulaTestsCalculateSafeLateralDistance, invalid_vehicle_state)
      17                 :            : {
      18                 :          1 :   Distance safeDistance(0.);
      19                 :            : 
      20         [ +  - ]:          1 :   VehicleState vehicle = createVehicleStateForLateralMotion(0.);
      21                 :          1 :   vehicle.dynamics.alphaLat.accelMax = Acceleration(-1.);
      22                 :          1 :   vehicle.dynamics.alphaLat.brakeMin = Acceleration(-1.);
      23         [ +  - ]:          1 :   VehicleState otherVehicle = createVehicleStateForLateralMotion(0.);
      24                 :            : 
      25   [ +  -  -  +  :          1 :   ASSERT_FALSE(calculateSafeLateralDistance(vehicle, otherVehicle, safeDistance));
          -  -  -  -  -  
                -  -  - ]
      26   [ +  -  -  +  :          1 :   ASSERT_FALSE(calculateSafeLateralDistance(otherVehicle, vehicle, safeDistance));
          -  -  -  -  -  
                -  -  - ]
      27   [ +  -  -  +  :          1 :   ASSERT_FALSE(calculateSafeLateralDistance(vehicle, vehicle, safeDistance));
          -  -  -  -  -  
                -  -  - ]
      28                 :            : }
      29                 :            : 
      30                 :          2 : TEST(RssFormulaTestsCalculateSafeLateralDistance, same_lateral_speed)
      31                 :            : {
      32                 :          1 :   Distance safeDistance(0.);
      33                 :            : 
      34         [ +  - ]:          1 :   std::vector<double> expectedSafeDistance = {1.1, 1.19, 1.19, 2.9, 2.9};
      35                 :          1 :   uint32_t expectedSafeDistanceIndex = 0u;
      36         [ +  + ]:          6 :   for (auto lateralSpeed : {0., 1., -1., 5., -5.})
      37                 :            :   {
      38         [ +  - ]:          5 :     VehicleState leftVehicle = createVehicleStateForLateralMotion(lateralSpeed);
      39         [ +  - ]:          5 :     VehicleState rightVehicle = createVehicleStateForLateralMotion(lateralSpeed);
      40                 :            : 
      41   [ +  -  -  +  :          5 :     ASSERT_TRUE(calculateSafeLateralDistance(leftVehicle, rightVehicle, safeDistance));
          -  -  -  -  -  
                -  -  - ]
      42   [ +  -  -  +  :          5 :     ASSERT_NEAR(static_cast<double>(safeDistance), expectedSafeDistance[expectedSafeDistanceIndex], 0.01);
          -  -  -  -  -  
                      - ]
      43                 :            : 
      44   [ +  -  -  +  :          5 :     ASSERT_TRUE(calculateSafeLateralDistance(rightVehicle, leftVehicle, safeDistance));
          -  -  -  -  -  
                -  -  - ]
      45   [ +  -  -  +  :          5 :     ASSERT_NEAR(static_cast<double>(safeDistance), expectedSafeDistance[expectedSafeDistanceIndex], 0.01);
          -  -  -  -  -  
                      - ]
      46                 :            : 
      47                 :          5 :     expectedSafeDistanceIndex++;
      48                 :            :   }
      49                 :            : }
      50                 :            : 
      51                 :          2 : TEST(RssFormulaTestsCalculateSafeLateralDistance, one_zero_lateral_speed)
      52                 :            : {
      53                 :          1 :   Distance safeDistance(0.);
      54                 :            : 
      55         [ +  - ]:          1 :   std::vector<double> expectedSafeDistanceLeft = {1.84, 0.45, 5.77, 0.};
      56         [ +  - ]:          1 :   std::vector<double> expectedSafeDistanceRight = {0.45, 1.84, 0., 5.77};
      57                 :          1 :   uint32_t expectedSafeDistanceIndex = 0u;
      58         [ +  + ]:          5 :   for (auto lateralSpeed : {1., -1., 5., -5.})
      59                 :            :   {
      60         [ +  - ]:          4 :     VehicleState leftVehicle = createVehicleStateForLateralMotion(lateralSpeed);
      61         [ +  - ]:          4 :     VehicleState rightVehicle = createVehicleStateForLateralMotion(0.);
      62                 :            : 
      63   [ +  -  -  +  :          4 :     ASSERT_TRUE(calculateSafeLateralDistance(leftVehicle, rightVehicle, safeDistance));
          -  -  -  -  -  
                -  -  - ]
      64   [ +  -  -  +  :          4 :     ASSERT_NEAR(static_cast<double>(safeDistance), expectedSafeDistanceLeft[expectedSafeDistanceIndex], 0.01);
          -  -  -  -  -  
                      - ]
      65                 :            : 
      66   [ +  -  -  +  :          4 :     ASSERT_TRUE(calculateSafeLateralDistance(rightVehicle, leftVehicle, safeDistance));
          -  -  -  -  -  
                -  -  - ]
      67   [ +  -  -  +  :          4 :     ASSERT_NEAR(static_cast<double>(safeDistance), expectedSafeDistanceRight[expectedSafeDistanceIndex], 0.01);
          -  -  -  -  -  
                      - ]
      68                 :            : 
      69                 :          4 :     expectedSafeDistanceIndex++;
      70                 :            :   }
      71                 :            : }
      72                 :            : 
      73                 :          2 : TEST(RssFormulaTestsCalculateSafeLateralDistance, lateral_fluctuation_margin_is_considered)
      74                 :            : {
      75                 :          1 :   Distance safeDistance(0.);
      76                 :            : 
      77         [ +  - ]:          1 :   std::vector<double> expectedSafeDistance = {1.1, 1.19, 1.19, 2.9, 2.9};
      78                 :          1 :   uint32_t expectedSafeDistanceIndex = 0u;
      79         [ +  + ]:          6 :   for (auto lateralSpeed : {0., 1., -1., 5., -5.})
      80                 :            :   {
      81         [ +  - ]:          5 :     VehicleState leftVehicle = createVehicleStateForLateralMotion(lateralSpeed);
      82         [ +  - ]:          5 :     VehicleState rightVehicle = createVehicleStateForLateralMotion(lateralSpeed);
      83                 :            : 
      84   [ +  -  -  +  :          5 :     ASSERT_TRUE(calculateSafeLateralDistance(leftVehicle, rightVehicle, safeDistance));
          -  -  -  -  -  
                -  -  - ]
      85   [ +  -  -  +  :          5 :     ASSERT_NEAR(static_cast<double>(safeDistance), expectedSafeDistance[expectedSafeDistanceIndex], 0.01);
          -  -  -  -  -  
                      - ]
      86                 :            : 
      87         [ +  - ]:          5 :     leftVehicle.dynamics.lateralFluctuationMargin += ad::physics::Distance(0.5);
      88                 :            : 
      89   [ +  -  -  +  :          5 :     ASSERT_TRUE(calculateSafeLateralDistance(rightVehicle, leftVehicle, safeDistance));
          -  -  -  -  -  
                -  -  - ]
      90   [ +  -  -  +  :          5 :     ASSERT_NEAR(static_cast<double>(safeDistance), expectedSafeDistance[expectedSafeDistanceIndex] + 0.25, 0.01);
          -  -  -  -  -  
                      - ]
      91                 :            : 
      92         [ +  - ]:          5 :     rightVehicle.dynamics.lateralFluctuationMargin += ad::physics::Distance(0.5);
      93                 :            : 
      94   [ +  -  -  +  :          5 :     ASSERT_TRUE(calculateSafeLateralDistance(rightVehicle, leftVehicle, safeDistance));
          -  -  -  -  -  
                -  -  - ]
      95   [ +  -  -  +  :          5 :     ASSERT_NEAR(static_cast<double>(safeDistance), expectedSafeDistance[expectedSafeDistanceIndex] + 0.5, 0.01);
          -  -  -  -  -  
                      - ]
      96                 :            : 
      97                 :          5 :     expectedSafeDistanceIndex++;
      98                 :            :   }
      99                 :            : }
     100                 :            : 
     101                 :            : } // namespace situation
     102                 :            : } // namespace rss
     103                 :            : } // namespace ad