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                 :            : /**
      10                 :            :  * @file
      11                 :            :  */
      12                 :            : 
      13                 :            : #pragma once
      14                 :            : 
      15                 :            : #include "ad/rss/core/RelativeObjectState.hpp"
      16                 :            : #include "ad/rss/world/ObjectId.hpp"
      17                 :            : 
      18                 :            : /*!
      19                 :            :  * @brief namespace ad
      20                 :            :  */
      21                 :            : namespace ad {
      22                 :            : /*!
      23                 :            :  * @brief namespace rss
      24                 :            :  */
      25                 :            : namespace rss {
      26                 :            : /*!
      27                 :            :  * @brief namespace structured
      28                 :            :  */
      29                 :            : namespace structured {
      30                 :            : 
      31                 :            : /*!
      32                 :            :  * @returns the constant ObjectId of the virtual right border
      33                 :            :  */
      34                 :     171967 : static inline world::ObjectId getRightBorderObjectId()
      35                 :            : {
      36                 :     171967 :   return std::numeric_limits<world::ObjectId>::max();
      37                 :            : }
      38                 :            : 
      39                 :            : /*!
      40                 :            :  * @returns the constant ObjectId of the virtual left border
      41                 :            :  */
      42                 :     170273 : static inline world::ObjectId getLeftBorderObjectId()
      43                 :            : {
      44                 :     170273 :   return std::numeric_limits<world::ObjectId>::max() - 1;
      45                 :            : }
      46                 :            : 
      47                 :            : /**
      48                 :            :  * @brief Calculate the longitudinal distance offset of a object when applying the \a "stated braking pattern" with
      49                 :            :  *        given deceleration
      50                 :            :  *
      51                 :            :  * @param[in]  currentLongitudinalSpeed      is the current longitudinal object speed
      52                 :            :  * @param[in]  max_speed_on_acceleration          is the maximum object speed to be considered in accelerated movement
      53                 :            :  * @param[in]  response_time      is the response time of the object
      54                 :            :  * @param[in]  acceleration      the acceleration of the object during response_time
      55                 :            :  * @param[in]  deceleration      is the applied breaking deceleration
      56                 :            :  * @param[out] distanceOffset    is the distance offset of the object from the current position after
      57                 :            :  *                               \a "the stated braking pattern"
      58                 :            :  *
      59                 :            :  * @return true on successful calculation, false otherwise
      60                 :            :  */
      61                 :            : bool calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern(physics::Speed const &currentLongitudinalSpeed,
      62                 :            :                                                                   physics::Speed const &max_speed_on_acceleration,
      63                 :            :                                                                   physics::Duration const &response_time,
      64                 :            :                                                                   physics::Acceleration const &acceleration,
      65                 :            :                                                                   physics::Acceleration const &deceleration,
      66                 :            :                                                                   physics::Distance &distanceOffset);
      67                 :            : 
      68                 :            : /**
      69                 :            :  * @brief Calculate the lateral distance offset of a object when applying the \a "stated braking pattern" with given
      70                 :            :  *        deceleration
      71                 :            :  *
      72                 :            :  * @param[in]  currentLateralSpeed  is the current lateral object speed
      73                 :            :  * @param[in]  response_time      is the response time of the object
      74                 :            :  * @param[in]  acceleration      the acceleration of the object during response_time
      75                 :            :  * @param[in]  deceleration      is the applied breaking deceleration
      76                 :            :  * @param[out] distanceOffset    is the distance offset of the object from the current position after
      77                 :            :  *                               \a "the stated braking pattern"
      78                 :            :  *
      79                 :            :  * @return true on successful calculation, false otherwise
      80                 :            :  */
      81                 :            : bool calculateLateralDistanceOffsetAfterStatedBrakingPattern(physics::Speed const &currentLateralSpeed,
      82                 :            :                                                              physics::Duration const &response_time,
      83                 :            :                                                              physics::Acceleration const &acceleration,
      84                 :            :                                                              physics::Acceleration const &deceleration,
      85                 :            :                                                              physics::Distance &distanceOffset);
      86                 :            : /**
      87                 :            :  * @brief Calculate the \a "safe longitudinal distance" between the two objects,
      88                 :            :  *        Assuming: Maximum deceleration for leading object, and \a "stated breaking pattern" for following object
      89                 :            :  *
      90                 :            :  *        ======================================================
      91                 :            :  *
      92                 :            :  *             FollowingObject --->          LeadingObject --->
      93                 :            :  *
      94                 :            :  *        ======================================================
      95                 :            :  *
      96                 :            :  * @param[in]  leadingObject      is the state of the leading object
      97                 :            :  * @param[in]  followingObject    is the state of the following object
      98                 :            :  * @param[out] safe_distance        is the calculated safe longitudinal distance
      99                 :            :  *
     100                 :            :  * @return true on successful calculation, false otherwise
     101                 :            :  */
     102                 :            : bool calculateSafeLongitudinalDistanceSameDirection(core::RelativeObjectState const &leadingObject,
     103                 :            :                                                     core::RelativeObjectState const &followingObject,
     104                 :            :                                                     physics::Distance &safe_distance);
     105                 :            : 
     106                 :            : /**
     107                 :            :  * @brief Check if the longitudinal distance between the two objects is safe.
     108                 :            :  *        Assuming: Maximum deceleration for leading object, and \a "stated breaking pattern" for following object
     109                 :            :  *
     110                 :            :  *        ======================================================
     111                 :            :  *
     112                 :            :  *             FollowingObject --->          LeadingObject --->
     113                 :            :  *
     114                 :            :  *        ======================================================
     115                 :            :  *
     116                 :            :  * @param[in]  leadingObject      is the state of the leading object
     117                 :            :  * @param[in]  followingObject    is the state of the following object
     118                 :            :  * @param[in]  objectDistance     the (positive) longitudinal distance between the two objects
     119                 :            :  * @param[out] safe_distance        is the calculated safe longitudinal distance
     120                 :            :  * @param[out] isDistanceSafe      true if the distance is safe, false otherwise
     121                 :            :  *
     122                 :            :  * @return true on successful calculation, false otherwise
     123                 :            :  */
     124                 :            : bool checkSafeLongitudinalDistanceSameDirection(core::RelativeObjectState const &leadingObject,
     125                 :            :                                                 core::RelativeObjectState const &followingObject,
     126                 :            :                                                 physics::Distance const &objectDistance,
     127                 :            :                                                 physics::Distance &safe_distance,
     128                 :            :                                                 bool &isDistanceSafe);
     129                 :            : 
     130                 :            : /**
     131                 :            :  * @brief Calculate  the safe longitudinal distance between to objects driving in opposite direction
     132                 :            :  * The calculation will assume that the correctObject is on the correct lane
     133                 :            :  * and oppositeObject on an opposite lane
     134                 :            :  *
     135                 :            :  * So calculation for the correctObject is performed with brake_min_correct and performed with brake_min
     136                 :            :  * for the oppositeObject
     137                 :            :  *
     138                 :            :  *        ======================================================
     139                 :            :  *
     140                 :            :  *             correctObject --->        <---  oppositeObject
     141                 :            :  *
     142                 :            :  *        ======================================================
     143                 :            :  *
     144                 :            :  * @param[in]  correctObject     is the state of the object driving in the correct lane
     145                 :            :  * @param[in]  oppositeObject    is the state of the object driving in the wrong lane
     146                 :            :  * @param[out] isDistanceSafe     true if the distance is safe, false otherwise
     147                 :            :  *
     148                 :            :  * @return true on successful calculation, false otherwise
     149                 :            :  */
     150                 :            : bool calculateSafeLongitudinalDistanceOppositeDirection(core::RelativeObjectState const &correctObject,
     151                 :            :                                                         core::RelativeObjectState const &oppositeObject,
     152                 :            :                                                         physics::Distance &safe_distance);
     153                 :            : 
     154                 :            : /**
     155                 :            :  * @brief Check if the longitudinal distance between to objects driving in opposite direction is safe.
     156                 :            :  * The check will assume that the correctObject is on the correct lane
     157                 :            :  * and oppositeObject on an opposite lane
     158                 :            :  *
     159                 :            :  * So check for the correctVehcile is performed with brake_min_correct and performed with brake_min
     160                 :            :  * for the oppositeObject
     161                 :            :  *
     162                 :            :  *        ======================================================
     163                 :            :  *
     164                 :            :  *             correctObject --->        <---  oppositeObject
     165                 :            :  *
     166                 :            :  *        ======================================================
     167                 :            :  *
     168                 :            :  * @param[in]  correctObject     is the state of the object driving in the correct lane
     169                 :            :  * @param[in]  oppositeObject    is the state of the object driving in the wrong lane
     170                 :            :  * @param[in]  objectDistance    the (positive) longitudinal distance between the two objects
     171                 :            :  * @param[out] safe_distance        is the calculated safe longitudinal distance
     172                 :            :  * @param[out] isDistanceSafe     true if the distance is safe, false otherwise
     173                 :            :  *
     174                 :            :  * @return true on successful calculation, false otherwise
     175                 :            :  */
     176                 :            : bool checkSafeLongitudinalDistanceOppositeDirection(core::RelativeObjectState const &correctObject,
     177                 :            :                                                     core::RelativeObjectState const &oppositeObject,
     178                 :            :                                                     physics::Distance const &objectDistance,
     179                 :            :                                                     physics::Distance &safe_distance,
     180                 :            :                                                     bool &isDistanceSafe);
     181                 :            : 
     182                 :            : /**
     183                 :            :  * @brief Check if the object can safely break longitudinaly in front of the intersection.
     184                 :            :  *        Assuming: Using \a "stated breaking pattern" for breaking
     185                 :            :  *
     186                 :            :  *        ======================================================
     187                 :            :  *
     188                 :            :  *             object --->             |  Intersection
     189                 :            :  *                                      |
     190                 :            :  *        ==============================              ===============
     191                 :            :  *                                      |            |
     192                 :            :  *                                      |            |
     193                 :            :  *
     194                 :            :  * @param[in]  object      is the state of the object
     195                 :            :  * @param[out] safe_distance      the safe distance according to the stated braking pattern
     196                 :            :  * @param[out] isDistanceSafe      true if the distance is safe, false otherwise
     197                 :            :  *
     198                 :            :  * @return true on successful calculation, false otherwise
     199                 :            :  */
     200                 :            : bool checkStopInFrontIntersection(core::RelativeObjectState const &object,
     201                 :            :                                   physics::Distance &safe_distance,
     202                 :            :                                   bool &isDistanceSafe);
     203                 :            : 
     204                 :            : /**
     205                 :            :  * @brief Calculate the \a "safe lateral distance" between the two objects,
     206                 :            :  *        Assuming: Both objects apply \a "stated breaking pattern"
     207                 :            :  *
     208                 :            :  *        ======================================================
     209                 :            :  *
     210                 :            :  *             Left object  -->
     211                 :            :  *                 |
     212                 :            :  *                 |
     213                 :            :  *                 v
     214                 :            :  *
     215                 :            :  *                 ^
     216                 :            :  *                 |
     217                 :            :  *                 |
     218                 :            :  *             Right object -->
     219                 :            :  *
     220                 :            :  *        ======================================================
     221                 :            :  *
     222                 :            :  * @param[in]  leftObject      is the state of the left object
     223                 :            :  * @param[in]  rightObject     is the state of the right object
     224                 :            :  * @param[out] safe_distance     is the calculated safe lateral distance
     225                 :            :  *
     226                 :            :  * @return true on successful calculation, false otherwise
     227                 :            :  */
     228                 :            : bool calculateSafeLateralDistance(core::RelativeObjectState const &leftObject,
     229                 :            :                                   core::RelativeObjectState const &rightObject,
     230                 :            :                                   physics::Distance &safe_distance);
     231                 :            : 
     232                 :            : /**
     233                 :            :  * @brief Check if the lateral distance between to objects is safe
     234                 :            :  *        Assuming: Both objects apply \a "stated breaking pattern"
     235                 :            :  *
     236                 :            :  *        ======================================================
     237                 :            :  *
     238                 :            :  *             Left object  -->
     239                 :            :  *                 |
     240                 :            :  *                 |
     241                 :            :  *                 v
     242                 :            :  *
     243                 :            :  *                 ^
     244                 :            :  *                 |
     245                 :            :  *                 |
     246                 :            :  *             Right object -->
     247                 :            :  *
     248                 :            :  *        ======================================================
     249                 :            :  *
     250                 :            :  * @param[in]  leftObject      is the state of the left object
     251                 :            :  * @param[in]  rightObject     is the state of the right object
     252                 :            :  * @param[in]  objectDistance  the (positive) lateral distance between the two objects
     253                 :            :  * @param[out] safe_distance     is the calculated safe lateral distance
     254                 :            :  * @param[out] isDistanceSafe   is true if the distance is safe, false otherwise
     255                 :            :  *
     256                 :            :  * @return true on successful calculation, false otherwise
     257                 :            :  */
     258                 :            : bool checkSafeLateralDistance(core::RelativeObjectState const &leftObject,
     259                 :            :                               core::RelativeObjectState const &rightObject,
     260                 :            :                               physics::Distance const &objectDistance,
     261                 :            :                               physics::Distance &safe_distance,
     262                 :            :                               bool &isDistanceSafe);
     263                 :            : 
     264                 :            : } // namespace structured
     265                 :            : } // namespace rss
     266                 :            : } // namespace ad