LCOV - code coverage report
Current view: top level - src/core - RssSituationExtraction.cpp (source / functions) Hit Total Coverage
Test: ad_rss Lines: 249 256 97.3 %
Date: 2024-03-25 17:12:56 Functions: 13 13 100.0 %
Branches: 212 266 79.7 %

           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                 :            : #include "ad/rss/core/RssSituationExtraction.hpp"
      10                 :            : #include <algorithm>
      11                 :            : #include "../world/RssSituationCoordinateSystemConversion.hpp"
      12                 :            : #include "../world/RssSituationIdProvider.hpp"
      13                 :            : #include "ad/rss/world/WorldModelValidInputRange.hpp"
      14                 :            : #include "spdlog/fmt/ostr.h"
      15                 :            : #include "spdlog/spdlog.h"
      16                 :            : 
      17                 :            : namespace ad {
      18                 :            : namespace rss {
      19                 :            : namespace core {
      20                 :            : 
      21                 :            : using physics::Distance;
      22                 :            : using physics::MetricRange;
      23                 :            : 
      24                 :        728 : RssSituationExtraction::RssSituationExtraction()
      25                 :            : {
      26                 :            :   try
      27                 :            :   {
      28         [ +  + ]:        728 :     mSituationIdProvider = std::unique_ptr<world::RssSituationIdProvider>(new world::RssSituationIdProvider());
      29                 :            :   }
      30         [ +  - ]:         26 :   catch (...)
      31                 :            :   {
      32         [ +  - ]:         13 :     spdlog::critical("RssSituationExtraction object initialization failed");
      33                 :         13 :     mSituationIdProvider = nullptr;
      34                 :            :   }
      35                 :        728 : }
      36                 :            : 
      37                 :        728 : RssSituationExtraction::~RssSituationExtraction()
      38                 :            : {
      39                 :        728 : }
      40                 :            : 
      41                 :       3981 : void RssSituationExtraction::calcluateRelativeLongitudinalPosition(
      42                 :            :   MetricRange const &egoMetricRange,
      43                 :            :   MetricRange const &otherMetricRange,
      44                 :            :   situation::LongitudinalRelativePosition &longitudinalPosition,
      45                 :            :   Distance &longitudinalDistance)
      46                 :            : {
      47         [ +  + ]:       3981 :   if (egoMetricRange.minimum > otherMetricRange.maximum)
      48                 :            :   {
      49                 :        714 :     longitudinalPosition = situation::LongitudinalRelativePosition::InFront;
      50                 :        714 :     longitudinalDistance = egoMetricRange.minimum - otherMetricRange.maximum;
      51                 :            :   }
      52         [ +  + ]:       3267 :   else if (otherMetricRange.minimum > egoMetricRange.maximum)
      53                 :            :   {
      54                 :       2104 :     longitudinalPosition = situation::LongitudinalRelativePosition::AtBack;
      55                 :       2104 :     longitudinalDistance = otherMetricRange.minimum - egoMetricRange.maximum;
      56                 :            :   }
      57                 :            :   else
      58                 :            :   {
      59                 :       1163 :     longitudinalDistance = Distance(0.);
      60   [ +  +  +  +  :       1163 :     if ((egoMetricRange.minimum > otherMetricRange.minimum) && (egoMetricRange.maximum > otherMetricRange.maximum))
                   +  + ]
      61                 :            :     {
      62                 :        105 :       longitudinalPosition = situation::LongitudinalRelativePosition::OverlapFront;
      63                 :            :     }
      64   [ +  +  +  +  :       1058 :     else if ((egoMetricRange.minimum < otherMetricRange.minimum) && (egoMetricRange.maximum < otherMetricRange.maximum))
                   +  + ]
      65                 :            :     {
      66                 :         19 :       longitudinalPosition = situation::LongitudinalRelativePosition::OverlapBack;
      67                 :            :     }
      68                 :            :     else
      69                 :            :     {
      70                 :       1039 :       longitudinalPosition = situation::LongitudinalRelativePosition::Overlap;
      71                 :            :     }
      72                 :            :   }
      73                 :       3834 : }
      74                 :            : 
      75                 :        799 : void RssSituationExtraction::calcluateRelativeLongitudinalPositionIntersection(
      76                 :            :   MetricRange const &egoMetricRange,
      77                 :            :   MetricRange const &otherMetricRange,
      78                 :            :   situation::LongitudinalRelativePosition &longitudinalPosition,
      79                 :            :   Distance &longitudinalDistance)
      80                 :            : {
      81         [ +  + ]:        799 :   if (egoMetricRange.maximum < otherMetricRange.minimum)
      82                 :            :   {
      83                 :        109 :     longitudinalPosition = situation::LongitudinalRelativePosition::InFront;
      84                 :        109 :     longitudinalDistance = otherMetricRange.minimum - egoMetricRange.maximum;
      85                 :            :   }
      86         [ +  + ]:        690 :   else if (otherMetricRange.maximum < egoMetricRange.minimum)
      87                 :            :   {
      88                 :        542 :     longitudinalPosition = situation::LongitudinalRelativePosition::AtBack;
      89                 :        542 :     longitudinalDistance = egoMetricRange.minimum - otherMetricRange.maximum;
      90                 :            :   }
      91                 :            :   else
      92                 :            :   {
      93                 :        148 :     longitudinalDistance = Distance(0.);
      94   [ +  +  +  +  :        148 :     if ((egoMetricRange.minimum < otherMetricRange.minimum) && (egoMetricRange.maximum < otherMetricRange.maximum))
                   +  + ]
      95                 :            :     {
      96                 :         52 :       longitudinalPosition = situation::LongitudinalRelativePosition::OverlapFront;
      97                 :            :     }
      98   [ +  +  +  -  :         96 :     else if ((egoMetricRange.minimum > otherMetricRange.minimum) && (egoMetricRange.maximum > otherMetricRange.maximum))
                   +  + ]
      99                 :            :     {
     100                 :         48 :       longitudinalPosition = situation::LongitudinalRelativePosition::OverlapBack;
     101                 :            :     }
     102                 :            :     else
     103                 :            :     {
     104                 :         48 :       longitudinalPosition = situation::LongitudinalRelativePosition::Overlap;
     105                 :            :     }
     106                 :            :   }
     107                 :        799 : }
     108                 :            : 
     109                 :       3834 : void RssSituationExtraction::calcluateRelativeLateralPosition(MetricRange const &egoMetricRange,
     110                 :            :                                                               MetricRange const &otherMetricRange,
     111                 :            :                                                               situation::LateralRelativePosition &lateralPosition,
     112                 :            :                                                               Distance &lateralDistance)
     113                 :            : {
     114         [ +  + ]:       3834 :   if (egoMetricRange.minimum > otherMetricRange.maximum)
     115                 :            :   {
     116                 :       1150 :     lateralPosition = situation::LateralRelativePosition::AtRight;
     117                 :       1150 :     lateralDistance = egoMetricRange.minimum - otherMetricRange.maximum;
     118                 :            :   }
     119         [ +  + ]:       2684 :   else if (otherMetricRange.minimum > egoMetricRange.maximum)
     120                 :            :   {
     121                 :        935 :     lateralPosition = situation::LateralRelativePosition::AtLeft;
     122                 :        935 :     lateralDistance = otherMetricRange.minimum - egoMetricRange.maximum;
     123                 :            :   }
     124                 :            :   else
     125                 :            :   {
     126                 :       1749 :     lateralDistance = Distance(0.);
     127   [ +  +  +  +  :       1749 :     if ((egoMetricRange.minimum > otherMetricRange.minimum) && (egoMetricRange.maximum > otherMetricRange.maximum))
                   +  + ]
     128                 :            :     {
     129                 :        105 :       lateralPosition = situation::LateralRelativePosition::OverlapRight;
     130                 :            :     }
     131   [ +  +  +  +  :       1644 :     else if ((egoMetricRange.minimum < otherMetricRange.minimum) && (egoMetricRange.maximum < otherMetricRange.maximum))
                   +  + ]
     132                 :            :     {
     133                 :        106 :       lateralPosition = situation::LateralRelativePosition::OverlapLeft;
     134                 :            :     }
     135                 :            :     else
     136                 :            :     {
     137                 :       1538 :       lateralPosition = situation::LateralRelativePosition::Overlap;
     138                 :            :     }
     139                 :            :   }
     140                 :       3834 : }
     141                 :            : 
     142                 :       3962 : bool RssSituationExtraction::convertObjectsNonIntersection(world::Scene const &currentScene,
     143                 :            :                                                            situation::Situation &situation)
     144                 :            : {
     145         [ +  + ]:       3962 :   if (!currentScene.intersectingRoad.empty())
     146                 :            :   {
     147         [ +  - ]:          1 :     spdlog::error("RssSituationExtraction::convertObjectsNonIntersection>> Intersecting road not empty {}",
     148                 :            :                   currentScene);
     149                 :          1 :     return false;
     150                 :            :   }
     151                 :            : 
     152                 :       3961 :   bool result = true;
     153                 :            : 
     154         [ +  - ]:       3961 :   world::ObjectDimensions egoVehicleDimension;
     155         [ +  - ]:       3961 :   world::ObjectDimensions objectToBeCheckedDimension;
     156         [ +  - ]:       3961 :   result = calculateObjectDimensions(currentScene, egoVehicleDimension, objectToBeCheckedDimension);
     157                 :            : 
     158                 :            :   situation::LongitudinalRelativePosition longitudinalPosition;
     159                 :       3961 :   Distance longitudinalDistance;
     160         [ +  + ]:       3961 :   calcluateRelativeLongitudinalPosition(egoVehicleDimension.longitudinalDimensions,
     161                 :            :                                         objectToBeCheckedDimension.longitudinalDimensions,
     162                 :            :                                         longitudinalPosition,
     163                 :            :                                         longitudinalDistance);
     164                 :            : 
     165                 :       3814 :   situation.relativePosition.longitudinalPosition = longitudinalPosition;
     166                 :       3814 :   situation.relativePosition.longitudinalDistance = longitudinalDistance;
     167                 :            : 
     168                 :       3814 :   situation.egoVehicleState.isInCorrectLane = !egoVehicleDimension.onNegativeLane;
     169                 :            : 
     170         [ +  + ]:       3814 :   if (currentScene.situationType == ::ad::rss::situation::SituationType::OppositeDirection)
     171                 :            :   {
     172                 :       1071 :     situation.otherVehicleState.isInCorrectLane = !objectToBeCheckedDimension.onPositiveLane;
     173                 :            :   }
     174                 :            :   else
     175                 :            :   {
     176                 :       2743 :     situation.otherVehicleState.isInCorrectLane = !objectToBeCheckedDimension.onNegativeLane;
     177                 :            :   }
     178                 :            : 
     179                 :            :   /**
     180                 :            :    * Set lateral restrictions
     181                 :            :    */
     182         [ +  - ]:       3814 :   if (result)
     183                 :            :   {
     184                 :            :     situation::LateralRelativePosition lateralPosition;
     185                 :       3814 :     Distance lateralDistance;
     186         [ +  - ]:       3814 :     calcluateRelativeLateralPosition(egoVehicleDimension.lateralDimensions,
     187                 :            :                                      objectToBeCheckedDimension.lateralDimensions,
     188                 :            :                                      lateralPosition,
     189                 :            :                                      lateralDistance);
     190                 :            : 
     191                 :       3814 :     situation.relativePosition.lateralPosition = lateralPosition;
     192                 :       3814 :     situation.relativePosition.lateralDistance = lateralDistance;
     193                 :            :   }
     194                 :       3814 :   return result;
     195                 :            : }
     196                 :            : 
     197                 :       1599 : void RssSituationExtraction::convertToIntersectionCentric(MetricRange const &objectDimension,
     198                 :            :                                                           MetricRange const &intersectionPosition,
     199                 :            :                                                           MetricRange &dimensionsIntersection)
     200                 :            : {
     201                 :       1599 :   dimensionsIntersection.maximum = intersectionPosition.minimum - objectDimension.minimum;
     202                 :       1599 :   dimensionsIntersection.minimum = intersectionPosition.minimum - objectDimension.maximum;
     203                 :       1598 : }
     204                 :            : 
     205                 :        843 : bool RssSituationExtraction::convertObjectsIntersection(world::Scene const &currentScene,
     206                 :            :                                                         situation::Situation &situation)
     207                 :            : {
     208         [ +  - ]:        843 :   world::ObjectDimensions egoVehicleDimension;
     209         [ +  - ]:        843 :   world::ObjectDimensions objectDimension;
     210                 :            : 
     211         [ +  - ]:        843 :   bool result = calculateObjectDimensions(currentScene.egoVehicle, currentScene.egoVehicleRoad, egoVehicleDimension);
     212                 :            : 
     213   [ +  +  +  -  :        843 :   result = result && calculateObjectDimensions(currentScene.object, currentScene.intersectingRoad, objectDimension);
                   +  + ]
     214                 :            : 
     215         [ +  + ]:        843 :   if (result)
     216                 :            :   {
     217                 :        800 :     situation.egoVehicleState.isInCorrectLane = !egoVehicleDimension.onNegativeLane;
     218                 :        800 :     situation.otherVehicleState.isInCorrectLane = !objectDimension.onNegativeLane;
     219                 :            : 
     220                 :            :     // For intersection the lanes don't have the same origin so the positions cannot be directly compared
     221                 :            :     // Intersection entry should be the common point so convert the positions to this reference point
     222         [ +  - ]:        800 :     MetricRange egoDimensionsIntersection;
     223         [ +  + ]:        800 :     convertToIntersectionCentric(
     224                 :            :       egoVehicleDimension.longitudinalDimensions, egoVehicleDimension.intersectionPosition, egoDimensionsIntersection);
     225                 :            : 
     226         [ +  - ]:        799 :     MetricRange objectDimensionsIntersection;
     227                 :            : 
     228         [ +  - ]:        799 :     convertToIntersectionCentric(
     229                 :            :       objectDimension.longitudinalDimensions, objectDimension.intersectionPosition, objectDimensionsIntersection);
     230                 :            : 
     231         [ +  - ]:        799 :     situation.egoVehicleState.distanceToEnterIntersection = std::max(Distance(0.), egoDimensionsIntersection.minimum);
     232                 :            :     situation.egoVehicleState.distanceToLeaveIntersection
     233         [ +  - ]:        799 :       = egoVehicleDimension.intersectionPosition.maximum - egoVehicleDimension.longitudinalDimensions.minimum;
     234                 :            : 
     235                 :            :     situation.otherVehicleState.distanceToEnterIntersection
     236         [ +  - ]:        799 :       = std::max(Distance(0.), objectDimensionsIntersection.minimum);
     237                 :            :     situation.otherVehicleState.distanceToLeaveIntersection
     238         [ +  - ]:        799 :       = objectDimension.intersectionPosition.maximum - objectDimension.longitudinalDimensions.minimum;
     239                 :            : 
     240                 :            :     situation::LongitudinalRelativePosition longitudinalPosition;
     241                 :        799 :     Distance longitudinalDistance;
     242         [ +  - ]:        799 :     calcluateRelativeLongitudinalPositionIntersection(
     243                 :            :       egoDimensionsIntersection, objectDimensionsIntersection, longitudinalPosition, longitudinalDistance);
     244                 :            : 
     245                 :        799 :     situation.relativePosition.longitudinalPosition = longitudinalPosition;
     246                 :        799 :     situation.relativePosition.longitudinalDistance = longitudinalDistance;
     247                 :            : 
     248                 :        799 :     situation.relativePosition.lateralPosition = situation::LateralRelativePosition::Overlap;
     249                 :        799 :     situation.relativePosition.lateralDistance = Distance(0.);
     250                 :            :   }
     251                 :            : 
     252         [ +  + ]:        842 :   if (currentScene.situationType == situation::SituationType::IntersectionEgoHasPriority)
     253                 :            :   {
     254                 :        408 :     situation.egoVehicleState.hasPriority = true;
     255                 :        408 :     situation.otherVehicleState.hasPriority = false;
     256                 :            :   }
     257         [ +  + ]:        434 :   else if (currentScene.situationType == situation::SituationType::IntersectionObjectHasPriority)
     258                 :            :   {
     259                 :        217 :     situation.egoVehicleState.hasPriority = false;
     260                 :        217 :     situation.otherVehicleState.hasPriority = true;
     261                 :            :   }
     262         [ +  - ]:        217 :   else if (currentScene.situationType == situation::SituationType::IntersectionSamePriority)
     263                 :            :   {
     264                 :        217 :     situation.egoVehicleState.hasPriority = false;
     265                 :        217 :     situation.otherVehicleState.hasPriority = false;
     266                 :            :   }
     267                 :            :   else
     268                 :            :   {
     269                 :            :     // This function should never be called if we are not in intersection situation
     270         [ #  # ]:          0 :     spdlog::error("RssSituationExtraction::convertObjectsIntersection>> Unexpected situationType {}", currentScene);
     271                 :            :     result = false; // LCOV_EXCL_LINE: unreachable code, keep to be on the safe side
     272                 :            :   }
     273                 :            : 
     274                 :        842 :   return result;
     275                 :            : }
     276                 :            : 
     277                 :       5021 : bool RssSituationExtraction::extractSituationInputRangeChecked(world::TimeIndex const &timeIndex,
     278                 :            :                                                                world::Scene const &currentScene,
     279                 :            :                                                                situation::Situation &situation)
     280                 :            : {
     281                 :            :   // ensure the object types are semantically correct
     282                 :            :   // @toDo: add this restriction to the data type model
     283                 :            :   //       and extend generated withinValidInputRange by these
     284         [ +  + ]:       5021 :   if (((currentScene.object.objectType != world::ObjectType::OtherVehicle)
     285         [ +  + ]:          6 :        && (currentScene.object.objectType != world::ObjectType::ArtificialObject)
     286         [ +  + ]:          4 :        && (currentScene.object.objectType != world::ObjectType::Pedestrian))
     287         [ +  + ]:       5018 :       || (currentScene.egoVehicle.objectType != world::ObjectType::EgoVehicle))
     288                 :            :   {
     289         [ +  - ]:          8 :     spdlog::error("RssSituationExtraction::extractSituationInputRangeChecked>> Invalid object type. Ego: {} Object: {}",
     290                 :          8 :                   currentScene.egoVehicle,
     291                 :          8 :                   currentScene.object);
     292                 :          8 :     return false;
     293                 :            :   }
     294         [ +  + ]:       5013 :   if (currentScene.object.objectId == currentScene.egoVehicle.objectId)
     295                 :            :   {
     296         [ +  - ]:          1 :     spdlog::error("RssSituationExtraction::extractSituationInputRangeChecked>> Object and ego vehicle must not have "
     297                 :            :                   "the same id. Ego: {} Object: {}",
     298                 :          1 :                   currentScene.egoVehicle,
     299                 :          1 :                   currentScene.object);
     300                 :          1 :     return false;
     301                 :            :   }
     302         [ +  + ]:       5012 :   if (!static_cast<bool>(mSituationIdProvider))
     303                 :            :   {
     304                 :         15 :     return false;
     305                 :            :   }
     306                 :            : 
     307                 :       4997 :   bool result = false;
     308                 :            : 
     309                 :            :   try
     310                 :            :   {
     311         [ +  + ]:       4997 :     situation.situationId = mSituationIdProvider->getSituationId(timeIndex, currentScene);
     312                 :       4952 :     situation.objectId = currentScene.object.objectId;
     313                 :       4952 :     situation.situationType = currentScene.situationType;
     314                 :            : 
     315                 :       4952 :     situation.egoVehicleState.objectType = currentScene.egoVehicle.objectType;
     316                 :       4952 :     situation.otherVehicleState.objectType = currentScene.object.objectType;
     317                 :            : 
     318                 :       4952 :     situation.egoVehicleState.objectState = currentScene.egoVehicle.state;
     319                 :       4952 :     situation.otherVehicleState.objectState = currentScene.object.state;
     320                 :            : 
     321                 :       4952 :     situation.egoVehicleState.hasPriority = false;
     322                 :       4952 :     situation.otherVehicleState.hasPriority = false;
     323                 :            : 
     324                 :       4952 :     situation.egoVehicleState.isInCorrectLane = true;
     325                 :       4952 :     situation.otherVehicleState.isInCorrectLane = true;
     326                 :            : 
     327                 :       4952 :     situation.egoVehicleState.distanceToEnterIntersection = Distance(0.);
     328                 :       4952 :     situation.egoVehicleState.distanceToLeaveIntersection = Distance(1000.);
     329                 :            : 
     330                 :       4952 :     situation.otherVehicleState.distanceToEnterIntersection = Distance(0.);
     331                 :       4952 :     situation.otherVehicleState.distanceToLeaveIntersection = Distance(1000.);
     332                 :            : 
     333         [ +  - ]:       4952 :     convertVehicleStateDynamics(currentScene.egoVehicle, currentScene.egoVehicleRssDynamics, situation.egoVehicleState);
     334         [ +  - ]:       4952 :     convertVehicleStateDynamics(currentScene.object, currentScene.objectRssDynamics, situation.otherVehicleState);
     335                 :            : 
     336   [ +  +  +  + ]:       4952 :     switch (currentScene.situationType)
     337                 :            :     {
     338                 :       3962 :       case ad::rss::situation::SituationType::SameDirection:
     339                 :            :       case ad::rss::situation::SituationType::OppositeDirection:
     340                 :            :       {
     341         [ +  + ]:       3962 :         result = convertObjectsNonIntersection(currentScene, situation);
     342                 :            : 
     343                 :       3815 :         break;
     344                 :            :       }
     345                 :        843 :       case ad::rss::situation::SituationType::IntersectionEgoHasPriority:
     346                 :            :       case ad::rss::situation::SituationType::IntersectionObjectHasPriority:
     347                 :            :       case ad::rss::situation::SituationType::IntersectionSamePriority:
     348                 :            :       {
     349         [ +  + ]:        843 :         result = convertObjectsIntersection(currentScene, situation);
     350                 :        842 :         break;
     351                 :            :       }
     352                 :        146 :       case ad::rss::situation::SituationType::NotRelevant:
     353                 :            :       case ad::rss::situation::SituationType::Unstructured:
     354                 :            :       {
     355                 :        146 :         result = true;
     356                 :        146 :         break;
     357                 :            :       }
     358                 :          1 :       default:
     359                 :            :       {
     360         [ +  - ]:          1 :         spdlog::error("RssSituationExtraction::extractSituationInputRangeChecked>> Invalid situation type {}",
     361                 :            :                       currentScene);
     362                 :          1 :         result = false;
     363                 :          1 :         break;
     364                 :            :       }
     365                 :            :     }
     366                 :            :   }
     367                 :        193 :   catch (std::exception &e)
     368                 :            :   {
     369         [ +  - ]:        193 :     spdlog::critical("RssSituationExtraction::extractSituationInputRangeChecked>> Exception caught '{}' {} {}",
     370                 :        193 :                      e.what(),
     371                 :            :                      timeIndex,
     372                 :            :                      currentScene);
     373                 :        193 :     result = false;
     374                 :            :   }
     375                 :          0 :   catch (...)
     376                 :            :   {
     377         [ -  - ]:          0 :     spdlog::critical(
     378                 :            :       "RssSituationExtraction::extractSituationInputRangeChecked>> Exception caught {} {}", timeIndex, currentScene);
     379                 :          0 :     result = false;
     380                 :            :   }
     381                 :            : 
     382                 :       4997 :   return result;
     383                 :            : }
     384                 :            : 
     385                 :         54 : bool RssSituationExtraction::mergeVehicleStates(MergeMode const &mergeMode,
     386                 :            :                                                 situation::VehicleState const &otherVehicleState,
     387                 :            :                                                 situation::VehicleState &mergedVehicleState)
     388                 :            : {
     389                 :            :   // on vehicle states there are only differences in intersection distances allowed due to different road definitions
     390                 :         54 :   if ((otherVehicleState.dynamics.alphaLat.accelMax != mergedVehicleState.dynamics.alphaLat.accelMax)
     391         [ +  + ]:         53 :       || (otherVehicleState.dynamics.alphaLat.brakeMin != mergedVehicleState.dynamics.alphaLat.brakeMin)
     392         [ +  + ]:         52 :       || (otherVehicleState.dynamics.alphaLon.accelMax != mergedVehicleState.dynamics.alphaLon.accelMax)
     393         [ +  + ]:         51 :       || (otherVehicleState.dynamics.alphaLon.brakeMinCorrect != mergedVehicleState.dynamics.alphaLon.brakeMinCorrect)
     394         [ +  + ]:         50 :       || (otherVehicleState.dynamics.alphaLon.brakeMin != mergedVehicleState.dynamics.alphaLon.brakeMin)
     395         [ +  + ]:         49 :       || (otherVehicleState.dynamics.alphaLon.brakeMax != mergedVehicleState.dynamics.alphaLon.brakeMax)
     396         [ +  + ]:         48 :       || (otherVehicleState.dynamics.lateralFluctuationMargin != mergedVehicleState.dynamics.lateralFluctuationMargin)
     397         [ +  + ]:         47 :       || (otherVehicleState.dynamics.responseTime != mergedVehicleState.dynamics.responseTime)
     398   [ +  +  -  +  :        107 :       || (otherVehicleState.hasPriority != mergedVehicleState.hasPriority))
                   +  + ]
     399                 :            :   {
     400                 :          8 :     return false;
     401                 :            :   }
     402                 :            :   // consider the worst cases
     403         [ +  + ]:         46 :   if (mergeMode == MergeMode::EgoVehicle)
     404                 :            :   {
     405                 :            :     // worst case for ego vehicle is not driving in correct lane
     406   [ +  -  +  + ]:         27 :     mergedVehicleState.isInCorrectLane = mergedVehicleState.isInCorrectLane && otherVehicleState.isInCorrectLane;
     407                 :            :   }
     408                 :            :   else
     409                 :            :   {
     410                 :            :     // worst case for other vehicle is driving in correct lane
     411   [ -  +  -  - ]:         19 :     mergedVehicleState.isInCorrectLane = mergedVehicleState.isInCorrectLane || otherVehicleState.isInCorrectLane;
     412                 :            :   }
     413                 :            : 
     414                 :            :   mergedVehicleState.distanceToEnterIntersection
     415                 :         46 :     = std::min(mergedVehicleState.distanceToEnterIntersection, otherVehicleState.distanceToEnterIntersection);
     416                 :            :   mergedVehicleState.distanceToLeaveIntersection
     417                 :         46 :     = std::max(mergedVehicleState.distanceToLeaveIntersection, otherVehicleState.distanceToLeaveIntersection);
     418                 :            :   mergedVehicleState.velocity.speedLon.minimum
     419                 :         46 :     = std::min(mergedVehicleState.velocity.speedLon.minimum, otherVehicleState.velocity.speedLon.minimum);
     420                 :            :   mergedVehicleState.velocity.speedLon.maximum
     421                 :         46 :     = std::max(mergedVehicleState.velocity.speedLon.maximum, otherVehicleState.velocity.speedLon.maximum);
     422                 :            :   mergedVehicleState.velocity.speedLat.minimum
     423                 :         46 :     = std::min(mergedVehicleState.velocity.speedLat.minimum, otherVehicleState.velocity.speedLat.minimum);
     424                 :            :   mergedVehicleState.velocity.speedLat.maximum
     425                 :         46 :     = std::max(mergedVehicleState.velocity.speedLat.maximum, otherVehicleState.velocity.speedLat.maximum);
     426                 :            : 
     427                 :         46 :   return true;
     428                 :            : }
     429                 :            : 
     430                 :         27 : bool RssSituationExtraction::mergeSituations(situation::Situation const &otherSituation,
     431                 :            :                                              situation::Situation &mergedSituation)
     432                 :            : {
     433                 :         27 :   if ( // basic data has to match
     434         [ +  - ]:         27 :     (otherSituation.situationId != mergedSituation.situationId) || (otherSituation.objectId != mergedSituation.objectId)
     435         [ +  - ]:         27 :     || (otherSituation.situationType != mergedSituation.situationType)
     436   [ +  -  +  - ]:         27 :     || !mergeVehicleStates(MergeMode::EgoVehicle, otherSituation.egoVehicleState, mergedSituation.egoVehicleState)
     437   [ +  -  +  + ]:         54 :     || !mergeVehicleStates(
     438   [ +  -  +  + ]:         54 :       MergeMode::OtherVehicle, otherSituation.otherVehicleState, mergedSituation.otherVehicleState))
     439                 :            :   {
     440                 :          8 :     return false;
     441                 :            :   }
     442                 :            : 
     443                 :            :   // worst case
     444                 :         19 :   mergedSituation.relativePosition.longitudinalDistance = std::min(
     445                 :         19 :     mergedSituation.relativePosition.longitudinalDistance, otherSituation.relativePosition.longitudinalDistance);
     446         [ +  + ]:         19 :   if (otherSituation.relativePosition.longitudinalPosition != mergedSituation.relativePosition.longitudinalPosition)
     447                 :            :   {
     448         [ +  + ]:          8 :     if ((mergedSituation.relativePosition.longitudinalPosition == situation::LongitudinalRelativePosition::Overlap)
     449         [ +  + ]:          7 :         || (otherSituation.relativePosition.longitudinalPosition == situation::LongitudinalRelativePosition::Overlap)
     450         [ +  + ]:          6 :         || ((mergedSituation.relativePosition.longitudinalPosition < situation::LongitudinalRelativePosition::Overlap)
     451         [ +  + ]:          3 :             && (otherSituation.relativePosition.longitudinalPosition
     452                 :            :                 > situation::LongitudinalRelativePosition::Overlap))
     453         [ +  + ]:          5 :         || ((mergedSituation.relativePosition.longitudinalPosition > situation::LongitudinalRelativePosition::Overlap)
     454         [ +  + ]:          3 :             && (otherSituation.relativePosition.longitudinalPosition
     455                 :            :                 < situation::LongitudinalRelativePosition::Overlap)))
     456                 :            :     {
     457                 :            :       // overlap is also worst case for contradicting input, ensure the longitudinal distance becomes 0.
     458                 :          4 :       mergedSituation.relativePosition.longitudinalPosition = situation::LongitudinalRelativePosition::Overlap;
     459                 :          4 :       mergedSituation.relativePosition.longitudinalDistance = Distance(0.);
     460                 :            :     }
     461         [ +  + ]:          4 :     else if ((mergedSituation.relativePosition.longitudinalPosition
     462                 :            :               == situation::LongitudinalRelativePosition::OverlapFront)
     463         [ +  + ]:          3 :              || (otherSituation.relativePosition.longitudinalPosition
     464                 :            :                  == situation::LongitudinalRelativePosition::OverlapFront))
     465                 :            :     {
     466                 :            :       // one of the both is overlap font (the other then only can be InFront in here)
     467                 :          2 :       mergedSituation.relativePosition.longitudinalPosition = situation::LongitudinalRelativePosition::OverlapFront;
     468                 :            :     }
     469         [ +  + ]:          2 :     else if ((mergedSituation.relativePosition.longitudinalPosition
     470                 :            :               == situation::LongitudinalRelativePosition::OverlapBack)
     471         [ +  - ]:          1 :              || (otherSituation.relativePosition.longitudinalPosition
     472                 :            :                  == situation::LongitudinalRelativePosition::OverlapBack))
     473                 :            :     {
     474                 :            :       // one of the both is overlap back (the other then only can be AtBack in here)
     475                 :          2 :       mergedSituation.relativePosition.longitudinalPosition = situation::LongitudinalRelativePosition::OverlapBack;
     476                 :            :     }
     477                 :            :     else
     478                 :            :     {
     479                 :            :       // LCOV_EXCL_START: unreachable code, keep to be on the safe side
     480                 :            :       // this is impossible to reach, set to overlap having longitudinal distance to 0. to be on the safe side
     481                 :            :       mergedSituation.relativePosition.longitudinalPosition = situation::LongitudinalRelativePosition::Overlap;
     482                 :            :       mergedSituation.relativePosition.longitudinalDistance = Distance(0.);
     483                 :            :       // LCOV_EXCL_STOP: unreachable code, keep to be on the safe side
     484                 :            :     }
     485                 :            :   }
     486                 :            : 
     487                 :            :   mergedSituation.relativePosition.lateralDistance
     488                 :         19 :     = std::min(mergedSituation.relativePosition.lateralDistance, otherSituation.relativePosition.lateralDistance);
     489         [ +  + ]:         19 :   if (otherSituation.relativePosition.lateralPosition != mergedSituation.relativePosition.lateralPosition)
     490                 :            :   {
     491         [ +  + ]:          8 :     if ((mergedSituation.relativePosition.lateralPosition == situation::LateralRelativePosition::Overlap)
     492         [ +  + ]:          7 :         || (otherSituation.relativePosition.lateralPosition == situation::LateralRelativePosition::Overlap)
     493         [ +  + ]:          6 :         || ((mergedSituation.relativePosition.lateralPosition < situation::LateralRelativePosition::Overlap)
     494         [ +  + ]:          3 :             && (otherSituation.relativePosition.lateralPosition > situation::LateralRelativePosition::Overlap))
     495         [ +  + ]:          5 :         || ((mergedSituation.relativePosition.lateralPosition > situation::LateralRelativePosition::Overlap)
     496         [ +  + ]:          3 :             && (otherSituation.relativePosition.lateralPosition < situation::LateralRelativePosition::Overlap)))
     497                 :            :     {
     498                 :            :       // overlap is also worst case for contradicting input, ensure the lateral distance becomes 0.
     499                 :          4 :       mergedSituation.relativePosition.lateralPosition = situation::LateralRelativePosition::Overlap;
     500                 :          4 :       mergedSituation.relativePosition.lateralDistance = Distance(0.);
     501                 :            :     }
     502         [ +  + ]:          4 :     else if ((mergedSituation.relativePosition.lateralPosition == situation::LateralRelativePosition::OverlapLeft)
     503         [ +  + ]:          3 :              || (otherSituation.relativePosition.lateralPosition == situation::LateralRelativePosition::OverlapLeft))
     504                 :            :     {
     505                 :            :       // one of the both is overlap left (the other then only can be AtLeft in here)
     506                 :          2 :       mergedSituation.relativePosition.lateralPosition = situation::LateralRelativePosition::OverlapLeft;
     507                 :            :     }
     508         [ +  + ]:          2 :     else if ((mergedSituation.relativePosition.lateralPosition == situation::LateralRelativePosition::OverlapRight)
     509         [ +  - ]:          1 :              || (otherSituation.relativePosition.lateralPosition == situation::LateralRelativePosition::OverlapRight))
     510                 :            :     {
     511                 :            :       // one of the both is overlap right (the other then only can be AtRight in here)
     512                 :          2 :       mergedSituation.relativePosition.lateralPosition = situation::LateralRelativePosition::OverlapRight;
     513                 :            :     }
     514                 :            :     else
     515                 :            :     {
     516                 :            :       // LCOV_EXCL_START: unreachable code, keep to be on the safe side
     517                 :            :       // this is impossible to reach, set to overlap having lateral distance to 0. to be on the safe side
     518                 :            :       mergedSituation.relativePosition.lateralPosition = situation::LateralRelativePosition::Overlap;
     519                 :            :       mergedSituation.relativePosition.lateralDistance = Distance(0.);
     520                 :            :       // LCOV_EXCL_STOP: unreachable code, keep to be on the safe side
     521                 :            :     }
     522                 :            :   }
     523                 :            : 
     524                 :         19 :   return true;
     525                 :            : }
     526                 :            : 
     527                 :       4263 : bool RssSituationExtraction::extractSituations(world::WorldModel const &worldModel,
     528                 :            :                                                situation::SituationSnapshot &situationSnapshot)
     529                 :            : {
     530         [ +  + ]:       4263 :   if (!withinValidInputRange(worldModel))
     531                 :            :   {
     532         [ +  - ]:         27 :     spdlog::error("RssSituationExtraction::extractSituation>> Invalid input {}", worldModel);
     533                 :         27 :     return false;
     534                 :            :   }
     535                 :            : 
     536                 :       4236 :   bool result = true;
     537                 :            :   try
     538                 :            :   {
     539                 :       4236 :     situationSnapshot.timeIndex = worldModel.timeIndex;
     540                 :       4236 :     situationSnapshot.defaultEgoVehicleRssDynamics = worldModel.defaultEgoVehicleRssDynamics;
     541                 :       4236 :     situationSnapshot.situations.clear();
     542         [ +  + ]:       9235 :     for (auto const &scene : worldModel.scenes)
     543                 :            :     {
     544         [ +  - ]:       5014 :       situation::Situation situation;
     545         [ +  - ]:       5014 :       bool const extractResult = extractSituationInputRangeChecked(worldModel.timeIndex, scene, situation);
     546         [ +  + ]:       5014 :       if (extractResult)
     547                 :            :       {
     548                 :            :         // situation id creation might detect that different scenes are representing identical situations
     549                 :            :         // ensure the situationSnapshot is unique while containing the worst-case situation
     550                 :            :         auto findResult = std::find_if(situationSnapshot.situations.begin(),
     551                 :            :                                        situationSnapshot.situations.end(),
     552                 :       5572 :                                        [&situation](ad::rss::situation::Situation const &checkSituation) {
     553                 :       5572 :                                          return checkSituation.situationId == situation.situationId;
     554         [ +  - ]:       4757 :                                        });
     555         [ +  + ]:       4757 :         if (findResult == situationSnapshot.situations.end())
     556                 :            :         {
     557         [ +  + ]:       4730 :           situationSnapshot.situations.push_back(situation);
     558                 :            :         }
     559   [ +  -  +  + ]:         27 :         else if (!mergeSituations(situation, *findResult))
     560                 :            :         {
     561                 :          8 :           result = false;
     562                 :            :         }
     563                 :            :       }
     564                 :            :       else
     565                 :            :       {
     566         [ +  - ]:        257 :         spdlog::error("RssSituationExtraction::extractSituations>> Extraction failed {}", scene);
     567                 :        257 :         result = false;
     568                 :            :       }
     569                 :            :     }
     570                 :            :   }
     571                 :         15 :   catch (std::exception &e)
     572                 :            :   {
     573         [ +  - ]:         30 :     spdlog::critical("RssSituationExtraction::extractSituations>> Exception caught '{}' {}", e.what(), worldModel);
     574                 :         15 :     result = false;
     575                 :            :   }
     576                 :          0 :   catch (...)
     577                 :            :   {
     578         [ -  - ]:          0 :     spdlog::critical("RssSituationExtraction::extractSituations>> Exception caught {}", worldModel);
     579                 :          0 :     result = false;
     580                 :            :   }
     581                 :       4236 :   return result;
     582                 :            : }
     583                 :            : 
     584                 :            : } // namespace core
     585                 :            : } // namespace rss
     586                 :            : } // namespace ad

Generated by: LCOV version 1.14