Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2019-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "RssSceneCreationTest.hpp"
      10                 :            : 
      11                 :            : struct RssSceneCreationTestTown04 : RssSceneCreationTest
      12                 :            : {
      13                 :          0 :   TestMode getTestMode() override
      14                 :            :   {
      15                 :          0 :     return TestMode::withRouteWithSpeedLimit;
      16                 :            :   }
      17                 :            : 
      18                 :          1 :   MapToLoad getMapToLoad() override
      19                 :            :   {
      20                 :          1 :     return MapToLoad::Town04;
      21                 :            :   }
      22                 :            : 
      23                 :          1 :   void initializeEgoVehicle() override
      24                 :            :   {
      25                 :          1 :     egoMatchObject.enuPosition.centerPoint.x = ::ad::map::point::ENUCoordinate(131.074);
      26                 :          1 :     egoMatchObject.enuPosition.centerPoint.y = ::ad::map::point::ENUCoordinate(302.408);
      27                 :          1 :     egoMatchObject.enuPosition.centerPoint.z = ::ad::map::point::ENUCoordinate(0);
      28         [ +  - ]:          1 :     egoMatchObject.enuPosition.heading = ::ad::map::point::createENUHeading(0.346132);
      29                 :            : 
      30         [ +  - ]:          1 :     initializeObjectENU(egoMatchObject.enuPosition.centerPoint, egoMatchObject.enuPosition.heading, egoMatchObject);
      31                 :            : 
      32                 :          1 :     egoSpeed = ::ad::physics::Speed(5.);
      33                 :          1 :     egoYawRate = ::ad::physics::AngularVelocity(0.);
      34                 :          1 :     egoSteeringAngle = ::ad::physics::Angle(0.);
      35                 :            : 
      36                 :          1 :     ::ad::map::point::ENUPoint target;
      37                 :          1 :     target.x = ::ad::map::point::ENUCoordinate(240.0);
      38                 :          1 :     target.y = ::ad::map::point::ENUCoordinate(307.0);
      39                 :          1 :     target.z = ::ad::map::point::ENUCoordinate(0.0);
      40         [ +  - ]:          1 :     auto positionEndGeo = ::ad::map::point::toGeo(target);
      41                 :            : 
      42                 :          1 :     egoRoute = ::ad::map::route::planning::planRoute(
      43                 :            :       egoMatchObject.mapMatchedBoundingBox
      44                 :          1 :         .referencePointPositions[static_cast<uint64_t>(::ad::map::match::ObjectReferencePoints::Center)][0]
      45         [ +  - ]:          1 :         .lanePoint.paraPoint,
      46                 :          1 :       positionEndGeo);
      47                 :          1 :   }
      48                 :            : 
      49                 :            :   ::ad::rss::core::RssCheck rssCheck;
      50                 :            : };
      51                 :            : 
      52                 :          2 : TEST_F(RssSceneCreationTestTown04, testVehicleBehindConnectingRoute)
      53                 :            : {
      54         [ +  - ]:          1 :   ::ad::rss::map::RssSceneCreation sceneCreation(1u, getEgoVehicleDynamics());
      55                 :            : 
      56         [ +  - ]:          2 :   spdlog::info("EgoMatchObject: {}", egoMatchObject);
      57         [ +  - ]:          2 :   spdlog::info("EgoRoute: {}", egoRoute);
      58                 :            : 
      59                 :          1 :   ::ad::rss::world::ObjectId otherVehicleId = ::ad::rss::world::ObjectId(10);
      60                 :          1 :   ::ad::physics::Speed otherVehicleSpeed{5.};
      61                 :          1 :   ::ad::physics::AngularVelocity otherVehicleYawRate{0.};
      62                 :          1 :   ::ad::physics::Angle otherVehicleSteeringAngle{0.};
      63                 :          1 :   ::ad::map::match::Object otherMatchObject;
      64                 :            : 
      65                 :          1 :   otherMatchObject.enuPosition.centerPoint.x = ::ad::map::point::ENUCoordinate(124.568);
      66                 :          1 :   otherMatchObject.enuPosition.centerPoint.y = ::ad::map::point::ENUCoordinate(299.829);
      67                 :          1 :   otherMatchObject.enuPosition.centerPoint.z = ::ad::map::point::ENUCoordinate(0);
      68         [ +  - ]:          1 :   otherMatchObject.enuPosition.heading = ::ad::map::point::createENUHeading(0.419017);
      69                 :            : 
      70         [ +  - ]:          1 :   initializeObjectENU(otherMatchObject.enuPosition.centerPoint, otherMatchObject.enuPosition.heading, otherMatchObject);
      71                 :            : 
      72                 :          1 :   ::ad::rss::map::RssObjectData egoObjectData;
      73                 :          1 :   egoObjectData.id = egoVehicleId;
      74                 :          1 :   egoObjectData.type = ::ad::rss::world::ObjectType::EgoVehicle;
      75         [ +  - ]:          1 :   egoObjectData.matchObject = egoMatchObject;
      76                 :          1 :   egoObjectData.speed = egoSpeed;
      77                 :          1 :   egoObjectData.yawRate = egoYawRate;
      78                 :          1 :   egoObjectData.steeringAngle = egoSteeringAngle;
      79                 :          1 :   egoObjectData.rssDynamics = getEgoVehicleDynamics();
      80                 :            : 
      81                 :          1 :   ::ad::rss::map::RssObjectData otherObjectData;
      82                 :          1 :   otherObjectData.id = otherVehicleId;
      83                 :          1 :   otherObjectData.type = ::ad::rss::world::ObjectType::OtherVehicle;
      84         [ +  - ]:          1 :   otherObjectData.matchObject = otherMatchObject;
      85                 :          1 :   otherObjectData.speed = otherVehicleSpeed;
      86                 :          1 :   otherObjectData.yawRate = otherVehicleYawRate;
      87                 :          1 :   otherObjectData.steeringAngle = otherVehicleSteeringAngle;
      88                 :          1 :   otherObjectData.rssDynamics = getObjectVehicleDynamics();
      89                 :            : 
      90   [ +  -  -  +  :          1 :   EXPECT_TRUE(
          -  -  -  -  -  
                -  -  - ]
      91                 :            :     sceneCreation.appendScenes(egoObjectData,
      92                 :            :                                egoRoute,
      93                 :            :                                otherObjectData,
      94                 :            :                                ::ad::rss::map::RssSceneCreation::RestrictSpeedLimitMode::IncreasedSpeedLimit10,
      95                 :            :                                ::ad::map::landmark::LandmarkIdSet(),
      96                 :            :                                ::ad::rss::map::RssMode::Structured));
      97                 :            : 
      98         [ +  - ]:          1 :   auto const worldModel = sceneCreation.getWorldModel();
      99   [ +  -  -  +  :          1 :   EXPECT_TRUE(withinValidInputRange(worldModel));
          -  -  -  -  -  
                -  -  - ]
     100                 :            : 
     101         [ +  - ]:          1 :   spdlog::info("WordModel: {}", worldModel);
     102   [ +  -  -  +  :          1 :   ASSERT_EQ(worldModel.scenes.size(), 1u);
          -  -  -  -  -  
                      - ]
     103                 :            : 
     104                 :          1 :   ::ad::rss::state::ProperResponse routeResponse;
     105                 :          1 :   ::ad::rss::situation::SituationSnapshot situationSnapshot;
     106                 :          1 :   ::ad::rss::state::RssStateSnapshot stateSnapshot;
     107   [ +  -  -  +  :          1 :   EXPECT_TRUE(rssCheck.calculateProperResponse(worldModel, situationSnapshot, stateSnapshot, routeResponse));
          -  -  -  -  -  
                -  -  - ]
     108                 :            : 
     109                 :            :   // not safe, since the one behind us is far too near
     110         [ +  - ]:          1 :   spdlog::info("RouteResponse: {}", routeResponse);
     111         [ +  - ]:          1 :   spdlog::info("StateSnapshot: {}", stateSnapshot);
     112         [ +  - ]:          1 :   spdlog::info("SituationSnapshot: {}", situationSnapshot);
     113   [ +  -  -  +  :          1 :   ASSERT_EQ(situationSnapshot.situations.size(), 1u);
          -  -  -  -  -  
                      - ]
     114                 :            : 
     115   [ -  +  -  -  :          1 :   EXPECT_FALSE(routeResponse.isSafe);
          -  -  -  -  -  
                      - ]
     116   [ +  -  -  +  :          1 :   EXPECT_EQ(::ad::rss::situation::LongitudinalRelativePosition::InFront,
          -  -  -  -  -  
                      - ]
     117                 :            :             situationSnapshot.situations.front().relativePosition.longitudinalPosition);
     118   [ +  -  -  +  :          1 :   EXPECT_EQ(::ad::rss::state::LongitudinalResponse::None, routeResponse.longitudinalResponse);
          -  -  -  -  -  
                      - ]
     119   [ +  -  -  +  :          1 :   EXPECT_EQ(::ad::rss::situation::LateralRelativePosition::OverlapLeft,
          -  -  -  -  -  
                      - ]
     120                 :            :             situationSnapshot.situations.front().relativePosition.lateralPosition);
     121   [ +  -  -  +  :          1 :   EXPECT_EQ(::ad::rss::state::LateralResponse::None, routeResponse.lateralResponseLeft);
          -  -  -  -  -  
                      - ]
     122   [ +  -  -  +  :          1 :   EXPECT_EQ(::ad::rss::state::LateralResponse::BrakeMin, routeResponse.lateralResponseRight);
          -  -  -  -  -  
                      - ]
     123                 :            : }