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                 :            :  * @file
      10                 :            :  */
      11                 :            : 
      12                 :            : #pragma once
      13                 :            : 
      14                 :            : #include <ad/map/route/FullRoute.hpp>
      15                 :            : #include <ad/rss/map/RssObjectData.hpp>
      16                 :            : #include <ad/rss/world/Object.hpp>
      17                 :            : #include <memory>
      18                 :            : 
      19                 :            : /*!
      20                 :            :  * @brief namespace ad
      21                 :            :  */
      22                 :            : namespace ad {
      23                 :            : /*!
      24                 :            :  * @brief namespace rss
      25                 :            :  */
      26                 :            : namespace rss {
      27                 :            : /*!
      28                 :            :  * @brief namespace map
      29                 :            :  */
      30                 :            : namespace map {
      31                 :            : 
      32                 :            : /*!
      33                 :            :  * @brief class providing support to convert ad::map objects into ad::rss objects
      34                 :            :  */
      35                 :            : class RssObjectConversion
      36                 :            : {
      37                 :            : public:
      38                 :            :   /*!
      39                 :            :    * \brief Smart pointer on RssObjectConversion
      40                 :            :    */
      41                 :            :   using Ptr = std::shared_ptr<RssObjectConversion>;
      42                 :            : 
      43                 :            :   /*!
      44                 :            :    * \brief Smart pointer on constant RssObjectConversion
      45                 :            :    */
      46                 :            :   using ConstPtr = std::shared_ptr<RssObjectConversion const>;
      47                 :            : 
      48                 :            :   /*!
      49                 :            :    * @brief constructor
      50                 :            :    *
      51                 :            :    * @param[in] object_data the object data
      52                 :            :    */
      53                 :            :   explicit RssObjectConversion(RssObjectData const &object_data);
      54                 :            : 
      55                 :            :   /*!
      56                 :            :    * @brief constructor with explicit occupied regions
      57                 :            :    *
      58                 :            :    * @param[in] object_data the object data
      59                 :            :    * @param[in] objectOccupiedRegions the object's occupied regions explicitly
      60                 :            :    */
      61                 :            :   RssObjectConversion(RssObjectData const &object_data,
      62                 :            :                       ::ad::rss::world::OccupiedRegionVector const &objectOccupiedRegions);
      63                 :            : 
      64                 :            :   /*!
      65                 :            :    * \brief standard copy constructor
      66                 :            :    */
      67                 :        861 :   RssObjectConversion(const RssObjectConversion &other) = default;
      68                 :            : 
      69                 :            :   /*!
      70                 :            :    * \brief standard move constructor
      71                 :            :    */
      72                 :            :   RssObjectConversion(RssObjectConversion &&other) = default;
      73                 :            : 
      74                 :            :   /*!
      75                 :            :    * \brief standard destructor
      76                 :            :    */
      77                 :       1392 :   ~RssObjectConversion() = default;
      78                 :            : 
      79                 :            :   /*!
      80                 :            :    * \brief delete standard assignment operator
      81                 :            :    */
      82                 :            :   RssObjectConversion &operator=(const RssObjectConversion &other) = delete;
      83                 :            : 
      84                 :            :   /*!
      85                 :            :    * \brief delete standard move assignment operator
      86                 :            :    */
      87                 :            :   RssObjectConversion &operator=(RssObjectConversion &&other) = delete;
      88                 :            : 
      89                 :            :   /** @returns RssDynamics of the object
      90                 :            :    *
      91                 :            :    * If updateSpeedLimit() was called in between the max_speed_on_acceleration value of the dynamics will be adapted to
      92                 :            :    * these.
      93                 :            :    */
      94                 :            :   ::ad::rss::world::RssDynamics getRssDynamics() const;
      95                 :            : 
      96                 :            :   /** @returns RSS Object description
      97                 :            :    *
      98                 :            :    * If laneIntervalAdded() or fillNotRelevantConstellationBoundingBox() was called in between the
      99                 :            :    * occupied regions have been filled accordingly. If not, these are empty (which is an invalid object!)
     100                 :            :    */
     101                 :            :   ::ad::rss::world::Object const &getRssObject() const;
     102                 :            : 
     103                 :            :   /** @returns Object id */
     104                 :            :   ::ad::rss::world::ObjectId getId() const;
     105                 :            : 
     106                 :            :   /**
     107                 :            :    * This is an estimate, that can be used to calculate the route calculation distances to be taken into account.
     108                 :            :    * It uses conservative expectations, i.e. brake_min_correct and no limitation of speed on acceleration.
     109                 :            :    *
     110                 :            :    * @param[in] object_id the object_id (for debug messages only)
     111                 :            :    * @param[in] current_speed the objects current speed
     112                 :            :    * @param[in] rss_dynamics the RssDynamics to consider
     113                 :            :    * @param[out] conservativeMinStoppingDistance the minimum distance for the object to stop calculated in a
     114                 :            :    * conservative way
     115                 :            :    *
     116                 :            :    * @returns boolean indicating success(\a true)/failure(\a  false) of the calculation
     117                 :            :    */
     118                 :            :   static bool calculateConservativeMinStoppingDistance(::ad::rss::world::ObjectId const &object_id,
     119                 :            :                                                        ::ad::physics::Speed const &current_speed,
     120                 :            :                                                        ::ad::rss::world::RssDynamics const &rss_dynamics,
     121                 :            :                                                        ::ad::physics::Distance &conservativeMinStoppingDistance);
     122                 :            :   /**
     123                 :            :    * This is an estimate, that can be used to calculate the route calculation distance to be taken into account.
     124                 :            :    * It uses conservative expectations, i.e. brake_min_correct and no limitation of speed on acceleration.
     125                 :            :    *
     126                 :            :    * @param[out] conservativeMinStoppingDistance the minimum distance for the object to stop calculated in a
     127                 :            :    * conservative way
     128                 :            :    *
     129                 :            :    * @returns boolean indicating success(\a true)/failure(\a  false) of the calculation
     130                 :            :    */
     131                 :            :   bool calculateConservativeMinStoppingDistance(::ad::physics::Distance &conservativeMinStoppingDistance) const;
     132                 :            : 
     133                 :            :   /** @brief update the max speed content
     134                 :            :    */
     135                 :            :   void updateSpeedLimit(::ad::physics::Speed const &max_speed_on_acceleration);
     136                 :            : 
     137                 :            :   /** @brief lane interval was added to the object route, so append relevant occupied regions
     138                 :            :    */
     139                 :            :   void laneIntervalAdded(::ad::map::route::LaneInterval const &lane_interval);
     140                 :            : 
     141                 :            :   /** @brief update the objects current velocity on the route considering the provided route_heading
     142                 :            :    *
     143                 :            :    *  Use this overload if the route is derived from a ad::map::route::ConnectingRoute by using the heading calculations
     144                 :            :    *  from the ConnectingRoute type, as the object bounding boxes are usually removed from the connecting route.
     145                 :            :    *  Like this, the object is more or less touching the route and the other overload taking the route as parameter
     146                 :            :    *  might not be able to extract the heading.
     147                 :            :    */
     148                 :            :   void updateVelocityOnRoute(::ad::map::point::ENUHeading const &route_heading);
     149                 :            : 
     150                 :            :   /** @brief update the objects current velocity on the route
     151                 :            :    *
     152                 :            :    *  The bounding box of the object has to be part of the provided route to be able to extract the route heading
     153                 :            :    * correctly. Therefore, use this overload if the route was calculated by the ad::map::route::planning::planRoute() or
     154                 :            :    * ad::map::route::planning::predictRoute() functions and no heading information is available yet by other means. If
     155                 :            :    * the route is derived from a ad::map::route::ConnectingRoute use the overload with the pre-calcuated heading.
     156                 :            :    *
     157                 :            :    *  @returns \c true if the operation succeeded
     158                 :            :    */
     159                 :            :   bool updateVelocityOnRoute(::ad::map::route::FullRoute const &route);
     160                 :            : 
     161                 :            :   /** @brief return the map matched position object this was created with (might be nullptr)
     162                 :            :    */
     163                 :       1027 :   ::ad::map::match::Object const *getObjectMapMatchedPosition() const
     164                 :            :   {
     165                 :       1027 :     return mObjectMapMatchedPosition;
     166                 :            :   }
     167                 :            : 
     168                 :            :   /** @brief check if the original input speed is in acceptable range
     169                 :            :    *
     170                 :            :    *  Negative speed is not supported by the RSS implementation; therefore negative input speeds are mapped to zero
     171                 :            :    * speed by this class.
     172                 :            :    *
     173                 :            :    *  But up to a certain small negative speed value, negative speeds still might want to be accepted to account for
     174                 :            :    * slowly backward drifting vehicles.
     175                 :            :    *  Therefore, the constructor of this class maps all negative speeds to zero, but stores the original provided speed
     176                 :            :    * for later analysis.
     177                 :            :    *  This function can be used to check for an acceptable speed.
     178                 :            :    *
     179                 :            :    * @param[in] acceptableNegativeSpeed a small negative speed value that should be allowed to be mapped to zero
     180                 :            :    * without error (default -0.5m/s).
     181                 :            :    *
     182                 :            :    * @returns \c true if the original speed equals the current internal object speed.
     183                 :            :    *  It also returns \c true if the original speed is equal or larger than the provided acceptableNegativeSpeed.
     184                 :            :    */
     185                 :            :   bool isOriginalSpeedAcceptable(::ad::physics::Speed const acceptableNegativeSpeed = ::ad::physics::Speed(-0.5)) const;
     186                 :            : 
     187                 :            :   /**
     188                 :            :    * @returns the original object speed provided as input
     189                 :            :    */
     190                 :          0 :   ::ad::physics::SpeedRange const &getOriginalObjectSpeed() const
     191                 :            :   {
     192                 :          0 :     return mOriginalObjectSpeed;
     193                 :            :   }
     194                 :            : 
     195                 :            :   /**
     196                 :            :    * @returns the distance estimate towards the other object
     197                 :            :    * The distance uses Euclidian distance of the center points as an estimate.
     198                 :            :    */
     199                 :            :   ::ad::physics::Distance getDistanceEstimate(RssObjectConversion::ConstPtr other) const;
     200                 :            : 
     201                 :            :   /**
     202                 :            :    * @returns the length of the vehicle
     203                 :            :    */
     204                 :        958 :   ::ad::physics::Distance getVehicleLength() const
     205                 :            :   {
     206                 :        958 :     return getRssObject().state.dimension.length;
     207                 :            :   }
     208                 :            : 
     209                 :            :   /**
     210                 :            :    * @returns the width of the vehicle
     211                 :            :    */
     212                 :        522 :   ::ad::physics::Distance getVehicleWidth() const
     213                 :            :   {
     214                 :        522 :     return getRssObject().state.dimension.width;
     215                 :            :   }
     216                 :            : 
     217                 :            : private:
     218                 :            :   ::ad::rss::world::Object mRssObject;
     219                 :            :   ::ad::map::match::Object const *mObjectMapMatchedPosition;
     220                 :            :   ::ad::physics::Speed mMaxSpeedOnAcceleration;
     221                 :            :   ::ad::physics::SpeedRange const mOriginalObjectSpeed;
     222                 :            :   ::ad::rss::world::RssDynamics const &mRssDynamics;
     223                 :            : 
     224                 :            :   void initializeRssObject(::ad::rss::world::ObjectId const &object_id,
     225                 :            :                            ::ad::rss::world::ObjectType const &object_type,
     226                 :            :                            ::ad::rss::world::OccupiedRegionVector const &objectOccupiedRegions,
     227                 :            :                            ::ad::map::match::ENUObjectPosition const &objectEnuPosition,
     228                 :            :                            ::ad::physics::SpeedRange const &objectSpeed,
     229                 :            :                            ::ad::physics::AngularVelocity const &objectYawRate,
     230                 :            :                            ::ad::physics::Angle const &objectSteeringAngle);
     231                 :            : 
     232                 :            :   void addRestrictedOccupiedRegion(::ad::map::match::LaneOccupiedRegion const &laneOccupiedRegion,
     233                 :            :                                    ::ad::map::route::LaneInterval const &lane_interval);
     234                 :            : };
     235                 :            : 
     236                 :            : } // namespace map
     237                 :            : } // namespace rss
     238                 :            : } // namespace ad