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 "ad/rss/structured/RssConstellationIdProvider.hpp"
      10                 :            : #include <algorithm>
      11                 :            : 
      12                 :            : namespace ad {
      13                 :            : namespace rss {
      14                 :            : namespace structured {
      15                 :            : 
      16                 :        878 : RssConstellationIdProvider::ConstellationData::ConstellationData(world::TimeIndex time_index,
      17                 :            :                                                                  core::RelativeConstellationId const constellation_id,
      18                 :        878 :                                                                  world::Constellation const &constellation)
      19                 :        878 :   : mTimeIndex(time_index)
      20                 :        878 :   , mConstellationType(constellation.constellation_type)
      21                 :        878 :   , mConstellationId(constellation_id)
      22                 :            : {
      23         [ +  + ]:        878 :   if ((mConstellationType == world::ConstellationType::IntersectionEgoHasPriority)
      24         [ +  + ]:        815 :       || (mConstellationType == world::ConstellationType::IntersectionObjectHasPriority)
      25         [ +  + ]:        774 :       || (mConstellationType == world::ConstellationType::IntersectionSamePriority))
      26                 :            :   {
      27         [ +  + ]:        144 :     mEgoVehicleIntersectionArea = getIntersectionArea(constellation.ego_vehicle_road);
      28         [ +  + ]:        141 :     mObjectIntersectionArea = getIntersectionArea(constellation.intersecting_road);
      29                 :            :   }
      30                 :        884 : }
      31                 :            : 
      32                 :            : RssConstellationIdProvider::ConstellationData::IntersectionArea
      33                 :       1759 : RssConstellationIdProvider::ConstellationData::getIntersectionArea(world::RoadArea const &roadArea)
      34                 :            : {
      35                 :       1759 :   IntersectionArea intersectionArea;
      36         [ +  + ]:       7191 :   for (auto &roadSegment : roadArea)
      37                 :            :   {
      38         [ +  + ]:       5438 :     if (roadSegment.type == world::RoadSegmentType::Intersection)
      39                 :            :     {
      40         [ +  + ]:       3512 :       for (auto &laneSegment : roadSegment.lane_segments)
      41                 :            :       {
      42         [ +  + ]:       1761 :         intersectionArea.insert(laneSegment.id);
      43                 :            :       }
      44                 :            :     }
      45                 :            :   }
      46                 :       1753 :   return intersectionArea;
      47                 :          6 : }
      48                 :            : 
      49                 :       1474 : bool RssConstellationIdProvider::ConstellationData::isSmallerOrEqual(
      50                 :            :   RssConstellationIdProvider::ConstellationData::IntersectionArea const &left,
      51                 :            :   RssConstellationIdProvider::ConstellationData::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         [ +  - ]:       1472 :   std::set_difference(right.begin(),
      60                 :            :                       right.end(),
      61                 :            :                       left.begin(),
      62                 :            :                       left.end(),
      63                 :            :                       std::insert_iterator<IntersectionArea>(differenceSet, differenceSet.begin()));
      64                 :       1472 :   return differenceSet.size() == expectedDifference;
      65                 :       1472 : }
      66                 :            : 
      67                 :       4145 : bool RssConstellationIdProvider::ConstellationData::updateConstellation(world::TimeIndex const time_index,
      68                 :            :                                                                         world::Constellation const &constellation)
      69                 :            : {
      70         [ +  + ]:       4145 :   if (constellation.constellation_type != mConstellationType)
      71                 :            :   {
      72                 :          6 :     return false;
      73                 :            :   }
      74                 :            : 
      75                 :       4139 :   IntersectionArea constellationEgoVehicleIntersectionArea;
      76                 :       4139 :   IntersectionArea constellationObjectIntersectionArea;
      77                 :            : 
      78         [ +  + ]:       4139 :   if ((mConstellationType == world::ConstellationType::IntersectionEgoHasPriority)
      79         [ +  + ]:       3763 :       || (mConstellationType == world::ConstellationType::IntersectionObjectHasPriority)
      80         [ +  + ]:       3582 :       || (mConstellationType == world::ConstellationType::IntersectionSamePriority))
      81                 :            :   {
      82                 :            :     // extract the intersection areas of the constellation
      83         [ +  - ]:        738 :     constellationEgoVehicleIntersectionArea = getIntersectionArea(constellation.ego_vehicle_road);
      84   [ +  -  +  + ]:        738 :     if (!isSmallerOrEqual(constellationEgoVehicleIntersectionArea, mEgoVehicleIntersectionArea))
      85                 :            :     {
      86                 :          2 :       return false;
      87                 :            :     }
      88                 :            : 
      89         [ +  - ]:        736 :     constellationObjectIntersectionArea = getIntersectionArea(constellation.intersecting_road);
      90   [ +  -  +  + ]:        736 :     if (!isSmallerOrEqual(constellationObjectIntersectionArea, mObjectIntersectionArea))
      91                 :            :     {
      92                 :          2 :       return false;
      93                 :            :     }
      94                 :            :   }
      95                 :            :   else
      96                 :            :   {
      97                 :            :     // there must be only one constellation of all other types between two objects
      98                 :            :     // therefore, the constellation is treated to be the same if the constellation type matches
      99                 :            :   }
     100                 :            : 
     101                 :       4135 :   mTimeIndex = time_index;
     102                 :       4135 :   return true;
     103                 :       4139 : }
     104                 :            : 
     105                 :       5013 : void RssConstellationIdProvider::updateTime(world::TimeIndex const &time_index)
     106                 :            : {
     107         [ +  + ]:       5013 :   if (time_index != mCurrentTime)
     108                 :            :   {
     109                 :       4211 :     mLastTime = mCurrentTime;
     110                 :       4211 :     mCurrentTime = time_index;
     111                 :            : 
     112                 :            :     // next time step, remove outdated data
     113         [ +  + ]:       8314 :     for (auto iter = mConstellationData.begin(); iter != mConstellationData.end();)
     114                 :            :     {
     115   [ +  +  +  -  :       4102 :       if ((iter->second.mTimeIndex != mLastTime) && (iter->second.mTimeIndex != mCurrentTime))
                   +  + ]
     116                 :            :       {
     117         [ +  - ]:          4 :         iter = mConstellationData.erase(iter);
     118                 :            :       }
     119                 :            :       else
     120                 :            :       {
     121                 :       4099 :         ++iter;
     122                 :            :       }
     123                 :            :     }
     124                 :            :   }
     125                 :       5012 : }
     126                 :            : 
     127                 :          0 : void RssConstellationIdProvider::dropConstellationIds(world::ObjectId const &object_id)
     128                 :            : {
     129                 :          0 :   mConstellationData.erase(object_id);
     130                 :          0 : }
     131                 :            : 
     132                 :       5013 : core::RelativeConstellationId RssConstellationIdProvider::getConstellationId(world::TimeIndex const &time_index,
     133                 :            :                                                                              world::Constellation const &constellation)
     134                 :            : {
     135         [ +  - ]:       5013 :   updateTime(time_index);
     136                 :            : 
     137         [ +  - ]:       5013 :   auto const objectDataRange = mConstellationData.equal_range(constellation.object.object_id);
     138                 :            :   auto findResult
     139         [ +  - ]:       5013 :     = std::find_if(objectDataRange.first,
     140                 :            :                    objectDataRange.second,
     141                 :       4145 :                    [&constellation, this](ConstellationDataMap::value_type &constellationData) {
     142                 :       4145 :                      return constellationData.second.updateConstellation(this->mCurrentTime, constellation);
     143                 :            :                    });
     144         [ +  + ]:       5013 :   if (findResult != objectDataRange.second)
     145                 :            :   {
     146                 :       4135 :     return findResult->second.mConstellationId;
     147                 :            :   }
     148                 :            :   auto insertResult
     149         [ +  + ]:        872 :     = mConstellationData.emplace_hint(objectDataRange.first,
     150                 :        872 :                                       constellation.object.object_id,
     151   [ +  -  +  + ]:       1765 :                                       ConstellationData(mCurrentTime, getFreeConstellationId(), constellation));
     152         [ +  - ]:        857 :   if (insertResult != mConstellationData.end())
     153                 :            :   {
     154                 :        857 :     return insertResult->second.mConstellationId;
     155                 :            :   }
     156                 :            :   // LCOV_EXCL_START: unreachable code, keep to be on the safe side
     157                 :            :   throw std::runtime_error(
     158                 :            :     "RssConstellationIdProvider::getConstellationId>> cannot add constellation to constellation map");
     159                 :            :   // LCOV_EXCL_STOP: unreachable code, keep to be on the safe side
     160                 :            : }
     161                 :            : 
     162                 :        878 : core::RelativeConstellationId RssConstellationIdProvider::getFreeConstellationId()
     163                 :            : {
     164                 :        878 :   bool constellationIdFound = false;
     165                 :            :   do // LCOV_EXCL_LINE: lcov analysis misses this line
     166                 :            :   {
     167                 :        878 :     mNextConstellationId++;
     168         [ +  - ]:        878 :     auto findResult = std::find_if(mConstellationData.begin(),
     169                 :            :                                    mConstellationData.end(),
     170                 :       5073 :                                    [this](ConstellationDataMap::value_type const &constellation) {
     171                 :       5073 :                                      return (constellation.second.mConstellationId == this->mNextConstellationId);
     172                 :            :                                    });
     173                 :        878 :     constellationIdFound = findResult == mConstellationData.end();
     174         [ -  + ]:        878 :   } while (!constellationIdFound);
     175                 :            : 
     176                 :        878 :   return mNextConstellationId;
     177                 :            : }
     178                 :            : 
     179                 :            : } // namespace structured
     180                 :            : } // namespace rss
     181                 :            : } // namespace ad