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/landmark/LandmarkOperation.hpp"
      10                 :            : #include "ad/map/access/Logging.hpp"
      11                 :            : #include "ad/map/access/Operation.hpp"
      12                 :            : #include "ad/map/access/Store.hpp"
      13                 :            : #include "ad/map/lane/LaneOperation.hpp"
      14                 :            : #include "ad/map/point/Operation.hpp"
      15                 :            : 
      16                 :            : namespace ad {
      17                 :            : namespace map {
      18                 :            : namespace landmark {
      19                 :            : 
      20                 :            : ////////////
      21                 :            : // Getters
      22                 :            : 
      23                 :         15 : point::ENUHeading getENUHeading(Landmark const &landmark)
      24                 :            : {
      25         [ +  - ]:         30 :   return point::createENUHeading(point::createECEFHeading(landmark.position, landmark.orientation));
      26                 :            : }
      27                 :            : 
      28                 :       9033 : Landmark::ConstPtr getLandmarkPtr(LandmarkId const &id)
      29                 :            : {
      30                 :       9033 :   auto landmarkPtr = access::getStore().getLandmarkPtr(id);
      31                 :            : 
      32         [ +  + ]:       9033 :   if (!bool(landmarkPtr))
      33                 :            :   {
      34         [ +  - ]:          1 :     throw std::invalid_argument("ad::map::landmark::getLandmarkPtr: LandmarkId not found in store");
      35                 :            :   }
      36                 :       9032 :   return landmarkPtr;
      37                 :            : }
      38                 :            : 
      39                 :         24 : Landmark const &getLandmark(LandmarkId const &id)
      40                 :            : {
      41                 :         24 :   return *getLandmarkPtr(id);
      42                 :            : }
      43                 :            : 
      44                 :         12 : landmark::LandmarkIdList getLandmarks()
      45                 :            : {
      46                 :         12 :   return access::getStore().getLandmarks();
      47                 :            : }
      48                 :            : 
      49                 :         10 : ENULandmark getENULandmark(LandmarkId const &id)
      50                 :            : {
      51         [ +  - ]:         20 :   auto const landmarkPtr = getLandmarkPtr(id);
      52         [ +  - ]:         10 :   ENULandmark landmark;
      53                 :         10 :   landmark.id = landmarkPtr->id;
      54                 :         10 :   landmark.type = landmarkPtr->type;
      55         [ +  - ]:         10 :   landmark.position = point::toENU(landmarkPtr->position);
      56                 :         10 :   landmark.trafficLightType = landmarkPtr->trafficLightType;
      57         [ +  - ]:         10 :   landmark.heading = getENUHeading(*landmarkPtr);
      58                 :            : 
      59                 :         20 :   return landmark;
      60                 :            : }
      61                 :            : 
      62                 :          2 : LandmarkIdList getVisibleLandmarks(lane::LaneId const &laneId)
      63                 :            : {
      64                 :          4 :   LandmarkIdList landmarks;
      65         [ +  + ]:          3 :   auto const lanePtr = lane::getLanePtr(laneId);
      66         [ -  + ]:          1 :   if (!bool(lanePtr))
      67                 :            :   {
      68         [ #  # ]:          0 :     throw std::invalid_argument("ad::map::landmark::getVisibleLandmarks: laneId not found in store");
      69                 :            :   }
      70         [ +  - ]:          2 :   return lanePtr->visibleLandmarks;
      71                 :            : }
      72                 :            : 
      73                 :          2 : LandmarkIdList getVisibleLandmarks(LandmarkType const &landmarkType, lane::LaneId const &laneId)
      74                 :            : {
      75                 :          2 :   LandmarkIdList landmarks;
      76         [ +  + ]:          3 :   auto const visibleLandmarkIds = getVisibleLandmarks(laneId);
      77         [ +  + ]:         26 :   for (const auto &landmarkId : visibleLandmarkIds)
      78                 :            :   {
      79         [ +  - ]:         50 :     auto const landmarkPtr = getLandmarkPtr(landmarkId);
      80   [ +  -  +  +  :         25 :     if (landmarkPtr && landmarkPtr->type == landmarkType)
                   +  + ]
      81                 :            :     {
      82         [ +  - ]:         10 :       landmarks.push_back(landmarkPtr->id);
      83                 :            :     }
      84                 :            :   }
      85                 :            : 
      86                 :          2 :   return landmarks;
      87                 :            : }
      88                 :            : 
      89                 :         44 : LandmarkId uniqueLandmarkId(point::GeoPoint const &geoPoint)
      90                 :            : {
      91                 :         46 :   LandmarkIdList landmarksIds;
      92                 :         44 :   LandmarkId id;
      93                 :            : 
      94   [ +  -  +  - ]:         44 :   landmarksIds = access::getStore().getLandmarks();
      95         [ +  + ]:         44 :   if (landmarksIds.size() == 0)
      96         [ +  - ]:          1 :     throw std::invalid_argument("There is no landmarks in the map.");
      97                 :            : 
      98         [ +  - ]:         43 :   point::ECEFPoint ecefDstPoint = toECEF(geoPoint);
      99                 :         43 :   physics::Distance minDistance = std::numeric_limits<physics::Distance>::max();
     100         [ +  + ]:       9015 :   for (auto const &landmarkId : landmarksIds)
     101                 :            :   {
     102         [ +  - ]:      17944 :     auto const landmarkPtr = getLandmarkPtr(landmarkId);
     103         [ +  - ]:       8972 :     point::GeoPoint geoSrcPoint = toGeo(landmarkPtr->position);
     104                 :       8972 :     geoSrcPoint.altitude = geoPoint.altitude;
     105         [ +  - ]:       8972 :     point::ECEFPoint ecefSrcPoint = toECEF(geoSrcPoint);
     106         [ +  - ]:       8972 :     auto distance = point::distance(ecefSrcPoint, ecefDstPoint);
     107                 :            : 
     108         [ +  + ]:       8972 :     if (landmarkPtr->boundingBox.isValid)
     109                 :            :     {
     110   [ +  -  +  +  :          2 :       if (distance <= landmarkPtr->boundingBox.length && distance < minDistance)
          +  -  +  -  +  
                      + ]
     111                 :            :       {
     112                 :          1 :         minDistance = distance;
     113                 :          1 :         id = landmarkPtr->id;
     114                 :            :       }
     115                 :            :     }
     116   [ +  -  +  +  :       8970 :     else if (distance <= physics::Distance(10) && distance < minDistance)
          +  -  +  +  +  
                      + ]
     117                 :            :     {
     118                 :         65 :       minDistance = distance;
     119                 :         65 :       id = landmarkPtr->id;
     120                 :            :     }
     121                 :            :   }
     122                 :            : 
     123         [ +  + ]:         43 :   if ((uint64_t)id == std::numeric_limits<uint64_t>::quiet_NaN())
     124         [ +  - ]:          1 :     throw std::invalid_argument("Cannot find any lardmark given geo point.");
     125                 :            : 
     126                 :         84 :   return id;
     127                 :            : }
     128                 :            : 
     129                 :            : } // namespace landmark
     130                 :            : } // namespace map
     131                 :            : } // namespace ad