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/RssResponseResolving.hpp"
      10                 :            : #include <algorithm>
      11                 :            : #include "ad/rss/core/Logging.hpp"
      12                 :            : #include "ad/rss/state/RssStateOperation.hpp"
      13                 :            : #include "ad/rss/state/RssStateSnapshotValidInputRange.hpp"
      14                 :            : #include "ad/rss/structured/RssFormulas.hpp"
      15                 :            : 
      16                 :            : namespace ad {
      17                 :            : namespace rss {
      18                 :            : namespace core {
      19                 :            : 
      20                 :       3886 : bool RssResponseResolving::provideProperResponse(state::RssStateSnapshot const &currentStateSnapshot,
      21                 :            :                                                  state::ProperResponse &response)
      22                 :            : {
      23         [ +  + ]:       3886 :   if (!withinValidInputRange(currentStateSnapshot))
      24                 :            :   {
      25         [ +  - ]:          1 :     core::getLogger()->error("RssResponseResolving::provideProperResponse>> Invalid input");
      26                 :          1 :     return false;
      27                 :            :   }
      28                 :            : 
      29                 :       3885 :   bool result = true;
      30                 :            :   // global try catch block to ensure this library call doesn't throw an exception
      31                 :            :   try
      32                 :            :   {
      33                 :       3885 :     response.time_index = currentStateSnapshot.time_index;
      34                 :       3885 :     response.is_safe = true;
      35                 :       3885 :     response.dangerous_objects.clear();
      36                 :       3885 :     response.longitudinal_response = state::LongitudinalResponse::None;
      37                 :       3885 :     response.lateral_response_left = state::LateralResponse::None;
      38                 :       3885 :     response.lateral_response_right = state::LateralResponse::None;
      39                 :       3885 :     response.unstructured_constellation_response = state::UnstructuredConstellationResponse::None;
      40                 :            : 
      41                 :            :     // absolute maxima are given by the default dynamics
      42                 :            :     response.acceleration_restrictions.longitudinal_range.maximum
      43                 :       3885 :       = currentStateSnapshot.default_ego_vehicle_rss_dynamics.alpha_lon.accel_max;
      44                 :            :     response.acceleration_restrictions.lateral_left_range.maximum
      45                 :       3885 :       = currentStateSnapshot.default_ego_vehicle_rss_dynamics.alpha_lat.accel_max;
      46                 :            :     response.acceleration_restrictions.lateral_right_range.maximum
      47                 :       3885 :       = currentStateSnapshot.default_ego_vehicle_rss_dynamics.alpha_lat.accel_max;
      48                 :            : 
      49                 :            :     // absolute minimum in longitudinal direction is given by brake_max
      50                 :            :     response.acceleration_restrictions.longitudinal_range.minimum
      51                 :       3885 :       = currentStateSnapshot.default_ego_vehicle_rss_dynamics.alpha_lon.brake_max;
      52                 :            : 
      53                 :            :     // in lateral dimension, this is handled differently
      54                 :            :     // because of the explicit split into left and right direction
      55                 :            :     //
      56                 :            :     // lowest() value used for minimum because, if the vehicle is driving to the left
      57                 :            :     // it can decelerate the movement to the left in an unbounded manner.
      58                 :            :     // At the point the vehicle direction is turning, the right restrictions are becoming valid.
      59                 :            :     //
      60                 :            :     // @todo: ideally we should try to come to a closed description in here, so that
      61                 :            :     // we only have ONE lateral acceleration range combining the whole result
      62                 :            :     // This will make it easier outside
      63                 :            :     // But: in this case we have to ensure that the orientation of the constellation
      64                 :            :     // is respected accordingly, as especially within intersections this might differ depending on the driven route!
      65                 :            :     // Furthermore, we might need to have some knowledge on the expected cycle time,
      66                 :            :     // to be able to restrict counter-steering at the right point in time BEFORE the turn over
      67                 :            :     // the minimum therefore, would then change dynamically while counter steering...
      68                 :            :     //
      69                 :            :     // Currently the final transformation back has to be performed outside which requires
      70                 :            :     // deeper understanding there; so the above is more or less up to the system integrator
      71                 :            :     // to calculate while transforming back from vehicle coordinates to control data
      72                 :            :     //
      73                 :            :     // => Would make it much easier to integrate in the outside if we can solve this internally in
      74                 :            :     // a robust and correct manner
      75                 :            :     response.acceleration_restrictions.lateral_left_range.minimum
      76                 :       3885 :       = std::numeric_limits<physics::Acceleration>::lowest();
      77                 :            :     response.acceleration_restrictions.lateral_right_range.minimum
      78                 :       3885 :       = std::numeric_limits<physics::Acceleration>::lowest();
      79                 :       3885 :     response.heading_ranges.clear();
      80                 :       3885 :     physics::Acceleration driveAwayBrakeMin = currentStateSnapshot.default_ego_vehicle_rss_dynamics.alpha_lon.accel_max;
      81                 :       3885 :     bool unstructuredDriveAwayToBrakeTransitionOccured = false;
      82                 :            : 
      83         [ +  + ]:       8479 :     for (auto const &currentState : currentStateSnapshot.individual_responses)
      84                 :            :     {
      85         [ +  + ]:       4594 :       if (isDangerous(currentState))
      86                 :            :       {
      87                 :       1529 :         bool const isLaneBoundariesObject = (currentState.object_id == structured::getLeftBorderObjectId())
      88   [ +  -  -  + ]:       1529 :           || (currentState.object_id == structured::getRightBorderObjectId());
      89                 :       1529 :         response.is_safe = false;
      90         [ +  - ]:       1529 :         if (std::find(response.dangerous_objects.begin(), response.dangerous_objects.end(), currentState.object_id)
      91         [ +  - ]:       3058 :             == response.dangerous_objects.end())
      92                 :            :         {
      93         [ +  - ]:       1529 :           response.dangerous_objects.push_back(currentState.object_id);
      94                 :            :         }
      95                 :            : 
      96         [ +  + ]:       1529 :         if (currentState.constellation_type == world::ConstellationType::Unstructured)
      97                 :            :         {
      98   [ +  -  +  - ]:          4 :           core::getLogger()->info("RssResponseResolving::provideProperResponse>> Unstructured state is dangerous: {}",
      99                 :            :                                   currentState);
     100                 :          2 :           combineState(currentState.unstructured_constellation_state,
     101                 :            :                        driveAwayBrakeMin,
     102                 :            :                        unstructuredDriveAwayToBrakeTransitionOccured,
     103                 :          2 :                        response.unstructured_constellation_response,
     104                 :          2 :                        response.heading_ranges,
     105         [ +  - ]:          2 :                        response.acceleration_restrictions.longitudinal_range);
     106                 :            :         }
     107                 :            :         else // structured
     108                 :            :         {
     109   [ +  -  +  - ]:       3054 :           core::getLogger()->info("RssResponseResolving::provideProperResponse>> Structured state is dangerous: {}",
     110                 :            :                                   currentState);
     111                 :            : 
     112                 :       1527 :           combineState(currentState.longitudinal_state,
     113                 :       1527 :                        response.longitudinal_response,
     114         [ +  - ]:       1527 :                        response.acceleration_restrictions.longitudinal_range);
     115                 :            : 
     116                 :            :           // we might need to check here if left or right is the dangerous side
     117                 :            :           // but for the combineLateralResponse will only respect the more severe response
     118                 :            :           // omitting the check should have the same result
     119                 :       1527 :           combineState(currentState.lateral_state_left,
     120                 :       1527 :                        response.lateral_response_left,
     121         [ +  - ]:       1527 :                        response.acceleration_restrictions.lateral_left_range,
     122                 :            :                        isLaneBoundariesObject);
     123                 :            : 
     124                 :       1527 :           combineState(currentState.lateral_state_right,
     125                 :       1527 :                        response.lateral_response_right,
     126         [ +  - ]:       1527 :                        response.acceleration_restrictions.lateral_right_range,
     127                 :            :                        isLaneBoundariesObject);
     128                 :            :         }
     129                 :            :       }
     130                 :            :     }
     131                 :            : 
     132         [ -  + ]:       3885 :     if (unstructuredDriveAwayToBrakeTransitionOccured
     133         [ #  # ]:          0 :         && (response.unstructured_constellation_response == state::UnstructuredConstellationResponse::DriveAway))
     134                 :            :     {
     135                 :          0 :       response.unstructured_constellation_response = state::UnstructuredConstellationResponse::Brake;
     136                 :            :       // brake should already be applied because at least one of the steering angles should have not been within the
     137                 :            :       // respective range to be on the safe side
     138                 :            :       response.acceleration_restrictions.longitudinal_range.maximum
     139         [ #  # ]:          0 :         = std::min(response.acceleration_restrictions.longitudinal_range.maximum, driveAwayBrakeMin);
     140                 :            :     }
     141                 :            :   }
     142         [ -  - ]:          0 :   catch (std::exception &e)
     143                 :            :   {
     144   [ -  -  -  - ]:          0 :     core::getLogger()->critical(
     145         [ -  - ]:          0 :       "RssResponseResolving::provideProperResponse>> Exception caught'{}' {}", e.what(), currentStateSnapshot);
     146                 :          0 :     result = false;
     147                 :          0 :   }
     148                 :          0 :   catch (...)
     149                 :            :   {
     150   [ -  -  -  - ]:          0 :     core::getLogger()->critical("RssResponseResolving::provideProperResponse>> Exception caught {}",
     151                 :            :                                 currentStateSnapshot);
     152                 :          0 :     result = false;
     153                 :          0 :   }
     154                 :            : 
     155                 :       3885 :   return result;
     156                 :            : }
     157                 :            : 
     158                 :            : } // namespace core
     159                 :            : } // namespace rss
     160                 :            : } // namespace ad