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                 :            :  * @file
      10                 :            :  */
      11                 :            : 
      12                 :            : #include "RssObjectPositionExtractor.hpp"
      13                 :            : #include <algorithm>
      14                 :            : #include "ad/physics/Operation.hpp"
      15                 :            : 
      16                 :            : namespace ad {
      17                 :            : namespace rss {
      18                 :            : namespace world {
      19                 :            : 
      20                 :            : using physics::Distance;
      21                 :            : using physics::MetricRange;
      22                 :            : 
      23                 :       9382 : RssObjectPositionExtractor::RssObjectPositionExtractor(OccupiedRegionVector const &occupiedRegions)
      24         [ +  - ]:       9382 :   : mOccupiedRegions(occupiedRegions)
      25                 :            : {
      26                 :       9354 :   mObjectDimensions.intersectionPosition.maximum = Distance(0.);
      27                 :       9354 : }
      28                 :            : 
      29                 :      35150 : bool RssObjectPositionExtractor::newRoadSegment(Distance const &longitudinalStartMin,
      30                 :            :                                                 Distance const &longitudinalStartMax)
      31                 :            : {
      32                 :      35150 :   bool result = true;
      33                 :            : 
      34                 :      35150 :   mCurrentLongitudinalMax = longitudinalStartMax;
      35                 :      35150 :   mCurrentLongitudinalMin = longitudinalStartMin;
      36                 :            : 
      37                 :      35150 :   return result;
      38                 :            : }
      39                 :            : 
      40                 :     254951 : bool RssObjectPositionExtractor::newLaneSegment(MetricRange lateralDistance, LaneSegment const &laneSegment)
      41                 :            : {
      42                 :     254951 :   bool result = true;
      43                 :            : 
      44                 :     254951 :   bool noAdditionalObjects = false;
      45   [ +  +  +  +  :     388247 :   while (!noAdditionalObjects && !mOccupiedRegions.empty())
                   +  + ]
      46                 :            :   {
      47                 :            :     auto const objectSegment
      48                 :     133298 :       = std::find_if(mOccupiedRegions.begin(), mOccupiedRegions.end(), [laneSegment](OccupiedRegion const &region) {
      49                 :     133298 :           return region.segmentId == laneSegment.id;
      50         [ +  - ]:     133298 :         });
      51         [ +  + ]:     133298 :     if (objectSegment == mOccupiedRegions.end())
      52                 :            :     {
      53                 :     123995 :       noAdditionalObjects = true;
      54                 :            :     }
      55                 :            :     else
      56                 :            :     {
      57   [ +  -  +  - ]:       9303 :       Distance latMinPosition = lateralDistance.minimum + (objectSegment->latRange.minimum * laneSegment.width.minimum);
      58   [ +  -  +  - ]:       9303 :       Distance latMaxPosition = lateralDistance.maximum + (objectSegment->latRange.maximum * laneSegment.width.maximum);
      59                 :            : 
      60                 :            :       Distance lonMinPosition
      61   [ +  +  +  - ]:       9303 :         = mCurrentLongitudinalMin + (objectSegment->lonRange.minimum * laneSegment.length.minimum);
      62                 :            :       Distance lonMaxPosition
      63   [ +  -  +  - ]:       9301 :         = mCurrentLongitudinalMax + (objectSegment->lonRange.maximum * laneSegment.length.maximum);
      64                 :            : 
      65                 :            :       mObjectDimensions.lateralDimensions.minimum
      66         [ +  - ]:       9301 :         = std::min(mObjectDimensions.lateralDimensions.minimum, latMinPosition);
      67                 :            :       mObjectDimensions.lateralDimensions.maximum
      68         [ +  - ]:       9301 :         = std::max(mObjectDimensions.lateralDimensions.maximum, latMaxPosition);
      69                 :            : 
      70                 :            :       mObjectDimensions.longitudinalDimensions.minimum
      71         [ +  - ]:       9301 :         = std::min(mObjectDimensions.longitudinalDimensions.minimum, lonMinPosition);
      72                 :            :       mObjectDimensions.longitudinalDimensions.maximum
      73         [ +  - ]:       9301 :         = std::max(mObjectDimensions.longitudinalDimensions.maximum, lonMaxPosition);
      74                 :            : 
      75         [ +  + ]:       9301 :       if (laneSegment.drivingDirection == LaneDrivingDirection::Positive)
      76                 :            :       {
      77                 :       7897 :         mObjectDimensions.onPositiveLane = true;
      78                 :            :       }
      79                 :            : 
      80         [ +  + ]:       9301 :       if (laneSegment.drivingDirection == LaneDrivingDirection::Negative)
      81                 :            :       {
      82                 :        652 :         mObjectDimensions.onNegativeLane = true;
      83                 :            :       }
      84                 :            : 
      85         [ +  - ]:       9301 :       mOccupiedRegions.erase(objectSegment);
      86                 :            :     }
      87                 :            :   }
      88                 :            : 
      89   [ +  +  +  -  :     254949 :   if (!mIntersectionReached && !mIntersectionEndReached && (laneSegment.type == LaneSegmentType::Intersection))
                   +  + ]
      90                 :            :   {
      91                 :       1625 :     mObjectDimensions.intersectionPosition.minimum = mCurrentLongitudinalMin;
      92                 :       1625 :     mObjectDimensions.intersectionPosition.maximum = std::max(
      93         [ +  - ]:       1625 :       Distance(mCurrentLongitudinalMax + laneSegment.length.maximum), mObjectDimensions.intersectionPosition.maximum);
      94                 :       1625 :     mIntersectionReached = true;
      95                 :       1625 :     mIntersectionEndReached = false;
      96                 :            :   }
      97   [ +  +  +  -  :     254949 :   if (mIntersectionReached && !mIntersectionEndReached && (laneSegment.type == LaneSegmentType::Intersection))
                   +  + ]
      98                 :            :   {
      99                 :       1625 :     mObjectDimensions.intersectionPosition.maximum = std::max(
     100         [ +  - ]:       1625 :       Distance(mCurrentLongitudinalMax + laneSegment.length.maximum), mObjectDimensions.intersectionPosition.maximum);
     101                 :            :   }
     102   [ +  +  +  -  :     254949 :   if (mIntersectionReached && !mIntersectionEndReached && (laneSegment.type != LaneSegmentType::Intersection))
                   +  + ]
     103                 :            :   {
     104                 :        182 :     mObjectDimensions.intersectionPosition.maximum = mCurrentLongitudinalMax;
     105                 :        182 :     mIntersectionEndReached = true;
     106                 :            :   }
     107                 :            : 
     108                 :     254949 :   return result;
     109                 :            : }
     110                 :            : 
     111                 :       9289 : bool RssObjectPositionExtractor::getObjectDimensions(ObjectDimensions &objectDimensions)
     112                 :            : {
     113                 :            :   // if mOccupiedRegions is not empty, something went wrong
     114         [ -  + ]:       9289 :   if (!mOccupiedRegions.empty())
     115                 :            :   {
     116                 :          0 :     return false;
     117                 :            :   }
     118                 :            : 
     119                 :            :   // if position.max < position.min, something went wrong
     120         [ -  + ]:       9289 :   if (mObjectDimensions.longitudinalDimensions.maximum < mObjectDimensions.longitudinalDimensions.minimum)
     121                 :            :   {
     122                 :            :     return false; // LCOV_EXCL_LINE: unreachable code, keep to be on the safe side
     123                 :            :   }
     124         [ -  + ]:       9289 :   if (mObjectDimensions.lateralDimensions.maximum < mObjectDimensions.lateralDimensions.minimum)
     125                 :            :   {
     126                 :            :     return false; // LCOV_EXCL_LINE: unreachable code, keep to be on the safe side
     127                 :            :   }
     128                 :            : 
     129                 :       9289 :   objectDimensions = mObjectDimensions;
     130                 :            : 
     131                 :       9289 :   return true;
     132                 :            : }
     133                 :            : 
     134                 :            : } // namespace world
     135                 :            : } // namespace rss
     136                 :            : } // namespace ad