Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2018-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: MIT
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "ad/map/route/Route.hpp"
      10                 :            : #include "ad/map/lane/LaneOperation.hpp"
      11                 :            : 
      12                 :            : namespace ad {
      13                 :            : namespace map {
      14                 :            : namespace route {
      15                 :            : namespace planning {
      16                 :            : 
      17                 :       2763 : Route::Route(const RoutingParaPoint &start,
      18                 :            :              const RoutingParaPoint &dest,
      19                 :            :              physics::Distance const &maxDistance,
      20                 :            :              physics::Duration const &maxDuration,
      21                 :       2763 :              Type const &routingType)
      22                 :            :   : mStart(start)
      23                 :            :   , mDest(dest)
      24                 :            :   , mMaxDistance(maxDistance)
      25                 :            :   , mMaxDuration(maxDuration)
      26                 :       2763 :   , mType(routingType)
      27                 :       2763 :   , mValid(false)
      28                 :            : {
      29         [ -  + ]:       2763 :   if (mType == Type::INVALID)
      30                 :            :   {
      31         [ #  # ]:          0 :     throw std::runtime_error("type INVALID");
      32                 :            :   }
      33                 :       2763 : }
      34                 :            : 
      35                 :     115518 : bool Route::laneDirectionIsIgnored() const
      36                 :            : {
      37         [ +  + ]:     115518 :   switch (mType)
      38                 :            :   {
      39                 :        912 :     case Type::SHORTEST_IGNORE_DIRECTION:
      40                 :        912 :       return true;
      41                 :     114606 :     case Type::SHORTEST:
      42                 :            :     default:
      43                 :     114606 :       return false;
      44                 :            :   }
      45                 :            : }
      46                 :            : 
      47                 :       2711 : const Route::RawRoute &Route::getRawRoute(size_t const routeIndex) const
      48                 :            : {
      49   [ +  +  +  - ]:       2711 :   static RawRoute const emptyRoute;
      50         [ +  - ]:       2711 :   if (mRawRoutes.size() > routeIndex)
      51                 :            :   {
      52                 :       2711 :     return mRawRoutes[routeIndex];
      53                 :            :   }
      54                 :          0 :   return emptyRoute;
      55                 :            : }
      56                 :            : 
      57                 :          2 : Route::BasicRoute Route::getBasicRoute(size_t const routeIndex) const
      58                 :            : {
      59   [ +  -  +  - ]:          4 :   auto rawRoute = getRawRoute(routeIndex);
      60                 :          2 :   BasicRoute fr;
      61         [ +  + ]:          9 :   for (size_t i = 0; i < rawRoute.paraPointList.size(); i++)
      62                 :            :   {
      63                 :          7 :     const point::ParaPoint &paraPoint = rawRoute.paraPointList[i];
      64                 :         14 :     point::ParaPointList pps;
      65         [ +  - ]:          7 :     pps.push_back(paraPoint);
      66         [ +  + ]:         21 :     for (auto contactLocation : {lane::ContactLocation::LEFT, lane::ContactLocation::RIGHT})
      67                 :            :     {
      68   [ +  -  +  + ]:         33 :       for (lane::Lane::ConstPtr lane = lane::getLanePtr(paraPoint.laneId); lane;)
      69                 :            :       {
      70                 :         19 :         lane::LaneDirection const direction = lane->direction;
      71         [ +  - ]:         38 :         lane::ContactLaneList const cl = getContactLanes(*lane, contactLocation);
      72                 :         19 :         lane = nullptr;
      73   [ +  +  +  - ]:         38 :         for (auto contact : cl)
      74                 :            :         {
      75                 :         19 :           lane::LaneId otherLaneId = contact.toLane;
      76                 :         19 :           bool isPrev = false;
      77         [ +  + ]:         19 :           if (i > 0)
      78                 :            :           {
      79         [ +  - ]:         13 :             isPrev = otherLaneId == rawRoute.paraPointList[i - 1].laneId;
      80                 :            :           }
      81                 :         19 :           bool isNext = false;
      82         [ +  + ]:         19 :           if (i + 1 < rawRoute.paraPointList.size())
      83                 :            :           {
      84         [ +  - ]:         13 :             isNext = otherLaneId == rawRoute.paraPointList[i + 1].laneId;
      85                 :            :           }
      86   [ +  +  +  + ]:         19 :           if (!isNext && !isPrev)
      87                 :            :           {
      88         [ +  - ]:         34 :             lane::Lane::ConstPtr other_lane = lane::getLanePtr(otherLaneId);
      89   [ +  +  +  -  :         17 :             if ((direction == other_lane->direction) || laneDirectionIsIgnored())
             -  +  +  + ]
      90                 :            :             {
      91         [ +  - ]:          5 :               point::ParaPoint ppt;
      92                 :          5 :               ppt.laneId = otherLaneId;
      93                 :          5 :               ppt.parametricOffset = paraPoint.parametricOffset;
      94         [ +  - ]:          5 :               pps.push_back(ppt);
      95                 :          5 :               lane = other_lane;
      96                 :            :             }
      97                 :            :           }
      98                 :            :         }
      99                 :            :       }
     100                 :            :     }
     101         [ +  - ]:          7 :     fr.push_back(pps);
     102                 :            :   }
     103                 :          4 :   return fr;
     104                 :            : }
     105                 :            : 
     106                 :          1 : std::vector<Route::BasicRoute> Route::getBasicRoutes() const
     107                 :            : {
     108                 :          1 :   std::vector<BasicRoute> routeVector;
     109         [ +  - ]:          1 :   routeVector.resize(mRawRoutes.size());
     110         [ +  + ]:          3 :   for (size_t i = 0u; i < mRawRoutes.size(); ++i)
     111                 :            :   {
     112         [ +  - ]:          2 :     routeVector[i] = getBasicRoute(i);
     113                 :            :   }
     114                 :          1 :   return routeVector;
     115                 :            : }
     116                 :            : 
     117                 :            : } // namespace planning
     118                 :            : } // namespace route
     119                 :            : } // namespace map
     120                 :            : } // namespace ad