Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2018-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "RssSituationIdProvider.hpp"
      10                 :            : #include <algorithm>
      11                 :            : 
      12                 :            : namespace ad {
      13                 :            : namespace rss {
      14                 :            : namespace world {
      15                 :            : 
      16                 :        924 : RssSituationIdProvider::SituationData::SituationData(TimeIndex timeIndex,
      17                 :            :                                                      situation::SituationId const situationId,
      18                 :        924 :                                                      Scene const &scene)
      19                 :            :   : mTimeIndex(timeIndex)
      20                 :        924 :   , mSituationType(scene.situationType)
      21                 :        984 :   , mSituationId(situationId)
      22                 :            : {
      23         [ +  + ]:        924 :   if ((mSituationType == situation::SituationType::IntersectionEgoHasPriority)
      24         [ +  + ]:        853 :       || (mSituationType == situation::SituationType::IntersectionObjectHasPriority)
      25         [ +  + ]:        805 :       || (mSituationType == situation::SituationType::IntersectionSamePriority))
      26                 :            :   {
      27   [ +  +  +  + ]:        169 :     mEgoVehicleIntersectionArea = getIntersectionArea(scene.egoVehicleRoad);
      28   [ +  +  +  + ]:        154 :     mObjectIntersectionArea = getIntersectionArea(scene.intersectingRoad);
      29                 :            :   }
      30                 :        894 : }
      31                 :            : 
      32                 :            : RssSituationIdProvider::SituationData::IntersectionArea
      33                 :       1767 : RssSituationIdProvider::SituationData::getIntersectionArea(RoadArea roadArea)
      34                 :            : {
      35                 :       1767 :   IntersectionArea intersectionArea;
      36         [ +  + ]:       7223 :   for (auto &roadSegment : roadArea)
      37                 :            :   {
      38         [ +  + ]:      10940 :     for (auto &laneSegment : roadSegment)
      39                 :            :     {
      40         [ +  + ]:       5484 :       if (laneSegment.type == LaneSegmentType::Intersection)
      41                 :            :       {
      42         [ +  + ]:       1767 :         intersectionArea.insert(laneSegment.id);
      43                 :            :       }
      44                 :            :     }
      45                 :            :   }
      46                 :       1761 :   return intersectionArea;
      47                 :            : }
      48                 :            : 
      49                 :       1474 : bool RssSituationIdProvider::SituationData::isSmallerOrEqual(
      50                 :            :   RssSituationIdProvider::SituationData::IntersectionArea const &left,
      51                 :            :   RssSituationIdProvider::SituationData::IntersectionArea const &right) const
      52                 :            : {
      53         [ +  + ]:       1474 :   if (right.size() < left.size())
      54                 :            :   {
      55                 :          2 :     return false;
      56                 :            :   }
      57                 :       1472 :   std::size_t const expectedDifference = right.size() - left.size();
      58                 :       1472 :   IntersectionArea differenceSet;
      59                 :            :   std::set_difference(right.begin(),
      60                 :            :                       right.end(),
      61                 :            :                       left.begin(),
      62                 :            :                       left.end(),
      63         [ +  - ]:       1472 :                       std::insert_iterator<IntersectionArea>(differenceSet, differenceSet.begin()));
      64                 :       1472 :   return differenceSet.size() == expectedDifference;
      65                 :            : }
      66                 :            : 
      67                 :       4102 : bool RssSituationIdProvider::SituationData::updateSituation(TimeIndex const timeIndex, Scene const &scene)
      68                 :            : {
      69         [ +  + ]:       4102 :   if (scene.situationType != mSituationType)
      70                 :            :   {
      71                 :          1 :     return false;
      72                 :            :   }
      73                 :            : 
      74                 :       8202 :   IntersectionArea sceneEgoVehicleIntersectionArea;
      75                 :       8202 :   IntersectionArea sceneObjectIntersectionArea;
      76                 :            : 
      77         [ +  + ]:       4101 :   if ((mSituationType == situation::SituationType::IntersectionEgoHasPriority)
      78         [ +  + ]:       3725 :       || (mSituationType == situation::SituationType::IntersectionObjectHasPriority)
      79         [ +  + ]:       3544 :       || (mSituationType == situation::SituationType::IntersectionSamePriority))
      80                 :            :   {
      81                 :            :     // extract the intersection areas of the scene
      82   [ +  -  +  - ]:        738 :     sceneEgoVehicleIntersectionArea = getIntersectionArea(scene.egoVehicleRoad);
      83   [ +  -  +  + ]:        738 :     if (!isSmallerOrEqual(sceneEgoVehicleIntersectionArea, mEgoVehicleIntersectionArea))
      84                 :            :     {
      85                 :          2 :       return false;
      86                 :            :     }
      87                 :            : 
      88   [ +  -  +  - ]:        736 :     sceneObjectIntersectionArea = getIntersectionArea(scene.intersectingRoad);
      89   [ +  -  +  + ]:        736 :     if (!isSmallerOrEqual(sceneObjectIntersectionArea, mObjectIntersectionArea))
      90                 :            :     {
      91                 :          2 :       return false;
      92                 :            :     }
      93                 :            :   }
      94                 :            :   else
      95                 :            :   {
      96                 :            :     // there must be only one situation of all other types between two objects
      97                 :            :     // therefore, the situation is treated to be the same if the situation type matches
      98                 :            :   }
      99                 :            : 
     100                 :       4097 :   mTimeIndex = timeIndex;
     101                 :       4097 :   return true;
     102                 :            : }
     103                 :            : 
     104                 :       5021 : void RssSituationIdProvider::updateTime(TimeIndex const &timeIndex)
     105                 :            : {
     106         [ +  + ]:       5021 :   if (timeIndex != mCurrentTime)
     107                 :            :   {
     108                 :       4229 :     mLastTime = mCurrentTime;
     109                 :       4229 :     mCurrentTime = timeIndex;
     110                 :            : 
     111                 :            :     // next time step, remove outdated data
     112         [ +  + ]:       8294 :     for (auto iter = mSituationData.begin(); iter != mSituationData.end();)
     113                 :            :     {
     114   [ +  +  +  -  :       4065 :       if ((iter->second.mTimeIndex != mLastTime) && (iter->second.mTimeIndex != mCurrentTime))
                   +  + ]
     115                 :            :       {
     116         [ +  - ]:          1 :         iter = mSituationData.erase(iter);
     117                 :            :       }
     118                 :            :       else
     119                 :            :       {
     120                 :       4064 :         ++iter;
     121                 :            :       }
     122                 :            :     }
     123                 :            :   }
     124                 :       5021 : }
     125                 :            : 
     126                 :       5021 : situation::SituationId RssSituationIdProvider::getSituationId(TimeIndex const &timeIndex, Scene const &scene)
     127                 :            : {
     128         [ +  - ]:       5021 :   updateTime(timeIndex);
     129                 :            : 
     130         [ +  - ]:       5021 :   auto const objectDataRange = mSituationData.equal_range(scene.object.objectId);
     131                 :            :   auto findResult = std::find_if(
     132                 :       4102 :     objectDataRange.first, objectDataRange.second, [&scene, this](SituationDataMap::value_type &situation) {
     133                 :       4102 :       return situation.second.updateSituation(this->mCurrentTime, scene);
     134         [ +  - ]:       5021 :     });
     135         [ +  + ]:       5021 :   if (findResult != objectDataRange.second)
     136                 :            :   {
     137                 :       4097 :     return findResult->second.mSituationId;
     138                 :            :   }
     139                 :            :   auto insertResult = mSituationData.emplace_hint(
     140   [ +  -  +  +  :        939 :     objectDataRange.first, scene.object.objectId, SituationData(mCurrentTime, getFreeSituationId(), scene));
                   +  + ]
     141         [ +  - ]:        879 :   if (insertResult != mSituationData.end())
     142                 :            :   {
     143                 :        879 :     return insertResult->second.mSituationId;
     144                 :            :   }
     145                 :            :   // LCOV_EXCL_START: unreachable code, keep to be on the safe side
     146                 :            :   throw std::runtime_error("RssSituationIdProvider::getSituationId>> cannot add scene to situation map");
     147                 :            :   // LCOV_EXCL_STOP: unreachable code, keep to be on the safe side
     148                 :            : }
     149                 :            : 
     150                 :        924 : situation::SituationId RssSituationIdProvider::getFreeSituationId()
     151                 :            : {
     152                 :        924 :   bool situationIdFound = false;
     153                 :            :   do // LCOV_EXCL_LINE: lcov analysis misses this line
     154                 :            :   {
     155                 :        924 :     mNextSituationId++;
     156                 :            :     auto findResult = std::find_if(
     157                 :       5076 :       mSituationData.begin(), mSituationData.end(), [this](SituationDataMap::value_type const &situation) {
     158                 :       5076 :         return (situation.second.mSituationId == this->mNextSituationId);
     159         [ +  - ]:        924 :       });
     160                 :        924 :     situationIdFound = findResult == mSituationData.end();
     161         [ -  + ]:        924 :   } while (!situationIdFound);
     162                 :            : 
     163                 :        924 :   return mNextSituationId;
     164                 :            : }
     165                 :            : 
     166                 :            : } // namespace world
     167                 :            : } // namespace rss
     168                 :            : } // namespace ad