Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2019-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "ad/rss/map/Logging.hpp"
      10                 :            : #include "ad/rss/map/RssRouteCheckerData.hpp"
      11                 :            : #include "ad/rss/map/RssWorldModelCreator.hpp"
      12                 :            : 
      13                 :            : namespace ad {
      14                 :            : namespace rss {
      15                 :            : namespace map {
      16                 :            : 
      17                 :            : template <class OBJECT_INSTANCE_TYPE> class ParallelChecker
      18                 :            : {
      19                 :            : public:
      20                 :         11 :   ParallelChecker(RssRouteCheckerEgoVehicleDataMap<OBJECT_INSTANCE_TYPE> const &egoVehicleMap)
      21                 :         11 :     : mEgoVehicleMap(egoVehicleMap)
      22                 :            :   {
      23                 :         11 :   }
      24                 :            : 
      25                 :            :   ParallelChecker(ParallelChecker const &other) = delete;
      26                 :            :   ParallelChecker &operator=(ParallelChecker const &other) = delete;
      27                 :            : 
      28                 :         28 :   void analyseResultOfRoute(RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &ego_vehicle,
      29                 :            :                             RssRouteCheckResult &situationCheckResult) const
      30                 :            :   {
      31         [ +  - ]:         28 :     if (situationCheckResult.is_valid)
      32                 :            :     {
      33                 :         28 :       situationCheckResult.result_analysis.dangerous_state = false;
      34                 :         28 :       situationCheckResult.result_analysis.dangerous_vehicle = false;
      35                 :         28 :       situationCheckResult.result_analysis.dangerous_opposite_state = false;
      36                 :         28 :       situationCheckResult.result_analysis.vehicle_crossing_road_boundaries = false;
      37                 :         28 :       bool leftBorderIsDangerous = false;
      38                 :         28 :       bool rightBorderIsDangerous = false;
      39                 :         28 :       bool vehicleTriggeredLeftResponse = false;
      40                 :         28 :       bool vehicleTriggeredRightResponse = false;
      41                 :         28 :       bool vehicleTriggeredLongitudinalResponse = false;
      42         [ +  + ]:         75 :       for (auto const &state : situationCheckResult.rss_state_snapshot.individual_responses)
      43                 :            :       {
      44         [ +  + ]:         47 :         if (::ad::rss::state::isDangerous(state))
      45                 :            :         {
      46                 :         13 :           situationCheckResult.result_analysis.dangerous_state = true;
      47                 :            :           auto dangerousSitationIter
      48         [ +  - ]:         13 :             = std::find_if(situationCheckResult.rss_situation_snapshot.constellations.begin(),
      49                 :            :                            situationCheckResult.rss_situation_snapshot.constellations.end(),
      50                 :         15 :                            [&state](::ad::rss::core::RelativeConstellation const &constellation) {
      51                 :         15 :                              return constellation.constellation_id == state.constellation_id;
      52                 :            :                            });
      53         [ +  - ]:         13 :           if (dangerousSitationIter != situationCheckResult.rss_situation_snapshot.constellations.end())
      54                 :            :           {
      55         [ -  + ]:         13 :             if (dangerousSitationIter->object_id == ::ad::rss::structured::getRightBorderObjectId())
      56                 :            :             {
      57                 :          0 :               rightBorderIsDangerous = true;
      58   [ #  #  #  # ]:          0 :               if (state.lateral_state_right.rss_state_information.current_distance == ::ad::physics::Distance(0.))
      59                 :            :               {
      60   [ #  #  #  # ]:          0 :                 getLogger()->debug("RssRouteChecker::analyseResultOfRoute[{}:{}] right border is crossed",
      61                 :          0 :                                    ego_vehicle.mRssObjectData.id,
      62         [ #  # ]:          0 :                                    situationCheckResult.rss_route.route_id);
      63                 :          0 :                 situationCheckResult.result_analysis.vehicle_crossing_road_boundaries = true;
      64                 :            :               }
      65                 :            :             }
      66         [ -  + ]:         13 :             else if (dangerousSitationIter->object_id == ::ad::rss::structured::getLeftBorderObjectId())
      67                 :            :             {
      68                 :          0 :               leftBorderIsDangerous = true;
      69   [ #  #  #  # ]:          0 :               if (state.lateral_state_left.rss_state_information.current_distance == ::ad::physics::Distance(0.))
      70                 :            :               {
      71   [ #  #  #  # ]:          0 :                 getLogger()->debug("RssRouteChecker::analyseResultOfRoute[{}:{}] left border is crossed",
      72                 :          0 :                                    ego_vehicle.mRssObjectData.id,
      73         [ #  # ]:          0 :                                    situationCheckResult.rss_route.route_id);
      74                 :          0 :                 situationCheckResult.result_analysis.vehicle_crossing_road_boundaries = true;
      75                 :            :               }
      76                 :            :             }
      77                 :            :             else
      78                 :            :             {
      79                 :         13 :               situationCheckResult.result_analysis.dangerous_vehicle = true;
      80         [ +  + ]:         13 :               if (state.longitudinal_state.response != ::ad::rss::state::LongitudinalResponse::None)
      81                 :            :               {
      82                 :          9 :                 vehicleTriggeredLongitudinalResponse = true;
      83                 :            :               }
      84         [ +  + ]:         13 :               if (state.lateral_state_left.response != ::ad::rss::state::LateralResponse::None)
      85                 :            :               {
      86                 :          2 :                 vehicleTriggeredLeftResponse = true;
      87                 :            :               }
      88         [ +  + ]:         13 :               if (state.lateral_state_right.response != ::ad::rss::state::LateralResponse::None)
      89                 :            :               {
      90                 :          2 :                 vehicleTriggeredRightResponse = true;
      91                 :            :               }
      92                 :            :             }
      93         [ -  + ]:         13 :             if (dangerousSitationIter->constellation_type == ::ad::rss::world::ConstellationType::OppositeDirection)
      94                 :            :             {
      95                 :          0 :               situationCheckResult.result_analysis.dangerous_opposite_state = true;
      96                 :            :             }
      97                 :            :           }
      98                 :            :         }
      99                 :            :       }
     100                 :            : 
     101                 :            :       // border are restricting potentially too much, fix this
     102         [ +  + ]:         27 :       if (!vehicleTriggeredLongitudinalResponse
     103         [ -  + ]:         18 :           && (situationCheckResult.proper_response.longitudinal_response
     104                 :            :               != ::ad::rss::state::LongitudinalResponse::None))
     105                 :            :       {
     106         [ #  # ]:          0 :         getLogger()->debug(
     107                 :            :           "RssRouteChecker::analyseResultOfRoute[{}:{}] longitudinal_response only triggered by borders: ignore",
     108                 :          0 :           ego_vehicle.mRssObjectData.id,
     109         [ #  # ]:          0 :           situationCheckResult.rss_route.route_id);
     110                 :          0 :         situationCheckResult.proper_response.longitudinal_response = ::ad::rss::state::LongitudinalResponse::None;
     111                 :          0 :         situationCheckResult.proper_response.acceleration_restrictions.longitudinal_range.maximum
     112                 :            :           = ego_vehicle.mRssObjectData.rss_dynamics.alpha_lon.accel_max;
     113                 :            :       }
     114   [ +  +  +  - ]:         27 :       if (!vehicleTriggeredLeftResponse && !leftBorderIsDangerous
     115         [ -  + ]:         25 :           && (situationCheckResult.proper_response.lateral_response_left != ::ad::rss::state::LateralResponse::None))
     116                 :            :       {
     117         [ #  # ]:          0 :         getLogger()->debug(
     118                 :            :           "RssRouteChecker::analyseResultOfRoute[{}:{}] lateral_response_left only triggered by right border: ignore",
     119                 :          0 :           ego_vehicle.mRssObjectData.id,
     120         [ #  # ]:          0 :           situationCheckResult.rss_route.route_id);
     121                 :          0 :         situationCheckResult.proper_response.lateral_response_left = ::ad::rss::state::LateralResponse::None;
     122                 :          0 :         situationCheckResult.proper_response.acceleration_restrictions.lateral_left_range.maximum
     123                 :            :           = ego_vehicle.mRssObjectData.rss_dynamics.alpha_lat.accel_max;
     124                 :          0 :         situationCheckResult.result_analysis.vehicle_crossing_road_boundaries = true;
     125                 :            :       }
     126   [ +  +  +  - ]:         27 :       if (!vehicleTriggeredRightResponse && !rightBorderIsDangerous
     127         [ -  + ]:         25 :           && (situationCheckResult.proper_response.lateral_response_right != ::ad::rss::state::LateralResponse::None))
     128                 :            :       {
     129         [ #  # ]:          0 :         getLogger()->debug(
     130                 :            :           "RssRouteChecker::analyseResultOfRoute[{}:{}] lateral_response_right only triggered by left border: ignore",
     131                 :          0 :           ego_vehicle.mRssObjectData.id,
     132         [ #  # ]:          0 :           situationCheckResult.rss_route.route_id);
     133                 :          0 :         situationCheckResult.proper_response.lateral_response_right = ::ad::rss::state::LateralResponse::None;
     134                 :          0 :         situationCheckResult.proper_response.acceleration_restrictions.lateral_right_range.maximum
     135                 :            :           = ego_vehicle.mRssObjectData.rss_dynamics.alpha_lat.accel_max;
     136                 :          0 :         situationCheckResult.result_analysis.vehicle_crossing_road_boundaries = true;
     137                 :            :       }
     138                 :            : 
     139         [ +  + ]:         27 :       if (!situationCheckResult.proper_response.is_safe)
     140                 :            :       {
     141         [ +  - ]:         26 :         getLogger()->debug("RssRouteChecker::analyseResultOfRoute[{}:{}] Unsafe route: {}\n {}",
     142                 :         13 :                            ego_vehicle.mRssObjectData.id,
     143                 :         13 :                            situationCheckResult.rss_route.route_id,
     144         [ +  - ]:         13 :                            situationCheckResult.proper_response,
     145                 :            :                            situationCheckResult);
     146                 :            :       }
     147                 :            :       else
     148                 :            :       {
     149         [ +  - ]:         29 :         getLogger()->trace("RssRouteChecker::analyseResults[{}:{}] RouteResponse: {}\n {}",
     150                 :         15 :                            ego_vehicle.mRssObjectData.id,
     151                 :         15 :                            situationCheckResult.rss_route.route_id,
     152         [ +  - ]:         15 :                            situationCheckResult.proper_response,
     153                 :            :                            situationCheckResult);
     154                 :            :       }
     155                 :            :     }
     156                 :         28 :   }
     157                 :            : 
     158                 :         17 :   void operator()(RssObjectResult &objectResult) const
     159                 :            :   {
     160         [ +  - ]:         17 :     auto egoVehicleFindResult = mEgoVehicleMap.find(objectResult.object_data.id);
     161         [ -  + ]:         17 :     if (egoVehicleFindResult == mEgoVehicleMap.end())
     162                 :            :     {
     163   [ #  #  #  # ]:          0 :       getLogger()->error("RssRouteChecker::ParallelChecker[{}] unexpectedly did not find checker for object",
     164         [ #  # ]:          0 :                          objectResult.object_data.id);
     165                 :          0 :       return;
     166                 :            :     }
     167                 :         17 :     auto &egoVehicleData = egoVehicleFindResult->second;
     168                 :            : 
     169         [ +  + ]:         44 :     for (auto &situationCheckResult : objectResult.situation_check_results)
     170                 :            :     {
     171         [ +  - ]:         28 :       auto rssCheckFindResult = egoVehicleData.mRssRouteCheckMap.find(situationCheckResult.rss_route.route_id);
     172         [ -  + ]:         28 :       if (rssCheckFindResult == egoVehicleData.mRssRouteCheckMap.end())
     173                 :            :       {
     174   [ #  #  #  # ]:          0 :         getLogger()->error("RssRouteChecker::ParallelChecker[{}:{}] unexpectedly did not find checker for route",
     175                 :          0 :                            objectResult.object_data.id,
     176         [ #  # ]:          0 :                            situationCheckResult.rss_route.route_id);
     177                 :          0 :         continue;
     178                 :            :       }
     179                 :         28 :       auto &rssCheck = rssCheckFindResult->second;
     180                 :            : 
     181                 :         28 :       situationCheckResult.is_valid = rssCheck->mSituationExtraction.extractSituation(
     182         [ +  - ]:         28 :         situationCheckResult.world_model, situationCheckResult.rss_situation_snapshot);
     183         [ -  + ]:         28 :       if (!situationCheckResult.is_valid)
     184                 :            :       {
     185   [ #  #  #  # ]:          0 :         getLogger()->warn("RssRouteChecker::ParallelChecker[{}:{}] extractSituation failed {}!",
     186                 :          0 :                           objectResult.object_data.id,
     187                 :          0 :                           situationCheckResult.rss_route.route_id,
     188         [ #  # ]:          0 :                           situationCheckResult.world_model);
     189                 :            :       }
     190                 :            : 
     191         [ +  - ]:         28 :       if (situationCheckResult.is_valid)
     192                 :            :       {
     193                 :         28 :         situationCheckResult.is_valid = rssCheck->mSituationChecking.checkSituation(
     194         [ +  - ]:         28 :           situationCheckResult.rss_situation_snapshot, situationCheckResult.rss_state_snapshot);
     195         [ -  + ]:         28 :         if (!situationCheckResult.is_valid)
     196                 :            :         {
     197   [ #  #  #  # ]:          0 :           getLogger()->warn("RssRouteChecker::ParallelChecker[{}:{}] checkSituation failed {}!",
     198                 :          0 :                             objectResult.object_data.id,
     199                 :          0 :                             situationCheckResult.rss_route.route_id,
     200         [ #  # ]:          0 :                             situationCheckResult.rss_situation_snapshot);
     201                 :            :         }
     202                 :            :       }
     203                 :            : 
     204         [ +  - ]:         28 :       if (situationCheckResult.is_valid)
     205                 :            :       {
     206                 :         28 :         situationCheckResult.is_valid = rssCheck->mResponseResolving.provideProperResponse(
     207         [ +  - ]:         28 :           situationCheckResult.rss_state_snapshot, situationCheckResult.proper_response);
     208         [ -  + ]:         28 :         if (!situationCheckResult.is_valid)
     209                 :            :         {
     210   [ #  #  #  # ]:          0 :           getLogger()->warn("RssRouteChecker::ParallelChecker[{}:{}] provideProperResponse failed {}!",
     211                 :          0 :                             objectResult.object_data.id,
     212                 :          0 :                             situationCheckResult.rss_route.route_id,
     213         [ #  # ]:          0 :                             situationCheckResult.rss_state_snapshot);
     214                 :            :         }
     215                 :            :       }
     216                 :            : 
     217         [ +  - ]:         28 :       analyseResultOfRoute(egoVehicleData, situationCheckResult);
     218                 :            :     }
     219                 :            :   }
     220                 :            : 
     221                 :            : private:
     222                 :            :   RssRouteCheckerEgoVehicleDataMap<OBJECT_INSTANCE_TYPE> const &mEgoVehicleMap;
     223                 :            : };
     224                 :            : 
     225                 :            : } // namespace map
     226                 :            : } // namespace rss
     227                 :            : } // namespace ad