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/DebugDrawing.hpp>
      15                 :            : #include <ad/geometry/GeometryOperation.hpp>
      16                 :            : #include <ad/geometry/Types.hpp>
      17                 :            : #include <ad/physics/RatioValue.hpp>
      18                 :            : #include <algorithm>
      19                 :            : #include <vector>
      20                 :            : #include "ad/rss/core/Physics.hpp"
      21                 :            : #include "ad/rss/core/RelativeObjectState.hpp"
      22                 :            : #include "ad/rss/unstructured/TrajectoryCommon.hpp"
      23                 :            : 
      24                 :            : /*!
      25                 :            :  * @brief namespace ad
      26                 :            :  */
      27                 :            : namespace ad {
      28                 :            : /*!
      29                 :            :  * @brief namespace rss
      30                 :            :  */
      31                 :            : namespace rss {
      32                 :            : /*!
      33                 :            :  * @brief namespace unstructured
      34                 :            :  */
      35                 :            : namespace unstructured {
      36                 :            : 
      37                 :            : /**
      38                 :            :  * @brief Calculates the trajectory sets of a vehicle
      39                 :            :  */
      40                 :            : class TrajectoryVehicle
      41                 :            : {
      42                 :            : public:
      43                 :         20 :   TrajectoryVehicle()
      44                 :            :   {
      45                 :         20 :   }
      46                 :            : 
      47                 :            :   /**
      48                 :            :    * @brief Calculate the trajectory sets for braking and continue forward behavior
      49                 :            :    *
      50                 :            :    * @param[in]  vehicleState current state of the vehicle
      51                 :            :    * @param[out] brakePolygon the trajectory set for braking behavior
      52                 :            :    * @param[out] continueForwardPolygon the trajectory set for continue-forward behavior
      53                 :            :    *
      54                 :            :    * @returns false if a failure occurred during calculations, true otherwise
      55                 :            :    */
      56                 :            :   bool calculateTrajectorySets(core::RelativeObjectState const &vehicleState,
      57                 :            :                                ::ad::geometry::Polygon &brakePolygon,
      58                 :            :                                ::ad::geometry::Polygon &continueForwardPolygon);
      59                 :            : 
      60                 :            : private:
      61                 :            :   /**
      62                 :            :    * @brief Calculate the yaw rate after a duration
      63                 :            :    *
      64                 :            :    * @param[in] yaw_rate     current yaw rate
      65                 :            :    * @param[in] timeInMovementUntilResponseTime         duration of yaw rate change
      66                 :            :    * @param[in] maxYawRateChange maximum yaw rate change per second
      67                 :            :    * @param[in] ratio            yaw rate change ratio
      68                 :            :    *
      69                 :            :    * @returns yaw rate
      70                 :            :    */
      71                 :            :   ad::physics::AngularVelocity calculateYawRate(ad::physics::AngularVelocity const &yaw_rate,
      72                 :            :                                                 ad::physics::Duration const &timeInMovementUntilResponseTime,
      73                 :            :                                                 ad::physics::AngularAcceleration const &maxYawRateChange,
      74                 :            :                                                 ad::physics::RatioValue const &ratio) const;
      75                 :            : 
      76                 :            :   /**
      77                 :            :    * @brief Calculate all trajectory points at response time
      78                 :            :    *
      79                 :            :    * @param[in]  vehicleState current state of the vehicle
      80                 :            :    * @param[out] frontSide the trajectory points defining the front
      81                 :            :    * @param[out] backSide the trajectory points defining the back
      82                 :            :    *
      83                 :            :    * @returns false if a failure occurred during calculations, true otherwise
      84                 :            :    */
      85                 :            :   bool getResponseTimeTrajectoryPoints(core::RelativeObjectState const &vehicleState,
      86                 :            :                                        TrajectorySetStep &frontSide,
      87                 :            :                                        TrajectorySetStep &backSide) const;
      88                 :            : 
      89                 :            :   /**
      90                 :            :    * @brief Calculate all trajectory points at response time
      91                 :            :    *
      92                 :            :    * @param[in]  vehicleState current state of the vehicle
      93                 :            :    * @param[in]  acceleration acceleration to use
      94                 :            :    * @param[in]  ratioDiff    yaw rate change ratio
      95                 :            :    * @param[in]  speedMode    use min or max speed of the speed_range
      96                 :            :    * @param[in]  step         resulting trajectory set step
      97                 :            :    *
      98                 :            :    * @returns false if a failure occurred during calculations, true otherwise
      99                 :            :    */
     100                 :            :   bool getResponseTimeTrajectoryPoints(core::RelativeObjectState const &vehicleState,
     101                 :            :                                        physics::Acceleration const &acceleration,
     102                 :            :                                        physics::RatioValue const &ratioDiff,
     103                 :            :                                        TrajectoryPoint::SpeedMode const &speedMode,
     104                 :            :                                        TrajectorySetStep &step) const;
     105                 :            : 
     106                 :            :   /**
     107                 :            :    * @brief Calculate a single trajectory point with changing radius
     108                 :            :    *
     109                 :            :    * @param[in] currentPoint           trajectory point to use for calculation
     110                 :            :    * @param[in]  dynamics              dynamics to use
     111                 :            :    * @param[in]  duration              time
     112                 :            :    * @param[in]  acceleration          acceleration to use
     113                 :            :    * @param[in]  yawRateChangeRatio    yaw rate change ratio
     114                 :            :    * @param[out] resultTrajectoryPoint resulting trajectory point
     115                 :            :    *
     116                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     117                 :            :    */
     118                 :            :   bool calculateTrajectoryPoint(TrajectoryPoint const &currentPoint,
     119                 :            :                                 world::RssDynamics const &dynamics,
     120                 :            :                                 ad::physics::Duration const &duration,
     121                 :            :                                 ad::physics::Acceleration const &acceleration,
     122                 :            :                                 ad::physics::RatioValue const &yawRateChangeRatio,
     123                 :            :                                 TrajectoryPoint &resultTrajectoryPoint) const;
     124                 :            : 
     125                 :            :   /**
     126                 :            :    * @brief Calculate a next trajectory point on a circle
     127                 :            :    *
     128                 :            :    * @param[inout] currentPoint        trajectory point to use for calculation
     129                 :            :    * @param[in]  acceleration          acceleration to use
     130                 :            :    * @param[in]  duration              duration of accelerated movement
     131                 :            :    * @param[in]  dynamics              dynamics to use
     132                 :            :    *
     133                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     134                 :            :    */
     135                 :            :   bool calculateTrajectoryPointOnCircle(TrajectoryPoint &currentPoint,
     136                 :            :                                         physics::Acceleration const &acceleration,
     137                 :            :                                         physics::Duration const &duration,
     138                 :            :                                         ::ad::rss::world::RssDynamics const &dynamics) const;
     139                 :            : 
     140                 :            :   /**
     141                 :            :    * @brief Calculate a time in movement until response time
     142                 :            :    *
     143                 :            :    * @param[in]  speed                      speed of the vehicle
     144                 :            :    * @param[in]  acceleration          acceleration to use
     145                 :            :    * @param[inout]  timeInMovement   resulting time in movement
     146                 :            :    *
     147                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     148                 :            :    */
     149                 :            :   bool getTimeInMovement(ad::physics::Speed const &speed,
     150                 :            :                          ad::physics::Acceleration const &acceleration,
     151                 :            :                          ad::physics::Duration &timeInMovement) const;
     152                 :            : 
     153                 :            :   /**
     154                 :            :    * @brief Calculate the brake trajectory set
     155                 :            :    *
     156                 :            :    * @param[in]  vehicleState                      current state of the vehicle
     157                 :            :    * @param[in]  timeAfterResponseTimeSpeedMax     time after the response time to move with speed max
     158                 :            :    * @param[in]  responseTimeFrontSide             the trajectory points defining the front
     159                 :            :    * @param[in]  timeAfterResponseTimeSpeedMin     time after the response time to move with speed min
     160                 :            :    * @param[in]  responseTimeBackSide              the trajectory points defining the back
     161                 :            :    * @param[out] resultPolygon                     the resulting brake polygon
     162                 :            :    * @param[out] brakeMinStepVehicleLocation       the vehicle locations for brake_min after response time
     163                 :            :    *
     164                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     165                 :            :    */
     166                 :            :   bool calculateBrake(core::RelativeObjectState const &vehicleState,
     167                 :            :                       ad::physics::Duration const &timeAfterResponseTimeSpeedMax,
     168                 :            :                       TrajectorySetStep const &responseTimeFrontSide,
     169                 :            :                       ad::physics::Duration const &timeAfterResponseTimeSpeedMin,
     170                 :            :                       TrajectorySetStep const &responseTimeBackSide,
     171                 :            :                       ::ad::geometry::Polygon &resultPolygon,
     172                 :            :                       TrajectorySetStepVehicleLocation &brakeMinStepVehicleLocation) const;
     173                 :            : 
     174                 :            :   /**
     175                 :            :    * @brief Calculate the continue forward trajectory set
     176                 :            :    *
     177                 :            :    * @param[in]  vehicleState                      current state of the vehicle
     178                 :            :    * @param[in]  timeAfterResponseTime             time after the response time to move
     179                 :            :    * @param[in]  responseTimeFrontSide             the trajectory points defining the front
     180                 :            :    * @param[in]  brakePolygon                      the polygon defining the brake trajectory set
     181                 :            :    * @param[in]  brakeMinStepVehicleLocation       the vehicle locations for brake_min after response time
     182                 :            :    * @param[out] resultPolygon                     the resulting continue forward polygon
     183                 :            :    *
     184                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     185                 :            :    */
     186                 :            :   bool calculateContinueForward(core::RelativeObjectState const &vehicleState,
     187                 :            :                                 physics::Duration const &timeAfterResponseTime,
     188                 :            :                                 TrajectorySetStep const &responseTimeFrontSide,
     189                 :            :                                 ::ad::geometry::Polygon const &brakePolygon,
     190                 :            :                                 TrajectorySetStepVehicleLocation const &brakeMinStepVehicleLocation,
     191                 :            :                                 ::ad::geometry::Polygon &resultPolygon) const;
     192                 :            : 
     193                 :            :   /**
     194                 :            :    * @brief Calculate the trajectory set step for a movement on a circle
     195                 :            :    *
     196                 :            :    * @param[in]  vehicleState                      current state of the vehicle
     197                 :            :    * @param[in]  timeAfterResponseTime             time after the response time to move
     198                 :            :    * @param[in]  acceleration                      acceleration to apply
     199                 :            :    * @param[in]  step                              resulting trajectory set step
     200                 :            :    *
     201                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     202                 :            :    */
     203                 :            :   bool calculateTrajectorySetStepOnCircle(core::RelativeObjectState const &vehicleState,
     204                 :            :                                           physics::Duration const &timeAfterResponseTime,
     205                 :            :                                           physics::Acceleration const &acceleration,
     206                 :            :                                           TrajectorySetStep &step) const;
     207                 :            : };
     208                 :            : 
     209                 :            : } // namespace unstructured
     210                 :            : } // namespace rss
     211                 :            : } // namespace ad