Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2020-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : /**
       9                 :            :  * @file
      10                 :            :  */
      11                 :            : 
      12                 :            : #pragma once
      13                 :            : 
      14                 :            : #include <ad/geometry/GeometryOperation.hpp>
      15                 :            : #include <ad/geometry/Types.hpp>
      16                 :            : #include <ad/physics/RatioValue.hpp>
      17                 :            : #include "ad/rss/core/RelativeObjectState.hpp"
      18                 :            : #include "ad/rss/unstructured/TrajectoryCommon.hpp"
      19                 :            : 
      20                 :            : /*!
      21                 :            :  * @brief namespace ad
      22                 :            :  */
      23                 :            : namespace ad {
      24                 :            : /*!
      25                 :            :  * @brief namespace rss
      26                 :            :  */
      27                 :            : namespace rss {
      28                 :            : /*!
      29                 :            :  * @brief namespace unstructured
      30                 :            :  */
      31                 :            : namespace unstructured {
      32                 :            : 
      33                 :            : /**
      34                 :            :  * @brief Calculates the trajectory sets of a pedestrian
      35                 :            :  */
      36                 :            : class TrajectoryPedestrian
      37                 :            : {
      38                 :            : public:
      39                 :            :   /**
      40                 :            :    * @brief The maximum radius before assuming forward movement
      41                 :            :    */
      42                 :            :   static const physics::Distance maxRadius;
      43                 :            : 
      44                 :            :   /**
      45                 :            :    * @brief Step width while calculating points on circle
      46                 :            :    */
      47                 :            :   static const physics::Angle circleStepWidth;
      48                 :            : 
      49                 :          0 :   TrajectoryPedestrian()
      50                 :            :   {
      51                 :          0 :   }
      52                 :            : 
      53                 :            :   /**
      54                 :            :    * @brief Calculate the trajectory sets for braking and continue forward behavior
      55                 :            :    *
      56                 :            :    * @param[in]  vehicleState           current state of the pedestrian
      57                 :            :    * @param[out] brakePolygon           the trajectory set for braking behavior
      58                 :            :    * @param[out] continueForwardPolygon the trajectory set for continue-forward behavior
      59                 :            :    *
      60                 :            :    * @returns false if a failure occurred during calculations, true otherwise
      61                 :            :    */
      62                 :            :   bool calculateTrajectorySets(core::RelativeObjectState const &vehicleState,
      63                 :            :                                ::ad::geometry::Polygon &brakePolygon,
      64                 :            :                                ::ad::geometry::Polygon &continueForwardPolygon);
      65                 :            : 
      66                 :            : private:
      67                 :            :   /**
      68                 :            :    * @brief Calculate all trajectory points at response time
      69                 :            :    *
      70                 :            :    * @param[in]  vehicleState current state of the vehicle
      71                 :            :    * @param[out] frontSide the trajectory points defining the front
      72                 :            :    * @param[out] backSide the trajectory points defining the back
      73                 :            :    *
      74                 :            :    * @returns false if a failure occurred during calculations, true otherwise
      75                 :            :    */
      76                 :            :   bool getResponseTimeTrajectoryPoints(core::RelativeObjectState const &vehicleState,
      77                 :            :                                        TrajectorySetStep &frontSide,
      78                 :            :                                        TrajectorySetStep &backSide) const;
      79                 :            : 
      80                 :            :   /**
      81                 :            :    * @brief Calculate a single trajectory point
      82                 :            :    *
      83                 :            :    * @param[inout] currentPoint     point to use for calculation
      84                 :            :    * @param[in]  dynamics           current dynamics of the vehicle
      85                 :            :    * @param[in]  duration           duration of movement
      86                 :            :    * @param[in]  acceleration       acceleration to apply
      87                 :            :    * @param[in]  yawRateChangeRatio heading change ratio
      88                 :            :    *
      89                 :            :    * @returns false if a failure occurred during calculations, true otherwise
      90                 :            :    */
      91                 :            :   bool calculateTrajectoryPoint(TrajectoryPoint &currentPoint,
      92                 :            :                                 world::RssDynamics const &dynamics,
      93                 :            :                                 physics::Duration const &duration,
      94                 :            :                                 ad::physics::Acceleration const &acceleration,
      95                 :            :                                 ad::physics::RatioValue const &yawRateChangeRatio) const;
      96                 :            : 
      97                 :            :   /**
      98                 :            :    * @brief Calculate a single trajectory point
      99                 :            :    *
     100                 :            :    * @param[inout] currentPoint     point to use for calculation
     101                 :            :    * @param[in]  dynamics           current dynamics of the vehicle
     102                 :            :    * @param[in]  distance           distance to move
     103                 :            :    * @param[in]  yawRateChangeRatio heading change ratio
     104                 :            :    *
     105                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     106                 :            :    */
     107                 :            :   void calculateTrajectoryPoint(TrajectoryPoint &currentPoint,
     108                 :            :                                 world::RssDynamics const &dynamics,
     109                 :            :                                 physics::Distance const &distance,
     110                 :            :                                 ad::physics::RatioValue const &angleChangeRatio) const;
     111                 :            : 
     112                 :            :   /**
     113                 :            :    * @brief Calculate a point on a straight line
     114                 :            :    *
     115                 :            :    * @param[in]  currentPoint point at response time
     116                 :            :    * @param[in]  distance     distance of the final point
     117                 :            :    * @param[out] resultPoint  resulting point
     118                 :            :    */
     119                 :            :   void calculateTrajectoryPointStraight(TrajectoryPoint const &currentPoint,
     120                 :            :                                         physics::Distance const &distance,
     121                 :            :                                         TrajectoryPoint &resultPoint) const;
     122                 :            : 
     123                 :            :   /**
     124                 :            :    * @brief Calculate points of trajectory set step on a straight line
     125                 :            :    *
     126                 :            :    * @param[in]  distance     distance of the final points
     127                 :            :    * @param[in]  step         current step
     128                 :            :    * @param[out] resultStep   resulting step
     129                 :            :    */
     130                 :            :   void calculateTrajectoryPointsStraight(physics::Distance const &distance,
     131                 :            :                                          TrajectorySetStep const &step,
     132                 :            :                                          TrajectorySetStep &resultStep) const;
     133                 :            : 
     134                 :            :   /**
     135                 :            :    * @brief Calculate a side polygon
     136                 :            :    *
     137                 :            :    * @param[in]  vehicleState          current state of the vehicle
     138                 :            :    * @param[in]  finalPointMin         minimal point
     139                 :            :    * @param[in]  finalPointMax         maximum point
     140                 :            :    *
     141                 :            :    * @returns polygon describing the side
     142                 :            :    */
     143                 :            :   ::ad::geometry::Polygon calculateSidePolygon(core::RelativeObjectState const &vehicleState,
     144                 :            :                                                TrajectoryPoint const &finalPointMin,
     145                 :            :                                                TrajectoryPoint const &finalPointMax) const;
     146                 :            : 
     147                 :            :   /**
     148                 :            :    * @brief Calculate the trajectory sets if pedestrian stands still
     149                 :            :    *
     150                 :            :    * @param[in]  vehicleState           current state of the pedestrian
     151                 :            :    * @param[in]  timeToStop             time to step with stated braking pattern
     152                 :            :    * @param[out] brakePolygon           the trajectory set for braking behavior
     153                 :            :    * @param[out] continueForwardPolygon the trajectory set for continue-forward behavior
     154                 :            :    *
     155                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     156                 :            :    */
     157                 :            :   bool calculateTrajectorySetsStandingStill(core::RelativeObjectState const &vehicleState,
     158                 :            :                                             physics::Duration const &timeToStop,
     159                 :            :                                             ::ad::geometry::Polygon &brakePolygon,
     160                 :            :                                             ::ad::geometry::Polygon &continueForwardPolygon) const;
     161                 :            : 
     162                 :            :   /**
     163                 :            :    * @brief Calculate the trajectory sets if pedestrian is currently moving
     164                 :            :    *
     165                 :            :    * @param[in]  vehicleState           current state of the pedestrian
     166                 :            :    * @param[in]  timeToStopSpeedMin     time to step with stated braking pattern on speed min
     167                 :            :    * @param[out] brakePolygon           the trajectory set for braking behavior
     168                 :            :    * @param[in]  timeToStopSpeedMax     time to step with stated braking pattern on speed max
     169                 :            :    * @param[out] continueForwardPolygon the trajectory set for continue-forward behavior
     170                 :            :    *
     171                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     172                 :            :    */
     173                 :            :   bool calculateTrajectorySetsMoving(core::RelativeObjectState const &vehicleState,
     174                 :            :                                      physics::Duration const &timeToStopSpeedMin,
     175                 :            :                                      ::ad::geometry::Polygon &brakePolygon,
     176                 :            :                                      physics::Duration const &timeToStopSpeedMax,
     177                 :            :                                      ::ad::geometry::Polygon &continueForwardPolygon) const;
     178                 :            : };
     179                 :            : 
     180                 :            : } // namespace unstructured
     181                 :            : } // namespace rss
     182                 :            : } // namespace ad