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                 :            : #include "ad/rss/map/Logging.hpp"
      10                 :            : #include "ad/rss/map/RssRouteCheckerData.hpp"
      11                 :            : #include "ad/rss/map/RssWorldModelCreator.hpp"
      12                 :            : 
      13                 :            : namespace ad {
      14                 :            : namespace rss {
      15                 :            : namespace map {
      16                 :            : 
      17                 :            : template <class OBJECT_INSTANCE_TYPE> class ParallelWorldModelCreator
      18                 :            : {
      19                 :            : public:
      20                 :         28 :   ParallelWorldModelCreator(::ad::rss::world::TimeIndex const time_index,
      21                 :            :                             RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &egoVehicleData,
      22                 :            :                             RssRoute const &rss_route,
      23                 :            :                             ConnectingRoutesCache &connecting_routes_cache,
      24                 :            :                             ::ad::map::lane::LaneIdSet const &mapAreaLanes)
      25                 :         28 :     : mEgoVehicleData(egoVehicleData)
      26                 :         28 :     , mRssRoute(rss_route)
      27                 :         28 :     , mWorldModelCreation(time_index, mEgoVehicleData.mRssObjectData.rss_dynamics, connecting_routes_cache)
      28                 :         28 :     , mMapAreaLanes(mapAreaLanes)
      29                 :            :   {
      30                 :         28 :     mWorldModelCreation.setRouteId(mRssRoute.route_id);
      31                 :         28 :     auto egoVehicleAdapter = mEgoVehicleData.getAdapter();
      32         [ -  + ]:         28 :     if (!bool(egoVehicleAdapter))
      33                 :            :     {
      34         [ #  # ]:          0 :       throw std::runtime_error("RssRouteChecker::ParallelWorldModelCreator>> ego_vehicle adapter invalid");
      35                 :            :     }
      36                 :         28 :     mWorldModelCreation.setMinimumDistanceToObjectsThatHaveToBeAnalyzed(
      37         [ +  - ]:         28 :       egoVehicleAdapter->getMinimumDistanceToObjectsThatHaveToBeAnalyzed());
      38                 :            : 
      39         [ +  - ]:         28 :     mGreenTrafficLightsOnRoute = egoVehicleAdapter->getGreenTrafficLightsOnRoute(mRssRoute.route);
      40         [ +  - ]:         28 :     auto const roadBoundariesMode = egoVehicleAdapter->getRoadBoundariesMode(mRssRoute.route);
      41         [ +  - ]:         28 :     ::ad::rss::map::RssObjectData egoObjectData = mEgoVehicleData.mRssObjectData;
      42                 :            :     // don't expect acceleration towards boundaries ('soft border')
      43                 :            :     // and ignore ego fluctuation margin to get smoother behavior
      44                 :         28 :     egoObjectData.rss_dynamics.alpha_lat.accel_max = ::ad::physics::Acceleration(0.);
      45                 :         28 :     egoObjectData.rss_dynamics.lateral_fluctuation_margin = ::ad::physics::Distance(0.);
      46         [ +  - ]:         28 :     mWorldModelCreation.appendRoadBoundaries(egoObjectData, mRssRoute.route, roadBoundariesMode);
      47                 :         28 :   }
      48                 :            : 
      49                 :            :   ParallelWorldModelCreator(ParallelWorldModelCreator const &other) = delete;
      50                 :            :   ParallelWorldModelCreator &operator=(ParallelWorldModelCreator const &other) = delete;
      51                 :            : 
      52                 :            :   void
      53                 :          0 :   operator()(typename RssRouteCheckerPedestrianDataMap<OBJECT_INSTANCE_TYPE>::value_type &pedestrianToProcess) const
      54                 :            :   {
      55         [ #  # ]:          0 :     if (pedestrianToProcess.second.mActive)
      56                 :            :     {
      57                 :          0 :       auto egoVehicleAdapter = mEgoVehicleData.getAdapter();
      58         [ #  # ]:          0 :       if (!bool(egoVehicleAdapter))
      59                 :            :       {
      60         [ #  # ]:          0 :         throw std::runtime_error("RssRouteChecker::ParallelWorldModelCreator>> ego_vehicle adapter invalid");
      61                 :            :       }
      62                 :          0 :       auto pedestrianAdapter = pedestrianToProcess.second.getAdapter();
      63         [ #  # ]:          0 :       if (!bool(pedestrianAdapter))
      64                 :            :       {
      65         [ #  # ]:          0 :         throw std::runtime_error("RssRouteChecker::ParallelWorldModelCreator>> pedestrian adapter invalid");
      66                 :            :       }
      67         [ #  # ]:          0 :       ::ad::rss::map::RssObjectData egoObjectData = mEgoVehicleData.mRssObjectData;
      68         [ #  # ]:          0 :       ::ad::rss::map::RssObjectData pedestrianData = pedestrianToProcess.second.mRssObjectData;
      69         [ #  # ]:          0 :       auto rssConstellationResult = egoVehicleAdapter->getPedestrianConstellationOnRoute(
      70                 :            :         mRssRoute, egoObjectData, pedestrianAdapter, pedestrianData);
      71                 :          0 :       egoObjectData.rss_dynamics = rssConstellationResult.ego_vehicle_dynamics;
      72                 :          0 :       pedestrianData.rss_dynamics = rssConstellationResult.object_dynamics;
      73                 :            : 
      74                 :          0 :       mWorldModelCreation.appendConstellations(egoObjectData,
      75                 :          0 :                                                mRssRoute.route,
      76                 :            :                                                pedestrianData,
      77                 :            :                                                rssConstellationResult.restrict_speed_limit_mode,
      78         [ #  # ]:          0 :                                                mGreenTrafficLightsOnRoute,
      79                 :            :                                                rssConstellationResult.constellation_creation_mode,
      80                 :            :                                                mMapAreaLanes);
      81                 :          0 :     }
      82                 :          0 :   }
      83                 :            : 
      84                 :          4 :   void operator()(
      85                 :            :     typename RssRouteCheckerArtificialObjectDataMap<OBJECT_INSTANCE_TYPE>::value_type &artificialObjectToProcess) const
      86                 :            :   {
      87         [ +  - ]:          4 :     if (artificialObjectToProcess.second.mActive)
      88                 :            :     {
      89                 :          4 :       auto egoVehicleAdapter = mEgoVehicleData.getAdapter();
      90         [ -  + ]:          4 :       if (!bool(egoVehicleAdapter))
      91                 :            :       {
      92         [ #  # ]:          0 :         throw std::runtime_error("RssRouteChecker::ParallelWorldModelCreator>> ego_vehicle adapter invalid");
      93                 :            :       }
      94                 :          4 :       auto artificialObjectAdapter = artificialObjectToProcess.second.getAdapter();
      95         [ -  + ]:          4 :       if (!bool(artificialObjectAdapter))
      96                 :            :       {
      97         [ #  # ]:          0 :         throw std::runtime_error("RssRouteChecker::ParallelWorldModelCreator>> artificialObject adapter invalid");
      98                 :            :       }
      99         [ +  - ]:          4 :       ::ad::rss::map::RssObjectData egoObjectData = mEgoVehicleData.mRssObjectData;
     100         [ +  - ]:          4 :       ::ad::rss::map::RssObjectData artificialObjectData = artificialObjectToProcess.second.mRssObjectData;
     101         [ +  - ]:          4 :       auto rssConstellationResult = egoVehicleAdapter->getArtificialObjectConstellationOnRoute(
     102                 :            :         mRssRoute, egoObjectData, artificialObjectAdapter, artificialObjectData);
     103                 :          4 :       egoObjectData.rss_dynamics = rssConstellationResult.ego_vehicle_dynamics;
     104                 :          4 :       artificialObjectData.rss_dynamics = rssConstellationResult.object_dynamics;
     105                 :            : 
     106                 :          4 :       mWorldModelCreation.appendConstellations(egoObjectData,
     107                 :          4 :                                                mRssRoute.route,
     108                 :            :                                                artificialObjectData,
     109                 :            :                                                rssConstellationResult.restrict_speed_limit_mode,
     110         [ +  - ]:          4 :                                                mGreenTrafficLightsOnRoute,
     111                 :            :                                                rssConstellationResult.constellation_creation_mode,
     112                 :            :                                                mMapAreaLanes);
     113                 :          4 :     }
     114                 :          4 :   }
     115                 :            : 
     116                 :         24 :   void operator()(typename RssRouteCheckerVehicleDataMap<OBJECT_INSTANCE_TYPE>::value_type &vehicleToProcess) const
     117                 :            :   {
     118         [ +  - ]:         24 :     if (vehicleToProcess.second.mActive)
     119                 :            :     {
     120                 :         24 :       process(vehicleToProcess.second);
     121                 :            :     }
     122                 :         24 :   }
     123                 :            : 
     124                 :         48 :   void operator()(typename RssRouteCheckerEgoVehicleDataMap<OBJECT_INSTANCE_TYPE>::value_type &vehicleToProcess) const
     125                 :            :   {
     126         [ +  - ]:         48 :     if ((vehicleToProcess.second.mActive)
     127         [ +  + ]:         48 :         && (vehicleToProcess.second.mRssObjectData.id != mEgoVehicleData.mRssObjectData.id))
     128                 :            :     {
     129                 :         20 :       process(vehicleToProcess.second);
     130                 :            :     }
     131                 :         48 :   }
     132                 :            : 
     133                 :         44 :   void process(RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE> const &vehicleToProcess) const
     134                 :            :   {
     135                 :         44 :     auto egoVehicleAdapter = mEgoVehicleData.getAdapter();
     136         [ -  + ]:         44 :     if (!bool(egoVehicleAdapter))
     137                 :            :     {
     138         [ #  # ]:          0 :       throw std::runtime_error("RssRouteChecker::ParallelWorldModelCreator>> ego_vehicle adapter invalid");
     139                 :            :     }
     140                 :         44 :     auto vehicleAdapter = vehicleToProcess.getAdapter();
     141         [ -  + ]:         44 :     if (!bool(vehicleAdapter))
     142                 :            :     {
     143         [ #  # ]:          0 :       throw std::runtime_error("RssRouteChecker::ParallelWorldModelCreator>> vehicle adapter invalid");
     144                 :            :     }
     145         [ +  - ]:         44 :     ::ad::rss::map::RssObjectData egoObjectData = mEgoVehicleData.mRssObjectData;
     146         [ +  - ]:         44 :     ::ad::rss::map::RssObjectData vehicleData = vehicleToProcess.mRssObjectData;
     147         [ +  + ]:         44 :     if (vehicleData.type == ::ad::rss::world::ObjectType::EgoVehicle)
     148                 :            :     {
     149                 :         20 :       vehicleData.type = ::ad::rss::world::ObjectType::OtherVehicle;
     150                 :            :     }
     151                 :            :     auto rssConstellationResult
     152         [ +  - ]:         44 :       = egoVehicleAdapter->getVehicleConstellationOnRoute(mRssRoute, egoObjectData, vehicleAdapter, vehicleData);
     153                 :         44 :     egoObjectData.rss_dynamics = rssConstellationResult.ego_vehicle_dynamics;
     154                 :         44 :     vehicleData.rss_dynamics = rssConstellationResult.object_dynamics;
     155                 :            : 
     156         [ +  - ]:         44 :     if ((mEgoVehicleData.mRouteExtensionMode
     157                 :            :          == RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::RouteExtensionMode::eAllowMultipleRoutes)
     158         [ +  - ]:         44 :         && (vehicleToProcess.mRouteExtensionMode
     159                 :            :             == RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::RouteExtensionMode::eAllowMultipleRoutes))
     160                 :            :     {
     161                 :            :       // only can deploy the routes if all routes have been considered
     162                 :         44 :       mWorldModelCreation.appendConstellations(egoObjectData,
     163                 :         44 :                                                mRssRoute.route,
     164                 :            :                                                vehicleData,
     165                 :            :                                                rssConstellationResult.restrict_speed_limit_mode,
     166                 :         44 :                                                mGreenTrafficLightsOnRoute,
     167                 :            :                                                rssConstellationResult.constellation_creation_mode,
     168                 :         44 :                                                mEgoVehicleData.mRoutes,
     169         [ +  - ]:         44 :                                                vehicleToProcess.mRoutes,
     170                 :            :                                                mMapAreaLanes);
     171                 :            :     }
     172                 :            :     else
     173                 :            :     {
     174                 :          0 :       mWorldModelCreation.appendConstellations(egoObjectData,
     175                 :          0 :                                                mRssRoute.route,
     176                 :            :                                                vehicleData,
     177                 :            :                                                rssConstellationResult.restrict_speed_limit_mode,
     178         [ #  # ]:          0 :                                                mGreenTrafficLightsOnRoute,
     179                 :            :                                                rssConstellationResult.constellation_creation_mode,
     180                 :            :                                                mMapAreaLanes);
     181                 :            :     }
     182                 :         44 :   }
     183                 :            : 
     184                 :         28 :   ::ad::rss::world::WorldModel getWorldModel()
     185                 :            :   {
     186                 :         28 :     return mWorldModelCreation.getWorldModel();
     187                 :            :   }
     188                 :            : 
     189                 :            : private:
     190                 :            :   RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &mEgoVehicleData;
     191                 :            :   RssRoute const &mRssRoute;
     192                 :            :   // the constellation creation calls are thread-safe, therefore mark mutable here is ok.
     193                 :            :   mutable RssWorldModelCreation mWorldModelCreation;
     194                 :            :   ::ad::map::landmark::LandmarkIdSet mGreenTrafficLightsOnRoute;
     195                 :            :   ::ad::map::lane::LaneIdSet const &mMapAreaLanes;
     196                 :            : };
     197                 :            : 
     198                 :            : } // namespace map
     199                 :            : } // namespace rss
     200                 :            : } // namespace ad