Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2019-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 <map>
      15                 :            : #include "ad/rss/core/RssCheck.hpp"
      16                 :            : #include "ad/rss/map/RssObjectAdapter.hpp"
      17                 :            : 
      18                 :            : /*!
      19                 :            :  * @brief namespace rss
      20                 :            :  */
      21                 :            : namespace ad {
      22                 :            : /*!
      23                 :            :  * @brief namespace rss
      24                 :            :  */
      25                 :            : namespace rss {
      26                 :            : /*!
      27                 :            :  * @brief namespace map
      28                 :            :  */
      29                 :            : namespace map {
      30                 :            : 
      31                 :            : /*!
      32                 :            :  * @brief Class to store data of RssObjectAdapter
      33                 :            :  */
      34                 :            : class RssRouteCheckerObjectData
      35                 :            : {
      36                 :            : public:
      37                 :            :   /*! @brief default constructor
      38                 :            :    */
      39                 :         12 :   RssRouteCheckerObjectData() = default;
      40                 :            : 
      41                 :            :   /*! @brief default copy constructor
      42                 :            :    */
      43                 :         24 :   RssRouteCheckerObjectData(const RssRouteCheckerObjectData &other) = default;
      44                 :            : 
      45                 :            :   /*! @brief default destrutor
      46                 :            :    */
      47                 :         36 :   ~RssRouteCheckerObjectData() = default;
      48                 :            : 
      49                 :            :   /*! @brief default assignment operator
      50                 :            :    */
      51                 :            :   RssRouteCheckerObjectData &operator=(const RssRouteCheckerObjectData &other) = default;
      52                 :            : 
      53                 :            :   /*! @brief sorting operator to enable map usage
      54                 :            :    */
      55                 :            :   bool operator<(RssRouteCheckerObjectData const &other)
      56                 :            :   {
      57                 :            :     return mRssObjectData.id < other.mRssObjectData.id;
      58                 :            :   }
      59                 :            : 
      60                 :            :   //! flag indicating if the object is active
      61                 :            :   bool mActive{false};
      62                 :            : 
      63                 :            :   //! flag indicating if the object's history has to be cleared
      64                 :            :   bool mClearObjectHistory{false};
      65                 :            : 
      66                 :            :   //! the actual RssObjectData of this
      67                 :            :   RssObjectData mRssObjectData;
      68                 :            : };
      69                 :            : 
      70                 :            : /*!
      71                 :            :  * @brief Class to store data of RssPedestrianAdapter
      72                 :            :  */
      73                 :            : template <class OBJECT_INSTANCE_TYPE> class RssRouteCheckerPedestrianData : public RssRouteCheckerObjectData
      74                 :            : {
      75                 :            : public:
      76                 :            :   /*! @brief default constructor
      77                 :            :    */
      78                 :          0 :   RssRouteCheckerPedestrianData() = default;
      79                 :            : 
      80                 :            :   /*! @brief default copy constructor
      81                 :            :    */
      82                 :          0 :   RssRouteCheckerPedestrianData(RssRouteCheckerPedestrianData const &other) = default;
      83                 :            : 
      84                 :            :   /*! @brief default destrutor
      85                 :            :    */
      86                 :          0 :   ~RssRouteCheckerPedestrianData() = default;
      87                 :            : 
      88                 :            :   /*! @brief default assignment operator
      89                 :            :    */
      90                 :            :   RssRouteCheckerPedestrianData &operator=(const RssRouteCheckerPedestrianData &other) = default;
      91                 :            : 
      92                 :            :   typedef RssPedestrianAdapter<OBJECT_INSTANCE_TYPE> RssPedestrianAdapterType;
      93                 :            : 
      94                 :            :   //! function to update the adapter class of this
      95                 :          0 :   void updateAdapter(typename RssPedestrianAdapterType::Ptr artificialObjectAdapter)
      96                 :            :   {
      97                 :          0 :     mPedestrianAdapter = artificialObjectAdapter;
      98                 :          0 :   }
      99                 :            : 
     100                 :            :   //! function to get the adapter class of this
     101                 :          0 :   typename RssPedestrianAdapterType::Ptr getAdapter()
     102                 :            :   {
     103                 :          0 :     return mPedestrianAdapter;
     104                 :            :   }
     105                 :            : 
     106                 :            :   //! function to get the adapter class of this
     107                 :            :   typename RssPedestrianAdapterType::ConstPtr getAdapter() const
     108                 :            :   {
     109                 :            :     return mPedestrianAdapter;
     110                 :            :   }
     111                 :            : 
     112                 :            :   using RssRouteCheckerObjectData::mActive;
     113                 :            :   using RssRouteCheckerObjectData::mRssObjectData;
     114                 :            : 
     115                 :            : protected:
     116                 :            :   //! the current RssPedestrianAdapter
     117                 :            :   typename RssPedestrianAdapterType::Ptr mPedestrianAdapter;
     118                 :            : };
     119                 :            : 
     120                 :            : /*!
     121                 :            :  * @brief Class to store data of RssVehicleAdapter
     122                 :            :  */
     123                 :            : template <class OBJECT_INSTANCE_TYPE> class RssRouteCheckerVehicleData : public RssRouteCheckerObjectData
     124                 :            : {
     125                 :            : public:
     126                 :            :   /*! @brief default constructor
     127                 :            :    */
     128                 :         10 :   RssRouteCheckerVehicleData() = default;
     129                 :            : 
     130                 :            :   /*! @brief default copy constructor
     131                 :            :    */
     132   [ +  -  +  - ]:         20 :   RssRouteCheckerVehicleData(RssRouteCheckerVehicleData const &other) = default;
     133                 :            : 
     134                 :            :   /*! @brief default destrutor
     135                 :            :    */
     136                 :            :   ~RssRouteCheckerVehicleData();
     137                 :            : 
     138                 :            :   /*! @brief default assignment operator
     139                 :            :    */
     140                 :            :   RssRouteCheckerVehicleData &operator=(const RssRouteCheckerVehicleData &other) = default;
     141                 :            : 
     142                 :            :   typedef RssVehicleAdapter<OBJECT_INSTANCE_TYPE> RssVehicleAdapterType;
     143                 :            : 
     144                 :            :   //! function to update the adapter class of this
     145                 :            : 
     146                 :         26 :   void updateAdapter(typename RssVehicleAdapterType::Ptr vehicleAdapter)
     147                 :            :   {
     148                 :         26 :     mVehicleAdapter = vehicleAdapter;
     149                 :         26 :   }
     150                 :            : 
     151                 :            :   //! function to get the adapter class of this
     152                 :        165 :   typename RssVehicleAdapterType::Ptr getAdapter()
     153                 :            :   {
     154                 :        165 :     return mVehicleAdapter;
     155                 :            :   }
     156                 :            : 
     157                 :            :   //! function to get the adapter class of this
     158                 :         44 :   typename RssVehicleAdapterType::ConstPtr getAdapter() const
     159                 :            :   {
     160                 :         44 :     return mVehicleAdapter;
     161                 :            :   }
     162                 :            : 
     163                 :            :   //! enumeration defining the mode on route extension
     164                 :            :   enum class RouteExtensionMode
     165                 :            :   {
     166                 :            :     eAllowMultipleRoutes,  //!< allow the creation of all possible route candidates
     167                 :            :     eAllowOnlySingleRoute, //!< only allow the processing of one single route, additional route candidates with lower
     168                 :            :                            //!< likelyhood are dropped
     169                 :            :     eAllowOnlyTargetRoute  //!< only allow the processing of the externally provided targets for routing.
     170                 :            :                            //!< No further automatic expansions, or route creations are performed.
     171                 :            :   };
     172                 :            : 
     173                 :            :   /*!
     174                 :            :    * @brief Supporting function to update the routes of this
     175                 :            :    *
     176                 :            :    * @param[in] routePreviewDistance indicator for the desired length of the routes
     177                 :            :    * @param[in] mapAreaLanes resticted set of lanes from a map to be considered (allows speed up)
     178                 :            :    * @param[in] routeExtensionMode defines the mode of the route extension (default:
     179                 :            :    * RouteExtensionMode::eAllowMultipleRoutes)
     180                 :            :    */
     181                 :            :   void updateRoutes(::ad::physics::Distance const routePreviewDistance,
     182                 :            :                     ::ad::map::lane::LaneIdSet const &mapAreaLanes,
     183                 :            :                     RouteExtensionMode routeExtensionMode = RouteExtensionMode::eAllowMultipleRoutes);
     184                 :            : 
     185                 :            :   /*!
     186                 :            :    * @brief Supporting function to normalize the sum of the route likelihood to 1.0 and reorder the list in descending
     187                 :            :    * likelihood order.
     188                 :            :    */
     189                 :            :   void normalizeAndOrderRoutes();
     190                 :            : 
     191                 :            :   //! the current active route extension mode
     192                 :            :   RouteExtensionMode mRouteExtensionMode;
     193                 :            :   //! the sorted list of routes
     194                 :            :   RssRouteList mRoutes;
     195                 :            : 
     196                 :            :   using RssRouteCheckerObjectData::mActive;
     197                 :            :   using RssRouteCheckerObjectData::mRssObjectData;
     198                 :            : 
     199                 :            :   //! the list of routing target to be used for routing purposes
     200                 :            :   std::vector<::ad::map::point::ENUPoint> mRoutingTargets;
     201                 :            : 
     202                 :            : protected:
     203                 :            :   /*!
     204                 :            :    * @brief Supporting function to process routing targets
     205                 :            :    */
     206                 :            :   std::vector<::ad::map::point::ENUPoint> handleRoutingTargets();
     207                 :            : 
     208                 :            :   /*!
     209                 :            :    * @brief Supporting function to create new routes
     210                 :            :    *
     211                 :            :    * If there are routing targets present, these are considered to create new routes to be considered
     212                 :            :    * (usually this results in one single route, but depending on the current vehicle positioning, this can also create a
     213                 :            :    * set of routes).
     214                 :            :    * Otherwise ::ad::map::route::planning::predictRoutesOnDistance() is used to create the routes.
     215                 :            :    *
     216                 :            :    * @param[in] routePreviewDistance indicator for the desired length of the routes
     217                 :            :    */
     218                 :            :   std::vector<::ad::map::route::FullRoute> createRoutes(::ad::physics::Distance const routePreviewDistance,
     219                 :            :                                                         ::ad::map::lane::LaneIdSet const &mapAreaLanes);
     220                 :            : 
     221                 :            :   /*!
     222                 :            :    * @brief Supporting function to shorten the predicted routes
     223                 :            :    *
     224                 :            :    * remove the parts of the route already taken, try to prepend route sections
     225                 :            :    * (i.e. when driving backwards a bit)
     226                 :            :    * try to ensure that the back of the vehicle is still within the route to
     227                 :            :    * support proper orientation calculations on route
     228                 :            :    */
     229                 :            :   void shortenRoutes();
     230                 :            : 
     231                 :            :   /*!
     232                 :            :    * @brief Supporting function to extend the predicted routes
     233                 :            :    *
     234                 :            :    * @param[in] routePreviewDistance indicator for the desired length of the routes
     235                 :            :    * @param[in] routingTargetsToAppend extend the route according to the routing targets to be appended
     236                 :            :    */
     237                 :            :   void extendRoutes(::ad::physics::Distance const routePreviewDistance,
     238                 :            :                     std::vector<::ad::map::point::ENUPoint> const &routingTargetsToAppend,
     239                 :            :                     ::ad::map::lane::LaneIdSet const &mapAreaLanes);
     240                 :            : 
     241                 :            :   RssRouteId mNextRouteId{1};
     242                 :            :   bool mExternalRoutes{false};
     243                 :            : 
     244                 :            :   //! the current RssVehicleAdapter
     245                 :            :   typename RssVehicleAdapterType::Ptr mVehicleAdapter;
     246                 :            : };
     247                 :            : 
     248                 :            : /*!
     249                 :            :  * @brief typedef for a RssCheck map in respect to the RssRouteId
     250                 :            :  */
     251                 :            : typedef std::map<RssRouteId, ::ad::rss::core::RssCheck::Ptr> RssRouteCheckMap;
     252                 :            : 
     253                 :            : /*!
     254                 :            :  * @brief Class to store data of RssVehicleAdapter
     255                 :            :  */
     256                 :            : template <class OBJECT_INSTANCE_TYPE>
     257                 :            : class RssRouteCheckerEgoVehicleData : public RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>
     258                 :            : {
     259                 :            : public:
     260                 :            :   /*! @brief default constructor
     261                 :            :    */
     262                 :          7 :   RssRouteCheckerEgoVehicleData() = default;
     263                 :            : 
     264                 :            :   /*! @brief default copy constructor
     265                 :            :    */
     266         [ +  - ]:         14 :   RssRouteCheckerEgoVehicleData(RssRouteCheckerEgoVehicleData const &other) = default;
     267                 :            : 
     268                 :            :   /*! @brief default destrutor
     269                 :            :    */
     270                 :         21 :   ~RssRouteCheckerEgoVehicleData() = default;
     271                 :            : 
     272                 :            :   /*! @brief default assignment operator
     273                 :            :    */
     274                 :            :   RssRouteCheckerEgoVehicleData &operator=(const RssRouteCheckerEgoVehicleData &other) = default;
     275                 :            : 
     276                 :            :   typedef RssEgoVehicleAdapter<OBJECT_INSTANCE_TYPE> RssEgoVehicleAdapterType;
     277                 :            : 
     278                 :            :   //! function to update the adapter class of this
     279                 :         17 :   void updateAdapter(typename RssEgoVehicleAdapterType::Ptr egoVehicleAdapter)
     280                 :            :   {
     281                 :         17 :     RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::updateAdapter(egoVehicleAdapter);
     282                 :         17 :     mEgoVehicleAdapter = egoVehicleAdapter;
     283                 :         17 :   }
     284                 :            : 
     285                 :            :   //! function to get the adapter class of this
     286                 :         17 :   typename RssEgoVehicleAdapterType::Ptr getAdapter()
     287                 :            :   {
     288                 :         17 :     return mEgoVehicleAdapter;
     289                 :            :   }
     290                 :            : 
     291                 :            :   //! function to get the adapter class of this
     292                 :         76 :   typename RssEgoVehicleAdapterType::ConstPtr getAdapter() const
     293                 :            :   {
     294                 :         76 :     return mEgoVehicleAdapter;
     295                 :            :   }
     296                 :            : 
     297                 :            :   /*! @brief Supporting function to update the vehicle dynamics on route
     298                 :            :    *
     299                 :            :    *  This function is used to update the vehicle dynamics on route of ego vehicles.
     300                 :            :    */
     301                 :            :   void updateVehicleDynamicsOnRoutes();
     302                 :            : 
     303                 :            :   /*!
     304                 :            :    * @brief Supporting function to evaluate current route likelihood in respect to the VehicleDynamicsOnRoute
     305                 :            :    *
     306                 :            :    * Has only effect if routes have been created internally.
     307                 :            :    */
     308                 :            :   void evaluateRoutes();
     309                 :            : 
     310                 :            :   //! the current RssRouteCheckMap
     311                 :            :   RssRouteCheckMap mRssRouteCheckMap;
     312                 :            : 
     313                 :            :   using RssRouteCheckerObjectData::mActive;
     314                 :            :   using RssRouteCheckerObjectData::mRssObjectData;
     315                 :            :   using RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::mRoutes;
     316                 :            : 
     317                 :            : protected:
     318                 :            :   //! the current RssEgoVehicleAdapter
     319                 :            :   typename RssEgoVehicleAdapterType::Ptr mEgoVehicleAdapter;
     320                 :            : 
     321                 :            :   using RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::mNextRouteId;
     322                 :            :   using RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::mRoutingTargets;
     323                 :            :   using RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::mExternalRoutes;
     324                 :            : };
     325                 :            : 
     326                 :            : /*!
     327                 :            :  * @brief Class to store data of RssArtificialObjectAdapter
     328                 :            :  */
     329                 :            : template <class OBJECT_INSTANCE_TYPE> class RssRouteCheckerArtificialObjectData : public RssRouteCheckerObjectData
     330                 :            : {
     331                 :            : public:
     332                 :            :   /*! @brief default constructor
     333                 :            :    */
     334                 :          2 :   RssRouteCheckerArtificialObjectData() = default;
     335                 :            : 
     336                 :            :   /*! @brief default copy constructor
     337                 :            :    */
     338                 :          4 :   RssRouteCheckerArtificialObjectData(RssRouteCheckerArtificialObjectData const &other) = default;
     339                 :            : 
     340                 :            :   /*! @brief default destrutor
     341                 :            :    */
     342                 :          6 :   ~RssRouteCheckerArtificialObjectData() = default;
     343                 :            : 
     344                 :            :   /*! @brief default assignment operator
     345                 :            :    */
     346                 :            :   RssRouteCheckerArtificialObjectData &operator=(const RssRouteCheckerArtificialObjectData &other) = default;
     347                 :            : 
     348                 :            :   //! function to update the adapter class of this
     349                 :          2 :   void updateAdapter(typename RssArtificialObjectAdapter<OBJECT_INSTANCE_TYPE>::Ptr artificialObjectAdapter)
     350                 :            :   {
     351                 :          2 :     mArtificialObjectAdapter = artificialObjectAdapter;
     352                 :          2 :   }
     353                 :            : 
     354                 :            :   //! function to get the adapter class of this
     355                 :          6 :   typename RssArtificialObjectAdapter<OBJECT_INSTANCE_TYPE>::Ptr getAdapter()
     356                 :            :   {
     357                 :          6 :     return mArtificialObjectAdapter;
     358                 :            :   }
     359                 :            : 
     360                 :            :   //! function to get the adapter class of this
     361                 :            :   typename RssArtificialObjectAdapter<OBJECT_INSTANCE_TYPE>::ConstPtr getAdapter() const
     362                 :            :   {
     363                 :            :     return mArtificialObjectAdapter;
     364                 :            :   }
     365                 :            : 
     366                 :            : protected:
     367                 :            :   //! the current RssArtificialObjectAdapter
     368                 :            :   typename RssArtificialObjectAdapter<OBJECT_INSTANCE_TYPE>::Ptr mArtificialObjectAdapter;
     369                 :            : };
     370                 :            : 
     371                 :            : /*! Typedef for a map on RssRouteCheckPedestrianData
     372                 :            :  */
     373                 :            : template <class OBJECT_INSTANCE_TYPE>
     374                 :            : using RssRouteCheckerPedestrianDataMap
     375                 :            :   = std::map<::ad::rss::world::ObjectId, RssRouteCheckerPedestrianData<OBJECT_INSTANCE_TYPE>>;
     376                 :            : 
     377                 :            : /*! Typedef for a map on RssRouteCheckVehicleData
     378                 :            :  */
     379                 :            : template <class OBJECT_INSTANCE_TYPE>
     380                 :            : using RssRouteCheckerVehicleDataMap
     381                 :            :   = std::map<::ad::rss::world::ObjectId, RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>>;
     382                 :            : 
     383                 :            : /*! Typedef for a map on RssRouteCheckEgoVehicleData
     384                 :            :  */
     385                 :            : template <class OBJECT_INSTANCE_TYPE>
     386                 :            : using RssRouteCheckerEgoVehicleDataMap
     387                 :            :   = std::map<::ad::rss::world::ObjectId, RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE>>;
     388                 :            : 
     389                 :            : /*! Typedef for a map on RssRouteCheckerArtificialObjectData
     390                 :            :  */
     391                 :            : template <class OBJECT_INSTANCE_TYPE>
     392                 :            : using RssRouteCheckerArtificialObjectDataMap
     393                 :            :   = std::map<::ad::rss::world::ObjectId, RssRouteCheckerArtificialObjectData<OBJECT_INSTANCE_TYPE>>;
     394                 :            : 
     395                 :            : } // namespace map
     396                 :            : } // namespace rss
     397                 :            : } // namespace ad
     398                 :            : 
     399                 :            : #include "ad/rss/map/detail/RssRouteCheckerDataDetail.hpp"