Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2019-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: MIT
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "DataTypeConversion.hpp"
      10                 :            : #include "ad/map/point/Operation.hpp"
      11                 :            : 
      12                 :            : namespace ad {
      13                 :            : namespace map {
      14                 :            : namespace opendrive {
      15                 :            : 
      16                 :       1222 : landmark::LandmarkType toLandmarkType(int type)
      17                 :            : {
      18                 :            :   // as defined on the OpenDRIVEv1.4 spec chapter 6.11 Signal Types
      19                 :            :   // and on the OpenDRIVEv1.5 spec chapter 6.11 Signal Types
      20      [ +  +  + ]:       1222 :   switch (type)
      21                 :            :   {
      22                 :       1199 :     case 1000001:
      23                 :            :     case 1000002:
      24                 :            :     case 1000007:
      25                 :            :     case 1000008:
      26                 :            :     case 1000009:
      27                 :            :     case 1000010:
      28                 :            :     case 1000011:
      29                 :            :     case 1000013:
      30                 :            :     case 1000014:
      31                 :            :     case 1000015:
      32                 :            :     case 1000016:
      33                 :            :     case 1000017:
      34                 :            :     case 1000018:
      35                 :            :     case 1000019:
      36                 :            :     case 1000020:
      37                 :       1199 :       return landmark::LandmarkType::TRAFFIC_LIGHT;
      38                 :         11 :     case 1000003: // pedestrian crossing
      39                 :            :     case 1000004: // bicycle crossing
      40                 :         11 :       return landmark::LandmarkType::OTHER;
      41                 :         12 :     default:
      42                 :         12 :       break;
      43                 :            :   }
      44                 :            : 
      45   [ +  +  +  - ]:         12 :   if ((type > 100) && (type <= 631)) // as defined by the StVO signal types
      46                 :            :   {
      47                 :         11 :     return landmark::LandmarkType::TRAFFIC_SIGN;
      48                 :            :   }
      49                 :            :   else
      50                 :            :   {
      51                 :          1 :     return landmark::LandmarkType::UNKNOWN;
      52                 :            :   }
      53                 :            : }
      54                 :            : 
      55                 :         37 : landmark::TrafficSignType toTrafficSignType(int type, int subtype)
      56                 :            : {
      57                 :            :   // as defined by the StVO signal type
      58   [ +  +  +  +  :         37 :   switch (type)
          +  +  +  +  +  
          +  +  +  +  +  
          +  +  +  +  +  
             +  +  +  +  
                      + ]
      59                 :            :   {
      60                 :         11 :     case 101:
      61                 :         11 :       return landmark::TrafficSignType::DANGER;
      62                 :          1 :     case 102:
      63                 :            :     case 103:
      64                 :            :     case 105:
      65                 :            :     case 108:
      66                 :            :     case 110:
      67                 :            :     case 112:
      68                 :            :     case 114:
      69                 :          1 :       return landmark::TrafficSignType::DANGER;
      70                 :          1 :     case 120:
      71                 :            :     case 121:
      72                 :          1 :       return landmark::TrafficSignType::LANES_MERGING;
      73                 :          1 :     case 133:
      74                 :          1 :       return landmark::TrafficSignType::CAUTION_PEDESTRIAN;
      75                 :          1 :     case 138:
      76                 :          1 :       return landmark::TrafficSignType::CAUTION_BICYCLE;
      77                 :          1 :     case 205:
      78                 :          1 :       return landmark::TrafficSignType::YIELD;
      79                 :          1 :     case 206:
      80                 :          1 :       return landmark::TrafficSignType::STOP;
      81                 :          1 :     case 215:
      82                 :          1 :       return landmark::TrafficSignType::ROUNDABOUT;
      83                 :          1 :     case 222:
      84                 :          1 :       return landmark::TrafficSignType::PASS_RIGHT;
      85                 :          1 :     case 250:
      86                 :          1 :       return landmark::TrafficSignType::ACCESS_FORBIDDEN;
      87                 :          1 :     case 251:
      88                 :          1 :       return landmark::TrafficSignType::ACCESS_FORBIDDEN_MOTORVEHICLES;
      89                 :          1 :     case 253:
      90                 :          1 :       return landmark::TrafficSignType::ACCESS_FORBIDDEN_TRUCKS;
      91                 :          1 :     case 254:
      92                 :          1 :       return landmark::TrafficSignType::ACCESS_FORBIDDEN_BICYCLE;
      93                 :          1 :     case 263:
      94                 :          1 :       return landmark::TrafficSignType::ACCESS_FORBIDDEN_WEIGHT;
      95                 :          1 :     case 264:
      96                 :          1 :       return landmark::TrafficSignType::ACCESS_FORBIDDEN_WIDTH;
      97                 :          1 :     case 265:
      98                 :          1 :       return landmark::TrafficSignType::ACCESS_FORBIDDEN_HEIGHT;
      99                 :          1 :     case 267:
     100                 :          1 :       return landmark::TrafficSignType::ACCESS_FORBIDDEN_WRONG_DIR;
     101                 :          4 :     case 274:
     102         [ +  + ]:          4 :       if (subtype == -1)
     103                 :            :       {
     104                 :          1 :         return landmark::TrafficSignType::MAX_SPEED;
     105                 :            :       }
     106         [ +  + ]:          3 :       else if (subtype == 1)
     107                 :            :       {
     108                 :          1 :         return landmark::TrafficSignType::SPEED_ZONE_30_BEGIN;
     109                 :            :       }
     110         [ +  + ]:          2 :       else if (subtype == 2)
     111                 :            :       {
     112                 :          1 :         return landmark::TrafficSignType::SPEED_ZONE_30_END;
     113                 :            :       }
     114                 :            :       else
     115                 :            :       {
     116                 :          1 :         return landmark::TrafficSignType::MAX_SPEED;
     117                 :            :       }
     118                 :          1 :     case 301:
     119                 :          1 :       return landmark::TrafficSignType::HAS_WAY_NEXT_INTERSECTION;
     120                 :          1 :     case 306:
     121                 :          1 :       return landmark::TrafficSignType::PRIORITY_WAY;
     122                 :          1 :     case 310:
     123                 :          1 :       return landmark::TrafficSignType::CITY_BEGIN;
     124                 :          1 :     case 311:
     125                 :          1 :       return landmark::TrafficSignType::CITY_END;
     126                 :          1 :     case 331:
     127                 :          1 :       return landmark::TrafficSignType::MOTORVEHICLE_BEGIN;
     128                 :          1 :     default:
     129                 :          1 :       return landmark::TrafficSignType::UNKNOWN;
     130                 :            :   }
     131                 :            : }
     132                 :            : 
     133                 :       2917 : lane::ContactType toContactType(int type)
     134                 :            : {
     135   [ +  +  +  +  :       2917 :   switch (type)
                   +  + ]
     136                 :            :   {
     137                 :          1 :     case 205:
     138                 :          1 :       return lane::ContactType::YIELD;
     139                 :          1 :     case 206:
     140                 :          1 :       return lane::ContactType::STOP;
     141                 :          1 :     case 301:
     142                 :          1 :       return lane::ContactType::RIGHT_OF_WAY; // next intersection only
     143                 :          1 :     case 306:
     144                 :          1 :       return lane::ContactType::RIGHT_OF_WAY;
     145                 :       2792 :     case 1000001:
     146                 :            :     case 1000008:
     147                 :            :     case 1000009:
     148                 :            :     case 1000010:
     149                 :            :     case 1000011:
     150                 :            :     case 1000012:
     151                 :       2792 :       return lane::ContactType::TRAFFIC_LIGHT;
     152                 :        121 :     default:
     153                 :        121 :       return lane::ContactType::UNKNOWN;
     154                 :            :   }
     155                 :            : }
     156                 :            : 
     157                 :       2915 : lane::ContactLocation toContactLocation(::opendrive::SignalReference const &signalReference, bool const &isJunction)
     158                 :            : {
     159         [ +  + ]:       2915 :   if (isJunction)
     160                 :            :   {
     161         [ +  + ]:       2733 :     if (signalReference.parametricPosition >= 0.5)
     162                 :            :     {
     163                 :       1345 :       return lane::ContactLocation::SUCCESSOR;
     164                 :            :     }
     165                 :            :     else
     166                 :            :     {
     167                 :       1388 :       return lane::ContactLocation::PREDECESSOR;
     168                 :            :     }
     169                 :            :   }
     170                 :            :   else
     171                 :            :   {
     172         [ +  + ]:        182 :     if (signalReference.inLaneOrientation)
     173                 :            :     {
     174                 :         91 :       return lane::ContactLocation::SUCCESSOR;
     175                 :            :     }
     176                 :            :     else
     177                 :            :     {
     178                 :         91 :       return lane::ContactLocation::PREDECESSOR;
     179                 :            :     }
     180                 :            :   }
     181                 :            : }
     182                 :            : 
     183                 :       1215 : landmark::TrafficLightType toTrafficLightType(int type, int subtype)
     184                 :            : {
     185   [ +  +  +  +  :       1215 :   switch (type)
          +  +  +  +  +  
                +  +  + ]
     186                 :            :   {
     187                 :       1199 :     case 1000001:
     188                 :       1199 :       return landmark::TrafficLightType::SOLID_RED_YELLOW_GREEN;
     189                 :          1 :     case 1000002:
     190                 :          1 :       return landmark::TrafficLightType::PEDESTRIAN_RED_GREEN;
     191                 :          1 :     case 1000007:
     192                 :          1 :       return landmark::TrafficLightType::BIKE_PEDESTRIAN_RED_GREEN;
     193                 :          1 :     case 1000008:
     194                 :          1 :       return landmark::TrafficLightType::UNKNOWN;
     195                 :          1 :     case 1000009:
     196                 :          1 :       return landmark::TrafficLightType::SOLID_RED_YELLOW;
     197                 :          1 :     case 1000010:
     198                 :          1 :       return landmark::TrafficLightType::UNKNOWN;
     199                 :          6 :     case 1000011:
     200         [ +  + ]:          6 :       if (subtype == 10)
     201                 :            :       {
     202                 :          1 :         return landmark::TrafficLightType::LEFT_RED_YELLOW_GREEN;
     203                 :            :       }
     204         [ +  + ]:          5 :       else if (subtype == 20)
     205                 :            :       {
     206                 :          1 :         return landmark::TrafficLightType::RIGHT_RED_YELLOW_GREEN;
     207                 :            :       }
     208         [ +  + ]:          4 :       else if (subtype == 30)
     209                 :            :       {
     210                 :          1 :         return landmark::TrafficLightType::STRAIGHT_RED_YELLOW_GREEN;
     211                 :            :       }
     212         [ +  + ]:          3 :       else if (subtype == 40)
     213                 :            :       {
     214                 :          1 :         return landmark::TrafficLightType::LEFT_STRAIGHT_RED_YELLOW_GREEN;
     215                 :            :       }
     216         [ +  + ]:          2 :       else if (subtype == 50)
     217                 :            :       {
     218                 :          1 :         return landmark::TrafficLightType::RIGHT_STRAIGHT_RED_YELLOW_GREEN;
     219                 :            :       }
     220                 :            :       else
     221                 :            :       {
     222                 :          1 :         return landmark::TrafficLightType::UNKNOWN;
     223                 :            :       }
     224                 :          1 :     case 1000012:
     225                 :          1 :       return landmark::TrafficLightType::UNKNOWN;
     226                 :          1 :     case 1000013:
     227                 :          1 :       return landmark::TrafficLightType::BIKE_RED_GREEN;
     228                 :          1 :     case 1000014:
     229                 :          1 :       return landmark::TrafficLightType::UNKNOWN;
     230                 :          1 :     case 1000015:
     231                 :          1 :       return landmark::TrafficLightType::UNKNOWN;
     232                 :          1 :     default:
     233                 :          1 :       return landmark::TrafficLightType::UNKNOWN;
     234                 :            :   }
     235                 :            : }
     236                 :            : 
     237                 :      23435 : lane::LaneType toLaneType(::opendrive::LaneType const &laneType)
     238                 :            : {
     239   [ +  +  -  +  :      23435 :   switch (laneType)
             -  -  +  - ]
     240                 :            :   {
     241                 :       9142 :     case ::opendrive::LaneType::Driving:
     242                 :            :     case ::opendrive::LaneType::Bidirectional:
     243                 :       9142 :       return lane::LaneType::NORMAL;
     244                 :       8063 :     case ::opendrive::LaneType::Shoulder:
     245                 :            :     case ::opendrive::LaneType::Border:
     246                 :            :     case ::opendrive::LaneType::Parking:
     247                 :       8063 :       return lane::LaneType::SHOULDER;
     248                 :          0 :     case ::opendrive::LaneType::Biking:
     249                 :          0 :       return lane::LaneType::BIKE;
     250                 :       4950 :     case ::opendrive::LaneType::Sidewalk:
     251                 :       4950 :       return lane::LaneType::PEDESTRIAN;
     252                 :          0 :     case ::opendrive::LaneType::Stop:
     253                 :          0 :       return lane::LaneType::EMERGENCY;
     254                 :          0 :     case ::opendrive::LaneType::Entry:
     255                 :            :     case ::opendrive::LaneType::Exit:
     256                 :            :     case ::opendrive::LaneType::OffRamp:
     257                 :            :     case ::opendrive::LaneType::OnRamp:
     258                 :          0 :       return lane::LaneType::TURN;
     259                 :       1280 :     case ::opendrive::LaneType::Restricted:
     260                 :            :     case ::opendrive::LaneType::Median:
     261                 :            :     case ::opendrive::LaneType::Special1:
     262                 :            :     case ::opendrive::LaneType::Special2:
     263                 :            :     case ::opendrive::LaneType::Special3:
     264                 :            :     case ::opendrive::LaneType::RoadWorks:
     265                 :            :     case ::opendrive::LaneType::Tram:
     266                 :            :     case ::opendrive::LaneType::Rail:
     267                 :            :     case ::opendrive::LaneType::None:
     268                 :       1280 :       return lane::LaneType::UNKNOWN;
     269                 :          0 :     default:
     270                 :          0 :       return lane::LaneType::INVALID;
     271                 :            :   }
     272                 :            : }
     273                 :            : 
     274                 :      23435 : lane::LaneDirection toLaneDirection(::opendrive::Lane const &lane, bool rightHandTraffic)
     275                 :            : {
     276                 :            :   lane::LaneDirection direction;
     277                 :            : 
     278                 :            :   // First determine the lane direction based on the position of the lane (right hand traffic)
     279         [ +  + ]:      23435 :   if (lane.index <= 0)
     280                 :            :   {
     281         [ +  - ]:      11278 :     if (rightHandTraffic)
     282                 :            :     {
     283                 :      11278 :       direction = lane::LaneDirection::POSITIVE;
     284                 :            :     }
     285                 :            :     else
     286                 :            :     {
     287                 :          0 :       direction = lane::LaneDirection::NEGATIVE;
     288                 :            :     }
     289                 :            :   }
     290                 :            :   else
     291                 :            :   {
     292         [ +  - ]:      12157 :     if (rightHandTraffic)
     293                 :            :     {
     294                 :      12157 :       direction = lane::LaneDirection::NEGATIVE;
     295                 :            :     }
     296                 :            :     else
     297                 :            :     {
     298                 :          0 :       direction = lane::LaneDirection::POSITIVE;
     299                 :            :     }
     300                 :            :   }
     301                 :            : 
     302                 :            :   // Then based on the lane type adjust the lane direction as needed
     303   [ +  +  +  -  :      23435 :   switch (lane.type)
                      - ]
     304                 :            :   {
     305                 :         84 :     case ::opendrive::LaneType::Bidirectional:
     306                 :         84 :       return lane::LaneDirection::BIDIRECTIONAL;
     307                 :       9142 :     case ::opendrive::LaneType::Driving:
     308                 :            :     case ::opendrive::LaneType::Biking:
     309                 :            :     case ::opendrive::LaneType::Stop:
     310                 :            :     case ::opendrive::LaneType::Entry:
     311                 :            :     case ::opendrive::LaneType::Exit:
     312                 :            :     case ::opendrive::LaneType::OffRamp:
     313                 :            :     case ::opendrive::LaneType::OnRamp:
     314                 :            :     case ::opendrive::LaneType::Restricted:
     315                 :            :     case ::opendrive::LaneType::Median:
     316                 :            :     case ::opendrive::LaneType::Special1:
     317                 :            :     case ::opendrive::LaneType::Special2:
     318                 :            :     case ::opendrive::LaneType::Special3:
     319                 :       9142 :       return direction;
     320                 :      14209 :     case ::opendrive::LaneType::None:
     321                 :            :     case ::opendrive::LaneType::Shoulder:
     322                 :            :     case ::opendrive::LaneType::Border:
     323                 :            :     case ::opendrive::LaneType::Sidewalk:
     324                 :            :     case ::opendrive::LaneType::Parking:
     325                 :      14209 :       return lane::LaneDirection::NONE;
     326                 :          0 :     case ::opendrive::LaneType::RoadWorks:
     327                 :            :     case ::opendrive::LaneType::Tram:
     328                 :            :     case ::opendrive::LaneType::Rail:
     329                 :          0 :       return lane::LaneDirection::UNKNOWN;
     330                 :          0 :     default:
     331                 :          0 :       return lane::LaneDirection::INVALID;
     332                 :            :   }
     333                 :            : }
     334                 :            : 
     335                 :     424010 : point::GeoPoint toGeo(::opendrive::Point const &point)
     336                 :            : {
     337         [ +  - ]:     424010 :   return point::createGeoPoint(point::Longitude(point.x), point::Latitude(point.y), point::Altitude(point.z));
     338                 :            : }
     339                 :            : 
     340                 :     424010 : point::ECEFPoint toECEF(::opendrive::Point const &point)
     341                 :            : {
     342                 :            :   // points expected to be geo points
     343         [ +  - ]:     848020 :   return point::toECEF(toGeo(point));
     344                 :            : }
     345                 :            : 
     346                 :      46870 : point::Geometry toGeometry(std::vector<::opendrive::Point> edgePoints)
     347                 :            : {
     348                 :      93740 :   point::ECEFEdge ecefEdge;
     349         [ +  + ]:     469662 :   for (auto const &point : edgePoints)
     350                 :            :   {
     351   [ +  -  +  - ]:     422792 :     ecefEdge.push_back(toECEF(point));
     352                 :            :   }
     353                 :            : 
     354         [ +  - ]:      93740 :   return point::createGeometry(ecefEdge, false);
     355                 :            : }
     356                 :            : 
     357                 :            : } // namespace opendrive
     358                 :            : } // namespace map
     359                 :            : } // namespace ad