Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2018-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: MIT
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "ad/map/access/Store.hpp"
      10                 :            : #include "ad/map/access/Logging.hpp"
      11                 :            : 
      12                 :            : #include <algorithm>
      13                 :            : #include <cstring>
      14                 :            : #include <string>
      15                 :            : #include "ad/map/lane/LaneOperation.hpp"
      16                 :            : #include "ad/map/point/BoundingSphereOperation.hpp"
      17                 :            : 
      18                 :            : namespace ad {
      19                 :            : namespace map {
      20                 :            : namespace access {
      21                 :            : 
      22                 :        483 : Store::Store()
      23                 :            : {
      24                 :        483 :   use_magic_ = false;
      25                 :        483 :   use_embedded_geometry_ = true;
      26                 :        483 :   use_geometry_store_ = false;
      27                 :        483 : }
      28                 :            : 
      29                 :        516 : Store::~Store()
      30                 :            : {
      31                 :        516 : }
      32                 :            : 
      33                 :          2 : bool Store::empty() const
      34                 :            : {
      35   [ +  +  +  - ]:          2 :   return lane_map_.empty() && landmark_map_.empty();
      36                 :            : }
      37                 :            : 
      38                 :            : ////////////////////
      39                 :            : // Access Operations
      40                 :        748 : MapMetaData const &Store::getMetaData() const
      41                 :            : {
      42                 :        748 :   return meta_data_;
      43                 :            : }
      44                 :            : 
      45                 :          2 : PartitionIdList Store::getPartitions() const
      46                 :            : {
      47                 :          2 :   PartitionIdList part_ids;
      48   [ +  +  +  - ]:          4 :   for (auto partition_id_and_ids : part_lane_map_)
      49                 :            :   {
      50         [ +  - ]:          2 :     part_ids.push_back(partition_id_and_ids.first);
      51                 :            :   }
      52   [ +  +  +  - ]:          4 :   for (auto partition_id_and_ids : part_landmark_map_)
      53                 :            :   {
      54   [ +  -  -  + ]:          2 :     if (std::find(part_ids.begin(), part_ids.end(), partition_id_and_ids.first) == part_ids.end())
      55                 :            :     {
      56         [ #  # ]:          0 :       part_ids.push_back(partition_id_and_ids.first);
      57                 :            :     }
      58                 :            :   }
      59                 :          2 :   return part_ids;
      60                 :            : }
      61                 :            : 
      62                 :     948061 : lane::Lane::ConstPtr Store::getLanePtr(lane::LaneId const &id) const
      63                 :            : {
      64                 :     948061 :   lane::Lane::ConstPtr lane_ptr;
      65         [ +  - ]:     948061 :   auto id_and_lane = lane_map_.find(id);
      66         [ +  + ]:     948061 :   if (id_and_lane != lane_map_.end())
      67                 :            :   {
      68                 :     948056 :     lane_ptr = id_and_lane->second;
      69                 :            :   }
      70                 :            :   else
      71                 :            :   {
      72   [ +  -  +  - ]:         10 :     access::getLogger()->warn("Lane not in the Store. ID: {}", id);
      73                 :            :   }
      74                 :    1896120 :   return lane_ptr;
      75                 :            : }
      76                 :            : 
      77                 :        902 : lane::LaneIdList Store::getLanes() const
      78                 :            : {
      79                 :        902 :   lane::LaneIdList ids;
      80   [ +  +  +  - ]:       1804 :   for (auto partition_id_and_ids : part_lane_map_)
      81                 :            :   {
      82                 :        902 :     const lane::LaneIdList &tids = partition_id_and_ids.second;
      83         [ +  - ]:        902 :     ids.insert(ids.end(), tids.begin(), tids.end());
      84                 :            :   }
      85                 :        902 :   return ids;
      86                 :            : }
      87                 :            : 
      88                 :          1 : lane::LaneIdList Store::getLanes(PartitionId const &partition_id) const
      89                 :            : {
      90         [ +  - ]:          1 :   auto partition_and_ids = part_lane_map_.find(partition_id);
      91         [ +  - ]:          1 :   if (partition_and_ids != part_lane_map_.end())
      92                 :            :   {
      93         [ +  - ]:          1 :     return partition_and_ids->second;
      94                 :            :   }
      95                 :          0 :   return lane::LaneIdList();
      96                 :            : }
      97                 :            : 
      98                 :          2 : lane::LaneIdList Store::getLanes(std::string const &type_filter, bool is_hov) const
      99                 :            : {
     100                 :          2 :   lane::LaneIdList ids;
     101         [ +  + ]:          4 :   for (auto id_and_lane : lane_map_)
     102                 :            :   {
     103                 :          4 :     lane::Lane::Ptr lane = id_and_lane.second;
     104   [ +  -  +  -  :          2 :     if (lane && lane::satisfiesFilter(*lane, type_filter, is_hov))
             +  +  +  + ]
     105                 :            :     {
     106         [ +  - ]:          1 :       ids.push_back(id_and_lane.first);
     107                 :            :     }
     108                 :            :   }
     109                 :          2 :   return ids;
     110                 :            : }
     111                 :            : 
     112                 :          2 : lane::LaneIdList Store::getLanes(PartitionId partition_id, std::string const &type_filter, bool is_hov) const
     113                 :            : {
     114                 :          2 :   lane::LaneIdList ids;
     115         [ +  - ]:          2 :   auto partition_and_ids = part_lane_map_.find(partition_id);
     116         [ +  - ]:          2 :   if (partition_and_ids != part_lane_map_.end())
     117                 :            :   {
     118         [ +  + ]:          4 :     for (auto lane_id : partition_and_ids->second)
     119                 :            :     {
     120         [ +  - ]:          2 :       auto findResult = lane_map_.find(lane_id);
     121         [ +  - ]:          2 :       if (findResult != lane_map_.end())
     122                 :            :       {
     123   [ +  -  +  + ]:          2 :         if (lane::satisfiesFilter(*findResult->second, type_filter, is_hov))
     124                 :            :         {
     125         [ +  - ]:          1 :           ids.push_back(lane_id);
     126                 :            :         }
     127                 :            :       }
     128                 :            :     }
     129                 :            :   }
     130                 :          4 :   return ids;
     131                 :            : }
     132                 :            : 
     133                 :          1 : landmark::LandmarkIdList Store::getLandmarks(PartitionId partition_id) const
     134                 :            : {
     135         [ +  - ]:          1 :   auto partition_and_landmark = part_landmark_map_.find(partition_id);
     136         [ +  - ]:          1 :   if (partition_and_landmark != part_landmark_map_.end())
     137                 :            :   {
     138         [ +  - ]:          1 :     return partition_and_landmark->second;
     139                 :            :   }
     140                 :          0 :   return landmark::LandmarkIdList();
     141                 :            : }
     142                 :            : 
     143                 :       9035 : landmark::Landmark::ConstPtr Store::getLandmarkPtr(landmark::LandmarkId id) const
     144                 :            : {
     145                 :       9035 :   landmark::Landmark::ConstPtr landmark_ptr;
     146         [ +  - ]:       9035 :   auto id_and_landmark = landmark_map_.find(id);
     147         [ +  + ]:       9035 :   if (id_and_landmark != landmark_map_.end())
     148                 :            :   {
     149                 :       9034 :     landmark_ptr = id_and_landmark->second;
     150                 :            :   }
     151                 :            :   else
     152                 :            :   {
     153   [ +  -  +  - ]:          2 :     access::getLogger()->warn("Landmark is not in the Store: {}", id);
     154                 :            :   }
     155                 :      18070 :   return landmark_ptr;
     156                 :            : }
     157                 :            : 
     158                 :         59 : landmark::LandmarkIdList Store::getLandmarks() const
     159                 :            : {
     160                 :         59 :   landmark::LandmarkIdList ids;
     161         [ +  + ]:       9120 :   for (auto id_and_landmark : landmark_map_)
     162                 :            :   {
     163         [ +  - ]:       9061 :     ids.push_back(id_and_landmark.first);
     164                 :            :   }
     165                 :         59 :   return ids;
     166                 :            : }
     167                 :            : 
     168                 :          1 : void Store::removePartition(PartitionId partition_id)
     169                 :            : {
     170                 :            :   {
     171                 :            :     // remove lanes
     172         [ +  - ]:          1 :     auto findLanesResult = part_lane_map_.find(partition_id);
     173         [ +  - ]:          1 :     if (findLanesResult != part_lane_map_.end())
     174                 :            :     {
     175         [ +  + ]:          4 :       for (auto lane_id : findLanesResult->second)
     176                 :            :       {
     177         [ +  - ]:          3 :         lane_map_.erase(lane_id);
     178                 :            :       }
     179         [ +  - ]:          1 :       part_lane_map_.erase(findLanesResult);
     180                 :            :     }
     181                 :            :   }
     182                 :            : 
     183                 :            :   {
     184                 :            :     // remove landmarks
     185         [ +  - ]:          1 :     auto findLandmarkResult = part_landmark_map_.find(partition_id);
     186         [ +  - ]:          1 :     if (findLandmarkResult != part_landmark_map_.end())
     187                 :            :     {
     188         [ +  + ]:          2 :       for (auto landmark_id : findLandmarkResult->second)
     189                 :            :       {
     190         [ +  - ]:          1 :         landmark_map_.erase(landmark_id);
     191                 :            :       }
     192         [ +  - ]:          1 :       part_landmark_map_.erase(findLandmarkResult);
     193                 :            :     }
     194                 :            :   }
     195                 :          1 : }
     196                 :            : 
     197                 :            : /////////////
     198                 :            : // Statistics
     199                 :            : 
     200                 :          1 : physics::Distance Store::getCumulativeLaneLength() const
     201                 :            : {
     202                 :          1 :   physics::Distance total(0);
     203         [ +  - ]:          1 :   lane::LaneIdList ids = getLanes();
     204         [ +  + ]:          2 :   for (auto id : ids)
     205                 :            :   {
     206         [ +  - ]:          2 :     lane::Lane::ConstPtr lane = getLanePtr(id);
     207         [ +  - ]:          1 :     if (lane)
     208                 :            :     {
     209         [ +  - ]:          1 :       total += lane->length;
     210                 :            :     }
     211                 :            :   }
     212                 :          2 :   return total;
     213                 :            : }
     214                 :            : 
     215                 :         25 : point::BoundingSphere Store::getBoundingSphere() const
     216                 :            : {
     217                 :         25 :   point::BoundingSphere boundingSphere;
     218         [ -  + ]:         25 :   if (lane_map_.empty())
     219                 :            :   {
     220                 :          0 :     return boundingSphere;
     221                 :            :   }
     222                 :         25 :   boundingSphere = lane_map_.begin()->second->boundingSphere;
     223         [ +  + ]:         66 :   for (auto id_and_lane : lane_map_)
     224                 :            :   {
     225         [ +  - ]:         41 :     boundingSphere = boundingSphere + id_and_lane.second->boundingSphere;
     226                 :            :   }
     227                 :         25 :   return boundingSphere;
     228                 :            : }
     229                 :            : 
     230                 :            : } // namespace access
     231                 :            : } // namespace map
     232                 :            : } // namespace ad