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/RssSituationChecking.hpp"
      10                 :            : #include <algorithm>
      11                 :            : #include <memory>
      12                 :            : #include "ad/rss/core/Logging.hpp"
      13                 :            : #include "ad/rss/core/RssSituationSnapshotValidInputRange.hpp"
      14                 :            : 
      15                 :            : namespace ad {
      16                 :            : namespace rss {
      17                 :            : namespace core {
      18                 :            : 
      19                 :            : enum class IsSafe
      20                 :            : {
      21                 :            :   Yes,
      22                 :            :   No
      23                 :            : };
      24                 :            : 
      25                 :      67236 : inline state::RssState createRssState(RelativeConstellationId const &constellation_id,
      26                 :            :                                       world::ConstellationType const &constellation_type,
      27                 :            :                                       world::ObjectId const &object_id,
      28                 :            :                                       world::ObjectId const &ego_id,
      29                 :            :                                       world::RssDynamics const &egoDynamics,
      30                 :            :                                       IsSafe const &isSafeValue)
      31                 :            : {
      32                 :      67236 :   bool const is_safe = (isSafeValue == IsSafe::Yes);
      33         [ +  - ]:      67236 :   state::RssStateInformation emptyRssStateInfo;
      34                 :      67236 :   emptyRssStateInfo.current_distance = std::numeric_limits<physics::Distance>::max();
      35                 :      67237 :   emptyRssStateInfo.safe_distance = std::numeric_limits<physics::Distance>::max();
      36                 :      67237 :   emptyRssStateInfo.evaluator = state::RssStateEvaluator::None;
      37                 :            : 
      38         [ +  - ]:      67237 :   state::RssState resultRssState;
      39                 :      67237 :   resultRssState.constellation_id = constellation_id;
      40                 :      67237 :   resultRssState.constellation_type = constellation_type;
      41                 :      67237 :   resultRssState.ego_id = ego_id;
      42                 :      67237 :   resultRssState.object_id = object_id;
      43                 :      67237 :   resultRssState.lateral_state_left.is_safe = is_safe;
      44                 :            :   resultRssState.lateral_state_left.response
      45         [ +  + ]:      67237 :     = is_safe ? (::ad::rss::state::LateralResponse::None) : (::ad::rss::state::LateralResponse::BrakeMin);
      46                 :      67237 :   resultRssState.lateral_state_left.alpha_lat = egoDynamics.alpha_lat;
      47                 :      67237 :   resultRssState.lateral_state_left.rss_state_information = emptyRssStateInfo;
      48                 :      67237 :   resultRssState.lateral_state_right.is_safe = is_safe;
      49                 :            :   resultRssState.lateral_state_right.response
      50         [ +  + ]:      67237 :     = is_safe ? (::ad::rss::state::LateralResponse::None) : (::ad::rss::state::LateralResponse::BrakeMin);
      51                 :      67237 :   resultRssState.lateral_state_right.alpha_lat = egoDynamics.alpha_lat;
      52                 :      67237 :   resultRssState.lateral_state_right.rss_state_information = emptyRssStateInfo;
      53                 :      67237 :   resultRssState.longitudinal_state.is_safe = is_safe;
      54                 :            :   resultRssState.longitudinal_state.response
      55         [ +  + ]:      67237 :     = is_safe ? (::ad::rss::state::LongitudinalResponse::None) : (::ad::rss::state::LongitudinalResponse::BrakeMin);
      56                 :      67237 :   resultRssState.longitudinal_state.alpha_lon = egoDynamics.alpha_lon;
      57                 :      67237 :   resultRssState.longitudinal_state.rss_state_information = emptyRssStateInfo;
      58                 :      67237 :   resultRssState.unstructured_constellation_state.heading_range.begin = ad::physics::Angle(0.0);
      59                 :      67236 :   resultRssState.unstructured_constellation_state.heading_range.end = ad::physics::c2PI;
      60                 :      67236 :   resultRssState.unstructured_constellation_state.alpha_lon = egoDynamics.alpha_lon;
      61                 :      67236 :   resultRssState.unstructured_constellation_state.is_safe = is_safe;
      62                 :      67236 :   resultRssState.unstructured_constellation_state.response = is_safe
      63         [ +  + ]:      67236 :     ? (::ad::rss::state::UnstructuredConstellationResponse::None)
      64                 :            :     : (::ad::rss::state::UnstructuredConstellationResponse::Brake);
      65                 :     134472 :   return resultRssState;
      66                 :            : }
      67                 :            : 
      68                 :      67080 : bool RssSituationChecking::checkConstellationInputRangeChecked(RelativeConstellation const &constellation,
      69                 :            :                                                                state::RssStateSnapshot &rssStateSnapshot)
      70                 :            : {
      71                 :      67080 :   bool result = false;
      72                 :            :   // global try catch block to ensure this library call doesn't throw an exception
      73                 :            :   try
      74                 :            :   {
      75                 :      67080 :     auto rssState = createRssState(constellation.constellation_id,
      76                 :      67080 :                                    constellation.constellation_type,
      77                 :      67080 :                                    constellation.object_id,
      78                 :      67080 :                                    constellation.ego_id,
      79                 :      67080 :                                    constellation.ego_state.dynamics,
      80         [ +  - ]:      67080 :                                    IsSafe::No);
      81                 :            : 
      82   [ +  +  +  +  :      67081 :     switch (constellation.constellation_type)
                      + ]
      83                 :            :     {
      84                 :        156 :       case world::ConstellationType::NotRelevant:
      85                 :        156 :         rssState = createRssState(constellation.constellation_id,
      86                 :        156 :                                   constellation.constellation_type,
      87                 :        156 :                                   constellation.object_id,
      88                 :        156 :                                   constellation.ego_id,
      89                 :        156 :                                   constellation.ego_state.dynamics,
      90         [ +  - ]:        156 :                                   IsSafe::Yes);
      91                 :        156 :         result = true;
      92                 :        156 :         break;
      93                 :      66134 :       case world::ConstellationType::SameDirection:
      94                 :            :       case world::ConstellationType::OppositeDirection:
      95         [ +  + ]:      66134 :         result = mNonIntersectionChecker.calculateRssStateNonIntersection(mCurrentTimeIndex, constellation, rssState);
      96                 :      66123 :         break;
      97                 :            : 
      98                 :        788 :       case world::ConstellationType::IntersectionEgoHasPriority:
      99                 :            :       case world::ConstellationType::IntersectionObjectHasPriority:
     100                 :            :       case world::ConstellationType::IntersectionSamePriority:
     101         [ +  - ]:        788 :         result = mIntersectionChecker.calculateRssStateIntersection(mCurrentTimeIndex, constellation, rssState);
     102                 :        788 :         break;
     103                 :          2 :       case world::ConstellationType::Unstructured:
     104                 :          4 :         result = mUnstructuredConstellationChecker.calculateRssStateUnstructured(
     105         [ +  - ]:          2 :           mCurrentTimeIndex, constellation, rssStateSnapshot.unstructured_constellation_ego_information, rssState);
     106                 :          2 :         break;
     107                 :          1 :       default:
     108   [ +  -  +  - ]:          2 :         core::getLogger()->error(
     109                 :            :           "RssSituationChecking::checkConstellationInputRangeChecked>> Invalid constellation type {}", constellation);
     110                 :          1 :         result = false;
     111                 :          1 :         break;
     112                 :            :     }
     113                 :            : 
     114         [ +  + ]:      67070 :     if (result)
     115                 :            :     {
     116         [ +  + ]:      55066 :       rssStateSnapshot.individual_responses.push_back(rssState);
     117                 :            :     }
     118                 :      67079 :   }
     119         [ +  - ]:         26 :   catch (std::exception &e)
     120                 :            :   {
     121   [ +  -  +  - ]:         52 :     core::getLogger()->critical(
     122         [ +  - ]:         52 :       "RssSituationChecking::checkConstellationInputRangeChecked>> Exception caught '{}' {}", e.what(), constellation);
     123                 :         26 :     result = false;
     124                 :         26 :   }
     125                 :          0 :   catch (...)
     126                 :            :   {
     127   [ -  -  -  - ]:          0 :     core::getLogger()->critical("RssSituationChecking::checkConstellationInputRangeChecked>> Exception caught {}",
     128                 :            :                                 constellation);
     129                 :          0 :     result = false;
     130                 :          0 :   }
     131                 :            : 
     132                 :      67079 :   return result;
     133                 :            : }
     134                 :            : 
     135                 :       4015 : bool RssSituationChecking::checkSituation(RssSituationSnapshot const &situationSnapshot,
     136                 :            :                                           state::RssStateSnapshot &rssStateSnapshot)
     137                 :            : {
     138         [ +  + ]:       4015 :   if (!withinValidInputRange(situationSnapshot))
     139                 :            :   {
     140         [ +  - ]:          2 :     core::getLogger()->error("RssSituationChecking::checkConstellations>> Invalid input {}", situationSnapshot);
     141                 :          1 :     return false;
     142                 :            :   }
     143         [ +  + ]:       4014 :   if (!checkTimeIncreasingConsistently(situationSnapshot.time_index))
     144                 :            :   {
     145         [ +  - ]:         50 :     core::getLogger()->error("RssSituationChecking::checkConstellations>> Inconsistent time {}",
     146         [ +  - ]:         25 :                              situationSnapshot.time_index);
     147                 :         25 :     return false;
     148                 :            :   }
     149                 :       3989 :   bool result = true;
     150                 :            :   // global try catch block to ensure this library call doesn't throw an exception
     151                 :            :   try
     152                 :            :   {
     153                 :       3989 :     rssStateSnapshot.time_index = situationSnapshot.time_index;
     154                 :       3989 :     rssStateSnapshot.default_ego_vehicle_rss_dynamics = situationSnapshot.default_ego_vehicle_rss_dynamics;
     155                 :       3989 :     rssStateSnapshot.individual_responses.clear();
     156                 :       3989 :     rssStateSnapshot.unstructured_constellation_ego_information.brake_trajectory_set.clear();
     157                 :       3989 :     rssStateSnapshot.unstructured_constellation_ego_information.continue_forward_trajectory_set.clear();
     158                 :            : 
     159   [ +  +  +  +  :      59062 :     for (auto it = situationSnapshot.constellations.begin(); (it != situationSnapshot.constellations.end()) && result;
                   +  + ]
     160                 :      55073 :          ++it)
     161                 :            :     {
     162         [ +  - ]:      55074 :       result = checkConstellationInputRangeChecked(*it, rssStateSnapshot);
     163                 :            :     }
     164                 :            :   }
     165                 :          0 :   catch (...)
     166                 :            :   {
     167   [ -  -  -  - ]:          0 :     core::getLogger()->critical("RssSituationChecking::checkConstellations>> Exception caught {}", situationSnapshot);
     168                 :          0 :     result = false;
     169                 :          0 :   }
     170         [ +  + ]:       3989 :   if (!result)
     171                 :            :   {
     172                 :         29 :     rssStateSnapshot.individual_responses.clear();
     173                 :            :   }
     174                 :       3989 :   return result;
     175                 :            : }
     176                 :            : 
     177                 :       4015 : bool RssSituationChecking::checkTimeIncreasingConsistently(world::TimeIndex const &nextTimeIndex)
     178                 :            : {
     179                 :       4015 :   bool timeIsIncreasing = false;
     180         [ +  + ]:       4015 :   if (mCurrentTimeIndex != nextTimeIndex)
     181                 :            :   {
     182                 :            :     // check for overflow
     183                 :       4011 :     world::TimeIndex const deltaTimeIndex = nextTimeIndex - mCurrentTimeIndex;
     184         [ +  + ]:       4011 :     if (deltaTimeIndex < (std::numeric_limits<world::TimeIndex>::max() / 2))
     185                 :            :     {
     186                 :       3990 :       timeIsIncreasing = true;
     187                 :            :     }
     188                 :            :   }
     189                 :       4015 :   mCurrentTimeIndex = nextTimeIndex;
     190                 :       4015 :   return timeIsIncreasing;
     191                 :            : }
     192                 :            : 
     193                 :            : } // namespace core
     194                 :            : } // namespace rss
     195                 :            : } // namespace ad