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                 :            : #pragma once
       9                 :            : 
      10                 :            : #include <ad/geometry/Types.hpp>
      11                 :            : #include <ad/physics/Operation.hpp>
      12                 :            : #include <cmath>
      13                 :            : #include <gtest/gtest.h>
      14                 :            : #include <limits>
      15                 :            : #include "RssTestParameters.hpp"
      16                 :            : #include "ad/rss/core/Physics.hpp"
      17                 :            : #include "ad/rss/core/RelativeConstellation.hpp"
      18                 :            : #include "ad/rss/core/RelativeObjectState.hpp"
      19                 :            : #include "ad/rss/core/RelativePosition.hpp"
      20                 :            : #include "ad/rss/state/ProperResponse.hpp"
      21                 :            : #include "ad/rss/state/RssState.hpp"
      22                 :            : #include "ad/rss/world/Object.hpp"
      23                 :            : #include "ad/rss/world/RoadSegment.hpp"
      24                 :            : 
      25                 :            : namespace ad {
      26                 :            : namespace rss {
      27                 :            : 
      28                 :            : // make the code more readable
      29                 :            : using physics::Acceleration;
      30                 :            : using physics::Distance;
      31                 :            : using physics::Duration;
      32                 :            : using physics::ParametricValue;
      33                 :            : using physics::Speed;
      34                 :            : 
      35                 :            : const double cDoubleNear(0.01);
      36                 :            : 
      37                 :            : #define ARRAYLEN(a) (sizeof(a) / sizeof(a[0]))
      38                 :            : 
      39                 :            : /**
      40                 :            :  * @brief resets the RSS state to it's safe state
      41                 :            :  *
      42                 :            :  * @param[in/out] state the longitudinal RSS state to be reset
      43                 :            :  */
      44                 :            : void resetRssState(state::LongitudinalRssState &state);
      45                 :            : 
      46                 :            : /**
      47                 :            :  * @brief resets the RSS state to it's safe state
      48                 :            :  *
      49                 :            :  * @param[in/out] state the lateral RSS state to be reset
      50                 :            :  */
      51                 :            : void resetRssState(state::LateralRssState &state);
      52                 :            : 
      53                 :            : /**
      54                 :            :  * @brief resets the RSS state to it's safe state
      55                 :            :  *
      56                 :            :  * @param[in/out] state the unstructured RSS state to be reset
      57                 :            :  */
      58                 :            : void resetRssState(state::UnstructuredConstellationRssState &state);
      59                 :            : 
      60                 :            : /**
      61                 :            :  * @brief resets the RSS state within the RssState to it's safe state
      62                 :            :  *
      63                 :            :  * @param[in/out] rssState the response state to be reset
      64                 :            :  * @param[in] constellation_id the constellation id to be set within the RssState
      65                 :            :  * @param[in] object_id the object id to be set within the RssState
      66                 :            :  * @param[in] constellation_type the constellation type to be set within the RssState
      67                 :            :  */
      68                 :            : void resetRssState(state::RssState &rssState,
      69                 :            :                    core::RelativeConstellationId const constellation_id,
      70                 :            :                    world::ObjectId const object_id,
      71                 :            :                    world::ConstellationType const constellation_type);
      72                 :            : 
      73                 :            : /**
      74                 :            :  * @brief resets the RSS state of the proper response to it's safe state
      75                 :            :  *
      76                 :            :  * @param[in/out] properResponse the the proper response to be reset
      77                 :            :  */
      78                 :            : void resetRssState(state::ProperResponse &properResponse);
      79                 :            : 
      80                 :            : /**
      81                 :            :  * @brief sets the RSS state to a specific unsafe state
      82                 :            :  *
      83                 :            :  * @param[in/out] state the lateral/longitudinal RSS state to be set
      84                 :            :  * @param[in] literal the response literal of the respective RSS state to be set
      85                 :            :  */
      86                 :            : template <typename RSSState, typename RSSStateLiteral>
      87                 :         34 : void setRssStateUnsafe(RSSState &state, RSSStateLiteral const literal)
      88                 :            : {
      89                 :         34 :   state.response = literal;
      90                 :         34 :   state.is_safe = false;
      91                 :         34 : }
      92                 :            : 
      93                 :            : /**
      94                 :            :  * @brief converts km/h to m/sec
      95                 :            :  *
      96                 :            :  * @param[in] speed the speed provided in km/h
      97                 :            :  *
      98                 :            :  * @returns the speed as physics::Speed
      99                 :            :  */
     100                 :      51426 : inline physics::Speed kmhToMeterPerSec(double const speed)
     101                 :            : {
     102                 :      51426 :   return Speed(speed / 3.6);
     103                 :            : }
     104                 :            : 
     105                 :            : /**
     106                 :            :  * @brief convert an object into an ego vehicle object
     107                 :            :  *
     108                 :            :  * @param[in] iObject the object to be taken as basis
     109                 :            :  *
     110                 :            :  * @returns a copy of the object with object type EgoVehicle
     111                 :            :  */
     112                 :        846 : inline world::Object objectAsEgo(world::Object const &iObject)
     113                 :            : {
     114                 :        846 :   world::Object object(iObject);
     115                 :        846 :   object.object_type = world::ObjectType::EgoVehicle;
     116                 :        846 :   return object;
     117                 :            : }
     118                 :            : 
     119                 :            : /**
     120                 :            :  * @return RssDynamics used within tests
     121                 :            :  *
     122                 :            :  */
     123                 :            : world::RssDynamics getObjectRssDynamics();
     124                 :            : 
     125                 :            : /**
     126                 :            :  * @return RssDynamics used within tests
     127                 :            :  *
     128                 :            :  */
     129                 :            : world::RssDynamics getEgoRssDynamics();
     130                 :            : 
     131                 :            : /**
     132                 :            :  * @brief create an object
     133                 :            :  *
     134                 :            :  * @param[in] lonVelocity the longitudinal velocity to be applied
     135                 :            :  * @param[in] latVelocity the lateral velocity to be applied
     136                 :            :  *
     137                 :            :  * @returns an object with applied lon/lat velocities
     138                 :            :  */
     139                 :            : world::Object createObject(double const lonVelocity, double const latVelocity);
     140                 :            : 
     141                 :            : /**
     142                 :            :  * @brief create a vehicle state
     143                 :            :  *
     144                 :            :  * @param[in] object_type type of object
     145                 :            :  * @param[in] lonVelocity the longitudinal velocity to be applied
     146                 :            :  * @param[in] latVelocity the lateral velocity to be applied
     147                 :            :  *
     148                 :            :  * @returns a vehicle state with applied lon/lat velocities
     149                 :            :  */
     150                 :            : core::RelativeObjectState
     151                 :            : createRelativeVehicleState(world::ObjectType const object_type, double const lonVelocity, double const latVelocity);
     152                 :            : 
     153                 :            : /**
     154                 :            :  * @brief create an object state
     155                 :            :  *
     156                 :            :  * @param[in] lonVelocity the longitudinal velocity to be applied
     157                 :            :  * @param[in] latVelocity the lateral velocity to be applied
     158                 :            :  *
     159                 :            :  * @returns an object state with applied lon/lat velocities
     160                 :            :  */
     161                 :            : world::ObjectState createObjectState(double const lonVelocity, double const latVelocity);
     162                 :            : 
     163                 :            : /**
     164                 :            :  * @brief create a vehicle state for longitudianl motion
     165                 :            :  *
     166                 :            :  * lateral velocity becomes 0.
     167                 :            :  *
     168                 :            :  * @param[in] lonVelocity the longitudinal velocity to be applied
     169                 :            :  *
     170                 :            :  * @returns a vehicle state with applied lon velocity
     171                 :            :  */
     172                 :        159 : inline core::RelativeObjectState createRelativeVehicleStateForLongitudinalMotion(double const lonVelocity)
     173                 :            : {
     174                 :        159 :   return createRelativeVehicleState(world::ObjectType::OtherVehicle, lonVelocity, 0.);
     175                 :            : }
     176                 :            : 
     177                 :            : /**
     178                 :            :  * @brief create a vehicle state for longitudianl motion
     179                 :            :  *
     180                 :            :  * longitudinal velocity becomes 0.
     181                 :            :  *
     182                 :            :  * @param[in] latVelocity the lateral velocity to be applied
     183                 :            :  *
     184                 :            :  * @returns a vehicle state with applied lat velocity
     185                 :            :  */
     186                 :         46 : inline core::RelativeObjectState createRelativeVehicleStateForLateralMotion(double const latVelocity)
     187                 :            : {
     188                 :         46 :   return createRelativeVehicleState(world::ObjectType::OtherVehicle, 0., latVelocity);
     189                 :            : }
     190                 :            : 
     191                 :            : /**
     192                 :            :  * @brief create a relative longitudinal position object
     193                 :            :  *
     194                 :            :  * @param[in] position the longitudinal relative position to be applied
     195                 :            :  * @param[in] distance the distance to be applied
     196                 :            :  *
     197                 :            :  * @returns a relative longitudinal position object
     198                 :            :  */
     199                 :            : core::RelativePosition createRelativeLongitudinalPosition(core::LongitudinalRelativePosition const &position,
     200                 :            :                                                           Distance const &distance = Distance(0.));
     201                 :            : 
     202                 :            : /**
     203                 :            :  * @brief create a relative lateral position object
     204                 :            :  *
     205                 :            :  * @param[in] position the lateral relative position to be applied
     206                 :            :  * @param[in] distance the distance to be applied
     207                 :            :  *
     208                 :            :  * @returns a relative lateral position object
     209                 :            :  */
     210                 :            : core::RelativePosition createRelativeLateralPosition(core::LateralRelativePosition const &position,
     211                 :            :                                                      Distance const &distance = Distance(0.));
     212                 :            : 
     213                 :            : /**
     214                 :            :  * @brief calculate the longitudinal stopping distance
     215                 :            :  *
     216                 :            :  * @param[in] objectSpeed the objectSpeed used for the calculation
     217                 :            :  * @param[in] objectSpeed the object maximum speed used for the calculation
     218                 :            :  * @param[in] acceleration the acceleration to be applied for acceleration within response time
     219                 :            :  * @param[in] deceleration the deceleration to be applied for braking
     220                 :            :  * @param[in] response_time the response_time to be applied before braking
     221                 :            :  *
     222                 :            :  * @returns the distance required to stop when applying accel_max during the response_time and then brake to the full
     223                 :            :  * stop
     224                 :            :  */
     225                 :            : Distance calculateLongitudinalStoppingDistance(Speed const &objectSpeed,
     226                 :            :                                                Speed const &objectMaxSpeedOnAcceleration,
     227                 :            :                                                Acceleration const &acceleration,
     228                 :            :                                                Acceleration const &deceleration,
     229                 :            :                                                Duration const &response_time);
     230                 :            : 
     231                 :            : /**
     232                 :            :  * @brief calculate the longitudinal min safe distance in following >>> leading configuration
     233                 :            :  *
     234                 :            :  * @param[in] followingObjectSpeed the object following at back: velocity
     235                 :            :  * @param[in] followingObjectRssDynamics the object following at back: RSS dynamics
     236                 :            :  * @param[in] leadingObjectSpeed the object leading in front: velocity
     237                 :            :  * @param[in] leadingObjectRssDynamics the object leading in front: RSS dynamics
     238                 :            :  *
     239                 :            :  * @returns the minimum safe distance required that the following object does not crash into the leading object
     240                 :            :  *  in case the leading object performs a brake with brake_max and following object performs a stated braking pattern
     241                 :            :  *  with (see calculateLongitudinalStoppingDistance) with brake_min
     242                 :            :  */
     243                 :            : Distance calculateLongitudinalMinSafeDistance(physics::Speed const &followingObjectSpeed,
     244                 :            :                                               world::RssDynamics const &followingObjectRssDynamics,
     245                 :            :                                               physics::Speed const &leadingObjectSpeed,
     246                 :            :                                               world::RssDynamics const &leadingObjectRssDynamics);
     247                 :            : 
     248                 :            : /**
     249                 :            :  * @brief calculate the longitudinal min safe distance in opposite direction configuration with
     250                 :            :  *   one object in correct lane and the other in the wrong
     251                 :            :  *
     252                 :            :  * @param[in] objectInCorrectLaneSpeed the object driving in the correct lane direction: velocity
     253                 :            :  * @param[in] objectInCorrectLaneRssDynamics the object driving in the correct lane direction: RSS dynamics
     254                 :            :  * @param[in] objectNotInCorrectLaneSpeed the object driving in the wrong lane: velocity
     255                 :            :  * @param[in] objectNotInCorrectLaneRssDynamics the object driving in the wrong lane: RSS dynamics
     256                 :            :  *
     257                 :            :  * @returns the minimum safe distance required that both objects are still able to break and not crash into each other.
     258                 :            :  *  The object in correct lane performs a stated braking pattern (see calculateLongitudinalStoppingDistance) with
     259                 :            :  * brake_min_correct;
     260                 :            :  *  the object in wrong lane performs a stated braking pattern with brake_min.
     261                 :            :  */
     262                 :            : Distance
     263                 :            : calculateLongitudinalMinSafeDistanceOppositeDirection(physics::Speed const &objectInCorrectLaneSpeed,
     264                 :            :                                                       world::RssDynamics const &objectInCorrectLaneRssDynamics,
     265                 :            :                                                       physics::Speed const &objectNotInCorrectLaneSpeed,
     266                 :            :                                                       world::RssDynamics const &objectNotInCorrectLaneRssDynamics);
     267                 :            : 
     268                 :            : /**
     269                 :            :  * @brief calculate the lateral min safe distance of two objects
     270                 :            :  *
     271                 :            :  * @param[in] leftObjectSpeed the object driving on the left side: velocity
     272                 :            :  * @param[in] leftObjectRssDynamics the object driving on the left side: RSS dynamics
     273                 :            :  * @param[in] rightObjectSpeed the object driving on the right side: velocity
     274                 :            :  * @param[in] rightObjectRssDynamics the object driving on the right side: RSS dynamics
     275                 :            :  *
     276                 :            :  * @returns the minimum safe distance required that both objects are still able to break and not crash into each other.
     277                 :            :  *  Both objects perform a stated braking pattern with brake_min in lateral direction.
     278                 :            :  */
     279                 :            : Distance calculateLateralMinSafeDistance(physics::Speed const &leftObjectSpeed,
     280                 :            :                                          world::RssDynamics const &leftObjectRssDynamics,
     281                 :            :                                          physics::Speed const &rightObjectSpeed,
     282                 :            :                                          world::RssDynamics const &rightObjectRssDynamics);
     283                 :            : 
     284                 :            : /**
     285                 :            :  * @brief update the vehicle state and unstructured constellation state info
     286                 :            :  *
     287                 :            :  * @param[in] backLeft position of the vehicle
     288                 :            :  * @param[in] positiveDirection direction selection
     289                 :            :  * @param[out] stateInfo unstructured state info
     290                 :            :  * @param[out] vehicleState resulting vehicle state
     291                 :            :  */
     292                 :            : void getUnstructuredVehicle(::ad::geometry::Point const &center_point,
     293                 :            :                             bool positiveDirection,
     294                 :            :                             state::UnstructuredConstellationStateInformation &stateInfo,
     295                 :            :                             core::RelativeObjectState &vehicleState);
     296                 :            : 
     297                 :            : /**
     298                 :            :  * @brief class providing constants for longitudinal/lateral RSS states
     299                 :            :  *
     300                 :            :  * The respective states can be augmented with expected constellation specific ResponseInformation data (see
     301                 :            :  * stateWithInformation() calls).
     302                 :            :  */
     303                 :            : class TestSupport
     304                 :            : {
     305                 :            : public:
     306                 :            :   /**
     307                 :            :    * @brief constructor
     308                 :            :    */
     309                 :            :   TestSupport();
     310                 :            : 
     311                 :            :   /**
     312                 :            :    * @brief augment a lateral RSS state with constellation specific ResponseInformation data
     313                 :            :    *
     314                 :            :    * @param[in] lateralState the lateral RSS state to be augmented by ResponseInformation
     315                 :            :    * @param[in] constellation the constellation used as basis for calculation of expected ResponseInformation data
     316                 :            :    *
     317                 :            :    * @returns the augmented RSS state is returned
     318                 :            :    */
     319                 :            :   static state::LateralRssState stateWithInformation(state::LateralRssState const &lateralState,
     320                 :            :                                                      core::RelativeConstellation const &constellation);
     321                 :            : 
     322                 :            :   /**
     323                 :            :    * @brief augment a longitudinal RSS state with constellation specific ResponseInformation data
     324                 :            :    *
     325                 :            :    * @param[in] longitudinal_state the longitudinal RSS state to be augmented by ResponseInformation
     326                 :            :    * @param[in] constellation the constellation used as basis for calculation of expected ResponseInformation data
     327                 :            :    *
     328                 :            :    * @returns the augmented RSS state is returned
     329                 :            :    */
     330                 :            :   static state::LongitudinalRssState stateWithInformation(state::LongitudinalRssState const &longitudinal_state,
     331                 :            :                                                           core::RelativeConstellation const &constellation);
     332                 :            : 
     333                 :            :   state::LongitudinalRssState cLongitudinalSafe;
     334                 :            :   state::LongitudinalRssState cLongitudinalNone;
     335                 :            :   state::LongitudinalRssState cLongitudinalBrakeMin;
     336                 :            :   state::LongitudinalRssState cLongitudinalBrakeMinCorrect;
     337                 :            :   state::LateralRssState cLateralSafe;
     338                 :            :   state::LateralRssState cLateralNone;
     339                 :            :   state::LateralRssState cLateralBrakeMin;
     340                 :            : };
     341                 :            : 
     342                 :            : extern const TestSupport cTestSupport;
     343                 :            : 
     344                 :          1 : inline world::RoadSegment longitudinalNoDifferenceRoadSegment()
     345                 :            : {
     346                 :          1 :   world::RoadSegment roadSegment;
     347                 :          1 :   world::LaneSegment laneSegment;
     348                 :            : 
     349                 :          1 :   laneSegment.id = 1;
     350                 :          1 :   laneSegment.length.minimum = Distance(10);
     351                 :          1 :   laneSegment.length.maximum = Distance(10);
     352                 :            : 
     353                 :          1 :   laneSegment.width.minimum = Distance(5);
     354                 :          1 :   laneSegment.width.maximum = Distance(5);
     355                 :            : 
     356         [ +  - ]:          1 :   roadSegment.lane_segments.push_back(laneSegment);
     357                 :          2 :   return roadSegment;
     358                 :          0 : }
     359                 :            : 
     360                 :          1 : inline world::RoadSegment longitudinalDifferenceRoadSegment()
     361                 :            : {
     362                 :          1 :   world::RoadSegment roadSegment;
     363                 :          1 :   world::LaneSegment laneSegment;
     364                 :            : 
     365                 :          1 :   laneSegment.id = 1;
     366                 :          1 :   laneSegment.length.minimum = Distance(5);
     367                 :          1 :   laneSegment.length.maximum = Distance(10);
     368                 :            : 
     369                 :          1 :   laneSegment.width.minimum = Distance(5);
     370                 :          1 :   laneSegment.width.maximum = Distance(5);
     371                 :            : 
     372         [ +  - ]:          1 :   roadSegment.lane_segments.push_back(laneSegment);
     373                 :          2 :   return roadSegment;
     374                 :          0 : }
     375                 :            : 
     376                 :            : } // namespace rss
     377                 :            : } // namespace ad