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                 :            :  * @file
      10                 :            :  */
      11                 :            : 
      12                 :            : #pragma once
      13                 :            : 
      14                 :            : #include <deque>
      15                 :            : #include <map>
      16                 :            : #include "ad/map/route/RouteExpander.hpp"
      17                 :            : #include "ad/map/route/Routing.hpp"
      18                 :            : 
      19                 :            : /** @brief namespace ad */
      20                 :            : namespace ad {
      21                 :            : /** @brief namespace map */
      22                 :            : namespace map {
      23                 :            : /** @brief namespace route */
      24                 :            : namespace route {
      25                 :            : /**
      26                 :            :  * @brief provides route planning capabilities on the road network of the map
      27                 :            :  */
      28                 :            : namespace planning {
      29                 :            : 
      30                 :            : struct RoutePredictionScore
      31                 :            : {
      32                 :            : };
      33                 :            : 
      34                 :            : /**
      35                 :            :  * @brief Implements route prediction on the lane network.
      36                 :            :  */
      37                 :            : class RoutePrediction : public RouteExpander<RoutePredictionScore>
      38                 :            : {
      39                 :            : public:
      40                 :            :   using RouteExpander::RoutingPoint;
      41                 :            : 
      42                 :            :   /**
      43                 :            :    * @brief Constructor.
      44                 :            :    *
      45                 :            :    * Calculates route predictions from a starting point until either the given predictionDistance or the given
      46                 :            :    * predictionDuration is reached.
      47                 :            :    * Note: Route predictions will not stop in the middle of an intersection.
      48                 :            :    *   They continue until the intersection is left again.
      49                 :            :    *
      50                 :            :    * @param[in] start Start point.
      51                 :            :    * @param[in] predictionDistance maximum prediction distance.
      52                 :            :    * @param[in] predictionDuration maximum prediction duration.
      53                 :            :    * @param[in] typ Type of the route to be calculated (default Route::Type::SHORTEST).
      54                 :            :    */
      55                 :            :   RoutePrediction(const RoutingParaPoint &start,
      56                 :            :                   physics::Distance const &predictionDistance,
      57                 :            :                   physics::Duration const &predictionDuration,
      58                 :            :                   Type typ = Route::Type::SHORTEST);
      59                 :            : 
      60                 :            :   /**
      61                 :            :    * @brief Constructor.
      62                 :            :    *
      63                 :            :    * Calculates route predictions from a starting point until the given predictionDistance is reached.
      64                 :            :    * Note: Route predictions will not stop in the middle of an intersection.
      65                 :            :    *   They continue until the intersection is left again.
      66                 :            :    *
      67                 :            :    * @param[in] start Start point.
      68                 :            :    * @param[in] predictionDistance maximum prediction distance.
      69                 :            :    * @param[in] typ Type of the route to be calculated (default Route::Type::SHORTEST).
      70                 :            :    */
      71                 :            :   RoutePrediction(const RoutingParaPoint &start,
      72                 :            :                   physics::Distance const &predictionDistance,
      73                 :            :                   Type typ = Route::Type::SHORTEST);
      74                 :            : 
      75                 :            :   /**
      76                 :            :    * @brief Constructor.
      77                 :            :    *
      78                 :            :    * Calculates route predictions from a starting point until the given predictionDuration is reached.
      79                 :            :    * Note: Route predictions will not stop in the middle of an intersection.
      80                 :            :    *   They continue until the intersection is left again.
      81                 :            :    *
      82                 :            :    * @param[in] start Start point.
      83                 :            :    * @param[in] predictionDuration maximum prediction duration.
      84                 :            :    * @param[in] typ Type of the route to be calculated (default Route::Type::SHORTEST).
      85                 :            :    */
      86                 :            :   RoutePrediction(const RoutingParaPoint &start,
      87                 :            :                   physics::Duration const &predictionDuration,
      88                 :            :                   Type typ = Route::Type::SHORTEST);
      89                 :            : 
      90                 :            :   /**
      91                 :            :    * @brief Calculates the route predictions using breadth search algorithm.
      92                 :            :    */
      93                 :            :   bool calculate() override;
      94                 :            : 
      95                 :            : private:
      96                 :            :   /**
      97                 :            :    * @brief Reimplemented from RouteExpander::AddNeighbor()
      98                 :            :    */
      99                 :            :   void addNeighbor(lane::Lane::ConstPtr originLane,
     100                 :            :                    RoutingPoint const &origin,
     101                 :            :                    lane::Lane::ConstPtr neighborLane,
     102                 :            :                    RoutingPoint const &neighbor,
     103                 :            :                    ExpandReason const &expandReason) override;
     104                 :            : 
     105                 :            :   /**
     106                 :            :    * @brief actually insert neighbor in data structures
     107                 :            :    */
     108                 :            :   void insertNeighbor(RoutingPoint const &origin, RoutingPoint const &neighbor);
     109                 :            : 
     110                 :            :   /**
     111                 :            :    * @brief Element of the routing tree
     112                 :            :    */
     113                 :            :   struct RouteTreeElement
     114                 :            :   {
     115                 :            :     //! Constructor.
     116                 :       2886 :     RouteTreeElement(RouteTreeElement const *parent, RoutingPoint const &iRoutingPoint)
     117                 :       2886 :       : routingPoint(iRoutingPoint)
     118                 :       2886 :       , theParent(parent)
     119                 :            :     {
     120                 :       2886 :     }
     121                 :            : 
     122                 :            :     //! comparing route tree elements by their actual route-point to ensure children are unique
     123                 :            :     struct RouteTreeElementCompare
     124                 :            :     {
     125                 :       8572 :       bool operator()(const std::shared_ptr<RouteTreeElement> &left,
     126                 :            :                       const std::shared_ptr<RouteTreeElement> &right) const
     127                 :            :       {
     128                 :       8572 :         return left->routingPoint.first < right->routingPoint.first;
     129                 :            :       }
     130                 :            :     };
     131                 :            : 
     132                 :            :     typedef std::set<std::shared_ptr<RouteTreeElement>, RouteTreeElementCompare> RouteTreeElementUniqueRoutePointSet;
     133                 :            : 
     134                 :            :     //! the routing point represented by this tree element
     135                 :            :     RoutingPoint routingPoint;
     136                 :            :     //! the children of the tree element. If children are empty, the element is a leaf of the tree
     137                 :            :     RouteTreeElementUniqueRoutePointSet children;
     138                 :            :     //! pointer to the parent(s) of this, required for later reconstruction of the paths
     139                 :            :     RouteTreeElement const *theParent;
     140                 :            :   };
     141                 :            : 
     142                 :            :   /**
     143                 :            :    * @brief reconstruct the path after search finished
     144                 :            :    */
     145                 :            :   void reconstructPaths();
     146                 :            : 
     147                 :            :   //! the tree root element
     148                 :            :   std::shared_ptr<RouteTreeElement> mRouteTreeRoot;
     149                 :            : 
     150                 :            :   /**
     151                 :            :    * @brief the already processed transitions from -> {to} (only process a transition once)
     152                 :            :    */
     153                 :            :   std::map<RoutingParaPoint, RouteTreeElement::RouteTreeElementUniqueRoutePointSet> mProcessedTransitions;
     154                 :            : 
     155                 :            :   //! the list of elements to be processed
     156                 :            :   std::deque<std::shared_ptr<RouteTreeElement>> mElementsToProcess;
     157                 :            : };
     158                 :            : 
     159                 :            : } // namespace planning
     160                 :            : } // namespace route
     161                 :            : } // namespace map
     162                 :            : } // namespace ad