..
  *******************************************************************************
  Copyright (c) 2021 in-tech GmbH
                2023 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)

  This program and the accompanying materials are made available under the
  terms of the Eclipse Public License 2.0 which is available at
  http://www.eclipse.org/legal/epl-2.0.

  SPDX-License-Identifier: EPL-2.0
  *******************************************************************************

.. _world_osi:

World_OSI
==========

.. _world_sampling:

Sampling of World Geometries
----------------------------

.. _OpenDRIVE: https://www.asam.net/standards/detail/opendrive/
.. _OpenSCENARIO: https://www.asam.net/standards/detail/openscenario/
.. _open simulation interface: https://github.com/OpenSimulationInterface
.. _Ramer-Douglas-Peucker algorithm: https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm

Roads are described following the `OpenDRIVE`_ specification.
There the geometry of a road is defined algebraically as lines, clothoids and by means of cubic polynomials, whereby primarily only one reference line is defined.
The lanes can be understood as a stack of parallel lanes, which are given by the width and the offset, now in relation to this reference line, which acts as the underlying coordinate system ('s/t' road coordinates with s along the road).
Here, too, width and offset are defined algebraically, which means that almost any boundary lines can be defined.

When the world is initialized, these boundary definitions (i.e. algebraic geometries) are converted into piecewise linear elements, which is called sampling.
The road is scanned at a constant interval along 's', which leads to four-sided (quadrangular) sections of the lanes at common 's' coordinates, so-called lane elements (see LaneElement).
The scanning is carried out at a scanning rate of 10 centimeters with the aim of achieving a total scanning error of less than 5 centimeters, as required by the representation used internally (c.f. `open simulation interface`_).
Note that this error is only guaranteed if geometries do not exhibit extreme curvatures, i.e. a deviation of more than 5 cm within two sampling points (10 cm along s).
The scanned points define so-called joints, which contain all scanned points at an 's' coordinate across all lane boundaries of the given road.
The number of these joints is reduced by a `Ramer-Douglas-Peucker algorithm`_, which ensures that the maximum lateral error of each individual point within a joint is less than 5 cm compared to the originally scanned points.
Note that (a) the boundary points of geometries are always retained and (b) additional points for lane marking transitions are also retained to ensure the maximum accuracy of these edge cases.
The lane elements are generated with two successive connections, which are ultimately used in the localization at runtime (see :ref:`world_localization`).

Internally, each lane element receives a constant direction, which is defined by the direction of the vector spanned between the centers of the corresponding connections.
Each joint also holds the corresponding curvature of the road, so that the curvature can be interpolated linearly within a lane element along the 's' coordinate.

.. _world_localization:

Localization
------------

Generally, the position of an agent is stored with respect to :ref:`world coordinates (x,y) <coordinatesystems_world>`.
As queries on the world operates in :ref:`road coordinates (s,t) <coordinatesystems_road>`, the position of the agent needs to be transformed.

This section describes the translation of coordinates (x,y) of an agent into RoadCoordinate (s,t), whereas the notion of (s,t) comes from the `OpenDRIVE` _ standard.

Basics
~~~~~~

The following image depics the basic principes of the localization which is rooted on the specifics of the OSI World Layer (aka OWL).

.. figure:: ./images/LocalizationBasics.svg

   Localization Basics

Given is a point P in cartesian coordinates (x/y).
The task is to assign the point to a lane, defined by a general road geometry and calculate the transformed Point P' in road coordinates (s/t).

Road geometry (based on `OpenDRIVE`_):

 - A road consists of several sections
 - Each section consists of several lanes
 - Within a section, the number of lanes is constant
 - Lanes can have one predecessor and one successor
 - The road follows a reference line, which is the reference for the *s*\ -coordinate.
   The *t*\ -coordinate is perpendicular to this line.

OWL specifics:

 - All lanes are sampled, generating a stream of generic quadrilaterals (LaneGeometryElements).
 - Within a section, the number of quads per lane is equal, and all lanes have the same length in s.
 - This is realized by a variable sampling width, determined by a constant sampling width along the longest arc.
 - Consequently, points perpendicular to the reference line (*t*\ -axis) have the same *s*\ -coordinate.

Note, that the final *t*\ -coordinate is calculated with respect to a virtual reference line for each lane.
This means, that points on the center of a lane have *t*\ -coordinate of 0.

Localization Sequence
~~~~~~~~~~~~~~~~~~~~~

.. _r-tree: https://www.boost.org/doc/libs/1_65_0/libs/geometry/doc/html/geometry/reference/spatial_indexes/boost__geometry__index__rtree.html

The following outlines the localization process implemented in openPASS, using a specific example of a bus changing lanes on a widening road.

.. figure:: ./images/localization_with_bus_step0.svg

   Bus changing lanes on a widening road

First, as described above LaneGeometryElements are generated for the road, here shown as a simplified red grid:

.. figure:: ./images/localization_with_bus_step1.svg

   LaneGeometryElements and bounding box of the bus

All geometries are internally managed by an `r-tree`_, used to check for overlaps between the bounding box of the bus and all stored objects in the tree, making checks for overlap extremely fast.
However, the r-tree comes with the restriction that all stored objects are stored by boxes orthogonal to ``x`` and ``y`` (= maximum Cartesian coordinates of the boundary points).

.. figure:: ./images/localization_with_bus_step2.svg

   r-Tree representation of the Bus and a single LaneGeometryElement

.. figure:: ./images/localization_with_bus_step3.svg

   r-Tree representation of all participating geometries

This introduces additional overlaps, resulting in false positives where the bus is located on segments it does not actually touch.

.. figure:: ./images/localization_with_bus_step4.svg

   Found overlaps with false positives

This is justified because the subsequent filtering can be carried out quickly since the possible geometries allow a simplified intersection check.
Finally, for each point of a non-empty intersection polygon, the ``s`` and ``t`` coordinates are calculated and aggregated with respect to the underlying lane.
For each touched lane, the minimum and maximum ``s`` coordinate, and the minimum and maximum lane remainder (delta t) is stored.

.. figure:: ./images/localization_with_bus_step5.svg

   Example for the calculation of s_min, s_max and delta_right

In addition, if the reference point (i.e. the middle of the rear axle) or the mainLaneLocator (i.e. the middle of the agent front) are located within a LaneGeometryElement, ``s/t/yaw`` is calculated of each point, respectively.
Further aggregation is done with respect to each road by calculating the minimum and maximum ``s`` for each road the agent intersects with.
For the current route of an agent, the following information is stored: ``s/t/yaw`` of the reference point and mainLaneLocator on the route (roads along a route are not allowed to intersect), distance from the lane boundary to the left and right for the road(s) along the route, and OpenDRIVE Ids of the lanes on the route that the agent touches.
The results also holds information whether both the reference point and the mainLaneLocator lay on the route.
In the current implementation, these points must be located - otherwise the agent is despawened, as the agent cannot execute distance queries without a relation to its current route.


.. _world_trafficsigns:

Traffic Signs, Road Markings and TrafficLights
----------------------------------------------

The world currently supports a variety of traffic signs, road markings and traffic lights.
All of these are defined in `OpenDRIVE`_ as "RoadSignal".
At the moment it can interpret traffic signs and road markings according to the German regulations, US regulations, and China regulations and traffic lights according the the `OpenDRIVE`_ appendix.
Traffic signs can contain optional supplementary traffic signs. Supplementary signs are dependent on a main traffic sign and contain additional information.

For Germany the types are based on the StVO (see: https://www.bast.de/BASt_2017/DE/Verkehrstechnik/Fachthemen/v1-verkehrszeichen/vz-download.html).
The following traffic signs are supported for Germany:

.. table::
   :class: tight-table

   ============================================= ============================================================================= ========= =========== =================================================================================
   TrafficSign                                   Image                                                                         StVo Type Subtype     Value and Units
   ============================================= ============================================================================= ========= =========== =================================================================================
   GiveWay                                       .. figure:: ./images/DE/GiveWay_205.svg                                       205
                                                   :width: 400
   Stop                                          .. figure:: ./images/DE/Stop_206.svg                                          206
                                                   :width: 400
   DoNotEnter                                    .. figure:: ./images/DE/DoNotEnter_267.svg                                    267
                                                   :width: 400
   EnvironmentalZoneBegin                        .. figure:: ./images/DE/EnvironmentalZoneBegin_270.1.svg                      270.1
                                                   :width: 400
   EnvironmentalZoneEnd                          .. figure:: ./images/DE/EnvironmentalZoneEnd_270.2.svg                        270.2
                                                   :width: 400
   MaximumSpeedLimit                             .. figure:: ./images/DE/MaximumSpeedLimit_274-5.svg                           274       X           The subtype "X" is used to define the speedlimit in km/h.
                                                   :width: 400                                                                                       Afterwards the world converts it to m/s.
                                                 .. figure:: ./images/DE/MaximumSpeedLimit_274-130.svg
                                                   :width: 400
   SpeedLimitZoneBegin                           .. figure:: ./images/DE/SpeedLimitZoneBegin_274.1.svg                         274.1     -/20        The subtype is used to define the speedlimit in km/h.
                                                   :width: 400                                                                                       Afterwards the world converts it to m/s.
                                                 .. figure:: ./images/DE/SpeedLimitZoneBegin_274.1-20.svg                                            No subtype = 30km/h, 20 = 20km/h
                                                   :width: 400
   SpeedLimitZoneEnd                             .. figure:: ./images/DE/SpeedLimitZoneEnd_274.2.svg                           274.2     -/20        The subtype is used to define the speedlimit in km/h.
                                                   :width: 400                                                                                       Afterwards the world converts it to m/s.
                                                 .. figure:: ./images/DE/SpeedLimitZoneEnd_274.2-20.svg                                              No subtype = 30km/h, 20 = 20km/h
                                                   :width: 400
   MinimumSpeedLimit                             .. figure:: ./images/DE/MinimumSpeedLimit_275.svg                             275       X           The subtype is used to define the speedlimit in km/h.
                                                   :width: 400                                                                                       Afterwards the world converts it to m/s.
   OvertakingBanBegin                            .. figure:: ./images/DE/OvertakingBanBegin_276.svg                            276
                                                   :width: 400
   OvertakingBanTrucksBegin                      .. figure:: ./images/DE/OvertakingBanTrucksBegin_277.svg                      277
                                                   :width: 400
   EndOfMaximumSpeedLimit                        .. figure:: ./images/DE/EndOfMaximumSpeedLimit_278-5.svg                      278       X           The subtype "X" is used to define the speedlimit in km/h.
                                                   :width: 400                                                                                       Afterwards the world converts it to m/s.
                                                 .. figure:: ./images/DE/EndOfMaximumSpeedLimit_278-130.svg
                                                   :width: 400
   EndOfMinimumSpeedLimit                        .. figure:: ./images/DE/EndOfMinimumSpeedLimit_279.svg                        279       X           The subtype "X" is used to define the speedlimit in km/h.
                                                   :width: 400                                                                                       Afterwards the world converts it to m/s.
   OvertakingBanEnd                              .. figure:: ./images/DE/OvertakingBanEnd_280.svg                              280
                                                   :width: 400
   OvertakingBanTrucksEnd                        .. figure:: ./images/DE/OvertakingBanTrucksEnd_281.svg                        281
                                                   :width: 400
   EndOffAllSpeedLimitsAndOvertakingRestrictions .. figure:: ./images/DE/EndOffAllSpeedLimitsAndOvertakingRestrictions_282.svg 282
                                                   :width: 400
   RightOfWayNextIntersection                    .. figure:: ./images/DE/RightOfWayNextIntersection_301.svg                    301
                                                   :width: 400
   RightOfWayBegin                               .. figure:: ./images/DE/RightOfWayBegin_306.svg                               306
                                                   :width: 400
   RightOfWayEnd                                 .. figure:: ./images/DE/RightOfWayEnd_307.svg                                 307
                                                   :width: 400
   TownBegin                                     .. figure:: ./images/DE/TownBegin_310.svg                                     310                   This sign contains a text describing the name of the town
                                                   :width: 400
   TownEnd                                       .. figure:: ./images/DE/TownEnd_311.svg                                       311                   This sign contains a text describing the name of the town
                                                   :width: 400
   TrafficCalmedDistrictBegin                    .. figure:: ./images/DE/TrafficCalmedDistrictBegin_325.1.svg                  325.1
                                                   :width: 400
   TrafficCalmedDistrictEnd                      .. figure:: ./images/DE/TrafficCalmedDistrictEnd_325.2.svg                    325.2
                                                   :width: 400
   HighWayBegin                                  .. figure:: ./images/DE/HighWayBegin_330.1.svg                                330.1
                                                   :width: 400
   HighWayEnd                                    .. figure:: ./images/DE/HighWayEnd_330.2.svg                                  330.2
                                                   :width: 400
   HighWayExit                                   .. figure:: ./images/DE/HighWayExit_333.svg                                   333
                                                   :width: 400
   AnnounceHighwayExit                           .. figure:: ./images/DE/AnnounceHighwayExit_448.svg                           448
                                                   :width: 400
   PreannounceHighwayExitDirections              .. figure:: ./images/DE/AnnounceHighwayExit_449.svg                           449
                                                   :width: 400
   HighwayExitPole                               .. figure:: ./images/DE/HighwayExitPole_450-50.svg                            450       50/51/52    The subtype describes the distance to the highway exit in m.
                                                   :width: 400                                                                                       50 = 100m, 51 = 200m, 52 = 300m
                                                 .. figure:: ./images/DE/HighwayExitPole_450-51.svg
                                                   :width: 400
                                                 .. figure:: ./images/DE/HighwayExitPole_450-52.svg
                                                   :width: 400
   AnnounceRightLaneEnd                          .. figure:: ./images/DE/AnnounceRightLaneEnd_531-10.svg                       531       10/11/12/13 The subtype describes the number of continuing lanes after the right lane ends.
                                                   :width: 400                                                                                       10 = 1 lane, 11 = 2 lanes, 12 = 3 lanes, 13 = 4 lanes
                                                 .. figure:: ./images/DE/AnnounceRightLaneEnd_531-11.svg
                                                   :width: 400
                                                 .. figure:: ./images/DE/AnnounceRightLaneEnd_531-12.svg
                                                   :width: 400
                                                 .. figure:: ./images/DE/AnnounceRightLaneEnd_531-13.svg
                                                   :width: 400
   AnnounceLeftLaneEnd                           .. figure:: ./images/DE/AnnounceLeftLaneEnd_531-20.svg                        531       20/21/22/23 The subtype describes the number of continuing lanes after the left lane ends.
                                                   :width: 400                                                                                       10 = 1 lane, 11 = 2 lanes, 12 = 3 lanes, 13 = 4 lanes
                                                 .. figure:: ./images/DE/AnnounceLeftLaneEnd_531-21.svg
                                                   :width: 400
                                                 .. figure:: ./images/DE/AnnounceLeftLaneEnd_531-22.svg
                                                   :width: 400
                                                 .. figure:: ./images/DE/AnnounceLeftLaneEnd_531-23.svg
                                                   :width: 400
   DistanceIndication                            .. figure:: ./images/DE/DistanceIndication_1004-30.svg                        1004      30/31/32    For subtype 30 the value describes the distance in m.
                                                   :width: 400                                                                                       For subtype 31 the value describes the distance in km.
                                                 .. figure:: ./images/DE/DistanceIndication_1004-31.svg                                              Subtype 32 has a STOP in 100m.
                                                   :width: 400
                                                 .. figure:: ./images/DE/DistanceIndication_1004-32.svg
                                                   :width: 400
   ============================================= ============================================================================= ========= =========== =================================================================================

The following road markings are supported for Germany:

.. table::
   :class: tight-table

   ======================= ================================================== ========= ======= ================
   RoadMarking             Image                                              StVo Type Subtype Value and Units
   ======================= ================================================== ========= ======= ================
   PedestrianCrossing      .. figure:: ./images/DE/PedestrianCrossing_293.svg 293
                             :width: 400
   Stop line               .. figure:: ./images/DE/Stop_line_294.svg          294
                             :width: 400
   ======================= ================================================== ========= ======= ================

The pedestrian crossing can also be defined in `OpenDRIVE`_ as object with type "crosswalk".

For United States the types are based on the "Manual on Uniform Traffic Control Devices" (see: https://mutcd.fhwa.dot.gov/pdfs/2009r1r2/pdf_index.htm).
The following traffic signs are supported for United States:

.. table::

   ============================================= ======================================================== ========= ========================================================
   TrafficSign                                   Image                                                    Type      Value and Units
   ============================================= ======================================================== ========= ========================================================
   HighWayExit                                   .. figure:: ./images/US/HighWayExit_E5-1.svg             E5-1
                                                   :width: 400
   HighWayExit                                   .. figure:: ./images/US/HighWayExit_E2-5.svg             E2-5
                                                   :width: 400
   AnnounceHighwayExit                           .. figure:: ./images/US/AnnounceHighwayExit_E1-2.svg     E1-2
                                                   :width: 400
   PreannounceHighwayExit                        .. figure:: ./images/US/PreannounceHighwayExit_E1-2a.svg E1-2a
                                                   :width: 400
   Stop                                          .. figure:: ./images/US/Stop_R1-1.svg                    R1-1
                                                   :width: 400
   GiveWay                                       .. figure:: ./images/US/GiveWay_R1-2.svg                 R1-2
                                                   :width: 400
   MaximumSpeedLimit                             .. figure:: ./images/US/MaximumSpeedLimit_R2-1.svg       R2-1      as defined via the "value" and "unit" attributes
                                                   :width: 400
   DoNotPass (OvertakingBan)                     .. figure:: ./images/US/DoNotPass_R4-1.svg               R4-1
                                                   :width: 400
   DoNotEnter                                    .. figure:: ./images/US/DoNotEnter_R5-1.svg              R5-1
                                                   :width: 400
   RightLaneEnd                                  .. figure:: ./images/US/RightLaneEnd_W4-2R.svg           W4-2R
                                                   :width: 400
   LeftLaneEnd                                   .. figure:: ./images/US/LeftLaneEnd_W4-2L.svg             W4-2L
                                                   :width: 400
   ============================================= ======================================================== ========= ========================================================

For China the types are based on the https://upload.wikimedia.org/wikipedia/commons/8/8c/China_GB_5768.2-2009.pdf.
The following traffic signs are supported for China:

.. table::

   ============================================= ======================================================= ============================= =========== ================================================================================
   TrafficSign                                   Image                                                   Type                          Subtype     Value and Units
   ============================================= ======================================================= ============================= =========== ================================================================================
   AnnounceRightLaneEnd                          .. figure:: ./images/CN/EndofLaneRight-32a.svg          EndofLane-32                  a/b         The subtype describes the number of continuing lanes after the right lane ends.
                                                   :width: 400                                                                                     a = 2 lanes, b = 3 lanes
                                                 .. figure:: ./images/CN/EndofLaneRight-32b.svg
                                                   :width: 400
   AnnounceLeftLaneEnd                           .. figure:: ./images/CN/EndofLaneLeft-32c.svg           EndofLane-32                  c/d         The subtype describes the number of continuing lanes after the left lane ends.
                                                   :width: 400                                                                                     c = 2 lanes, d = 3 lanes
                                                 .. figure:: ./images/CN/EndofLaneLeft-32d.svg
                                                   :width: 400
   MaximumSpeedLimit                             .. figure:: ./images/CN/MaximumSpeedLimit-38.svg        SpeedLimit-38                             as defined via the "value" and "unit" attributes
                                                   :width: 400
   EndOfMaximumSpeedLimit                        .. figure:: ./images/CN/EndOfMaximumSpeedLimit-39.svg   EndOfSpeedLimit-39                        as defined via the "value" and "unit" attributes
                                                   :width: 400
   AnnounceHighwayExit (1km)                     .. figure:: ./images/CN/AnnouceHighwayExit1km-50b.svg   AnnounceHighwayExit-50b
                                                   :width: 400
   AnnounceHighwayExit (500m)                    .. figure:: ./images/CN/AnnouceHighwayExit500m-50c.svg  AnnounceHighwayExit-50c
                                                   :width: 400
   HighWayExit                                   .. figure:: ./images/CN/HighwayExit-50d.svg             HighwayExit-50d
                                                   :width: 400
   ============================================= ======================================================= ============================= =========== ================================================================================

The following traffic lights are supported:

.. table::
   :class: tight-table

   ============================================= =============== ======= ===============
   TrafficLight                                  OpenDRIVE Type  Subtype Value and Units
   ============================================= =============== ======= ===============
   Standard traffic light (red, yellow, green)   1.000.001       \-      \-
   Left arrows                                   1.000.011       10      \-
   Right arrows                                  1.000.011       20      \-
   Upwards arrows                                1.000.011       30      \-
   Left und upwards arrows                       1.000.011       40      \-
   Right und upwards arrows                      1.000.011       50      \-
   ============================================= =============== ======= ===============

These traffic lights are controlled by `OpenSCENARIO`_.

.. _world_lanemarkings:

Lane Markings
-------------

The world also supports lane markings (i.e. printed lines between two lanes) according to the `OpenDRIVE`_ standard.
The following attributes of the "roadMark" tag in the scenery file are stored in the world and can be retrieved by the GetLaneMarkings query: sOffset, type, weight, color.
The weight is converted into a width in meter: 0.15 for standard and 0.3 for bold. Lane markings are also converted to OSI LaneBoundaries.
For the OpenDRIVE type "solid solid", "solid broken", "broken solid", and "broken broken" two LaneBoundaries are created in OSI with a fixed lateral distance of 0.15m.

.. _world_getobstruction:

GetObstruction
--------------

The GetObstruction function calculates the lateral distance an agent must travel in order to align with either the left or right boundary of a target object occupying the same lane.

The calculation adheres to the following process:

#) Project the agent's MainLaneLocator along the lane to the nearest and furthest s-coordinate of the target object, capturing the projected points
#) Create a straight line from the two captured points
#) Calculate the Euclidean distance of each of the target object's corners to the created line
#) Return the left-most and right-most points with respect to the created line

If the first step fails, because the ego lane does not extend to the object's position (i.e. it ends prematurely or the objects is outside the ego's route), then the result is invalid, which is indicated by a separate valid flag.

.. figure:: ./images/GetObstruction.svg

   Example for the calculation of GetObstruction
