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/RssLaneCoordinateSystemConversion.hpp"
      13                 :            : #include <algorithm>
      14                 :            : #include <limits>
      15                 :            : #include <vector>
      16                 :            : #include "ad/rss/core/Logging.hpp"
      17                 :            : 
      18                 :            : namespace ad {
      19                 :            : namespace rss {
      20                 :            : namespace structured {
      21                 :            : 
      22                 :            : using physics::Distance;
      23                 :            : using physics::MetricRange;
      24                 :            : 
      25                 :       5574 : bool calculateLateralDimensions(world::RoadArea const &roadArea, std::vector<MetricRange> &lateralRanges)
      26                 :            : {
      27                 :       5574 :   bool result = true;
      28         [ +  + ]:       5574 :   if (roadArea.empty())
      29                 :            :   {
      30         [ +  - ]:          2 :     core::getLogger()->error("RssLaneCoordinateSystemConversion::calculateLateralDimensions>> road area empty");
      31                 :          2 :     return false;
      32                 :            :   }
      33                 :            : 
      34                 :            :   try
      35                 :            :   {
      36         [ +  - ]:       5572 :     MetricRange currentLateralPosition;
      37                 :            : 
      38                 :       5572 :     currentLateralPosition.maximum = Distance(0.);
      39                 :       5572 :     currentLateralPosition.minimum = Distance(0.);
      40                 :            : 
      41                 :       5572 :     std::size_t currentLateralIndex = 0u;
      42                 :       5572 :     bool roadSegmentFound = true;
      43         [ +  + ]:      25949 :     while (roadSegmentFound)
      44                 :            :     {
      45                 :      20422 :       roadSegmentFound = false;
      46         [ +  + ]:      20422 :       lateralRanges.push_back(currentLateralPosition);
      47                 :            : 
      48                 :      20376 :       Distance lateralDistanceMax = Distance(0.);
      49                 :      20376 :       Distance lateralDistanceMin = std::numeric_limits<Distance>::max();
      50         [ +  + ]:     176044 :       for (const auto &roadSegment : roadArea)
      51                 :            :       {
      52         [ -  + ]:     155670 :         if (roadSegment.lane_segments.empty())
      53                 :            :         {
      54   [ #  #  #  # ]:          0 :           core::getLogger()->error(
      55                 :            :             "RssLaneCoordinateSystemConversion::calculateLateralDimensions>> road segment empty");
      56                 :          0 :           return false;
      57                 :            :         }
      58         [ +  + ]:     155670 :         if (roadSegment.lane_segments.size() > currentLateralIndex)
      59                 :            :         {
      60                 :     130313 :           roadSegmentFound = true;
      61                 :            :           lateralDistanceMax
      62         [ +  - ]:     130313 :             = std::max(lateralDistanceMax, roadSegment.lane_segments[currentLateralIndex].width.maximum);
      63                 :            :           lateralDistanceMin
      64         [ +  - ]:     130312 :             = std::min(lateralDistanceMin, roadSegment.lane_segments[currentLateralIndex].width.minimum);
      65                 :            :         }
      66                 :            :       }
      67                 :            : 
      68         [ +  + ]:      20377 :       if (roadSegmentFound)
      69                 :            :       {
      70         [ +  - ]:      14850 :         currentLateralPosition.maximum += lateralDistanceMax;
      71         [ +  - ]:      14850 :         currentLateralPosition.minimum += lateralDistanceMin;
      72                 :      14850 :         currentLateralIndex++;
      73                 :            :       }
      74                 :            :     }
      75                 :            :   }
      76                 :         45 :   catch (...)
      77                 :            :   {
      78                 :         45 :     return false;
      79                 :         45 :   }
      80                 :            : 
      81                 :       5527 :   return result;
      82                 :            : }
      83                 :            : 
      84                 :            : /**
      85                 :            :  * The RoadArea describes the relation between object and ego_vehicle.
      86                 :            :  * The RoadArea can be regarded as as matrix.
      87                 :            :  * Each cell of the matrix is a laneSegment.
      88                 :            :  * The rows of a cell are combined to a roadSegment.
      89                 :            :  *
      90                 :            :  * The objects are defined by occupied_regions. These are the cells that are overlapped by the objects (marked with x
      91                 :            :  * and
      92                 :            :  * o in the following sketch).
      93                 :            :  *
      94                 :            :  *                 ___________________________
      95                 :            :  *                |         |       _|_       |
      96                 :            :  *                |         |      |o|o|      |
      97                 :            :  *                ----------------------------
      98                 :            :  *                |        _|_     |o|o|      |
      99                 :            :  *                |       |x|x|     -|-       |
     100                 :            :  *                 ---------------------------
     101                 :            :  *                |       |x|x|      |        |
     102                 :            :  *                |        -|-       |        |   roadSegment
     103                 :            :  *                |         |        |        |
     104                 :            :  *               0 ----------------------------
     105                 :            :  *                 0
     106                 :            :  *
     107                 :            :  *
     108                 :            :  *  Each laneSegment has a minimum and maximum length and width.
     109                 :            :  *  In order to determine the dimension of the objects it is required to calculate the minimum and maximum
     110                 :            :  *  length and width towards the object borders.
     111                 :            :  *  For the longitudinal case:
     112                 :            :  *    for each roadSegment the maximum and minimum length are extracted.
     113                 :            :  *    The starting minimum and maximum length of a roadSegment is the sum of the minimum or maximum length of all the
     114                 :            :  * previous road_segments
     115                 :            :  *  This happens on the fly in calculateObjectDimensions.
     116                 :            :  *
     117                 :            :  *  For lateral case (calculateLateralDimensions):
     118                 :            :  *   for each column of the matrix minimum and maximum width needs to be determined
     119                 :            :  *   For the first column this is the minimum and maximum of all the first laneSegments within every roadSegment.
     120                 :            :  *   For the next column this is the minimum and maximum of all the second laneSegmetns within every roadSegment plus
     121                 :            :  * the minimum / maximum value of the first column
     122                 :            :  *   and so on...
     123                 :            :  *
     124                 :            :  *  These values needs to be determined in a separate processing step (calculateLateralDimensions)
     125                 :            :  *
     126                 :            :  *  The minimum and maximum distances to the begin of each laneSegment are then passed together with the laneSegement to
     127                 :            :  * the RssObjectPostionExtractor.
     128                 :            :  *  The RssObjectPostionExtractor checks whether the laneSegment is occupied by the object and if so minimum and maximum
     129                 :            :  * object dimensions
     130                 :            :  *  of the object area in the laneSegment are calculated and combined to a global dimension of the object.
     131                 :            :  *
     132                 :            :  */
     133                 :            : 
     134                 :       5574 : bool calculateObjectDimensions(std::vector<world::Object> const &objects,
     135                 :            :                                world::RoadArea const &roadArea,
     136                 :            :                                std::vector<ObjectDimensions> &objectDimensions)
     137                 :            : {
     138                 :       5574 :   bool result = true;
     139                 :            : 
     140                 :            :   try
     141                 :            :   {
     142                 :       5574 :     std::vector<MetricRange> lateralRanges;
     143         [ +  - ]:       5574 :     result = calculateLateralDimensions(roadArea, lateralRanges);
     144         [ +  + ]:       5574 :     if (result)
     145                 :            :     {
     146         [ +  - ]:       5527 :       MetricRange longitudinalDimensions;
     147                 :            : 
     148                 :       5527 :       longitudinalDimensions.maximum = Distance(0.);
     149                 :       5527 :       longitudinalDimensions.minimum = Distance(0.);
     150                 :            : 
     151                 :       5527 :       std::vector<RssObjectPositionExtractor> extractors;
     152         [ +  + ]:      14857 :       for (const auto &object : objects)
     153                 :            :       {
     154         [ +  + ]:       9387 :         if (object.occupied_regions.empty())
     155                 :            :         {
     156   [ +  -  +  - ]:          2 :           core::getLogger()->error("RssLaneCoordinateSystemConversion::calculateObjectDimensions>> occupied region of "
     157                 :            :                                    "object {} empty",
     158         [ +  - ]:          1 :                                    object.object_id);
     159                 :          1 :           return false;
     160                 :            :         }
     161   [ +  +  +  + ]:       9414 :         extractors.push_back(RssObjectPositionExtractor(object.occupied_regions));
     162                 :            :       }
     163                 :            : 
     164         [ +  + ]:      25568 :       for (auto const &roadSegment : roadArea)
     165                 :            :       {
     166         [ +  + ]:      55238 :         for (auto &extractor : extractors)
     167                 :            :         {
     168   [ +  -  +  -  :      35138 :           result = result && extractor.newRoadSegment(longitudinalDimensions, roadSegment);
                   +  - ]
     169                 :            :         }
     170                 :            : 
     171                 :            :         // This is needed, because we want to look for the minimum
     172                 :      20100 :         Distance longitudinalDistanceMax = Distance(0.);
     173                 :      20100 :         Distance longitudinalDistanceMin = std::numeric_limits<Distance>::max();
     174                 :            : 
     175   [ +  +  +  -  :     149906 :         for (std::size_t i = 0u; i < roadSegment.lane_segments.size() && result; i++)
                   +  + ]
     176                 :            :         {
     177         [ +  - ]:     129808 :           if (i < lateralRanges.size())
     178                 :            :           {
     179         [ +  + ]:     384352 :             for (auto &extractor : extractors)
     180                 :            :             {
     181   [ +  -  +  +  :     254546 :               result = result && extractor.newLaneSegment(lateralRanges[i], roadSegment.lane_segments[i]);
                   +  - ]
     182                 :            :             }
     183                 :            :           }
     184                 :            :           else
     185                 :            :           {
     186                 :            :             result = false; // LCOV_EXCL_LINE: unreachable code, keep to be on the safe side
     187                 :            :           }
     188                 :            : 
     189         [ +  - ]:     129806 :           longitudinalDistanceMax = std::max(longitudinalDistanceMax, roadSegment.lane_segments[i].length.maximum);
     190         [ +  - ]:     129806 :           longitudinalDistanceMin = std::min(longitudinalDistanceMin, roadSegment.lane_segments[i].length.minimum);
     191                 :            :         }
     192                 :            : 
     193         [ +  - ]:      20098 :         if (result)
     194                 :            :         {
     195         [ +  - ]:      20098 :           longitudinalDimensions.maximum += longitudinalDistanceMax;
     196         [ +  - ]:      20098 :           longitudinalDimensions.minimum += longitudinalDistanceMin;
     197                 :            :         }
     198                 :            :       }
     199                 :            : 
     200         [ +  - ]:       5468 :       if (result)
     201                 :            :       {
     202         [ +  + ]:      14732 :         for (auto &extractor : extractors)
     203                 :            :         {
     204         [ +  - ]:       9293 :           ObjectDimensions extractedDimensions;
     205   [ +  -  +  -  :       9292 :           result = result && extractor.getObjectDimensions(extractedDimensions);
                   +  - ]
     206         [ +  + ]:       9293 :           objectDimensions.push_back(extractedDimensions);
     207                 :            :         }
     208                 :            :       }
     209         [ +  + ]:       5526 :     }
     210                 :            :     else
     211                 :            :     {
     212   [ +  -  +  - ]:         94 :       core::getLogger()->error(
     213                 :            :         "RssLaneCoordinateSystemConversion::calculateObjectDimensions>> calculateLateralDimensions of "
     214                 :            :         "object {} failed",
     215         [ +  - ]:         47 :         objects.front().object_id);
     216                 :            :     }
     217         [ +  - ]:       5573 :   }
     218                 :         86 :   catch (...)
     219                 :            :   {
     220                 :         86 :     return false;
     221                 :         86 :   }
     222                 :            : 
     223                 :       5487 :   return result;
     224                 :            : }
     225                 :            : 
     226                 :       3961 : bool calculateObjectDimensions(world::Constellation const &currentConstellation,
     227                 :            :                                ObjectDimensions &egoVehiclePosition,
     228                 :            :                                ObjectDimensions &objectPosition)
     229                 :            : {
     230                 :       3961 :   bool result = true;
     231                 :            : 
     232                 :            :   try
     233                 :            :   {
     234                 :       3961 :     std::vector<world::Object> objects;
     235         [ +  + ]:       3961 :     objects.push_back(currentConstellation.ego_vehicle);
     236         [ +  + ]:       3939 :     objects.push_back(currentConstellation.object);
     237                 :            : 
     238                 :       3917 :     std::vector<ObjectDimensions> objectDimensions;
     239         [ +  - ]:       3917 :     result = calculateObjectDimensions(objects, currentConstellation.ego_vehicle_road, objectDimensions);
     240                 :            : 
     241   [ +  +  +  -  :       3917 :     if (result && (objectDimensions.size() == 2))
                   +  + ]
     242                 :            :     {
     243                 :       3814 :       egoVehiclePosition = objectDimensions[0];
     244                 :       3814 :       objectPosition = objectDimensions[1];
     245                 :            :     }
     246                 :            :     else
     247                 :            :     {
     248                 :        103 :       result = false;
     249                 :            :     }
     250                 :       3961 :   }
     251                 :         44 :   catch (...)
     252                 :            :   {
     253                 :         44 :     return false;
     254                 :         44 :   }
     255                 :            : 
     256                 :       3917 :   return result;
     257                 :            : }
     258                 :            : 
     259                 :       1669 : bool calculateObjectDimensions(world::Object const &object,
     260                 :            :                                world::RoadArea const &roadArea,
     261                 :            :                                ObjectDimensions &objectPosition)
     262                 :            : {
     263                 :       1669 :   bool result = true;
     264                 :            : 
     265                 :            :   try
     266                 :            :   {
     267                 :       1669 :     std::vector<world::Object> objects;
     268         [ +  + ]:       1669 :     objects.push_back(object);
     269                 :            : 
     270                 :       1657 :     std::vector<ObjectDimensions> objectDimensions;
     271         [ +  - ]:       1657 :     result = calculateObjectDimensions(objects, roadArea, objectDimensions);
     272                 :            : 
     273   [ +  +  +  -  :       1657 :     if (result && (objectDimensions.size() == 1))
                   +  + ]
     274                 :            :     {
     275                 :       1626 :       objectPosition = objectDimensions[0];
     276                 :            :     }
     277                 :            :     else
     278                 :            :     {
     279                 :         31 :       result = false;
     280                 :            :     }
     281                 :       1669 :   }
     282                 :         12 :   catch (...)
     283                 :            :   {
     284                 :         12 :     return false;
     285                 :         12 :   }
     286                 :            : 
     287                 :       1657 :   return result;
     288                 :            : }
     289                 :            : 
     290                 :       9936 : void convertVehicleStateDynamics(world::Object const &object,
     291                 :            :                                  world::RssDynamics const &rssDynamics,
     292                 :            :                                  core::RelativeObjectState &vehicleState)
     293                 :            : {
     294                 :       9936 :   vehicleState.dynamics = rssDynamics;
     295                 :       9936 :   vehicleState.structured_object_state.velocity = object.velocity;
     296                 :       9936 : }
     297                 :            : 
     298                 :            : } // namespace structured
     299                 :            : } // namespace rss
     300                 :            : } // namespace ad