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 <map>
      15                 :            : #include <memory>
      16                 :            : #include <sstream>
      17                 :            : #include <string>
      18                 :            : #include <vector>
      19                 :            : #include "ad/map/lane/Types.hpp"
      20                 :            : #include "ad/map/match/Types.hpp"
      21                 :            : #include "ad/map/point/Types.hpp"
      22                 :            : #include "ad/map/route/RouteOperation.hpp"
      23                 :            : 
      24                 :            : /** @brief namespace ad */
      25                 :            : namespace ad {
      26                 :            : /** @brief namespace map */
      27                 :            : namespace map {
      28                 :            : /** @brief namespace intersection */
      29                 :            : namespace intersection {
      30                 :            : 
      31                 :            : /**
      32                 :            :  * @brief forward declaration of CoreIntersection class
      33                 :            :  */
      34                 :            : class CoreIntersection;
      35                 :            : 
      36                 :            : /**
      37                 :            :  * @brief typedef for shared_ptr of CoreIntersection class
      38                 :            :  */
      39                 :            : typedef std::shared_ptr<CoreIntersection> CoreIntersectionPtr;
      40                 :            : 
      41                 :            : /**
      42                 :            :  * @brief typedef for shared_ptr of const CoreIntersection class
      43                 :            :  */
      44                 :            : typedef std::shared_ptr<CoreIntersection const> CoreIntersectionConstPtr;
      45                 :            : 
      46                 :            : /**
      47                 :            :  * Class to provide basic intersection information.
      48                 :            :  *
      49                 :            :  *  The CoreIntersection class provides:
      50                 :            :  *  - the list of all lanes of the intersection and their extends (see CoreIntersection::internalLanes() and
      51                 :            :  * CoreIntersection::getBoundingSphere())
      52                 :            :  *  - the list of lanes entering the intersection (see CoreIntersection::entryLanes() and
      53                 :            :  * CoreIntersection::entryParaPoints())
      54                 :            :  *  - the list of lanes exiting the intersection (see CoreIntersection::exitLanes() and
      55                 :            :  * CoreIntersection::exitParaPoints())
      56                 :            :  *  - supporting functions to check how lanes, routes, objects are interacting with the intersection
      57                 :            :  */
      58                 :            : class CoreIntersection
      59                 :            : {
      60                 :            : public:
      61                 :            :   /**
      62                 :            :    * @brief check if a lane is part of any intersection
      63                 :            :    */
      64                 :            :   static bool isLanePartOfAnIntersection(lane::LaneId const laneId);
      65                 :            : 
      66                 :            :   /**
      67                 :            :    * @brief check if any lane in the route is part of any intersection
      68                 :            :    *
      69                 :            :    * @param[in] route planned route
      70                 :            :    *
      71                 :            :    * @return If there is an intersection within the route, \c true is returned.
      72                 :            :    */
      73                 :            :   static bool isRoutePartOfAnIntersection(route::FullRoute const &route);
      74                 :            : 
      75                 :            :   /**
      76                 :            :    * @brief check if there is an intersection for the given route
      77                 :            :    *
      78                 :            :    * @param[in] route planned route
      79                 :            :    *
      80                 :            :    * @return If there is an intersection within the route, \c true is returned.
      81                 :            :    */
      82                 :            :   static bool isIntersectionOnRoute(route::FullRoute const &route);
      83                 :            : 
      84                 :            :   /**
      85                 :            :    * @brief check if the road segment enters an intersection
      86                 :            :    *
      87                 :            :    * @param[in] routeIterator the route iterator of the road segment
      88                 :            :    * @param[out] routePreviousSegmentIter if an intersectin is entered,
      89                 :            :    *   this holds the previous route segment that is part of the transition
      90                 :            :    *
      91                 :            :    * @returns \c true if given routeIterator enters an intersection
      92                 :            :    */
      93                 :            :   static bool isRoadSegmentEnteringIntersection(route::RouteIterator const &routeIterator,
      94                 :            :                                                 route::RoadSegmentList::const_iterator &routePreviousSegmentIter);
      95                 :            : 
      96                 :            :   /**
      97                 :            :    * @returns \c true if the provided \c objectRoute contains an internal lane
      98                 :            :    *
      99                 :            :    * This is the case if one of the internalLanes()
     100                 :            :    * is part of the objectRoute.
     101                 :            :    */
     102                 :            :   bool objectRouteCrossesIntersection(route::FullRoute const &objectRoute) const;
     103                 :            : 
     104                 :            :   /** @brief calculate and return the physics::Distance of the object to the intersection */
     105                 :            :   physics::Distance objectDistanceToIntersection(match::Object const &object) const;
     106                 :            : 
     107                 :            :   /** @returns \c true if the object is within the intersection (touches one of the internalLanes) */
     108                 :            :   bool objectWithinIntersection(match::MapMatchedObjectBoundingBox const &object) const;
     109                 :            : 
     110                 :            :   /** @return all lanes that are inside the intersection (independent of route) */
     111                 :            :   lane::LaneIdSet const &internalLanes() const;
     112                 :            : 
     113                 :            :   /** @return all lanes that lead into the intersection  (not part of the intersection themselves) */
     114                 :            :   lane::LaneIdSet const &entryLanes() const;
     115                 :            : 
     116                 :            :   /** @return the border points of all lanes that lead into the intersection as ParaPoint's */
     117                 :            :   point::ParaPointList const &entryParaPoints() const;
     118                 :            : 
     119                 :            :   /** @return all lanes that lead out of the intersection (not part of the intersection themselves) */
     120                 :            :   lane::LaneIdSet const &exitLanes() const;
     121                 :            : 
     122                 :            :   /** @return the border points of all lanes that lead out of the intersection as ParaPoint's */
     123                 :            :   point::ParaPointList const &exitParaPoints() const;
     124                 :            : 
     125                 :            :   /** @return the bounding sphere of all the inner lanes of the intersection **/
     126                 :          0 :   point::BoundingSphere const &getBoundingSphere() const
     127                 :            :   {
     128                 :          0 :     return mInternalLanesBoundingSphere;
     129                 :            :   }
     130                 :            : 
     131                 :            :   /**
     132                 :            :    * @brief retrieve the core intersection for the given laneId
     133                 :            :    *
     134                 :            :    * @param[in] laneId the laneId to use for creation of the CoreIntersection
     135                 :            :    *
     136                 :            :    * @return if the provided \a laneId is part of an intersection a CoreIntersection object is created and returned.
     137                 :            :    */
     138                 :            :   static CoreIntersectionPtr getCoreIntersectionFor(lane::LaneId const &laneId);
     139                 :            : 
     140                 :            :   /**
     141                 :            :    * @brief retrieve all CoreIntersection objects for the given lane Ids
     142                 :            :    *
     143                 :            :    * @param[in] laneIds the set of laneIds to use for creation of the CoreIntersection
     144                 :            :    *
     145                 :            :    * @return if one of the provided lane Ids is part of an intersection a CoreIntersection object is created and
     146                 :            :    * returned.
     147                 :            :    * if multiple lane Ids are part of the same CoreIntersection only one single CoreIntersection is returned.
     148                 :            :    * if lane Ids are part of different CoreIntersections, multiple CoreIntersections are returned.
     149                 :            :    * In summary: the CoreIntersections of all lanedIds are returned, so that
     150                 :            :    * every laneId of the provided \a laneIds set that belongs to an intersection is present in one of the returned
     151                 :            :    * CoreIntersection::internalLanes() set.
     152                 :            :    */
     153                 :            :   static std::vector<CoreIntersectionPtr> getCoreIntersectionsFor(lane::LaneIdSet const &laneIds);
     154                 :            : 
     155                 :            :   /**
     156                 :            :    * @brief retrieve all CoreIntersection objects for the given lane Ids
     157                 :            :    *
     158                 :            :    * @param[in] laneIds the vector of laneIds to use for creation of the CoreIntersection
     159                 :            :    *
     160                 :            :    * @return if one of the provided lane Ids is part of an intersection a CoreIntersection object is created and
     161                 :            :    * returned.
     162                 :            :    * if multiple lane Ids are part of the same CoreIntersection only one single CoreIntersection is returned.
     163                 :            :    * if lane Ids are part of different CoreIntersections, multiple CoreIntersections are returned.
     164                 :            :    * In summary: the CoreIntersections of all lanedIds are returned, so that
     165                 :            :    * every laneId of the provided \a laneIds set that belongs to an intersection is present in one of the returned
     166                 :            :    * CoreIntersection::internalLanes() set.
     167                 :            :    */
     168                 :            :   static std::vector<CoreIntersectionPtr> getCoreIntersectionsFor(lane::LaneIdList const &laneIds);
     169                 :            : 
     170                 :            :   /**
     171                 :            :    * @brief retrieve all CoreIntersection objects of the map
     172                 :            :    *
     173                 :            :    * @return getCoreIntersectionsFor(lane::getLanes())
     174                 :            :    */
     175                 :            :   static std::vector<CoreIntersectionPtr> getCoreIntersectionsForMap();
     176                 :            : 
     177                 :            :   /**
     178                 :            :    * @brief retrieve the core intersection for the given mapMatchedPosition
     179                 :            :    *
     180                 :            :    * @returns getCoreIntersectionFor(mapMatchedPosition.lanePoint.paraPoint.laneId)
     181                 :            :    **/
     182                 :            :   static CoreIntersectionPtr getCoreIntersectionFor(match::MapMatchedPosition const &mapMatchedPosition);
     183                 :            : 
     184                 :            :   /**
     185                 :            :    * @brief retrieve the core intersection for the given ENU \a position
     186                 :            :    *
     187                 :            :    * Performs map matching of the given \a position and
     188                 :            :    * returns getCoreIntersectionsForInLaneMatches(MapMatchedPositionConfidenceList) of the matching result.
     189                 :            :    */
     190                 :            :   static std::vector<CoreIntersectionPtr> getCoreIntersectionsForInLaneMatches(point::ENUPoint const &position);
     191                 :            : 
     192                 :            :   /**
     193                 :            :    * @brief retrieve the core intersection for the given mapMatchedPositionConfidenceList
     194                 :            :    *
     195                 :            :    * Collects all in lane matches part of the \a mapMatchedPositionConfidenceList in a LaneIdSet and returns the vector
     196                 :            :    *of CoreIntersection
     197                 :            :    * returned by getCoreIntersectionsFor(laneIdSet).
     198                 :            :    * Usually the result vector size is zero or one. In cases where multiple intersections are on top of each other and
     199                 :            :    *the map matching
     200                 :            :    * distance exceeded the altitude difference of the intersections, multiple CoreIntersection results are possible.
     201                 :            :    **/
     202                 :            :   static std::vector<CoreIntersectionPtr>
     203                 :            :   getCoreIntersectionsForInLaneMatches(match::MapMatchedPositionConfidenceList const &mapMatchedPositionConfidenceList);
     204                 :            : 
     205                 :            :   /**
     206                 :            :    * @brief retrieve the core intersection for the given object
     207                 :            :    *
     208                 :            :    * @returns Collects all in lane matches part of the \a MapMatchedObjectBoundingBox in a LaneIdSet and returns the
     209                 :            :    *vector of CoreIntersection
     210                 :            :    * returned by getCoreIntersectionsFor(laneIdSet).
     211                 :            :    * Usually the result vector size is zero or one. In cases where multiple intersections are on top of each other and
     212                 :            :    *the map matching
     213                 :            :    * distance exceeded the altitude difference of the intersections, multiple CoreIntersection results are possible.
     214                 :            :    **/
     215                 :            :   static std::vector<CoreIntersectionPtr>
     216                 :            :   getCoreIntersectionsForInLaneMatches(match::MapMatchedObjectBoundingBox const &object);
     217                 :            : 
     218                 :            : protected:
     219         [ +  - ]:        111 :   CoreIntersection() = default;
     220                 :            : 
     221                 :            :   //! check if given lane is inside the intersection
     222                 :            :   bool isLanePartOfCoreIntersection(lane::LaneId const laneId) const;
     223                 :            : 
     224                 :            :   void extractLanesOfCoreIntersection(lane::LaneId const laneId);
     225                 :            : 
     226                 :            :   enum SuccessorMode
     227                 :            :   {
     228                 :            :     OwnIntersection,
     229                 :            :     AnyIntersection
     230                 :            :   };
     231                 :            : 
     232                 :            :   bool isLanePartOfIntersection(lane::LaneId const laneId, SuccessorMode const successorMode) const;
     233                 :            : 
     234                 :            :   /**
     235                 :            :    * @brief Provide the direct successor lane segments in lane direction within and outside of the intersection.
     236                 :            :    *
     237                 :            :    * @param laneId LaneId
     238                 :            :    * @return a pair with <the laneID segments within the intersection, the laneID segments outside the intersection>
     239                 :            :    */
     240                 :            :   std::pair<lane::LaneIdSet, lane::LaneIdSet>
     241                 :            :   getDirectSuccessorsInLaneDirection(lane::LaneId const laneId, SuccessorMode const successorMode) const;
     242                 :            : 
     243                 :            :   /**
     244                 :            :    * @brief Provide the direct successor lane segments in lane direction within the intersection.
     245                 :            :    *
     246                 :            :    * @param laneId LaneId
     247                 :            :    * @return the laneID segments within the intersection.
     248                 :            :    */
     249                 :            :   lane::LaneIdSet getDirectSuccessorsInLaneDirectionWithinIntersection(lane::LaneId const laneId,
     250                 :            :                                                                        SuccessorMode const successorMode) const;
     251                 :            : 
     252                 :            :   /**
     253                 :            :    * @brief Provide the successor lane segments in lane direction within the intersection recursively until the
     254                 :            :    * intersection is left
     255                 :            :    *
     256                 :            :    * @param laneId LaneId
     257                 :            :    * @return the laneID segments within the intersection.
     258                 :            :    */
     259                 :            :   lane::LaneIdSet getAllSuccessorsInLaneDirectionWithinIntersection(lane::LaneId const laneId,
     260                 :            :                                                                     SuccessorMode const successorMode) const;
     261                 :            : 
     262                 :            :   /**
     263                 :            :    * @brief Provide the successor lane segments in lane direction within the intersection recursively until the
     264                 :            :    * intersection is left including the input laneId if part of the inner lanes.
     265                 :            :    *
     266                 :            :    * @param laneId LaneId
     267                 :            :    * @return the laneID segments within the intersection.
     268                 :            :    */
     269                 :            :   lane::LaneIdSet getLaneAndAllSuccessorsInLaneDirectionWithinIntersection(lane::LaneId const laneId,
     270                 :            :                                                                            SuccessorMode const successorMode) const;
     271                 :            : 
     272                 :            :   /**
     273                 :            :    * @brief Provide the outgoing lane segments that are reachable in lane direction
     274                 :            :    *
     275                 :            :    * @param laneId LaneId
     276                 :            :    * @return the outgoing laneID segments outside the intersection.
     277                 :            :    */
     278                 :            :   lane::LaneIdSet getAllReachableOutgoingLanes(lane::LaneId const laneId, SuccessorMode const successorMode) const;
     279                 :            : 
     280                 :            :   /**
     281                 :            :    * @brief Provide the outgoing lane segments that are reachable in lane direction as well as the intersection internal
     282                 :            :    * lanes
     283                 :            :    *
     284                 :            :    * @param laneId LaneId
     285                 :            :    * @return a pair with <the laneID segments within the intersection, the laneID segments outside the intersection>
     286                 :            :    */
     287                 :            :   std::pair<lane::LaneIdSet, lane::LaneIdSet>
     288                 :            :   getAllReachableInternalAndOutgoingLanes(lane::LaneId const laneId, SuccessorMode const successorMode) const;
     289                 :            : 
     290                 :            :   point::ParaPoint getEntryParaPointOfExternalLane(lane::LaneId const &laneId) const;
     291                 :            :   point::ParaPoint getExitParaPointOfExternalLane(lane::LaneId const &laneId) const;
     292                 :            :   point::ParaPoint getEntryParaPointOfInternalLane(lane::LaneId const &laneId) const;
     293                 :            :   point::ParaPoint getExitParaPointOfInternalLane(lane::LaneId const &laneId) const;
     294                 :            : 
     295                 :            :   //! all lanes inside the intersection
     296                 :            :   lane::LaneIdSet mInternalLanes{};
     297                 :            : 
     298                 :            :   //! lanes going into the intersection
     299                 :            :   lane::LaneIdSet mEntryLanes{};
     300                 :            : 
     301                 :            :   //! lanes going into the intersection represented as ParaPoint
     302                 :            :   point::ParaPointList mEntryParaPoints{};
     303                 :            : 
     304                 :            :   //! lanes going out of the intersection
     305                 :            :   lane::LaneIdSet mExitLanes{};
     306                 :            : 
     307                 :            :   //! lanes going out of the intersection represented as ParaPoint
     308                 :            :   point::ParaPointList mExitParaPoints{};
     309                 :            : 
     310                 :            :   /**
     311                 :            :    * Managing relations between lanes through separate maps with sets. Reading:
     312                 :            :    * key: id of lane
     313                 :            :    * value: set of lanes that relate with this one (e.g. overlap)
     314                 :            :    */
     315                 :            :   std::map<lane::LaneId, lane::LaneIdSet> mOverlapping;
     316                 :            :   std::map<lane::LaneId, lane::LaneIdSet> mSuccessor;
     317                 :            :   std::map<lane::LaneId, lane::LaneIdSet> mPredecessor;
     318                 :            : 
     319                 :            :   point::BoundingSphere mInternalLanesBoundingSphere;
     320                 :            : 
     321                 :            : private:
     322                 :            :   explicit CoreIntersection(lane::LaneId const &laneId);
     323                 :            : 
     324                 :            :   void checkAndInsertEntryLane(lane::LaneId const laneId);
     325                 :            :   void checkAndInsertExitLane(lane::LaneId const laneId);
     326                 :            :   void processContactsForLane(lane::Lane const &lane, lane::ContactLane const &contact);
     327                 :            : 
     328                 :            :   template <typename CONTAINER>
     329                 :            :   static std::vector<CoreIntersectionPtr> getCoreIntersectionsForLaneIds(CONTAINER const &laneIds);
     330                 :            : };
     331                 :            : 
     332                 :            : } // namespace intersection
     333                 :            : } // namespace map
     334                 :            : } // namespace ad
     335                 :            : 
     336                 :            : namespace std {
     337                 :            : 
     338                 :            : /**
     339                 :            :  * \brief standard ostream operator for ad::map::intersection::CoreIntersection
     340                 :            :  *
     341                 :            :  * \param[in] os The output stream to write to
     342                 :            :  * \param[in] intersection The core intersection object
     343                 :            :  *
     344                 :            :  * \returns The stream object.
     345                 :            :  *
     346                 :            :  */
     347                 :            : std::ostream &operator<<(std::ostream &os, ::ad::map::intersection::CoreIntersection const &intersection);
     348                 :            : 
     349                 :            : /**
     350                 :            :  * \brief overload of the std::to_string for ad::map::intersection::CoreIntersection
     351                 :            :  */
     352                 :            : static inline std::string to_string(::ad::map::intersection::CoreIntersection const &intersection)
     353                 :            : {
     354                 :            :   stringstream sstream;
     355                 :            :   sstream << intersection;
     356                 :            :   return sstream.str();
     357                 :            : }
     358                 :            : } // namespace std