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 "ad/rss/structured/RssObjectPositionExtractor.hpp"
      13                 :            : #include <ad/physics/Operation.hpp>
      14                 :            : #include <algorithm>
      15                 :            : 
      16                 :            : namespace ad {
      17                 :            : namespace rss {
      18                 :            : namespace structured {
      19                 :            : 
      20                 :            : using physics::Distance;
      21                 :            : using physics::MetricRange;
      22                 :            : 
      23                 :       9386 : RssObjectPositionExtractor::RssObjectPositionExtractor(world::OccupiedRegionVector const &occupied_regions)
      24   [ +  -  +  - ]:       9386 :   : mOccupiedRegions(occupied_regions)
      25                 :            : {
      26                 :       9358 :   mObjectDimensions.intersectionPosition.maximum = Distance(0.);
      27                 :       9358 :   mCurrentLongitudinalRoadSegmentStart.minimum = Distance(0.);
      28                 :       9358 :   mCurrentLongitudinalRoadSegmentStart.maximum = Distance(0.);
      29                 :       9358 : }
      30                 :            : 
      31                 :      35138 : bool RssObjectPositionExtractor::newRoadSegment(physics::MetricRange const &longitudinalStart,
      32                 :            :                                                 world::RoadSegment const &roadSegment)
      33                 :            : {
      34                 :      35138 :   mCurrentLongitudinalRoadSegmentStart = longitudinalStart;
      35                 :      35138 :   mRoadSegmentMinLengthAfterIntersectingArea = roadSegment.minimum_length_after_intersecting_area;
      36                 :            : 
      37      [ +  +  - ]:      35138 :   switch (mIntersectionState)
      38                 :            :   {
      39                 :      34956 :     case IntersectionState::BeforeIntersection:
      40         [ +  + ]:      34956 :       if (roadSegment.type == world::RoadSegmentType::Intersection)
      41                 :            :       {
      42                 :            :         // fix minimum
      43                 :            :         mObjectDimensions.intersectionPosition.minimum
      44                 :       1631 :           = mCurrentLongitudinalRoadSegmentStart.minimum + roadSegment.minimum_length_before_intersecting_area;
      45                 :            :         // and ensure max is larger than min and larger than max of current segment start
      46                 :       4893 :         mObjectDimensions.intersectionPosition.maximum = std::max(
      47                 :       1631 :           mObjectDimensions.intersectionPosition.minimum,
      48                 :       1631 :           std::max(mCurrentLongitudinalRoadSegmentStart.maximum, mObjectDimensions.intersectionPosition.maximum));
      49                 :       1631 :         mIntersectionState = IntersectionState::WithinIntersection;
      50                 :            :       }
      51                 :      34956 :       break;
      52                 :        182 :     case IntersectionState::WithinIntersection:
      53         [ +  - ]:        182 :       if (roadSegment.type != world::RoadSegmentType::Intersection)
      54                 :            :       {
      55                 :        182 :         mIntersectionState = IntersectionState::AfterIntersection;
      56                 :            :       }
      57                 :        182 :       break;
      58                 :          0 :     default:
      59                 :          0 :       break;
      60                 :            :   }
      61                 :            : 
      62                 :      35138 :   return true;
      63                 :            : }
      64                 :            : 
      65                 :     254547 : bool RssObjectPositionExtractor::newLaneSegment(MetricRange lateral_distance, world::LaneSegment const &laneSegment)
      66                 :            : {
      67                 :     254547 :   bool noAdditionalObjects = false;
      68   [ +  +  +  +  :     387628 :   while (!noAdditionalObjects && !mOccupiedRegions.empty())
                   +  + ]
      69                 :            :   {
      70         [ +  - ]:     133084 :     auto const objectSegment = std::find_if(
      71                 :     133083 :       mOccupiedRegions.begin(), mOccupiedRegions.end(), [laneSegment](world::OccupiedRegion const &region) {
      72                 :     133083 :         return region.segment_id == laneSegment.id;
      73                 :            :       });
      74         [ +  + ]:     133084 :     if (objectSegment == mOccupiedRegions.end())
      75                 :            :     {
      76                 :     123770 :       noAdditionalObjects = true;
      77                 :            :     }
      78                 :            :     else
      79                 :            :     {
      80                 :            :       Distance latMinPosition
      81   [ +  -  +  - ]:       9314 :         = lateral_distance.minimum + (objectSegment->lat_range.minimum * laneSegment.width.minimum);
      82                 :            :       Distance latMaxPosition
      83   [ +  -  +  - ]:       9315 :         = lateral_distance.maximum + (objectSegment->lat_range.maximum * laneSegment.width.maximum);
      84                 :            : 
      85                 :            :       Distance lonMinPosition = mCurrentLongitudinalRoadSegmentStart.minimum
      86   [ +  +  +  - ]:       9313 :         + (objectSegment->lon_range.minimum * laneSegment.length.minimum);
      87                 :            :       Distance lonMaxPosition = mCurrentLongitudinalRoadSegmentStart.maximum
      88   [ +  -  +  - ]:       9313 :         + (objectSegment->lon_range.maximum * laneSegment.length.maximum);
      89                 :            : 
      90                 :            :       mObjectDimensions.lateralDimensions.minimum
      91         [ +  - ]:       9313 :         = std::min(mObjectDimensions.lateralDimensions.minimum, latMinPosition);
      92                 :            :       mObjectDimensions.lateralDimensions.maximum
      93         [ +  - ]:       9313 :         = std::max(mObjectDimensions.lateralDimensions.maximum, latMaxPosition);
      94                 :            : 
      95                 :            :       mObjectDimensions.longitudinalDimensions.minimum
      96         [ +  - ]:       9313 :         = std::min(mObjectDimensions.longitudinalDimensions.minimum, lonMinPosition);
      97                 :            :       mObjectDimensions.longitudinalDimensions.maximum
      98         [ +  - ]:       9312 :         = std::max(mObjectDimensions.longitudinalDimensions.maximum, lonMaxPosition);
      99                 :            : 
     100         [ +  + ]:       9312 :       if (laneSegment.driving_direction == world::LaneDrivingDirection::Positive)
     101                 :            :       {
     102                 :       7920 :         mObjectDimensions.onPositiveLane = true;
     103                 :            :       }
     104                 :            : 
     105         [ +  + ]:       9312 :       if (laneSegment.driving_direction == world::LaneDrivingDirection::Negative)
     106                 :            :       {
     107                 :        646 :         mObjectDimensions.onNegativeLane = true;
     108                 :            :       }
     109                 :            : 
     110         [ +  - ]:       9312 :       mOccupiedRegions.erase(objectSegment);
     111                 :            :     }
     112                 :            :   }
     113                 :            : 
     114         [ +  + ]:     254544 :   if (mIntersectionState == IntersectionState::WithinIntersection)
     115                 :            :   {
     116                 :            :     // only parts of the lane segment might be relevant
     117                 :            :     auto laneSegmentMax
     118   [ +  -  +  - ]:       1631 :       = std::max(physics::Distance(0.), laneSegment.length.maximum - mRoadSegmentMinLengthAfterIntersectingArea);
     119         [ +  - ]:       1631 :     mObjectDimensions.intersectionPosition.maximum = std::max(
     120         [ +  - ]:       3262 :       mCurrentLongitudinalRoadSegmentStart.maximum + laneSegmentMax, mObjectDimensions.intersectionPosition.maximum);
     121                 :            :   }
     122                 :            : 
     123                 :     254544 :   return true;
     124                 :            : }
     125                 :            : 
     126                 :       9292 : bool RssObjectPositionExtractor::getObjectDimensions(ObjectDimensions &objectDimensions)
     127                 :            : {
     128                 :            :   // if mOccupiedRegions is not empty, something went wrong
     129         [ -  + ]:       9292 :   if (!mOccupiedRegions.empty())
     130                 :            :   {
     131                 :          0 :     return false;
     132                 :            :   }
     133                 :            : 
     134                 :            :   // if position.max < position.min, something went wrong
     135         [ -  + ]:       9292 :   if (mObjectDimensions.longitudinalDimensions.maximum < mObjectDimensions.longitudinalDimensions.minimum)
     136                 :            :   {
     137                 :            :     return false; // LCOV_EXCL_LINE: unreachable code, keep to be on the safe side
     138                 :            :   }
     139         [ -  + ]:       9293 :   if (mObjectDimensions.lateralDimensions.maximum < mObjectDimensions.lateralDimensions.minimum)
     140                 :            :   {
     141                 :            :     return false; // LCOV_EXCL_LINE: unreachable code, keep to be on the safe side
     142                 :            :   }
     143                 :            : 
     144                 :       9293 :   objectDimensions = mObjectDimensions;
     145                 :            : 
     146                 :       9293 :   return true;
     147                 :            : }
     148                 :            : 
     149                 :            : } // namespace structured
     150                 :            : } // namespace rss
     151                 :            : } // namespace ad