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/RoutePrediction.hpp"
      10                 :            : 
      11                 :            : // need to make RouteTreeElement public if debug output is required!
      12                 :            : #define DEBUG_OUTPUT 0
      13                 :            : 
      14                 :            : #if DEBUG_OUTPUT
      15                 :            : #include <iostream>
      16                 :            : 
      17                 :            : namespace std {
      18                 :            : 
      19                 :            : inline ostream &operator<<(ostream &out, ad::map::route::planning::RoutePrediction::RouteTreeElement const &value)
      20                 :            : {
      21                 :            :   out << value.routingPoint.first.point << "|" << static_cast<int16_t>(value.routingPoint.first.direction) << " ("
      22                 :            :       << static_cast<double>(value.routingPoint.second.routeDistance) << "meters ,"
      23                 :            :       << static_cast<double>(value.routingPoint.second.routeDuration) << " seconds) [";
      24                 :            :   auto current = value.theParent;
      25                 :            :   while (current != nullptr)
      26                 :            :   {
      27                 :            :     out << current->routingPoint.first.point << ", ";
      28                 :            :     current = current->theParent;
      29                 :            :   }
      30                 :            :   out << "]";
      31                 :            :   return out;
      32                 :            : }
      33                 :            : 
      34                 :            : } // namespace std
      35                 :            : #endif
      36                 :            : 
      37                 :            : namespace ad {
      38                 :            : namespace map {
      39                 :            : namespace route {
      40                 :            : namespace planning {
      41                 :            : 
      42                 :         39 : RoutePrediction::RoutePrediction(const RoutingParaPoint &start,
      43                 :            :                                  physics::Distance const &predictionDistance,
      44                 :            :                                  physics::Duration const &predictionDuration,
      45                 :         39 :                                  Type typ)
      46         [ +  - ]:         39 :   : RouteExpander(start, start, predictionDistance, predictionDuration, typ)
      47                 :            : {
      48                 :         39 : }
      49                 :            : 
      50                 :          1 : RoutePrediction::RoutePrediction(const RoutingParaPoint &start, physics::Distance const &predictionDistance, Type typ)
      51   [ +  -  +  - ]:          1 :   : RouteExpander(start, start, predictionDistance, physics::Duration::getMax(), typ)
      52                 :            : {
      53                 :          1 : }
      54                 :            : 
      55                 :          0 : RoutePrediction::RoutePrediction(const RoutingParaPoint &start, physics::Duration const &predictionDuration, Type typ)
      56   [ #  #  #  # ]:          0 :   : RouteExpander(start, start, physics::Distance::getMax(), predictionDuration, typ)
      57                 :            : {
      58                 :          0 : }
      59                 :            : 
      60                 :         40 : bool RoutePrediction::calculate()
      61                 :            : {
      62                 :         40 :   mProcessedTransitions.clear();
      63                 :         40 :   mElementsToProcess.clear();
      64                 :         40 :   mRawRoutes.clear();
      65                 :            : 
      66         [ +  - ]:         40 :   RoutingPoint start;
      67                 :         40 :   start.first = getRoutingStart();
      68                 :         40 :   start.second = RoutingCost();
      69                 :            : 
      70         [ +  + ]:         40 :   if (getRoutingStart().direction == RoutingDirection::DONT_CARE)
      71                 :            :   {
      72                 :            :     // Don't care routing direction means the vehicle can drive forward or backward.
      73                 :            :     // Within the routing we have to replace this by concrete directions,
      74                 :            :     // otherwise the vehicle would be able to switch between forward/backward while driving on the route
      75                 :            :     // which especially is not meant by the don't care flag.
      76                 :          9 :     start.first.direction = RoutingDirection::POSITIVE;
      77                 :            :   }
      78         [ +  - ]:         40 :   mRouteTreeRoot = std::make_shared<RouteTreeElement>(nullptr, start);
      79         [ +  - ]:         40 :   mElementsToProcess.push_back(mRouteTreeRoot);
      80                 :            : 
      81         [ +  + ]:         40 :   if (getRoutingStart().direction == RoutingDirection::DONT_CARE)
      82                 :            :   {
      83                 :            :     // add the negative concrete start of prediction option by changing direction without cost
      84                 :          9 :     auto startOtherDirection = start;
      85                 :          9 :     startOtherDirection.first.direction = RoutingDirection::NEGATIVE;
      86         [ +  - ]:          9 :     insertNeighbor(start, startOtherDirection);
      87                 :            :   }
      88                 :            : 
      89         [ +  + ]:       1712 :   while (!mElementsToProcess.empty())
      90                 :            :   {
      91         [ +  - ]:       1672 :     expandNeighbors(mElementsToProcess.front()->routingPoint);
      92                 :       1672 :     mElementsToProcess.pop_front();
      93                 :            :   }
      94                 :            : 
      95         [ +  - ]:         40 :   reconstructPaths();
      96                 :         40 :   mRouteTreeRoot.reset();
      97                 :         80 :   return isValid();
      98                 :            : }
      99                 :            : 
     100                 :       2884 : void RoutePrediction::addNeighbor(lane::Lane::ConstPtr originLane,
     101                 :            :                                   RoutingPoint const &origin,
     102                 :            :                                   lane::Lane::ConstPtr neighborLane,
     103                 :            :                                   RoutingPoint const &neighbor,
     104                 :            :                                   ExpandReason const &expandReason)
     105                 :            : {
     106                 :            :   (void)originLane;
     107                 :            :   (void)neighborLane;
     108                 :            : 
     109         [ +  + ]:       2884 :   if (expandReason == ExpandReason::Destination)
     110                 :            :   {
     111                 :         47 :     return;
     112                 :            :   }
     113                 :            : 
     114                 :       2837 :   insertNeighbor(origin, neighbor);
     115                 :            : }
     116                 :            : 
     117                 :       2846 : void RoutePrediction::insertNeighbor(RoutingPoint const &origin, RoutingPoint const &neighbor)
     118                 :            : {
     119                 :       2846 :   auto currentTreeElement = mElementsToProcess.front();
     120                 :            : 
     121         [ +  - ]:       2846 :   auto newChildElement = std::make_shared<RouteTreeElement>(currentTreeElement.get(), neighbor);
     122   [ +  -  +  - ]:       2846 :   auto insertProcessedResult = mProcessedTransitions[origin.first].insert(newChildElement);
     123         [ +  + ]:       2846 :   if (insertProcessedResult.second)
     124                 :            :   {
     125         [ +  - ]:       1632 :     auto insertToProcessResult = currentTreeElement->children.insert(newChildElement);
     126         [ -  + ]:       1632 :     if (!insertToProcessResult.second)
     127                 :            :     {
     128                 :            :       // this child is already in the current element, nothing to be done
     129                 :          0 :       return;
     130                 :            :     }
     131                 :            :   }
     132                 :            :   else
     133                 :            :   {
     134                 :            :     // a similar child (a twin) was already already expanded, take that one as our child
     135         [ +  - ]:       1214 :     currentTreeElement->children.insert(*insertProcessedResult.first);
     136                 :            :     // nothing further to be done since already processed
     137                 :       1214 :     return;
     138                 :            :   }
     139                 :            : 
     140                 :            : #if DEBUG_OUTPUT
     141                 :            :   std::cout << "Adding neighbor" << *(newChildElement.get()) << std::endl;
     142                 :            : #endif
     143         [ +  - ]:       1632 :   mElementsToProcess.push_back(newChildElement);
     144                 :            : }
     145                 :            : 
     146                 :         40 : void RoutePrediction::reconstructPaths()
     147                 :            : {
     148                 :         40 :   mValid = bool(mRouteTreeRoot);
     149         [ +  - ]:         40 :   if (mValid)
     150                 :            :   {
     151         [ +  + ]:        984 :     for (auto const &processedTransition : mProcessedTransitions)
     152                 :            :     {
     153         [ +  + ]:       2576 :       for (auto const &element : processedTransition.second)
     154                 :            :       {
     155         [ +  + ]:       1632 :         if (element->children.empty())
     156                 :            :         {
     157                 :            :           // a leaf in the route tree
     158                 :        308 :           RawRoute rawRoute;
     159                 :        154 :           rawRoute.routeDistance = element->routingPoint.second.routeDistance;
     160                 :        154 :           rawRoute.routeDuration = element->routingPoint.second.routeDuration;
     161                 :            : #if DEBUG_OUTPUT
     162                 :            :           std::cout << "Leaf " << *element << std::endl;
     163                 :            : #endif
     164                 :        154 :           RouteTreeElement const *parent = element.get();
     165         [ +  + ]:       3333 :           while (parent != nullptr)
     166                 :            :           {
     167         [ +  - ]:       3179 :             rawRoute.paraPointList.insert(rawRoute.paraPointList.begin(), parent->routingPoint.first.point);
     168                 :       3179 :             parent = parent->theParent;
     169                 :            :           }
     170         [ +  - ]:        154 :           mRawRoutes.push_back(rawRoute);
     171                 :            : #if DEBUG_OUTPUT
     172                 :            :           std::cout << "Added path [" << mRawRoutes.size() << "]" << std::endl;
     173                 :            :           for (auto &point : rawRoute.paraPointList)
     174                 :            :           {
     175                 :            :             std::cout << " " << point << std::endl;
     176                 :            :           }
     177                 :            :           std::cout << "----------" << std::endl;
     178                 :            : #endif
     179                 :            :         }
     180                 :            :       }
     181                 :            :     }
     182                 :            :   }
     183                 :         40 : }
     184                 :            : 
     185                 :            : } // namespace planning
     186                 :            : } // namespace route
     187                 :            : } // namespace map
     188                 :            : } // namespace ad