├── opendriveparser ├── elements │ ├── __init__.py │ ├── roadElevationProfile.py │ ├── eulerspiral.py │ ├── roadType.py │ ├── road.py │ ├── junction.py │ ├── openDrive.py │ ├── roadLink.py │ ├── roadLateralProfile.py │ ├── roadPlanView.py │ └── roadLanes.py ├── __init__.py ├── README.md ├── .gitignore └── parser.py ├── opendrive2lanelet ├── plane_elements │ ├── __init__.py │ ├── border.py │ ├── plane_group.py │ └── plane.py ├── __init__.py ├── utils.py ├── .gitignore ├── README.md ├── commonroad.py ├── XML_commonRoad_XSD.xsd └── network.py ├── requirements.txt ├── gui_screenshot.png ├── test ├── opendrive-1-rn.dump ├── opendrive-1-sc.dump ├── convert.py └── opendrive-1.py ├── .travis.yml ├── setup.py ├── LICENSE.md ├── .gitignore ├── README.md ├── gui.py └── viewer.py /opendriveparser/elements/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /opendrive2lanelet/plane_elements/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | scipy 3 | lxml 4 | matplotlib 5 | PyQt5 6 | -------------------------------------------------------------------------------- /opendrive2lanelet/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | from opendrive2lanelet.network import Network 3 | -------------------------------------------------------------------------------- /opendriveparser/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | from opendriveparser.parser import parse_opendrive 3 | -------------------------------------------------------------------------------- /gui_screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wenlong-dev/opendrive2lanelets-converter/HEAD/gui_screenshot.png -------------------------------------------------------------------------------- /test/opendrive-1-rn.dump: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wenlong-dev/opendrive2lanelets-converter/HEAD/test/opendrive-1-rn.dump -------------------------------------------------------------------------------- /test/opendrive-1-sc.dump: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wenlong-dev/opendrive2lanelets-converter/HEAD/test/opendrive-1-sc.dump -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | sudo: false 3 | python: 4 | - "2.7" 5 | - "3.4" 6 | - "3.5" 7 | - "3.6" 8 | install: 9 | - pip install . 10 | - pip install -r requirements.txt 11 | script: 12 | - python -m unittest 13 | -------------------------------------------------------------------------------- /test/convert.py: -------------------------------------------------------------------------------- 1 | from lxml import etree 2 | from opendriveparser import parse_opendrive 3 | from opendrive2lanelet import Network 4 | 5 | 6 | fh = open("test/opendrive-1.xodr", 'r') 7 | openDrive = parse_opendrive(etree.parse(fh).getroot()) 8 | fh.close() 9 | 10 | roadNetwork = Network() 11 | roadNetwork.loadOpenDrive(openDrive) 12 | 13 | scenario = roadNetwork.exportCommonRoadScenario() 14 | 15 | fh = open("test/opendrive-1.xml", "wb") 16 | fh.write(scenario.export_to_string()) 17 | fh.close() 18 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup, find_packages 4 | 5 | setup(name='opendrive2lanelets', 6 | version='1.0', 7 | description='Parser and converter from OpenDRIVE to lanelets', 8 | author='Stefan Urban', 9 | author_email='stefan.urban@tum.de', 10 | url='https://gitlab.lrz.de/koschi/converter', 11 | packages=find_packages(), 12 | install_requires=[ 13 | "numpy", 14 | "lxml", 15 | "scipy", 16 | ] 17 | ) 18 | -------------------------------------------------------------------------------- /opendrive2lanelet/utils.py: -------------------------------------------------------------------------------- 1 | 2 | def encode_road_section_lane_width_id(roadId, sectionId, laneId, widthId): 3 | return ".".join([str(roadId), str(sectionId), str(laneId), str(widthId)]) 4 | 5 | def decode_road_section_lane_width_id(encodedString): 6 | 7 | parts = encodedString.split(".") 8 | 9 | if len(parts) != 4: 10 | raise Exception() 11 | 12 | return (int(parts[0]), int(parts[1]), int(parts[2]), int(parts[3])) 13 | 14 | def allCloseToZero(a): 15 | """ Tests if all elements of a are close to zero. """ 16 | 17 | import numpy 18 | return numpy.allclose(a, numpy.zeros(numpy.shape(a))) 19 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | Copyright (c) 2018 Stefan Urban 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 4 | 5 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 6 | 7 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. -------------------------------------------------------------------------------- /opendriveparser/elements/roadElevationProfile.py: -------------------------------------------------------------------------------- 1 | 2 | class ElevationProfile(object): 3 | 4 | def __init__(self): 5 | self._elevations = [] 6 | 7 | @property 8 | def elevations(self): 9 | return self._elevations 10 | 11 | 12 | class Elevation(object): 13 | 14 | def __init__(self): 15 | self._sPos = None 16 | self._a = None 17 | self._b = None 18 | self._c = None 19 | self._d = None 20 | 21 | @property 22 | def sPos(self): 23 | return self._sPos 24 | 25 | @sPos.setter 26 | def sPos(self, value): 27 | self._sPos = float(value) 28 | 29 | @property 30 | def a(self): 31 | return self._a 32 | 33 | @a.setter 34 | def a(self, value): 35 | self._a = float(value) 36 | 37 | @property 38 | def b(self): 39 | return self._b 40 | 41 | @b.setter 42 | def b(self, value): 43 | self._b = float(value) 44 | 45 | @property 46 | def c(self): 47 | return self._c 48 | 49 | @c.setter 50 | def c(self, value): 51 | self._c = float(value) 52 | 53 | @property 54 | def d(self): 55 | return self._d 56 | 57 | @d.setter 58 | def d(self, value): 59 | self._d = float(value) 60 | -------------------------------------------------------------------------------- /test/opendrive-1.py: -------------------------------------------------------------------------------- 1 | 2 | import pickle 3 | import os 4 | import unittest 5 | 6 | from lxml import etree 7 | from opendriveparser import parse_opendrive 8 | from opendrive2lanelet import Network 9 | 10 | class OpenDrive1Test(unittest.TestCase): 11 | 12 | def setUp(self): 13 | 14 | # Write CommonRoad scenario to file 15 | fh = open(os.path.dirname(os.path.realpath(__file__)) + "/opendrive-1.xodr", 'r') 16 | openDrive = parse_opendrive(etree.parse(fh).getroot()) 17 | fh.close() 18 | 19 | self.roadNetwork = Network() 20 | self.roadNetwork.loadOpenDrive(openDrive) 21 | 22 | self.scenario = self.roadNetwork.exportCommonRoadScenario() 23 | 24 | #pickle.dump(self.roadNetwork, open(os.path.dirname(os.path.realpath(__file__)) + "opendrive-1-rn.dump", "wb")) 25 | #pickle.dump(self.scenario, open(os.path.dirname(os.path.realpath(__file__)) + "opendrive-1-sc.dump", "wb")) 26 | 27 | 28 | def test_planes(self): 29 | 30 | with open(os.path.dirname(os.path.realpath(__file__)) + "/opendrive-1-rn.dump", "rb") as fh: 31 | self.assertEqual(self.roadNetwork, pickle.load(fh)) 32 | 33 | def test_lanelets(self): 34 | 35 | with open(os.path.dirname(os.path.realpath(__file__)) + "/opendrive-1-sc.dump", "rb") as fh: 36 | self.assertEqual(self.scenario, pickle.load(fh)) 37 | 38 | 39 | 40 | if __name__ == '__main__': 41 | unittest.main() 42 | -------------------------------------------------------------------------------- /opendriveparser/elements/eulerspiral.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | from scipy.special import fresnel 4 | 5 | class EulerSpiral(object): 6 | 7 | def __init__(self, gamma): 8 | self._gamma = gamma 9 | 10 | @staticmethod 11 | def createFromLengthAndCurvature(length, curvStart, curvEnd): 12 | return EulerSpiral(1 * (curvEnd - curvStart) / length) 13 | 14 | def calc(self, s, x0=0, y0=0, kappa0=0, theta0=0): 15 | 16 | # Start 17 | C0 = x0 + 1j * y0 18 | 19 | if self._gamma == 0 and kappa0 == 0: 20 | # Straight line 21 | Cs = C0 + np.exp(1j * theta0 * s) 22 | 23 | elif self._gamma == 0 and kappa0 != 0: 24 | # Arc 25 | Cs = C0 + np.exp(1j * theta0) / kappa0 * (np.sin(kappa0 * s) + 1j * (1 - np.cos(kappa0 * s))) 26 | 27 | else: 28 | # Fresnel integrals 29 | Sa, Ca = fresnel((kappa0 + self._gamma * s) / np.sqrt(np.pi * np.abs(self._gamma))) 30 | Sb, Cb = fresnel(kappa0 / np.sqrt(np.pi * np.abs(self._gamma))) 31 | 32 | # Euler Spiral 33 | Cs1 = np.sqrt(np.pi / np.abs(self._gamma)) * np.exp(1j * (theta0 - kappa0**2 / 2 / self._gamma)) 34 | Cs2 = np.sign(self._gamma) * (Ca - Cb) + 1j * Sa - 1j * Sb 35 | 36 | Cs = C0 + Cs1 * Cs2 37 | 38 | # Tangent at each point 39 | theta = self._gamma * s**2 / 2 + kappa0 * s + theta0 40 | 41 | return (Cs.real, Cs.imag, theta) 42 | -------------------------------------------------------------------------------- /opendriveparser/elements/roadType.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | class Type(object): 4 | 5 | allowedTypes = ["unknown", "rural", "motorway", "town", "lowSpeed", "pedestrian", "bicycle"] 6 | 7 | def __init__(self): 8 | self._sPos = None 9 | self._type = None 10 | self._speed = None 11 | 12 | @property 13 | def sPos(self): 14 | return self._sPos 15 | 16 | @sPos.setter 17 | def sPos(self, value): 18 | self._sPos = float(value) 19 | 20 | @property 21 | def type(self): 22 | return self._type 23 | 24 | @type.setter 25 | def type(self, value): 26 | if value not in self.allowedTypes: 27 | raise AttributeError("Type not allowed.") 28 | 29 | self._type = value 30 | 31 | @property 32 | def speed(self): 33 | return self._speed 34 | 35 | @speed.setter 36 | def speed(self, value): 37 | if not isinstance(value, Speed): 38 | raise TypeError("Value must be instance of Speed.") 39 | 40 | self._speed = value 41 | 42 | 43 | class Speed(object): 44 | 45 | def __init__(self): 46 | self._max = None 47 | self._unit = None 48 | 49 | @property 50 | def max(self): 51 | return self._max 52 | 53 | @max.setter 54 | def max(self, value): 55 | self._max = str(value) 56 | 57 | @property 58 | def unit(self): 59 | return self._unit 60 | 61 | @unit.setter 62 | def unit(self, value): 63 | # TODO validate input 64 | self._unit = str(value) 65 | -------------------------------------------------------------------------------- /opendriveparser/README.md: -------------------------------------------------------------------------------- 1 | # OpenDRIVE parser 2 | 3 | ## Usage 4 | 5 | ```python 6 | from lxml import etree 7 | from opendriveparser import parse_opendrive 8 | 9 | fh = open("input_opendrive.xodr", 'r') 10 | openDrive = parse_opendrive(etree.parse(fh).getroot()) 11 | fh.close() 12 | 13 | # Now do stuff with the data 14 | for road in openDrive.roads: 15 | print("Road ID: {}".format(road.id)) 16 | ``` 17 | 18 | ## License 19 | 20 | Copyright (c) 2018 Stefan Urban 21 | 22 | Permission is hereby granted, free of charge, to any person obtaining a copy 23 | of this software and associated documentation files (the "Software"), to deal 24 | in the Software without restriction, including without limitation the rights 25 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 26 | copies of the Software, and to permit persons to whom the Software is 27 | furnished to do so, subject to the following conditions: 28 | 29 | The above copyright notice and this permission notice shall be included in 30 | all copies or substantial portions of the Software. 31 | 32 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 33 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 34 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 35 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 36 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 37 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 38 | THE SOFTWARE. 39 | 40 | -------------------------------------------------------------------------------- /opendrive2lanelet/.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | 27 | # PyInstaller 28 | # Usually these files are written by a python script from a template 29 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 30 | *.manifest 31 | *.spec 32 | 33 | # Installer logs 34 | pip-log.txt 35 | pip-delete-this-directory.txt 36 | 37 | # Unit test / coverage reports 38 | htmlcov/ 39 | .tox/ 40 | .coverage 41 | .coverage.* 42 | .cache 43 | nosetests.xml 44 | coverage.xml 45 | *.cover 46 | .hypothesis/ 47 | 48 | # Translations 49 | *.mo 50 | *.pot 51 | 52 | # Django stuff: 53 | *.log 54 | local_settings.py 55 | 56 | # Flask stuff: 57 | instance/ 58 | .webassets-cache 59 | 60 | # Scrapy stuff: 61 | .scrapy 62 | 63 | # Sphinx documentation 64 | docs/_build/ 65 | 66 | # PyBuilder 67 | target/ 68 | 69 | # Jupyter Notebook 70 | .ipynb_checkpoints 71 | 72 | # pyenv 73 | .python-version 74 | 75 | # celery beat schedule file 76 | celerybeat-schedule 77 | 78 | # SageMath parsed files 79 | *.sage.py 80 | 81 | # Environments 82 | .env 83 | .venv 84 | env/ 85 | venv/ 86 | ENV/ 87 | 88 | # Spyder project settings 89 | .spyderproject 90 | .spyproject 91 | 92 | # Rope project settings 93 | .ropeproject 94 | 95 | # mkdocs documentation 96 | /site 97 | 98 | # mypy 99 | .mypy_cache/ 100 | -------------------------------------------------------------------------------- /opendriveparser/.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | 27 | # PyInstaller 28 | # Usually these files are written by a python script from a template 29 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 30 | *.manifest 31 | *.spec 32 | 33 | # Installer logs 34 | pip-log.txt 35 | pip-delete-this-directory.txt 36 | 37 | # Unit test / coverage reports 38 | htmlcov/ 39 | .tox/ 40 | .coverage 41 | .coverage.* 42 | .cache 43 | nosetests.xml 44 | coverage.xml 45 | *.cover 46 | .hypothesis/ 47 | 48 | # Translations 49 | *.mo 50 | *.pot 51 | 52 | # Django stuff: 53 | *.log 54 | local_settings.py 55 | 56 | # Flask stuff: 57 | instance/ 58 | .webassets-cache 59 | 60 | # Scrapy stuff: 61 | .scrapy 62 | 63 | # Sphinx documentation 64 | docs/_build/ 65 | 66 | # PyBuilder 67 | target/ 68 | 69 | # Jupyter Notebook 70 | .ipynb_checkpoints 71 | 72 | # pyenv 73 | .python-version 74 | 75 | # celery beat schedule file 76 | celerybeat-schedule 77 | 78 | # SageMath parsed files 79 | *.sage.py 80 | 81 | # Environments 82 | .env 83 | .venv 84 | env/ 85 | venv/ 86 | ENV/ 87 | 88 | # Spyder project settings 89 | .spyderproject 90 | .spyproject 91 | 92 | # Rope project settings 93 | .ropeproject 94 | 95 | # mkdocs documentation 96 | /site 97 | 98 | # mypy 99 | .mypy_cache/ 100 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.xodr 2 | *.xml 3 | venv/ 4 | .vscode/ 5 | 6 | # Byte-compiled / optimized / DLL files 7 | __pycache__/ 8 | *.py[cod] 9 | *$py.class 10 | 11 | # C extensions 12 | *.so 13 | 14 | # Distribution / packaging 15 | .Python 16 | build/ 17 | develop-eggs/ 18 | dist/ 19 | downloads/ 20 | eggs/ 21 | .eggs/ 22 | lib/ 23 | lib64/ 24 | parts/ 25 | sdist/ 26 | var/ 27 | wheels/ 28 | *.egg-info/ 29 | .installed.cfg 30 | *.egg 31 | 32 | # PyInstaller 33 | # Usually these files are written by a python script from a template 34 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 35 | *.manifest 36 | *.spec 37 | 38 | # Installer logs 39 | pip-log.txt 40 | pip-delete-this-directory.txt 41 | 42 | # Unit test / coverage reports 43 | htmlcov/ 44 | .tox/ 45 | .coverage 46 | .coverage.* 47 | .cache 48 | nosetests.xml 49 | coverage.xml 50 | *.cover 51 | .hypothesis/ 52 | 53 | # Translations 54 | *.mo 55 | *.pot 56 | 57 | # Django stuff: 58 | *.log 59 | local_settings.py 60 | 61 | # Flask stuff: 62 | instance/ 63 | .webassets-cache 64 | 65 | # Scrapy stuff: 66 | .scrapy 67 | 68 | # Sphinx documentation 69 | docs/_build/ 70 | 71 | # PyBuilder 72 | target/ 73 | 74 | # Jupyter Notebook 75 | .ipynb_checkpoints 76 | 77 | # pyenv 78 | .python-version 79 | 80 | # celery beat schedule file 81 | celerybeat-schedule 82 | 83 | # SageMath parsed files 84 | *.sage.py 85 | 86 | # Environments 87 | .env 88 | .venv 89 | env/ 90 | venv/ 91 | ENV/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | -------------------------------------------------------------------------------- /opendrive2lanelet/README.md: -------------------------------------------------------------------------------- 1 | # OpenDRIVE 2 Lanelets - Converter 2 | 3 | ## Requirements 4 | 5 | - Python 3.x 6 | - numpy 7 | - opendriveparser 8 | 9 | ## Usage 10 | 11 | ```python 12 | from lxml import etree 13 | from opendriveparser import parse_opendrive 14 | from opendrive2lanelet import Network 15 | 16 | fh = open("input_opendrive.xodr", 'r') 17 | openDrive = parse_opendrive(etree.parse(fh).getroot()) 18 | fh.close() 19 | 20 | roadNetwork = Network() 21 | roadNetwork.loadOpenDrive(openDrive) 22 | 23 | scenario = roadNetwork.exportCommonRoadScenario() 24 | 25 | # TODO extract common road writer from fvks 26 | writer = CommonRoadFileWriter(scenario, None) 27 | writer.write_scenario_to_file("output_commonroad_file.xml") 28 | ``` 29 | 30 | ## License 31 | 32 | Copyright (c) 2018 Stefan Urban 33 | 34 | Permission is hereby granted, free of charge, to any person obtaining a copy 35 | of this software and associated documentation files (the "Software"), to deal 36 | in the Software without restriction, including without limitation the rights 37 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 38 | copies of the Software, and to permit persons to whom the Software is 39 | furnished to do so, subject to the following conditions: 40 | 41 | The above copyright notice and this permission notice shall be included in 42 | all copies or substantial portions of the Software. 43 | 44 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 45 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 46 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 47 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 48 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 49 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 50 | THE SOFTWARE. 51 | -------------------------------------------------------------------------------- /opendriveparser/elements/road.py: -------------------------------------------------------------------------------- 1 | 2 | from opendriveparser.elements.roadPlanView import PlanView 3 | from opendriveparser.elements.roadLink import Link 4 | from opendriveparser.elements.roadLanes import Lanes 5 | from opendriveparser.elements.roadElevationProfile import ElevationProfile 6 | from opendriveparser.elements.roadLateralProfile import LateralProfile 7 | from opendriveparser.elements.junction import Junction 8 | 9 | class Road(object): 10 | 11 | def __init__(self): 12 | self._id = None 13 | self._name = None 14 | self._junction = None 15 | self._length = None 16 | 17 | self._header = None # TODO 18 | self._link = Link() 19 | self._types = [] 20 | self._planView = PlanView() 21 | self._elevationProfile = ElevationProfile() 22 | self._lateralProfile = LateralProfile() 23 | self._lanes = Lanes() 24 | 25 | @property 26 | def id(self): 27 | return self._id 28 | 29 | @id.setter 30 | def id(self, value): 31 | self._id = int(value) 32 | 33 | @property 34 | def name(self): 35 | return self._name 36 | 37 | @name.setter 38 | def name(self, value): 39 | self._name = str(value) 40 | 41 | @property 42 | def junction(self): 43 | return self._junction 44 | 45 | @junction.setter 46 | def junction(self, value): 47 | if not isinstance(value, Junction) and value is not None: 48 | raise TypeError("Property must be a Junction or NoneType") 49 | 50 | if value == -1: 51 | value = None 52 | 53 | self._junction = value 54 | 55 | @property 56 | def link(self): 57 | return self._link 58 | 59 | @property 60 | def types(self): 61 | return self._types 62 | 63 | @property 64 | def planView(self): 65 | return self._planView 66 | 67 | @property 68 | def elevationProfile(self): 69 | return self._elevationProfile 70 | 71 | @property 72 | def lateralProfile(self): 73 | return self._lateralProfile 74 | 75 | @property 76 | def lanes(self): 77 | return self._lanes 78 | -------------------------------------------------------------------------------- /opendrive2lanelet/plane_elements/border.py: -------------------------------------------------------------------------------- 1 | 2 | from functools import lru_cache 3 | import numpy as np 4 | 5 | from opendriveparser.elements.roadPlanView import PlanView 6 | 7 | class Border(object): 8 | """ 9 | A lane border defines a path along a whole lane section 10 | - a lane always used an inner and outer lane border 11 | - the reference can be another lane border or a plan view 12 | """ 13 | 14 | def __init__(self): 15 | 16 | self._refOffset = 0.0 17 | 18 | self._coeffsOffsets = [] 19 | self._coeffs = [] 20 | 21 | self._reference = None 22 | 23 | def __str__(self): 24 | return str(self._refOffset) 25 | 26 | @property 27 | def reference(self): 28 | return self._reference 29 | 30 | @reference.setter 31 | def reference(self, value): 32 | if not isinstance(value, Border) and not isinstance(value, PlanView): 33 | raise TypeError("Value must be instance of Border or PlanView") 34 | 35 | self._reference = value 36 | 37 | @property 38 | def refOffset(self): 39 | """ The reference offset """ 40 | return self._refOffset 41 | 42 | @refOffset.setter 43 | def refOffset(self, value): 44 | self._refOffset = float(value) 45 | 46 | @property 47 | def coeffs(self): 48 | """ It is assumed the coeffs are added in ascending order! ([0] + [1] * x + [2] * x**2 + ...) """ 49 | return self._coeffs 50 | 51 | @property 52 | def coeffsOffsets(self): 53 | """ Offsets for coeffs """ 54 | return self._coeffsOffsets 55 | 56 | @lru_cache(maxsize=200000) 57 | def calc(self, sPos, addOffset=0.0): 58 | """ Calculate the border """ 59 | 60 | if isinstance(self._reference, PlanView): 61 | # Last reference has to be a reference geometry 62 | refPos, refTang = self._reference.calc(self._refOffset + sPos) 63 | 64 | elif isinstance(self._reference, Border): 65 | # Offset of all inner lanes 66 | refPos, refTang = self._reference.calc(self._refOffset + sPos) 67 | 68 | else: 69 | raise Exception("Reference must be plan view or other lane border.") 70 | 71 | if not self._coeffs or not self._coeffsOffsets: 72 | raise Exception("No entries for width definitions.") 73 | 74 | # Find correct coefficients 75 | widthIdx = next((self._coeffsOffsets.index(n) for n in self._coeffsOffsets[::-1] if n <= sPos), len(self._coeffsOffsets)) 76 | 77 | # Calculate width at sPos 78 | distance = np.polynomial.polynomial.polyval(sPos - self._coeffsOffsets[widthIdx], self._coeffs[widthIdx]) + addOffset 79 | 80 | # New point is in orthogonal direction 81 | ortho = refTang + np.pi / 2 82 | newPos = refPos + np.array([distance * np.cos(ortho), distance * np.sin(ortho)]) 83 | 84 | return newPos, refTang 85 | -------------------------------------------------------------------------------- /opendriveparser/elements/junction.py: -------------------------------------------------------------------------------- 1 | 2 | class Junction(object): 3 | # TODO priority 4 | # TODO controller 5 | 6 | def __init__(self): 7 | self._id = None 8 | self._name = None 9 | self._connections = [] 10 | 11 | @property 12 | def id(self): 13 | return self._id 14 | 15 | @id.setter 16 | def id(self, value): 17 | self._id = int(value) 18 | 19 | @property 20 | def name(self): 21 | return self._name 22 | 23 | @name.setter 24 | def name(self, value): 25 | self._name = str(value) 26 | 27 | @property 28 | def connections(self): 29 | return self._connections 30 | 31 | def addConnection(self, connection): 32 | if not isinstance(connection, Connection): 33 | raise TypeError("Has to be of instance Connection") 34 | 35 | self._connections.append(connection) 36 | 37 | 38 | class Connection(object): 39 | 40 | def __init__(self): 41 | self._id = None 42 | self._incomingRoad = None 43 | self._connectingRoad = None 44 | self._contactPoint = None 45 | self._laneLinks = [] 46 | 47 | @property 48 | def id(self): 49 | return self._id 50 | 51 | @id.setter 52 | def id(self, value): 53 | self._id = int(value) 54 | 55 | @property 56 | def incomingRoad(self): 57 | return self._incomingRoad 58 | 59 | @incomingRoad.setter 60 | def incomingRoad(self, value): 61 | self._incomingRoad = int(value) 62 | 63 | @property 64 | def connectingRoad(self): 65 | return self._connectingRoad 66 | 67 | @connectingRoad.setter 68 | def connectingRoad(self, value): 69 | self._connectingRoad = int(value) 70 | 71 | @property 72 | def contactPoint(self): 73 | return self._contactPoint 74 | 75 | @contactPoint.setter 76 | def contactPoint(self, value): 77 | if value not in ["start", "end"]: 78 | raise AttributeError("Contact point can only be start or end.") 79 | 80 | self._contactPoint = value 81 | 82 | @property 83 | def laneLinks(self): 84 | return self._laneLinks 85 | 86 | def addLaneLink(self, laneLink): 87 | if not isinstance(laneLink, LaneLink): 88 | raise TypeError("Has to be of instance LaneLink") 89 | 90 | self._laneLinks.append(laneLink) 91 | 92 | 93 | class LaneLink(object): 94 | 95 | def __init__(self): 96 | self._from = None 97 | self._to = None 98 | 99 | def __str__(self): 100 | return str(self._from) + " > " + str(self._to) 101 | 102 | @property 103 | def fromId(self): 104 | return self._from 105 | 106 | @fromId.setter 107 | def fromId(self, value): 108 | self._from = int(value) 109 | 110 | @property 111 | def toId(self): 112 | return self._to 113 | 114 | @toId.setter 115 | def toId(self, value): 116 | self._to = int(value) 117 | -------------------------------------------------------------------------------- /opendriveparser/elements/openDrive.py: -------------------------------------------------------------------------------- 1 | 2 | class OpenDrive(object): 3 | 4 | def __init__(self): 5 | self._header = Header() 6 | self._roads = [] 7 | self._controllers = [] 8 | self._junctions = [] 9 | self._junctionGroups = [] 10 | self._stations = [] 11 | 12 | @property 13 | def header(self): 14 | return self._header 15 | 16 | @property 17 | def roads(self): 18 | return self._roads 19 | 20 | def getRoad(self, id): 21 | for road in self._roads: 22 | if road.id == id: 23 | return road 24 | 25 | return None 26 | 27 | @property 28 | def controllers(self): 29 | return self._controllers 30 | 31 | @property 32 | def junctions(self): 33 | return self._junctions 34 | 35 | def getJunction(self, junctionId): 36 | for junction in self._junctions: 37 | if junction.id == junctionId: 38 | return junction 39 | return None 40 | 41 | @property 42 | def junctionGroups(self): 43 | return self._junctionGroups 44 | 45 | @property 46 | def stations(self): 47 | return self._stations 48 | 49 | 50 | class Header(object): 51 | 52 | def __init__(self): 53 | self._revMajor = None 54 | self._revMinor = None 55 | self._name = None 56 | self._version = None 57 | self._date = None 58 | self._north = None 59 | self._south = None 60 | self._east = None 61 | self._west = None 62 | self._vendor = None 63 | 64 | @property 65 | def revMajor(self): 66 | return self._revMajor 67 | 68 | @revMajor.setter 69 | def revMajor(self, value): 70 | self._revMajor = value 71 | 72 | @property 73 | def revMinor(self): 74 | return self._revMinor 75 | 76 | @revMinor.setter 77 | def revMinor(self, value): 78 | self._revMinor = value 79 | 80 | @property 81 | def name(self): 82 | return self._name 83 | 84 | @name.setter 85 | def name(self, value): 86 | self._name = value 87 | 88 | @property 89 | def version(self): 90 | return self._version 91 | 92 | @version.setter 93 | def version(self, value): 94 | self._version = value 95 | 96 | @property 97 | def date(self): 98 | return self._date 99 | 100 | @date.setter 101 | def date(self, value): 102 | self._date = value 103 | 104 | @property 105 | def north(self): 106 | return self._north 107 | 108 | @north.setter 109 | def north(self, value): 110 | self._north = value 111 | 112 | @property 113 | def south(self): 114 | return self._south 115 | 116 | @south.setter 117 | def south(self, value): 118 | self._south = value 119 | 120 | @property 121 | def east(self): 122 | return self._east 123 | 124 | @east.setter 125 | def east(self, value): 126 | self._east = value 127 | 128 | @property 129 | def west(self): 130 | return self._west 131 | 132 | @west.setter 133 | def west(self, value): 134 | self._west = value 135 | 136 | @property 137 | def vendor(self): 138 | return self._vendor 139 | 140 | @vendor.setter 141 | def vendor(self, value): 142 | self._vendor = value 143 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # OpenDRIVE 2 Lanelets - Converter 2 | 3 | This repository provides the code for an OpenDRIVE ([www.opendrive.org](http://www.opendrive.org)) to lanelets ([www.mrt.kit.edu/software/liblanelet](https://www.mrt.kit.edu/software/libLanelet/libLanelet.html)) converter. 4 | 5 | ## Installation 6 | 7 | The following python packages have to be available: 8 | - numpy 9 | - scipy 10 | - lxml 11 | - matplotlib (only for GUI) 12 | - PyQt5 (only for GUI) 13 | 14 | If needed, the converter libraries can be installed using ```pip```: 15 | 16 | ```bash 17 | git clone https://gitlab.lrz.de/koschi/converter.git 18 | cd converter && pip install . 19 | ``` 20 | 21 | ## Example OpenDRIVE Files 22 | 23 | Download example files from: http://opendrive.org/download.html 24 | 25 | ## Usage 26 | 27 | ### Using our provided GUI 28 | 29 | Additional requirement: PyQt5. Start the GUI with ```python gui.py``` 30 | 31 | ![GUI screenshot](gui_screenshot.png "Screenshot of converter GUI") 32 | 33 | ### Using the library in your own scripts 34 | 35 | ```python 36 | from lxml import etree 37 | from opendriveparser import parse_opendrive 38 | from opendrive2lanelet import Network 39 | 40 | # Import, parse and convert OpenDRIVE file 41 | fh = open("input_opendrive.xodr", 'r') 42 | openDrive = parse_opendrive(etree.parse(fh).getroot()) 43 | fh.close() 44 | 45 | roadNetwork = Network() 46 | roadNetwork.loadOpenDrive(openDrive) 47 | 48 | scenario = roadNetwork.exportCommonRoadScenario() 49 | 50 | # Write CommonRoad scenario to file 51 | fh = open("output_commonroad_file.xml", "wb") 52 | fh.write(scenario.export_to_string()) 53 | fh.close() 54 | ``` 55 | 56 | 57 | ## Known Problems 58 | 59 | - When trying to use the gui.py under Wayland, the following error occurs: 60 | ``` 61 | This application failed to start because it could not find or load the Qt platform plugin "wayland" in "". 62 | Available platform plugins are: eglfs, linuxfb, minimal, minimalegl, offscreen, vnc, xcb. 63 | Reinstalling the application may fix this problem. 64 | ``` 65 | Set the platform to *xcb* using this command: ```export QT_QPA_PLATFORM="xcb"``` 66 | 67 | ## ToDo 68 | 69 | - CommonRoad does not support lane types right now. When this is implemented, the OpenDRIVE types have to be mapped onto the CommonRoad ones and added to the lanelet output. 70 | 71 | - Adjacent lanes are not working properly yet, support is not included in software right now. 72 | 73 | - Lane merges may cause problems with predecessor or successor definition. 74 | 75 | - Unit-Tests 76 | 77 | ## License (MIT) 78 | 79 | Copyright (c) 2018 Stefan Urban 80 | 81 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 82 | 83 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 84 | 85 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 86 | -------------------------------------------------------------------------------- /opendriveparser/elements/roadLink.py: -------------------------------------------------------------------------------- 1 | 2 | class Link(object): 3 | 4 | def __init__(self): 5 | self._id = None 6 | self._predecessor = None 7 | self._successor = None 8 | self._neighbors = [] 9 | 10 | def __str__(self): 11 | return " > link id " + str(self._id) + " | successor: " + str(self._successor) 12 | 13 | @property 14 | def id(self): 15 | return self._id 16 | 17 | @id.setter 18 | def id(self, value): 19 | self._id = int(value) 20 | 21 | @property 22 | def predecessor(self): 23 | return self._predecessor 24 | 25 | @predecessor.setter 26 | def predecessor(self, value): 27 | if not isinstance(value, Predecessor): 28 | raise TypeError("Value must be Predecessor") 29 | 30 | self._predecessor = value 31 | 32 | @property 33 | def successor(self): 34 | return self._successor 35 | 36 | @successor.setter 37 | def successor(self, value): 38 | if not isinstance(value, Successor): 39 | raise TypeError("Value must be Successor") 40 | 41 | self._successor = value 42 | 43 | @property 44 | def neighbors(self): 45 | return self._neighbors 46 | 47 | @neighbors.setter 48 | def neighbors(self, value): 49 | if not isinstance(value, list) or not all(isinstance(x, Neighbor) for x in value): 50 | raise TypeError("Value must be list of instances of Neighbor.") 51 | 52 | self._neighbors = value 53 | 54 | def addNeighbor(self, value): 55 | if not isinstance(value, Neighbor): 56 | raise TypeError("Value must be Neighbor") 57 | 58 | self._neighbors.append(value) 59 | 60 | 61 | class Predecessor(object): 62 | 63 | def __init__(self): 64 | self._elementType = None 65 | self._elementId = None 66 | self._contactPoint = None 67 | 68 | def __str__(self): 69 | return str(self._elementType) + " with id " + str(self._elementId) + " contact at " + str(self._contactPoint) 70 | 71 | @property 72 | def elementType(self): 73 | return self._elementType 74 | 75 | @elementType.setter 76 | def elementType(self, value): 77 | if value not in ["road", "junction"]: 78 | raise AttributeError("Value must be road or junction") 79 | 80 | self._elementType = value 81 | 82 | @property 83 | def elementId(self): 84 | return self._elementId 85 | 86 | @elementId.setter 87 | def elementId(self, value): 88 | self._elementId = int(value) 89 | 90 | @property 91 | def contactPoint(self): 92 | return self._contactPoint 93 | 94 | @contactPoint.setter 95 | def contactPoint(self, value): 96 | if value not in ["start", "end"] and value is not None: 97 | raise AttributeError("Value must be start or end") 98 | 99 | self._contactPoint = value 100 | 101 | class Successor(Predecessor): 102 | pass 103 | 104 | class Neighbor(object): 105 | 106 | def __init__(self): 107 | self._side = None 108 | self._elementId = None 109 | self._direction = None 110 | 111 | @property 112 | def side(self): 113 | return self._side 114 | 115 | @side.setter 116 | def side(self, value): 117 | if value not in ["left", "right"]: 118 | raise AttributeError("Value must be left or right") 119 | 120 | self._side = value 121 | 122 | @property 123 | def elementId(self): 124 | return self._elementId 125 | 126 | @elementId.setter 127 | def elementId(self, value): 128 | self._elementId = int(value) 129 | 130 | @property 131 | def direction(self): 132 | return self._direction 133 | 134 | @direction.setter 135 | def direction(self, value): 136 | if value not in ["same", "opposite"]: 137 | raise AttributeError("Value must be same or opposite") 138 | 139 | self._direction = value 140 | -------------------------------------------------------------------------------- /opendrive2lanelet/plane_elements/plane_group.py: -------------------------------------------------------------------------------- 1 | 2 | from opendrive2lanelet.commonroad import Lanelet 3 | 4 | class PLaneGroup(object): 5 | """ A group of pLanes can be converted to a lanelet just like a single pLane """ 6 | 7 | def __init__(self, id=None, pLanes=None, innerNeighbour=None, innerNeighbourSameDirection=True, outerNeighbour=None, reverse = False): 8 | 9 | self._pLanes = [] 10 | self._id = id 11 | self._innerNeighbour = innerNeighbour 12 | self._innerNeighbourSameDirection = innerNeighbourSameDirection 13 | self._outerNeighbour = outerNeighbour 14 | 15 | self._reverse = reverse 16 | 17 | if pLanes is not None: 18 | 19 | if isinstance(pLanes, list): 20 | self._pLanes.extend(pLanes) 21 | else: 22 | self._pLanes.append(pLanes) 23 | 24 | def append(self, pLane): 25 | self._pLanes.append(pLane) 26 | 27 | @property 28 | def id(self): 29 | if self._id is not None: 30 | return self._id 31 | 32 | raise Exception() 33 | 34 | @property 35 | def type(self): 36 | return self._pLanes[0]._type 37 | 38 | @property 39 | def length(self): 40 | return sum([x.length for x in self._pLanes]) 41 | 42 | def convertToLanelet(self, precision=0.5, ref=None, refDistance=[0.0, 0.0]): 43 | 44 | lanelet = None 45 | y1 = refDistance[0] 46 | x = 0 47 | 48 | for pLane in self._pLanes: 49 | 50 | x += pLane.length 51 | y2 = (refDistance[1] - refDistance[0]) / self.length * x + refDistance[0] 52 | 53 | # First lanelet 54 | if lanelet is None: 55 | lanelet = pLane.convertToLanelet(precision=precision, ref=ref, refDistance=[y1, y2]) 56 | lanelet.lanelet_id = self.id 57 | continue 58 | 59 | # Append all following lanelets 60 | newLanelet = pLane.convertToLanelet(precision=precision, ref=ref, refDistance=[y1, y2]) 61 | 62 | lanelet = lanelet.concatenate(newLanelet, self.id) 63 | 64 | y1 = y2 65 | 66 | if lanelet is None: 67 | raise Exception("Lanelet concatenation problem") 68 | 69 | # Adjacent lanes 70 | if self.innerNeighbour is not None: 71 | lanelet.adj_left = self.innerNeighbour 72 | lanelet.adj_left_same_direction = self.innerNeighbourSameDirection 73 | 74 | if self.outerNeighbour is not None: 75 | lanelet.adj_right = self.outerNeighbour 76 | lanelet.adj_right_same_direction = True 77 | 78 | if self._reverse: 79 | newLanelet = Lanelet( 80 | left_vertices=lanelet.right_vertices[::-1], 81 | center_vertices=lanelet.center_vertices[::-1], 82 | right_vertices=lanelet.left_vertices[::-1], 83 | lanelet_id=lanelet.lanelet_id 84 | ) 85 | 86 | newLanelet.adj_left = lanelet.adj_left 87 | newLanelet.adj_left_same_direction = lanelet.adj_left_same_direction 88 | newLanelet.adj_right = lanelet.adj_right 89 | newLanelet.adj_right_same_direction = lanelet.adj_right_same_direction 90 | 91 | lanelet = newLanelet 92 | 93 | return lanelet 94 | 95 | @property 96 | def innerNeighbour(self): 97 | return self._innerNeighbour 98 | 99 | @innerNeighbour.setter 100 | def innerNeighbour(self, value): 101 | self._innerNeighbour = value 102 | 103 | @property 104 | def innerNeighbourSameDirection(self): 105 | return self._innerNeighbourSameDirection 106 | 107 | @innerNeighbourSameDirection.setter 108 | def innerNeighbourSameDirection(self, value): 109 | self._innerNeighbourSameDirection = value 110 | 111 | @property 112 | def outerNeighbour(self): 113 | """ Outer neighbour has always the same driving direction """ 114 | return self._outerNeighbour 115 | 116 | @outerNeighbour.setter 117 | def outerNeighbour(self, value): 118 | self._outerNeighbour = value 119 | -------------------------------------------------------------------------------- /opendriveparser/elements/roadLateralProfile.py: -------------------------------------------------------------------------------- 1 | 2 | class LateralProfile(object): 3 | 4 | def __init__(self): 5 | self._superelevations = [] 6 | self._crossfalls = [] 7 | self._shapes = [] 8 | 9 | @property 10 | def superelevations(self): 11 | return self._superelevations 12 | 13 | @superelevations.setter 14 | def superelevations(self, value): 15 | if not isinstance(value, list) or not all(isinstance(x, Superelevation) for x in value): 16 | raise TypeError("Value must be an instance of Superelevation.") 17 | 18 | self._superelevations = value 19 | 20 | @property 21 | def crossfalls(self): 22 | return self._crossfalls 23 | 24 | @crossfalls.setter 25 | def crossfalls(self, value): 26 | if not isinstance(value, list) or not all(isinstance(x, Crossfall) for x in value): 27 | raise TypeError("Value must be an instance of Crossfall.") 28 | 29 | self._crossfalls = value 30 | 31 | @property 32 | def shapes(self): 33 | return self._shapes 34 | 35 | @shapes.setter 36 | def shapes(self, value): 37 | if not isinstance(value, list) or not all(isinstance(x, Shape) for x in value): 38 | raise TypeError("Value must be a list of instances of Shape.") 39 | 40 | self._shapes = value 41 | 42 | 43 | class Superelevation(object): 44 | 45 | def __init__(self): 46 | self._sPos = None 47 | self._a = None 48 | self._b = None 49 | self._c = None 50 | self._d = None 51 | 52 | @property 53 | def sPos(self): 54 | return self._sPos 55 | 56 | @sPos.setter 57 | def sPos(self, value): 58 | self._sPos = float(value) 59 | 60 | @property 61 | def a(self): 62 | return self._a 63 | 64 | @a.setter 65 | def a(self, value): 66 | self._a = float(value) 67 | 68 | @property 69 | def b(self): 70 | return self._b 71 | 72 | @b.setter 73 | def b(self, value): 74 | self._b = float(value) 75 | 76 | @property 77 | def c(self): 78 | return self._c 79 | 80 | @c.setter 81 | def c(self, value): 82 | self._c = float(value) 83 | 84 | @property 85 | def d(self): 86 | return self._d 87 | 88 | @d.setter 89 | def d(self, value): 90 | self._d = float(value) 91 | 92 | 93 | class Crossfall(object): 94 | 95 | def __init__(self): 96 | self._side = None 97 | self._sPos = None 98 | self._a = None 99 | self._b = None 100 | self._c = None 101 | self._d = None 102 | 103 | @property 104 | def side(self): 105 | return self._side 106 | 107 | @side.setter 108 | def side(self, value): 109 | if value not in ["left", "right", "both"]: 110 | raise TypeError("Value must be string with content 'left', 'right' or 'both'.") 111 | 112 | self._side = value 113 | 114 | @property 115 | def sPos(self): 116 | return self._sPos 117 | 118 | @sPos.setter 119 | def sPos(self, value): 120 | self._sPos = float(value) 121 | 122 | @property 123 | def a(self): 124 | return self._a 125 | 126 | @a.setter 127 | def a(self, value): 128 | self._a = float(value) 129 | 130 | @property 131 | def b(self): 132 | return self._b 133 | 134 | @b.setter 135 | def b(self, value): 136 | self._b = float(value) 137 | 138 | @property 139 | def c(self): 140 | return self._c 141 | 142 | @c.setter 143 | def c(self, value): 144 | self._c = float(value) 145 | 146 | @property 147 | def d(self): 148 | return self._d 149 | 150 | @d.setter 151 | def d(self, value): 152 | self._d = float(value) 153 | 154 | 155 | class Shape(object): 156 | 157 | def __init__(self): 158 | self._sPos = None 159 | self._t = None 160 | self._a = None 161 | self._b = None 162 | self._c = None 163 | self._d = None 164 | 165 | @property 166 | def sPos(self): 167 | return self._sPos 168 | 169 | @sPos.setter 170 | def sPos(self, value): 171 | self._sPos = float(value) 172 | 173 | @property 174 | def t(self): 175 | return self._t 176 | 177 | @t.setter 178 | def t(self, value): 179 | self._t = float(value) 180 | 181 | @property 182 | def a(self): 183 | return self._a 184 | 185 | @a.setter 186 | def a(self, value): 187 | self._a = float(value) 188 | 189 | @property 190 | def b(self): 191 | return self._b 192 | 193 | @b.setter 194 | def b(self, value): 195 | self._b = float(value) 196 | 197 | @property 198 | def c(self): 199 | return self._c 200 | 201 | @c.setter 202 | def c(self, value): 203 | self._c = float(value) 204 | 205 | @property 206 | def d(self): 207 | return self._d 208 | 209 | @d.setter 210 | def d(self, value): 211 | self._d = float(value) 212 | -------------------------------------------------------------------------------- /opendrive2lanelet/plane_elements/plane.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | from opendrive2lanelet.plane_elements.border import Border 5 | from opendrive2lanelet.commonroad import Lanelet 6 | 7 | class PLane(object): 8 | """ A lane defines a part of a road along a reference trajectory (plan view), using lane borders and start/stop positions (parametric) """ 9 | 10 | def __init__(self, id, type): 11 | """ Each lane is defined with a starting point, an offset to the reference trajectory and a lane width """ 12 | 13 | self._id = id 14 | self._type = type 15 | self._length = None 16 | self._innerBorder = None 17 | self._innerBorderOffset = None 18 | self._outerBorder = None 19 | self._outerBorderOffset = None 20 | 21 | self._isNotExistent = False 22 | self._innerNeighbours = [] 23 | self._outerNeighbours = [] 24 | 25 | self._successors = [] 26 | self._predecessors = [] 27 | 28 | @property 29 | def id(self): 30 | return self._id 31 | 32 | @property 33 | def type(self): 34 | return self._type 35 | 36 | @property 37 | def length(self): 38 | return self._length 39 | 40 | @length.setter 41 | def length(self, value): 42 | self._length = float(value) 43 | 44 | @property 45 | def innerBorder(self): 46 | return self._innerBorder 47 | 48 | @innerBorder.setter 49 | def innerBorder(self, value): 50 | if not isinstance(value, Border): 51 | raise TypeError("Value must be instance of _LaneBorder.") 52 | 53 | self._innerBorder = value 54 | 55 | def calcInnerBorder(self, sPos, addOffset=0.0): 56 | return self._innerBorder.calc(self._innerBorderOffset + sPos, addOffset=addOffset) 57 | 58 | @property 59 | def outerBorder(self): 60 | return self._outerBorder 61 | 62 | @property 63 | def innerBorderOffset(self): 64 | return self._innerBorderOffset 65 | 66 | @innerBorderOffset.setter 67 | def innerBorderOffset(self, value): 68 | self._innerBorderOffset = float(value) 69 | 70 | @outerBorder.setter 71 | def outerBorder(self, value): 72 | if not isinstance(value, Border): 73 | raise TypeError("Value must be instance of Border.") 74 | 75 | self._outerBorder = value 76 | 77 | def calcOuterBorder(self, sPos, addOffset=0.0): 78 | return self._outerBorder.calc(self._outerBorderOffset + sPos, addOffset=addOffset) 79 | 80 | @property 81 | def outerBorderOffset(self): 82 | return self._outerBorderOffset 83 | 84 | @outerBorderOffset.setter 85 | def outerBorderOffset(self, value): 86 | self._outerBorderOffset = float(value) 87 | 88 | def calcWidth(self, sPos): 89 | innerCoords = self.calcInnerBorder(sPos) 90 | outerCoords = self.calcOuterBorder(sPos) 91 | 92 | return np.linalg.norm(innerCoords[0] - outerCoords[0]) 93 | 94 | def convertToLanelet(self, precision=0.5, ref=None, refDistance=[0.0, 0.0], refMinDistance=3.0): 95 | # Define calculation points 96 | # TODO dependent on max error 97 | numSteps = max(2, np.ceil(self._length / float(precision))) 98 | poses = np.linspace(0, self._length, numSteps) 99 | 100 | left_vertices = [] 101 | right_vertices = [] 102 | 103 | for pos in poses: 104 | if ref is None: 105 | left_vertices.append(self.calcInnerBorder(pos)[0]) 106 | right_vertices.append(self.calcOuterBorder(pos)[0]) 107 | else: 108 | x = pos 109 | m = (refDistance[1] - refDistance[0]) / self._length 110 | t = refDistance[0] 111 | 112 | d = m*x + t 113 | 114 | if ref == "left": 115 | left_vertices.append(self.calcInnerBorder(pos)[0]) 116 | right_vertices.append(self.calcOuterBorder(pos, d)[0]) 117 | elif ref == "right": 118 | left_vertices.append(self.calcInnerBorder(pos, d)[0]) 119 | right_vertices.append(self.calcOuterBorder(pos)[0]) 120 | 121 | center_vertices = [(l + r) / 2 for (l, r) in zip(left_vertices, right_vertices)] 122 | 123 | return Lanelet( 124 | left_vertices=np.array([np.array([x, y]) for x, y in left_vertices]), 125 | center_vertices=np.array([np.array([x, y]) for x, y in center_vertices]), 126 | right_vertices=np.array([np.array([x, y]) for x, y in right_vertices]), 127 | lanelet_id=self._id 128 | ) 129 | 130 | @property 131 | def isNotExistent(self): 132 | return self._isNotExistent 133 | 134 | @isNotExistent.setter 135 | def isNotExistent(self, value): 136 | self._isNotExistent = bool(value) 137 | 138 | @property 139 | def innerNeighbours(self): 140 | return self._innerNeighbours 141 | 142 | @innerNeighbours.setter 143 | def innerNeighbours(self, value): 144 | self._innerNeighbours = value 145 | 146 | @property 147 | def outerNeighbours(self): 148 | return self._outerNeighbours 149 | 150 | @outerNeighbours.setter 151 | def outerNeighbours(self, value): 152 | self._outerNeighbours = value 153 | 154 | @property 155 | def successors(self): 156 | return self._successors 157 | 158 | @property 159 | def predecessors(self): 160 | return self._predecessors 161 | -------------------------------------------------------------------------------- /opendriveparser/elements/roadPlanView.py: -------------------------------------------------------------------------------- 1 | 2 | import abc 3 | import numpy as np 4 | 5 | from opendriveparser.elements.eulerspiral import EulerSpiral 6 | 7 | 8 | class PlanView(object): 9 | 10 | def __init__(self): 11 | self._geometries = [] 12 | 13 | def addLine(self, startPosition, heading, length): 14 | self._geometries.append(Line(startPosition, heading, length)) 15 | 16 | def addSpiral(self, startPosition, heading, length, curvStart, curvEnd): 17 | self._geometries.append(Spiral(startPosition, heading, length, curvStart, curvEnd)) 18 | 19 | def addArc(self, startPosition, heading, length, curvature): 20 | self._geometries.append(Arc(startPosition, heading, length, curvature)) 21 | 22 | def addParamPoly3(self, startPosition, heading, length, aU, bU, cU, dU, aV, bV, cV, dV, pRange): 23 | self._geometries.append(ParamPoly3(startPosition, heading, length, aU, bU, cU, dU, aV, bV, cV, dV, pRange)) 24 | 25 | def getLength(self): 26 | """ Get length of whole plan view """ 27 | 28 | length = 0 29 | 30 | for geometry in self._geometries: 31 | length += geometry.getLength() 32 | 33 | return length 34 | 35 | def calc(self, sPos): 36 | """ Calculate position and tangent at sPos """ 37 | 38 | for geometry in self._geometries: 39 | if geometry.getLength() < sPos and not np.isclose(geometry.getLength(), sPos): 40 | sPos -= geometry.getLength() 41 | continue 42 | return geometry.calcPosition(sPos) 43 | 44 | # TODO 45 | return self._geometries[-1].calcPosition(self._geometries[-1].getLength()) 46 | 47 | # raise Exception("Tried to calculate a position outside of the borders of the reference path at s=" + str(sPos) + " but path has only length of l=" + str(self.getLength())) 48 | 49 | class Geometry(object): 50 | __metaclass__ = abc.ABCMeta 51 | 52 | @abc.abstractmethod 53 | def getStartPosition(self): 54 | """ Returns the overall geometry length """ 55 | return 56 | 57 | @abc.abstractmethod 58 | def getLength(self): 59 | """ Returns the overall geometry length """ 60 | return 61 | 62 | @abc.abstractmethod 63 | def calcPosition(self, s): 64 | """ Calculates the position of the geometry as if the starting point is (0/0) """ 65 | return 66 | 67 | class Line(Geometry): 68 | 69 | def __init__(self, startPosition, heading, length): 70 | self.startPosition = np.array(startPosition) 71 | self.heading = heading 72 | self.length = length 73 | 74 | def getStartPosition(self): 75 | return self.startPosition 76 | 77 | def getLength(self): 78 | return self.length 79 | 80 | def calcPosition(self, s): 81 | pos = self.startPosition + np.array([s * np.cos(self.heading), s * np.sin(self.heading)]) 82 | tangent = self.heading 83 | 84 | return (pos, tangent) 85 | 86 | class Arc(Geometry): 87 | 88 | def __init__(self, startPosition, heading, length, curvature): 89 | self.startPosition = np.array(startPosition) 90 | self.heading = heading 91 | self.length = length 92 | self.curvature = curvature 93 | 94 | def getStartPosition(self): 95 | return self.startPosition 96 | 97 | def getLength(self): 98 | return self.length 99 | 100 | def calcPosition(self, s): 101 | c = self.curvature 102 | hdg = self.heading - np.pi / 2 103 | 104 | a = 2 / c * np.sin(s * c / 2) 105 | alpha = (np.pi - s * c) / 2 - hdg 106 | 107 | dx = -1 * a * np.cos(alpha) 108 | dy = a * np.sin(alpha) 109 | 110 | pos = self.startPosition + np.array([dx, dy]) 111 | tangent = self.heading + s * self.curvature 112 | 113 | return (pos, tangent) 114 | 115 | class Spiral(Geometry): 116 | 117 | def __init__(self, startPosition, heading, length, curvStart, curvEnd): 118 | self._startPosition = np.array(startPosition) 119 | self._heading = heading 120 | self._length = length 121 | self._curvStart = curvStart 122 | self._curvEnd = curvEnd 123 | 124 | self._spiral = EulerSpiral.createFromLengthAndCurvature(self._length, self._curvStart, self._curvEnd) 125 | 126 | def getStartPosition(self): 127 | return self._startPosition 128 | 129 | def getLength(self): 130 | return self._length 131 | 132 | def calcPosition(self, s): 133 | (x, y, t) = self._spiral.calc(s, self._startPosition[0], self._startPosition[1], self._curvStart, self._heading) 134 | 135 | return (np.array([x, y]), t) 136 | 137 | class Poly3(Geometry): 138 | 139 | def __init__(self, startPosition, heading, length, a, b, c, d): 140 | self._startPosition = np.array(startPosition) 141 | self._heading = heading 142 | self._length = length 143 | self._a = a 144 | self._b = b 145 | self._c = c 146 | self._d = d 147 | 148 | raise NotImplementedError() 149 | 150 | def getStartPosition(self): 151 | return self._startPosition 152 | 153 | def getLength(self): 154 | return self._length 155 | 156 | def calcPosition(self, s): 157 | # TODO untested 158 | 159 | # Calculate new point in s/t coordinate system 160 | coeffs = [self._a, self._b, self._c, self._d] 161 | 162 | t = np.polynomial.polynomial.polyval(s, coeffs) 163 | 164 | # Rotate and translate 165 | srot = s * np.cos(self._heading) - t * np.sin(self._heading) 166 | trot = s * np.sin(self._heading) + t * np.cos(self._heading) 167 | 168 | # Derivate to get heading change 169 | dCoeffs = coeffs[1:] * np.array(np.arange(1, len(coeffs))) 170 | tangent = np.polynomial.polynomial.polyval(s, dCoeffs) 171 | 172 | return (self._startPosition + np.array([srot, trot]), self._heading + tangent) 173 | 174 | class ParamPoly3(Geometry): 175 | 176 | def __init__(self, startPosition, heading, length, aU, bU, cU, dU, aV, bV, cV, dV, pRange): 177 | self._startPosition = np.array(startPosition) 178 | self._heading = heading 179 | self._length = length 180 | 181 | self._aU = aU 182 | self._bU = bU 183 | self._cU = cU 184 | self._dU = dU 185 | self._aV = aV 186 | self._bV = bV 187 | self._cV = cV 188 | self._dV = dV 189 | 190 | if pRange is None: 191 | self._pRange = 1.0 192 | else: 193 | self._pRange = pRange 194 | 195 | def getStartPosition(self): 196 | return self._startPosition 197 | 198 | def getLength(self): 199 | return self._length 200 | 201 | def calcPosition(self, s): 202 | 203 | # Position 204 | pos = (s / self._length) * self._pRange 205 | 206 | coeffsU = [self._aU, self._bU, self._cU, self._dU] 207 | coeffsV = [self._aV, self._bV, self._cV, self._dV] 208 | 209 | x = np.polynomial.polynomial.polyval(pos, coeffsU) 210 | y = np.polynomial.polynomial.polyval(pos, coeffsV) 211 | 212 | xrot = x * np.cos(self._heading) - y * np.sin(self._heading) 213 | yrot = x * np.sin(self._heading) + y * np.cos(self._heading) 214 | 215 | # Tangent is defined by derivation 216 | dCoeffsU = coeffsU[1:] * np.array(np.arange(1, len(coeffsU))) 217 | dCoeffsV = coeffsV[1:] * np.array(np.arange(1, len(coeffsV))) 218 | 219 | dx = np.polynomial.polynomial.polyval(pos, dCoeffsU) 220 | dy = np.polynomial.polynomial.polyval(pos, dCoeffsV) 221 | 222 | tangent = np.arctan2(dy, dx) 223 | 224 | 225 | return (self._startPosition + np.array([xrot, yrot]), self._heading + tangent) 226 | -------------------------------------------------------------------------------- /gui.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | import signal 4 | import sys 5 | 6 | from lxml import etree 7 | 8 | from PyQt5.QtCore import Qt 9 | from PyQt5.QtWidgets import QApplication, QWidget, QLineEdit, QFileDialog, QMainWindow 10 | from PyQt5.QtWidgets import QPushButton, QMessageBox, QLabel 11 | 12 | from opendriveparser import parse_opendrive 13 | from opendrive2lanelet import Network 14 | 15 | from viewer import MainWindow as ViewerWidget 16 | 17 | class MainWindow(QWidget): 18 | 19 | def __init__(self, argv): 20 | super().__init__() 21 | 22 | self.loadedRoadNetwork = None 23 | 24 | self._initUserInterface() 25 | self.show() 26 | 27 | if len(argv) >= 2: 28 | self.loadOpenDriveFile(argv[1]) 29 | self.viewLaneletNetwork() 30 | 31 | def _initUserInterface(self): 32 | 33 | self.setWindowTitle("OpenDRIVE 2 Lanelets Converter") 34 | 35 | self.setFixedSize(560, 345) 36 | 37 | self.loadButton = QPushButton('Load OpenDRIVE', self) 38 | self.loadButton.setToolTip('Load a OpenDRIVE scenario within a *.xodr file') 39 | self.loadButton.move(10, 10) 40 | self.loadButton.resize(130, 35) 41 | self.loadButton.clicked.connect(self.openOpenDriveFileDialog) 42 | 43 | self.inputOpenDriveFile = QLineEdit(self) 44 | self.inputOpenDriveFile.move(150, 10) 45 | self.inputOpenDriveFile.resize(400, 35) 46 | self.inputOpenDriveFile.setReadOnly(True) 47 | 48 | self.statsText = QLabel(self) 49 | self.statsText.move(10, 55) 50 | self.statsText.resize(540, 235) 51 | self.statsText.setAlignment(Qt.AlignLeft | Qt.AlignTop) 52 | self.statsText.setTextFormat(Qt.RichText) 53 | 54 | self.exportCommonRoadButton = QPushButton('Export as CommonRoad', self) 55 | self.exportCommonRoadButton.move(10, 300) 56 | self.exportCommonRoadButton.resize(170, 35) 57 | self.exportCommonRoadButton.setDisabled(True) 58 | self.exportCommonRoadButton.clicked.connect(self.exportAsCommonRoad) 59 | 60 | self.viewOutputButton = QPushButton('View Road Network', self) 61 | self.viewOutputButton.move(190, 300) 62 | self.viewOutputButton.resize(170, 35) 63 | self.viewOutputButton.setDisabled(True) 64 | self.viewOutputButton.clicked.connect(self.viewLaneletNetwork) 65 | 66 | def resetOutputElements(self): 67 | self.exportCommonRoadButton.setDisabled(True) 68 | self.viewOutputButton.setDisabled(True) 69 | 70 | def openOpenDriveFileDialog(self): 71 | self.resetOutputElements() 72 | 73 | path, _ = QFileDialog.getOpenFileName( 74 | self, 75 | "QFileDialog.getOpenFileName()", 76 | "", 77 | "OpenDRIVE files *.xodr (*.xodr)", 78 | options=QFileDialog.Options() 79 | ) 80 | 81 | if not path: 82 | return 83 | 84 | self.loadOpenDriveFile(path) 85 | 86 | def loadOpenDriveFile(self, path): 87 | 88 | filename = os.path.basename(path) 89 | self.inputOpenDriveFile.setText(filename) 90 | 91 | # Load road network and print some statistics 92 | try: 93 | fh = open(path, 'r') 94 | openDriveXml = parse_opendrive(etree.parse(fh).getroot()) 95 | fh.close() 96 | except (etree.XMLSyntaxError) as e: 97 | errorMsg = 'XML Syntax Error: {}'.format(e) 98 | QMessageBox.warning(self, 'OpenDRIVE error', 'There was an error during the loading of the selected OpenDRIVE file.\n\n{}'.format(errorMsg), QMessageBox.Ok) 99 | return 100 | except (TypeError, AttributeError, ValueError) as e: 101 | errorMsg = 'Value Error: {}'.format(e) 102 | QMessageBox.warning(self, 'OpenDRIVE error', 'There was an error during the loading of the selected OpenDRIVE file.\n\n{}'.format(errorMsg), QMessageBox.Ok) 103 | return 104 | 105 | self.loadedRoadNetwork = Network() 106 | self.loadedRoadNetwork.loadOpenDrive(openDriveXml) 107 | 108 | self.statsText.setText("Name: {}
Version: {}
Date: {}

OpenDRIVE Version {}.{}

Number of roads: {}
Total length of road network: {:.2f} meters".format( 109 | openDriveXml.header.name if openDriveXml.header.name else "unset", 110 | openDriveXml.header.version, 111 | openDriveXml.header.date, 112 | openDriveXml.header.revMajor, 113 | openDriveXml.header.revMinor, 114 | len(openDriveXml.roads), 115 | sum([road.length for road in openDriveXml.roads]) 116 | )) 117 | 118 | self.exportCommonRoadButton.setDisabled(False) 119 | self.viewOutputButton.setDisabled(False) 120 | 121 | def exportAsCommonRoad(self): 122 | 123 | if not self.loadedRoadNetwork: 124 | return 125 | 126 | path, _ = QFileDialog.getSaveFileName( 127 | self, 128 | "QFileDialog.getSaveFileName()", 129 | "", 130 | "CommonRoad files *.xml (*.xml)", 131 | options=QFileDialog.Options() 132 | ) 133 | 134 | if not path: 135 | return 136 | 137 | try: 138 | fh = open(path, "wb") 139 | fh.write(self.loadedRoadNetwork.exportCommonRoadScenario().export_to_string()) 140 | fh.close() 141 | except (IOError) as e: 142 | QMessageBox.critical(self, 'CommonRoad file not created!', 'The CommonRoad file was not exported due to an error.\n\n{}'.format(e), QMessageBox.Ok) 143 | return 144 | 145 | QMessageBox.information(self, 'CommonRoad file created!', 'The CommonRoad file was successfully exported.', QMessageBox.Ok) 146 | 147 | def viewLaneletNetwork(self): 148 | 149 | class ViewerWindow(QMainWindow): 150 | def __init__(self, parent=None): 151 | super(ViewerWindow, self).__init__(parent) 152 | self.viewer = ViewerWidget(self) 153 | 154 | self.setCentralWidget(self.viewer) 155 | 156 | viewer = ViewerWindow(self) 157 | viewer.viewer.openScenario(self.loadedRoadNetwork.exportCommonRoadScenario()) 158 | viewer.show() 159 | 160 | # def viewLaneletNetwork(self): 161 | # import matplotlib.pyplot as plt 162 | # from fvks.visualization.draw_dispatch import draw_object 163 | # from fvks.scenario.lanelet import Lanelet as FvksLanelet 164 | 165 | # def convert_to_fvks_lanelet(ll): 166 | # return FvksLanelet( 167 | # left_vertices=ll.left_vertices, 168 | # center_vertices=ll.center_vertices, 169 | # right_vertices=ll.right_vertices, 170 | # lanelet_id=ll.lanelet_id 171 | # ) 172 | 173 | # scenario = self.loadedRoadNetwork.exportCommonRoadScenario(filterTypes=[ 174 | # 'driving', 175 | # 'onRamp', 176 | # 'offRamp', 177 | # 'stop', 178 | # 'parking', 179 | # 'special1', 180 | # 'special2', 181 | # 'special3', 182 | # 'entry', 183 | # 'exit', 184 | # ]) 185 | 186 | # # Visualization 187 | # fig = plt.figure() 188 | # ax = fig.add_subplot(111) 189 | 190 | # for lanelet in scenario.lanelet_network.lanelets: 191 | # draw_object(convert_to_fvks_lanelet(lanelet), ax=ax) 192 | 193 | # ax.set_aspect('equal', 'datalim') 194 | # plt.axis('off') 195 | 196 | # plt.show() 197 | 198 | 199 | if __name__ == '__main__': 200 | # Make it possible to exit application with ctrl+c on console 201 | signal.signal(signal.SIGINT, signal.SIG_DFL) 202 | 203 | # Startup application 204 | app = QApplication(sys.argv) 205 | ex = MainWindow(sys.argv) 206 | sys.exit(app.exec_()) 207 | -------------------------------------------------------------------------------- /opendriveparser/elements/roadLanes.py: -------------------------------------------------------------------------------- 1 | 2 | class Lanes(object): 3 | 4 | def __init__(self): 5 | self._laneOffsets = [] 6 | self._laneSections = [] 7 | 8 | @property 9 | def laneOffsets(self): 10 | self._laneOffsets.sort(key=lambda x: x.sPos) 11 | return self._laneOffsets 12 | 13 | @property 14 | def laneSections(self): 15 | self._laneSections.sort(key=lambda x: x.sPos) 16 | return self._laneSections 17 | 18 | def getLaneSection(self, laneSectionIdx): 19 | for laneSection in self.laneSections: 20 | if laneSection.idx == laneSectionIdx: 21 | return laneSection 22 | 23 | return None 24 | 25 | def getLastLaneSectionIdx(self): 26 | 27 | numLaneSections = len(self.laneSections) 28 | 29 | if numLaneSections > 1: 30 | return numLaneSections - 1 31 | 32 | return 0 33 | 34 | class LaneOffset(object): 35 | 36 | def __init__(self): 37 | self._sPos = None 38 | self._a = None 39 | self._b = None 40 | self._c = None 41 | self._d = None 42 | 43 | @property 44 | def sPos(self): 45 | return self._sPos 46 | 47 | @sPos.setter 48 | def sPos(self, value): 49 | self._sPos = float(value) 50 | 51 | @property 52 | def a(self): 53 | return self._a 54 | 55 | @a.setter 56 | def a(self, value): 57 | self._a = float(value) 58 | 59 | @property 60 | def b(self): 61 | return self._b 62 | 63 | @b.setter 64 | def b(self, value): 65 | self._b = float(value) 66 | 67 | @property 68 | def c(self): 69 | return self._c 70 | 71 | @c.setter 72 | def c(self, value): 73 | self._c = float(value) 74 | 75 | @property 76 | def d(self): 77 | return self._d 78 | 79 | @d.setter 80 | def d(self, value): 81 | self._d = float(value) 82 | 83 | @property 84 | def coeffs(self): 85 | """ Array of coefficients for usage with numpy.polynomial.polynomial.polyval """ 86 | return [self._a, self._b, self._c, self._d] 87 | 88 | 89 | class LaneSection(object): 90 | 91 | def __init__(self, road=None): 92 | self._idx = None 93 | self._sPos = None 94 | self._singleSide = None 95 | self._leftLanes = LeftLanes() 96 | self._centerLanes = CenterLanes() 97 | self._rightLanes = RightLanes() 98 | 99 | self._parentRoad = road 100 | 101 | @property 102 | def idx(self): 103 | return self._idx 104 | 105 | @idx.setter 106 | def idx(self, value): 107 | self._idx = int(value) 108 | 109 | @property 110 | def sPos(self): 111 | return self._sPos 112 | 113 | @sPos.setter 114 | def sPos(self, value): 115 | self._sPos = float(value) 116 | 117 | @property 118 | def length(self): 119 | return self._length 120 | 121 | @length.setter 122 | def length(self, value): 123 | self._length = float(value) 124 | 125 | @property 126 | def singleSide(self): 127 | return self._singleSide 128 | 129 | @singleSide.setter 130 | def singleSide(self, value): 131 | if value not in ["true", "false"] and value is not None: 132 | raise AttributeError("Value must be true or false.") 133 | 134 | self._singleSide = (value == "true") 135 | 136 | @property 137 | def leftLanes(self): 138 | """ Get list of sorted lanes always starting in the middle (lane id -1) """ 139 | return self._leftLanes.lanes 140 | 141 | @property 142 | def centerLanes(self): 143 | return self._centerLanes.lanes 144 | 145 | @property 146 | def rightLanes(self): 147 | """ Get list of sorted lanes always starting in the middle (lane id 1) """ 148 | return self._rightLanes.lanes 149 | 150 | @property 151 | def allLanes(self): 152 | """ Attention! lanes are not sorted by id """ 153 | return self._leftLanes.lanes + self._centerLanes.lanes + self._rightLanes.lanes 154 | 155 | def getLane(self, laneId): 156 | for lane in self.allLanes: 157 | if lane.id == laneId: 158 | return lane 159 | 160 | return None 161 | 162 | @property 163 | def parentRoad(self): 164 | return self._parentRoad 165 | 166 | 167 | class LeftLanes(object): 168 | 169 | sort_direction = False 170 | 171 | def __init__(self): 172 | self._lanes = [] 173 | 174 | @property 175 | def lanes(self): 176 | self._lanes.sort(key=lambda x: x.id, reverse=self.sort_direction) 177 | return self._lanes 178 | 179 | class CenterLanes(LeftLanes): 180 | pass 181 | 182 | class RightLanes(LeftLanes): 183 | sort_direction = True 184 | 185 | 186 | class Lane(object): 187 | 188 | laneTypes = [ 189 | "none", "driving", "stop", "shoulder", "biking", "sidewalk", "border", 190 | "restricted", "parking", "bidirectional", "median", "special1", "special2", 191 | "special3", "roadWorks", "tram", "rail", "entry", "exit", "offRamp", "onRamp" 192 | ] 193 | 194 | def __init__(self, parentRoad): 195 | self._parent_road = parentRoad 196 | self._id = None 197 | self._type = None 198 | self._level = None 199 | self._link = LaneLink() 200 | self._widths = [] 201 | self._borders = [] 202 | 203 | @property 204 | def parentRoad(self): 205 | return self._parent_road 206 | 207 | @property 208 | def id(self): 209 | return self._id 210 | 211 | @id.setter 212 | def id(self, value): 213 | self._id = int(value) 214 | 215 | @property 216 | def type(self): 217 | return self._type 218 | 219 | @type.setter 220 | def type(self, value): 221 | if value not in self.laneTypes: 222 | raise Exception() 223 | 224 | self._type = str(value) 225 | 226 | @property 227 | def level(self): 228 | return self._level 229 | 230 | @level.setter 231 | def level(self, value): 232 | if value not in ["true", "false"] and value is not None: 233 | raise AttributeError("Value must be true or false.") 234 | 235 | self._level = (value == "true") 236 | 237 | @property 238 | def link(self): 239 | return self._link 240 | 241 | @property 242 | def widths(self): 243 | self._widths.sort(key=lambda x: x.sOffset) 244 | return self._widths 245 | 246 | def getWidth(self, widthIdx): 247 | for width in self._widths: 248 | if width.idx == widthIdx: 249 | return width 250 | 251 | return None 252 | 253 | def getLastLaneWidthIdx(self): 254 | """ Returns the index of the last width sector of the lane """ 255 | 256 | numWidths = len(self._widths) 257 | 258 | if numWidths > 1: 259 | return numWidths - 1 260 | 261 | return 0 262 | 263 | @property 264 | def borders(self): 265 | return self._borders 266 | 267 | 268 | class LaneLink(object): 269 | 270 | def __init__(self): 271 | self._predecessor = None 272 | self._successor = None 273 | 274 | @property 275 | def predecessorId(self): 276 | return self._predecessor 277 | 278 | @predecessorId.setter 279 | def predecessorId(self, value): 280 | self._predecessor = int(value) 281 | 282 | @property 283 | def successorId(self): 284 | return self._successor 285 | 286 | @successorId.setter 287 | def successorId(self, value): 288 | self._successor = int(value) 289 | 290 | 291 | class LaneWidth(object): 292 | 293 | def __init__(self): 294 | self._idx = None 295 | self._sOffset = None 296 | self._a = None 297 | self._b = None 298 | self._c = None 299 | self._d = None 300 | 301 | @property 302 | def idx(self): 303 | return self._idx 304 | 305 | @idx.setter 306 | def idx(self, value): 307 | self._idx = int(value) 308 | 309 | @property 310 | def sOffset(self): 311 | return self._sOffset 312 | 313 | @sOffset.setter 314 | def sOffset(self, value): 315 | self._sOffset = float(value) 316 | 317 | @property 318 | def a(self): 319 | return self._a 320 | 321 | @a.setter 322 | def a(self, value): 323 | self._a = float(value) 324 | 325 | @property 326 | def b(self): 327 | return self._b 328 | 329 | @b.setter 330 | def b(self, value): 331 | self._b = float(value) 332 | 333 | @property 334 | def c(self): 335 | return self._c 336 | 337 | @c.setter 338 | def c(self, value): 339 | self._c = float(value) 340 | 341 | @property 342 | def d(self): 343 | return self._d 344 | 345 | @d.setter 346 | def d(self, value): 347 | self._d = float(value) 348 | 349 | @property 350 | def coeffs(self): 351 | """ Array of coefficients for usage with numpy.polynomial.polynomial.polyval """ 352 | return [self._a, self._b, self._c, self._d] 353 | 354 | class LaneBorder(LaneWidth): 355 | pass 356 | -------------------------------------------------------------------------------- /viewer.py: -------------------------------------------------------------------------------- 1 | 2 | import math 3 | import signal 4 | import sys 5 | import os 6 | 7 | from lxml import etree 8 | 9 | import numpy as np 10 | 11 | import matplotlib 12 | matplotlib.use('Qt5Agg') 13 | 14 | from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas 15 | from matplotlib.figure import Figure 16 | from matplotlib.path import Path 17 | from matplotlib.patches import PathPatch, Polygon 18 | 19 | from PyQt5.QtCore import Qt, pyqtSlot 20 | from PyQt5.QtWidgets import QApplication, QWidget, QTableWidget, QPushButton, QLineEdit, QFileDialog, QLabel, QMessageBox 21 | from PyQt5.QtWidgets import QSizePolicy, QHBoxLayout, QVBoxLayout, QTableWidgetItem, QAbstractItemView 22 | 23 | from opendrive2lanelet.commonroad import Lanelet, LaneletNetwork, Scenario, ScenarioError 24 | 25 | 26 | class Canvas(FigureCanvas): 27 | """Ultimately, this is a QWidget """ 28 | 29 | def __init__(self, parent=None, width=5, height=5, dpi=100): 30 | 31 | self.ax = None 32 | 33 | self.fig = Figure(figsize=(width, height), dpi=dpi) 34 | 35 | super(Canvas, self).__init__(self.fig) 36 | self.setParent(parent) 37 | self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) 38 | 39 | self.clear_axes() 40 | 41 | def clear_axes(self): 42 | if self.ax is not None: 43 | self.ax.clear() 44 | else: 45 | 46 | self.ax = self.fig.add_subplot(111) 47 | self.ax.set_aspect('equal', 'datalim') 48 | self.ax.set_axis_off() 49 | 50 | self.draw() 51 | 52 | def get_axes(self): 53 | return self.ax 54 | 55 | def update_plot(self): 56 | self.draw() 57 | 58 | class MainWindow(QWidget): 59 | 60 | def __init__(self, parent=None, path=None): 61 | super().__init__(parent) 62 | 63 | self.current_scenario = None 64 | self.selected_lanelet_id = None 65 | 66 | self._initUserInterface() 67 | self.show() 68 | 69 | if path is not None: 70 | self.openPath(path) 71 | 72 | def _initUserInterface(self): 73 | 74 | self.setWindowTitle("CommonRoad XML Viewer") 75 | 76 | self.setMinimumSize(1000, 600) 77 | 78 | #self.testButton = QPushButton('test', self) 79 | #self.testButton.clicked.connect(self.testCmd) 80 | 81 | self.loadButton = QPushButton('Load CommonRoad', self) 82 | self.loadButton.setToolTip('Load a CommonRoad scenario within a *.xml file') 83 | self.loadButton.clicked.connect(self.openCommonRoadFile) 84 | 85 | self.inputCommonRoadFile = QLineEdit(self) 86 | self.inputCommonRoadFile.setReadOnly(True) 87 | 88 | hbox = QHBoxLayout() 89 | hbox.addWidget(self.loadButton) 90 | hbox.addWidget(self.inputCommonRoadFile) 91 | 92 | self.dynamic = Canvas(self, width=5, height=10, dpi=100) 93 | 94 | vbox = QVBoxLayout() 95 | vbox.addLayout(hbox, 0) 96 | vbox.addWidget(self.dynamic, 1) 97 | 98 | vbox.setAlignment(Qt.AlignTop) 99 | 100 | self.laneletsList = QTableWidget(self) 101 | self.laneletsList.setSelectionBehavior(QAbstractItemView.SelectRows) 102 | self.laneletsList.clicked.connect(self.onClickLanelet) 103 | 104 | hbox2 = QHBoxLayout(self) 105 | hbox2.addLayout(vbox, 1) 106 | hbox2.addWidget(self.laneletsList, 0) 107 | 108 | self.setLayout(hbox2) 109 | 110 | def openCommonRoadFile(self): 111 | 112 | path, _ = QFileDialog.getOpenFileName( 113 | self, 114 | "QFileDialog.getOpenFileName()", 115 | "", 116 | "CommonRoad scenario files *.xml (*.xml)", 117 | options=QFileDialog.Options() 118 | ) 119 | 120 | if not path: 121 | return 122 | 123 | self.openPath(path) 124 | 125 | def openPath(self, path): 126 | 127 | filename = os.path.basename(path) 128 | self.inputCommonRoadFile.setText(filename) 129 | 130 | try: 131 | fh = open(path, 'rb') 132 | data = fh.read() 133 | fh.close() 134 | 135 | scenario = Scenario.read_from_string(data) 136 | except etree.XMLSyntaxError as e: 137 | errorMsg = 'Syntax Error: {}'.format(e) 138 | QMessageBox.warning(self, 'CommonRoad XML error', 'There was an error during the loading of the selected CommonRoad file.\n\n{}'.format(errorMsg), QMessageBox.Ok) 139 | return 140 | except Exception as e: 141 | errorMsg = '{}'.format(e) 142 | QMessageBox.warning(self, 'CommonRoad XML error', 'There was an error during the loading of the selected CommonRoad file.\n\n{}'.format(errorMsg), QMessageBox.Ok) 143 | return 144 | 145 | self.openScenario(scenario) 146 | 147 | def openScenario(self, scenario): 148 | self.current_scenario = scenario 149 | self.update_plot() 150 | 151 | @pyqtSlot() 152 | def onClickLanelet(self): 153 | self.dynamic.clear_axes() 154 | 155 | selectedLanelets = self.laneletsList.selectedItems() 156 | 157 | if not selectedLanelets: 158 | self.selected_lanelet_id = None 159 | return 160 | 161 | self.selected_lanelet_id = int(selectedLanelets[0].text()) 162 | self.update_plot() 163 | 164 | def update_plot(self): 165 | 166 | if self.current_scenario is None: 167 | self.dynamic.clear_axes() 168 | return 169 | 170 | try: 171 | selected_lanelet = self.current_scenario.lanelet_network.find_lanelet_by_id(self.selected_lanelet_id) 172 | 173 | except ScenarioError: 174 | selected_lanelet = None 175 | 176 | ax = self.dynamic.get_axes() 177 | 178 | xlim1 = float('Inf') 179 | xlim2 = -float('Inf') 180 | 181 | ylim1 = float('Inf') 182 | ylim2 = -float('Inf') 183 | 184 | for lanelet in self.current_scenario.lanelet_network.lanelets: 185 | 186 | # Selected lanelet 187 | if selected_lanelet is not None: 188 | 189 | draw_arrow = True 190 | 191 | if lanelet.lanelet_id == selected_lanelet.lanelet_id: 192 | color = 'red' 193 | alpha = 0.7 194 | zorder = 10 195 | label = '{} selected'.format(lanelet.lanelet_id) 196 | elif lanelet.lanelet_id in selected_lanelet.predecessor: 197 | color = 'blue' 198 | alpha = 0.5 199 | zorder = 5 200 | label = '{} predecessor of {}'.format(lanelet.lanelet_id, selected_lanelet.lanelet_id) 201 | elif lanelet.lanelet_id in selected_lanelet.successor: 202 | color = 'green' 203 | alpha = 0.5 204 | zorder = 5 205 | label = '{} successor of {}'.format(lanelet.lanelet_id, selected_lanelet.lanelet_id) 206 | elif lanelet.lanelet_id == selected_lanelet.adj_left: 207 | color = 'yellow' 208 | alpha = 0.5 209 | zorder = 5 210 | label = '{} adj left of {} ({})'.format(lanelet.lanelet_id, selected_lanelet.lanelet_id, "same" if selected_lanelet.adj_left_same_direction else "opposite") 211 | elif lanelet.lanelet_id == selected_lanelet.adj_right: 212 | color = 'orange' 213 | alpha = 0.5 214 | zorder = 5 215 | label = '{} adj right of {} ({})'.format(lanelet.lanelet_id, selected_lanelet.lanelet_id, "same" if selected_lanelet.adj_right_same_direction else "opposite") 216 | else: 217 | color = 'gray' 218 | alpha = 0.3 219 | zorder = 0 220 | label = None 221 | draw_arrow = False 222 | 223 | else: 224 | color = 'gray' 225 | alpha = 0.3 226 | zorder = 0 227 | label = None 228 | draw_arrow = False 229 | 230 | 231 | verts = [] 232 | codes = [Path.MOVETO] 233 | 234 | # TODO efficiency 235 | 236 | for x, y in np.vstack([lanelet.left_vertices, lanelet.right_vertices[::-1]]): 237 | verts.append([x, y]) 238 | codes.append(Path.LINETO) 239 | 240 | #if color != 'gray': 241 | xlim1 = min(xlim1, x) 242 | xlim2 = max(xlim2, x) 243 | 244 | ylim1 = min(ylim1, y) 245 | ylim2 = max(ylim2, y) 246 | 247 | verts.append(verts[0]) 248 | codes[-1] = Path.CLOSEPOLY 249 | 250 | path = Path(verts, codes) 251 | 252 | ax.add_patch(PathPatch(path, facecolor=color, edgecolor="black", lw=0.0, alpha=alpha, zorder=zorder, label=label)) 253 | 254 | ax.plot([x for x, y in lanelet.left_vertices], [y for x, y in lanelet.left_vertices], color="black", lw=0.1) 255 | ax.plot([x for x, y in lanelet.right_vertices], [y for x, y in lanelet.right_vertices], color="black", lw=0.1) 256 | 257 | if draw_arrow: 258 | idx = 0 259 | 260 | ml = lanelet.left_vertices[idx] 261 | mr = lanelet.right_vertices[idx] 262 | mc = lanelet.center_vertices[min(len(lanelet.center_vertices) - 1, idx + 10)] 263 | 264 | ax.plot([ml[0], mr[0], mc[0], ml[0]], [ml[1], mr[1], mc[1], ml[1]], color="black", lw=0.3, zorder=15) 265 | 266 | handles, labels = self.dynamic.get_axes().get_legend_handles_labels() 267 | self.dynamic.get_axes().legend(handles, labels) 268 | 269 | if xlim1 != float('Inf') and xlim2 != float('Inf') and ylim1 != float('Inf') and ylim2 != float('Inf'): 270 | self.dynamic.get_axes().set_xlim([xlim1, xlim2]) 271 | self.dynamic.get_axes().set_ylim([ylim1, ylim2]) 272 | 273 | self.dynamic.update_plot() 274 | 275 | 276 | self.laneletsList.setRowCount(len(self.current_scenario.lanelet_network.lanelets)) 277 | self.laneletsList.setColumnCount(2) 278 | 279 | for idx, lanelet in enumerate(self.current_scenario.lanelet_network.lanelets): 280 | self.laneletsList.setItem(idx - 1, 0, QTableWidgetItem("{}".format(lanelet.lanelet_id))) 281 | self.laneletsList.setItem(idx - 1, 1, QTableWidgetItem("{}".format(lanelet.description))) 282 | 283 | #def testCmd(self): 284 | # self.dynamic.fig.savefig("foo.pdf", bbox_inches='tight') 285 | 286 | 287 | 288 | if __name__ == '__main__': 289 | # Make it possible to exit application with ctrl+c on console 290 | signal.signal(signal.SIGINT, signal.SIG_DFL) 291 | 292 | # Startup application 293 | app = QApplication(sys.argv) 294 | 295 | if len(sys.argv) >= 2: 296 | ex = MainWindow(path=sys.argv[1]) 297 | else: 298 | ex = MainWindow() 299 | 300 | sys.exit(app.exec_()) 301 | -------------------------------------------------------------------------------- /opendrive2lanelet/commonroad.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | import time 4 | 5 | import numpy as np 6 | from lxml import etree, objectify 7 | 8 | from lxml import etree 9 | from lxml.builder import E 10 | 11 | 12 | 13 | class Scenario(object): 14 | 15 | def __init__(self, dt, benchmark_id=None): 16 | self.dt = dt 17 | self.lanelet_network = LaneletNetwork() 18 | self.benchmark_id = benchmark_id 19 | 20 | def add_objects(self, o): 21 | if type(o) == list: 22 | for oo in o: 23 | self.add_objects(oo) 24 | elif type(o) == LaneletNetwork: 25 | for l in o.lanelets: 26 | self.lanelet_network.add_lanelet(l) 27 | elif type(o) == Lanelet: 28 | self.lanelet_network.add_lanelet(o) 29 | else: 30 | raise ScenarioError 31 | 32 | def export_to_string(self, benchmarkId=None, date=None, timeStepSize=None, author=None, affiliation=None, source=None, tags=None): 33 | 34 | rootElement = E( 35 | "commonRoad", 36 | benchmarkID="unknown" if benchmarkId is None else benchmarkId, 37 | commonRoadVersion="2018a", 38 | date=time.strftime("%Y-%m-%d") if date is None else date, 39 | timeStepSize="0.1" if timeStepSize is None else timeStepSize, 40 | author="" if author is None else author, 41 | affiliation="" if affiliation is None else affiliation, 42 | source="" if source is None else source, 43 | tags="" if tags is None else tags 44 | ) 45 | 46 | # Road network 47 | for lanelet in self.lanelet_network.lanelets: 48 | 49 | # Bounds 50 | leftPointsElements = E("leftBound") 51 | 52 | for (x, y) in lanelet.left_vertices: 53 | leftPointsElements.append(E("point", E("x", str(x)), E("y", str(y)))) 54 | 55 | rightPointsElements = E("rightBound") 56 | 57 | for (x, y) in lanelet.right_vertices: 58 | rightPointsElements.append(E("point", E("x", str(x)), E("y", str(y)))) 59 | 60 | laneletElement = E("lanelet", leftPointsElements, rightPointsElements) 61 | laneletElement.set("id", str(int(lanelet.lanelet_id))) 62 | 63 | for predecessor in lanelet.predecessor: 64 | laneletElement.append(E("predecessor", ref=str(predecessor))) 65 | 66 | for successor in lanelet.successor: 67 | laneletElement.append(E("successor", ref=str(successor))) 68 | 69 | if lanelet.adj_left is not None: 70 | laneletElement.append(E("adjacentLeft", ref=str(lanelet.adj_left), drivingDir=str("same" if lanelet.adj_left_same_direction else "opposite"))) 71 | 72 | if lanelet.adj_right is not None: 73 | laneletElement.append(E("adjacentRight", ref=str(lanelet.adj_right), drivingDir=str("same" if lanelet.adj_right_same_direction else "opposite"))) 74 | 75 | rootElement.append(laneletElement) 76 | 77 | # Dummy planning problem 78 | planningProblem = E("planningProblem", id=str(0)) 79 | 80 | initialState = E("initialState") 81 | initialState.append(E('position', E('point', E('x', str(0.0)), E('y', str(0.0))))) 82 | initialState.append(E('velocity', E('exact', str(0.0)))) 83 | initialState.append(E('orientation', E('exact', str(0.0)))) 84 | initialState.append(E('yawRate', E('exact', str(0.0)))) 85 | initialState.append(E('slipAngle', E('exact', str(0.0)))) 86 | initialState.append(E('time', E('exact', str(0.0)))) 87 | 88 | planningProblem.append(initialState) 89 | 90 | goalState = E("goalState") 91 | goalState.append(E('position', E('circle', E('radius', str(1.0)), E('center', E('x', str(0.0)), E('y', str(0.0)))))) 92 | 93 | goalState.append(E('time', E('intervalStart', str(0.0)), E('intervalEnd', str(1.0)))) 94 | goalState.append(E('orientation', E('intervalStart', str(0.0)), E('intervalEnd', str(1.0)))) 95 | goalState.append(E('velocity', E('intervalStart', str(0.0)), E('intervalEnd', str(1.0)))) 96 | 97 | planningProblem.append(goalState) 98 | 99 | rootElement.append(planningProblem) 100 | 101 | commonRoadScenarioStr = etree.tostring(rootElement, pretty_print=True, xml_declaration=True, encoding='utf-8') 102 | 103 | # Make sure the XML is a valid 104 | schema = etree.XMLSchema(file=open(os.path.dirname(os.path.abspath(__file__)) + "/XML_commonRoad_XSD.xsd", "rb")) 105 | parser = objectify.makeparser(schema=schema, encoding='utf-8') 106 | 107 | try: 108 | etree.fromstring(commonRoadScenarioStr, parser) 109 | except etree.XMLSyntaxError as e: 110 | raise Exception('Could not produce valid CommonRoad file! Error: {}'.format(e.msg)) 111 | 112 | return commonRoadScenarioStr 113 | 114 | @staticmethod 115 | def read_from_string(input_string, dt=0.1): 116 | 117 | # Parse XML using CommonRoad schema 118 | schema = etree.XMLSchema(file=open(os.path.dirname(os.path.abspath(__file__)) + "/XML_commonRoad_XSD.xsd", "rb")) 119 | parser = objectify.makeparser(schema=schema, encoding='utf-8') 120 | 121 | root = objectify.fromstring(input_string, parser=parser) 122 | 123 | # Create scenario 124 | scenario = Scenario( 125 | dt=dt, 126 | benchmark_id=root.attrib['benchmarkID'] 127 | ) 128 | 129 | for lanelet in root.iterchildren('lanelet'): 130 | 131 | left_vertices = [] 132 | right_vertices = [] 133 | 134 | for pt in lanelet.leftBound.getchildren(): 135 | left_vertices.append(np.array([float(pt.x), float(pt.y)])) 136 | 137 | for pt in lanelet.rightBound.getchildren(): 138 | right_vertices.append(np.array([float(pt.x), float(pt.y)])) 139 | 140 | center_vertices = [(l + r) / 2 for (l, r) in zip(left_vertices, right_vertices)] 141 | 142 | scenario.lanelet_network.add_lanelet(Lanelet( 143 | left_vertices=np.array([np.array([x, y]) for x, y in left_vertices]), 144 | center_vertices=np.array([np.array([x, y]) for x, y in center_vertices]), 145 | right_vertices=np.array([np.array([x, y]) for x, y in right_vertices]), 146 | lanelet_id=int(lanelet.attrib['id']), 147 | predecessor=[int(el.attrib['ref']) for el in lanelet.iterchildren(tag='predecessor')], 148 | successor=[int(el.attrib['ref']) for el in lanelet.iterchildren(tag='successor')], 149 | adjacent_left=int(lanelet.adjacentLeft.attrib['ref']) if lanelet.find('adjacentLeft') is not None else None, 150 | adjacent_left_same_direction=lanelet.adjacentLeft.attrib['drivingDir'] == "same" if lanelet.find('adjacentLeft') is not None else None, 151 | adjacent_right=int(lanelet.adjacentRight.attrib['ref']) if lanelet.find('adjacentRight') is not None else None, 152 | adjacent_right_same_direction=lanelet.adjacentRight.attrib['drivingDir'] == "same" if lanelet.find('adjacentRight') is not None else None 153 | )) 154 | 155 | return scenario 156 | 157 | 158 | class ScenarioError(Exception): 159 | """Base class for exceptions in this module.""" 160 | pass 161 | 162 | class LaneletNetwork(object): 163 | 164 | def __init__(self): 165 | self.lanelets = [] 166 | 167 | def find_lanelet_by_id(self, lanelet_id): 168 | for l in self.lanelets: 169 | if l.lanelet_id == lanelet_id: 170 | return l 171 | raise ScenarioError 172 | 173 | def add_lanelet(self, lanelet): 174 | if type(lanelet) == list: 175 | for l in lanelet: 176 | self.add_lanelet(l) 177 | else: 178 | try: 179 | self.find_lanelet_by_id(lanelet.lanelet_id) 180 | except ScenarioError: 181 | self.lanelets.append(lanelet) 182 | else: 183 | raise Exception("Lanelet with id {} already in network.".format(lanelet.lanelet_id)) 184 | 185 | class Lanelet(object): 186 | 187 | def __init__(self, left_vertices, center_vertices, right_vertices, 188 | lanelet_id, 189 | predecessor=None, successor=None, 190 | adjacent_left=None, adjacent_left_same_direction=None, 191 | adjacent_right=None, adjacent_right_same_direction=None, 192 | speed_limit=None): 193 | if (len(left_vertices) != len(center_vertices) and 194 | len(center_vertices) != len(right_vertices)): 195 | raise ScenarioError 196 | self.left_vertices = left_vertices 197 | self.center_vertices = center_vertices 198 | self.right_vertices = right_vertices 199 | if predecessor is None: 200 | self.predecessor = [] 201 | else: 202 | self.predecessor = predecessor 203 | if successor is None: 204 | self.successor = [] 205 | else: 206 | self.successor = successor 207 | self.adj_left = adjacent_left 208 | self.adj_left_same_direction = adjacent_left_same_direction 209 | self.adj_right = adjacent_right 210 | self.adj_right_same_direction = adjacent_right_same_direction 211 | 212 | self.lanelet_id = lanelet_id 213 | self.speed_limit = speed_limit 214 | self.description = "" 215 | 216 | self.distance = [0] 217 | for i in range(1, len(self.center_vertices)): 218 | self.distance.append(self.distance[i-1] + 219 | np.linalg.norm(self.center_vertices[i] - 220 | self.center_vertices[i-1])) 221 | self.distance = np.array(self.distance) 222 | 223 | def calc_width_at_start(self): 224 | return np.linalg.norm(self.left_vertices[0], self.right_vertices[0]) 225 | 226 | def calc_width_at_end(self): 227 | return np.linalg.norm(np.array(self.left_vertices[-1]) - np.array(self.right_vertices[-1])) 228 | 229 | def concatenate(self, lanelet, lanelet_id=-1): 230 | laneletA = self 231 | laneletB = lanelet 232 | 233 | float_tolerance = 1e-6 234 | if (np.linalg.norm(laneletA.center_vertices[-1] - laneletB.center_vertices[0]) > float_tolerance or 235 | np.linalg.norm(laneletA.left_vertices[-1] - laneletB.left_vertices[0]) > float_tolerance or 236 | np.linalg.norm(laneletA.right_vertices[-1] - laneletB.right_vertices[0]) > float_tolerance): 237 | pass 238 | # TODO 239 | # raise Exception("no way {} {} {}".format( 240 | # np.linalg.norm(laneletA.center_vertices[-1] - laneletB.center_vertices[0]), 241 | # np.linalg.norm(laneletA.left_vertices[-1] - laneletB.left_vertices[0]), 242 | # np.linalg.norm(laneletA.right_vertices[-1] - laneletB.right_vertices[0]) 243 | # )) 244 | #return None 245 | 246 | left_vertices = np.vstack((laneletA.left_vertices, 247 | laneletB.left_vertices[1:])) 248 | center_vertices = np.vstack((laneletA.center_vertices, 249 | laneletB.center_vertices[1:])) 250 | right_vertices = np.vstack((laneletA.right_vertices, 251 | laneletB.right_vertices[1:])) 252 | return Lanelet(center_vertices=center_vertices, 253 | left_vertices=left_vertices, 254 | right_vertices=right_vertices, 255 | predecessor=laneletA.predecessor.copy(), 256 | successor=laneletB.successor.copy(), 257 | adjacent_left=None, 258 | adjacent_left_same_direction=None, 259 | adjacent_right=None, 260 | adjacent_right_same_direction=None, 261 | lanelet_id=lanelet_id, 262 | speed_limit=None) 263 | -------------------------------------------------------------------------------- /opendrive2lanelet/XML_commonRoad_XSD.xsd: -------------------------------------------------------------------------------- 1 | 2 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 195 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 223 | 225 | 227 | 229 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 304 | 306 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | -------------------------------------------------------------------------------- /opendriveparser/parser.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | from lxml import etree 4 | 5 | from opendriveparser.elements.openDrive import OpenDrive 6 | from opendriveparser.elements.road import Road 7 | from opendriveparser.elements.roadLink import Predecessor as RoadLinkPredecessor, Successor as RoadLinkSuccessor, Neighbor as RoadLinkNeighbor 8 | from opendriveparser.elements.roadType import Type as RoadType, Speed as RoadTypeSpeed 9 | from opendriveparser.elements.roadElevationProfile import Elevation as RoadElevationProfileElevation 10 | from opendriveparser.elements.roadLateralProfile import Superelevation as RoadLateralProfileSuperelevation, Crossfall as RoadLateralProfileCrossfall, Shape as RoadLateralProfileShape 11 | from opendriveparser.elements.roadLanes import LaneOffset as RoadLanesLaneOffset, Lane as RoadLaneSectionLane, LaneSection as RoadLanesSection, LaneWidth as RoadLaneSectionLaneWidth, LaneBorder as RoadLaneSectionLaneBorder 12 | from opendriveparser.elements.junction import Junction, Connection as JunctionConnection, LaneLink as JunctionConnectionLaneLink 13 | 14 | 15 | def parse_opendrive(rootNode): 16 | """ Tries to parse XML tree, returns OpenDRIVE object """ 17 | 18 | # Only accept lxml element 19 | if not etree.iselement(rootNode): 20 | raise TypeError("Argument rootNode is not a xml element") 21 | 22 | newOpenDrive = OpenDrive() 23 | 24 | # Header 25 | header = rootNode.find("header") 26 | 27 | if header is not None: 28 | 29 | if header.get('revMajor') is not None: 30 | newOpenDrive.header.revMajor = str(header.get('revMajor')) 31 | 32 | if header.get('revMinor') is not None: 33 | newOpenDrive.header.revMinor = str(header.get('revMinor')) 34 | 35 | if header.get('name') is not None: 36 | newOpenDrive.header.name = str(header.get('name')) 37 | 38 | if header.get('version') is not None: 39 | newOpenDrive.header.version = str(header.get('version')) 40 | 41 | if header.get('date') is not None: 42 | newOpenDrive.header.date = str(header.get('date')) 43 | 44 | if header.get('north') is not None: 45 | newOpenDrive.header.north = str(header.get('north')) 46 | 47 | if header.get('south') is not None: 48 | newOpenDrive.header.south = str(header.get('south')) 49 | 50 | if header.get('east') is not None: 51 | newOpenDrive.header.east = str(header.get('east')) 52 | 53 | if header.get('west') is not None: 54 | newOpenDrive.header.west = str(header.get('west')) 55 | 56 | if header.get('vendor') is not None: 57 | newOpenDrive.header.vendor = str(header.get('vendor')) 58 | 59 | # Reference 60 | if header.find("geoReference") is not None: 61 | pass 62 | # TODO not implemented 63 | 64 | # Junctions 65 | for junction in rootNode.findall("junction"): 66 | 67 | newJunction = Junction() 68 | 69 | newJunction.id = int(junction.get("id")) 70 | newJunction.name = str(junction.get("name")) 71 | 72 | for connection in junction.findall("connection"): 73 | 74 | newConnection = JunctionConnection() 75 | 76 | newConnection.id = connection.get("id") 77 | newConnection.incomingRoad = connection.get("incomingRoad") 78 | newConnection.connectingRoad = connection.get("connectingRoad") 79 | newConnection.contactPoint = connection.get("contactPoint") 80 | 81 | for laneLink in connection.findall("laneLink"): 82 | 83 | newLaneLink = JunctionConnectionLaneLink() 84 | 85 | newLaneLink.fromId = laneLink.get("from") 86 | newLaneLink.toId = laneLink.get("to") 87 | 88 | newConnection.addLaneLink(newLaneLink) 89 | 90 | newJunction.addConnection(newConnection) 91 | 92 | newOpenDrive.junctions.append(newJunction) 93 | 94 | # Load roads 95 | for road in rootNode.findall("road"): 96 | 97 | newRoad = Road() 98 | 99 | newRoad.id = int(road.get("id")) 100 | newRoad.name = road.get("name") 101 | 102 | junctionId = int(road.get("junction")) if road.get("junction") != "-1" else None 103 | 104 | if junctionId: 105 | newRoad.junction = newOpenDrive.getJunction(junctionId) 106 | 107 | # TODO verify road length 108 | newRoad.length = float(road.get("length")) 109 | 110 | # Links 111 | if road.find("link") is not None: 112 | 113 | predecessor = road.find("link").find("predecessor") 114 | 115 | if predecessor is not None: 116 | 117 | newPredecessor = RoadLinkPredecessor() 118 | 119 | newPredecessor.elementType = predecessor.get("elementType") 120 | newPredecessor.elementId = predecessor.get("elementId") 121 | newPredecessor.contactPoint = predecessor.get("contactPoint") 122 | 123 | newRoad.link.predecessor = newPredecessor 124 | 125 | successor = road.find("link").find("successor") 126 | 127 | if successor is not None: 128 | 129 | newSuccessor = RoadLinkSuccessor() 130 | 131 | newSuccessor.elementType = successor.get("elementType") 132 | newSuccessor.elementId = successor.get("elementId") 133 | newSuccessor.contactPoint = successor.get("contactPoint") 134 | 135 | newRoad.link.successor = newSuccessor 136 | 137 | for neighbor in road.find("link").findall("neighbor"): 138 | 139 | newNeighbor = RoadLinkNeighbor() 140 | 141 | newNeighbor.side = neighbor.get("side") 142 | newNeighbor.elementId = neighbor.get("elementId") 143 | newNeighbor.direction = neighbor.get("direction") 144 | 145 | newRoad.link.neighbors.append(newNeighbor) 146 | 147 | # Type 148 | for roadType in road.findall("type"): 149 | 150 | newType = RoadType() 151 | 152 | newType.sPos = roadType.get("s") 153 | newType.type = roadType.get("type") 154 | 155 | if roadType.find("speed"): 156 | 157 | newSpeed = RoadTypeSpeed() 158 | 159 | newSpeed.max = roadType.find("speed").get("max") 160 | newSpeed.unit = roadType.find("speed").get("unit") 161 | 162 | newType.speed = newSpeed 163 | 164 | newRoad.types.append(newType) 165 | 166 | # Plan view 167 | for geometry in road.find("planView").findall("geometry"): 168 | 169 | startCoord = [float(geometry.get("x")), float(geometry.get("y"))] 170 | 171 | if geometry.find("line") is not None: 172 | newRoad.planView.addLine(startCoord, float(geometry.get("hdg")), float(geometry.get("length"))) 173 | 174 | elif geometry.find("spiral") is not None: 175 | newRoad.planView.addSpiral(startCoord, float(geometry.get("hdg")), float(geometry.get("length")), float(geometry.find("spiral").get("curvStart")), float(geometry.find("spiral").get("curvEnd"))) 176 | 177 | elif geometry.find("arc") is not None: 178 | newRoad.planView.addArc(startCoord, float(geometry.get("hdg")), float(geometry.get("length")), float(geometry.find("arc").get("curvature"))) 179 | 180 | elif geometry.find("poly3") is not None: 181 | raise NotImplementedError() 182 | 183 | elif geometry.find("paramPoly3") is not None: 184 | if geometry.find("paramPoly3").get("pRange"): 185 | 186 | if geometry.find("paramPoly3").get("pRange") == "arcLength": 187 | pMax = float(geometry.get("length")) 188 | else: 189 | pMax = None 190 | else: 191 | pMax = None 192 | 193 | newRoad.planView.addParamPoly3( \ 194 | startCoord, \ 195 | float(geometry.get("hdg")), \ 196 | float(geometry.get("length")), \ 197 | float(geometry.find("paramPoly3").get("aU")), \ 198 | float(geometry.find("paramPoly3").get("bU")), \ 199 | float(geometry.find("paramPoly3").get("cU")), \ 200 | float(geometry.find("paramPoly3").get("dU")), \ 201 | float(geometry.find("paramPoly3").get("aV")), \ 202 | float(geometry.find("paramPoly3").get("bV")), \ 203 | float(geometry.find("paramPoly3").get("cV")), \ 204 | float(geometry.find("paramPoly3").get("dV")), \ 205 | pMax \ 206 | ) 207 | 208 | else: 209 | raise Exception("invalid xml") 210 | 211 | # Elevation profile 212 | if road.find("elevationProfile") is not None: 213 | 214 | for elevation in road.find("elevationProfile").findall("elevation"): 215 | 216 | newElevation = RoadElevationProfileElevation() 217 | 218 | newElevation.sPos = elevation.get("s") 219 | newElevation.a = elevation.get("a") 220 | newElevation.b = elevation.get("b") 221 | newElevation.c = elevation.get("c") 222 | newElevation.d = elevation.get("d") 223 | 224 | newRoad.elevationProfile.elevations.append(newElevation) 225 | 226 | # Lateral profile 227 | if road.find("lateralProfile") is not None: 228 | 229 | for superelevation in road.find("lateralProfile").findall("superelevation"): 230 | 231 | newSuperelevation = RoadLateralProfileSuperelevation() 232 | 233 | newSuperelevation.sPos = superelevation.get("s") 234 | newSuperelevation.a = superelevation.get("a") 235 | newSuperelevation.b = superelevation.get("b") 236 | newSuperelevation.c = superelevation.get("c") 237 | newSuperelevation.d = superelevation.get("d") 238 | 239 | newRoad.lateralProfile.superelevations.append(newSuperelevation) 240 | 241 | for crossfall in road.find("lateralProfile").findall("crossfall"): 242 | 243 | newCrossfall = RoadLateralProfileCrossfall() 244 | 245 | newCrossfall.side = crossfall.get("side") 246 | newCrossfall.sPos = crossfall.get("s") 247 | newCrossfall.a = crossfall.get("a") 248 | newCrossfall.b = crossfall.get("b") 249 | newCrossfall.c = crossfall.get("c") 250 | newCrossfall.d = crossfall.get("d") 251 | 252 | newRoad.lateralProfile.crossfalls.append(newCrossfall) 253 | 254 | for shape in road.find("lateralProfile").findall("shape"): 255 | 256 | newShape = RoadLateralProfileShape() 257 | 258 | newShape.sPos = shape.get("s") 259 | newShape.t = shape.get("t") 260 | newShape.a = shape.get("a") 261 | newShape.b = shape.get("b") 262 | newShape.c = shape.get("c") 263 | newShape.d = shape.get("d") 264 | 265 | newRoad.lateralProfile.shapes.append(newShape) 266 | 267 | # Lanes 268 | lanes = road.find("lanes") 269 | 270 | if lanes is None: 271 | raise Exception("Road must have lanes element") 272 | 273 | # Lane offset 274 | for laneOffset in lanes.findall("laneOffset"): 275 | 276 | newLaneOffset = RoadLanesLaneOffset() 277 | 278 | newLaneOffset.sPos = laneOffset.get("s") 279 | newLaneOffset.a = laneOffset.get("a") 280 | newLaneOffset.b = laneOffset.get("b") 281 | newLaneOffset.c = laneOffset.get("c") 282 | newLaneOffset.d = laneOffset.get("d") 283 | 284 | newRoad.lanes.laneOffsets.append(newLaneOffset) 285 | 286 | # Lane sections 287 | for laneSectionIdx, laneSection in enumerate(road.find("lanes").findall("laneSection")): 288 | 289 | newLaneSection = RoadLanesSection(road=newRoad) 290 | 291 | # Manually enumerate lane sections for referencing purposes 292 | newLaneSection.idx = laneSectionIdx 293 | 294 | newLaneSection.sPos = laneSection.get("s") 295 | newLaneSection.singleSide = laneSection.get("singleSide") 296 | 297 | sides = dict( 298 | left=newLaneSection.leftLanes, 299 | center=newLaneSection.centerLanes, 300 | right=newLaneSection.rightLanes 301 | ) 302 | 303 | for sideTag, newSideLanes in sides.items(): 304 | 305 | side = laneSection.find(sideTag) 306 | 307 | # It is possible one side is not present 308 | if side is None: 309 | continue 310 | 311 | for lane in side.findall("lane"): 312 | 313 | newLane = RoadLaneSectionLane( 314 | parentRoad=newRoad 315 | ) 316 | 317 | newLane.id = lane.get("id") 318 | newLane.type = lane.get("type") 319 | 320 | # In some sample files the level is not specified according to the OpenDRIVE spec 321 | newLane.level = "true" if lane.get("level") in [1, '1', 'true'] else "false" 322 | 323 | # Lane Links 324 | if lane.find("link") is not None: 325 | 326 | if lane.find("link").find("predecessor") is not None: 327 | newLane.link.predecessorId = lane.find("link").find("predecessor").get("id") 328 | 329 | if lane.find("link").find("successor") is not None: 330 | newLane.link.successorId = lane.find("link").find("successor").get("id") 331 | 332 | # Width 333 | for widthIdx, width in enumerate(lane.findall("width")): 334 | 335 | newWidth = RoadLaneSectionLaneWidth() 336 | 337 | newWidth.idx = widthIdx 338 | newWidth.sOffset = width.get("sOffset") 339 | newWidth.a = width.get("a") 340 | newWidth.b = width.get("b") 341 | newWidth.c = width.get("c") 342 | newWidth.d = width.get("d") 343 | 344 | newLane.widths.append(newWidth) 345 | 346 | # Border 347 | for borderIdx, border in enumerate(lane.findall("border")): 348 | 349 | newBorder = RoadLaneSectionLaneBorder() 350 | 351 | newBorder.idx = borderIdx 352 | newBorder.sPos = border.get("sOffset") 353 | newBorder.a = border.get("a") 354 | newBorder.b = border.get("b") 355 | newBorder.c = border.get("c") 356 | newBorder.d = border.get("d") 357 | 358 | newLane.borders.append(newBorder) 359 | 360 | # Road Marks 361 | # TODO implementation 362 | 363 | # Material 364 | # TODO implementation 365 | 366 | # Visiblility 367 | # TODO implementation 368 | 369 | # Speed 370 | # TODO implementation 371 | 372 | # Access 373 | # TODO implementation 374 | 375 | # Lane Height 376 | # TODO implementation 377 | 378 | # Rules 379 | # TODO implementation 380 | 381 | newSideLanes.append(newLane) 382 | 383 | newRoad.lanes.laneSections.append(newLaneSection) 384 | 385 | # OpenDRIVE does not provide lane section lengths by itself, calculate them by ourselves 386 | for laneSection in newRoad.lanes.laneSections: 387 | 388 | # Last lane section in road 389 | if laneSection.idx + 1 >= len(newRoad.lanes.laneSections): 390 | laneSection.length = newRoad.planView.getLength() - laneSection.sPos 391 | 392 | # All but the last lane section end at the succeeding one 393 | else: 394 | laneSection.length = newRoad.lanes.laneSections[laneSection.idx + 1].sPos - laneSection.sPos 395 | 396 | # OpenDRIVE does not provide lane width lengths by itself, calculate them by ourselves 397 | for laneSection in newRoad.lanes.laneSections: 398 | for lane in laneSection.allLanes: 399 | widthsPoses = np.array([x.sOffset for x in lane.widths] + [laneSection.length]) 400 | widthsLengths = widthsPoses[1:] - widthsPoses[:-1] 401 | 402 | for widthIdx, width in enumerate(lane.widths): 403 | width.length = widthsLengths[widthIdx] 404 | 405 | # Objects 406 | # TODO implementation 407 | 408 | # Signals 409 | # TODO implementation 410 | 411 | newOpenDrive.roads.append(newRoad) 412 | 413 | return newOpenDrive 414 | -------------------------------------------------------------------------------- /opendrive2lanelet/network.py: -------------------------------------------------------------------------------- 1 | 2 | import copy 3 | 4 | import numpy as np 5 | 6 | from opendriveparser.elements.openDrive import OpenDrive 7 | 8 | from opendrive2lanelet.plane_elements.plane import PLane 9 | from opendrive2lanelet.plane_elements.plane_group import PLaneGroup 10 | from opendrive2lanelet.plane_elements.border import Border 11 | from opendrive2lanelet.utils import encode_road_section_lane_width_id, decode_road_section_lane_width_id, allCloseToZero 12 | 13 | from opendrive2lanelet.commonroad import LaneletNetwork, Scenario, ScenarioError 14 | 15 | 16 | class Network(object): 17 | """ Represents a network of parametric lanes """ 18 | 19 | def __init__(self): 20 | self._planes = [] 21 | self._linkIndex = None 22 | 23 | def loadOpenDrive(self, openDrive): 24 | """ Load all elements of an OpenDRIVE network to a parametric lane representation """ 25 | 26 | if not isinstance(openDrive, OpenDrive): 27 | raise TypeError() 28 | 29 | self._linkIndex = self.createLinkIndex(openDrive) 30 | 31 | # Convert all parts of a road to parametric lanes (planes) 32 | for road in openDrive.roads: 33 | 34 | # The reference border is the base line for the whole road 35 | referenceBorder = Network.createReferenceBorder(road.planView, road.lanes.laneOffsets) 36 | 37 | # A lane section is the smallest part that can be converted at once 38 | for laneSection in road.lanes.laneSections: 39 | 40 | pLanes = Network.laneSectionToPLanes(laneSection, referenceBorder) 41 | 42 | self._planes.extend(pLanes) 43 | 44 | def addPLane(self, pLane): 45 | if not isinstance(pLane, PLane): 46 | raise TypeError() 47 | self._planes.append(pLane) 48 | 49 | def exportLaneletNetwork(self, filterTypes=None): 50 | """ Export lanelet as lanelet network """ 51 | 52 | # Convert groups to lanelets 53 | laneletNetwork = LaneletNetwork() 54 | 55 | for pLane in self._planes: 56 | if filterTypes is not None and pLane.type not in filterTypes: 57 | continue 58 | 59 | lanelet = pLane.convertToLanelet() 60 | 61 | lanelet.predecessor = self._linkIndex.getPredecessors(pLane.id) 62 | lanelet.successor = self._linkIndex.getSuccessors(pLane.id) 63 | 64 | lanelet.refPLane = pLane 65 | 66 | laneletNetwork.add_lanelet(lanelet) 67 | 68 | # Prune all not existing references 69 | lanelet_ids = [x.lanelet_id for x in laneletNetwork.lanelets] 70 | 71 | for lanelet in laneletNetwork.lanelets: 72 | for predecessor in lanelet.predecessor: 73 | if predecessor not in lanelet_ids: 74 | lanelet.predecessor.remove(predecessor) 75 | for successor in lanelet.successor: 76 | if successor not in lanelet_ids: 77 | lanelet.successor.remove(successor) 78 | if lanelet.adj_left not in lanelet_ids: 79 | lanelet.adj_left = None 80 | if lanelet.adj_right not in lanelet_ids: 81 | lanelet.adj_right = None 82 | 83 | # Perform lane merges 84 | # Condition for lane merge: 85 | # - Lanelet ends (has no successor or predecessor) 86 | # - Has an adjacent (left or right) with same type 87 | if True: 88 | for lanelet in laneletNetwork.lanelets: 89 | 90 | if len(lanelet.successor) == 0: 91 | 92 | if lanelet.adj_left is not None: 93 | adj_left_lanelet = laneletNetwork.find_lanelet_by_id(lanelet.adj_left) 94 | 95 | newLanelet = lanelet.refPLane.convertToLanelet( 96 | ref="right", 97 | refDistance=[adj_left_lanelet.calc_width_at_end(), 0.0] 98 | ) 99 | 100 | lanelet.left_vertices = newLanelet.left_vertices 101 | lanelet.center_vertices = newLanelet.center_vertices 102 | lanelet.right_vertices = newLanelet.right_vertices 103 | 104 | lanelet.successor.extend(adj_left_lanelet.successor) 105 | 106 | if lanelet.adj_right is not None: 107 | adj_right_lanelet = laneletNetwork.find_lanelet_by_id(lanelet.adj_right) 108 | 109 | newLanelet = lanelet.refPLane.convertToLanelet( 110 | ref="left", 111 | refDistance=[-1 * adj_right_lanelet.calc_width_at_end(), 0.0] 112 | ) 113 | 114 | lanelet.left_vertices = newLanelet.left_vertices 115 | lanelet.center_vertices = newLanelet.center_vertices 116 | lanelet.right_vertices = newLanelet.right_vertices 117 | 118 | lanelet.successor.extend(adj_right_lanelet.successor) 119 | 120 | if len(lanelet.predecessor) == 0: 121 | 122 | if lanelet.adj_left is not None: 123 | adj_left_lanelet = laneletNetwork.find_lanelet_by_id(lanelet.adj_left) 124 | 125 | newLanelet = lanelet.refPLane.convertToLanelet( 126 | ref="right", 127 | refDistance=[0.0, -1 * adj_left_lanelet.calc_width_at_end()] 128 | ) 129 | 130 | lanelet.left_vertices = newLanelet.left_vertices 131 | lanelet.center_vertices = newLanelet.center_vertices 132 | lanelet.right_vertices = newLanelet.right_vertices 133 | 134 | lanelet.predecessor.extend(adj_left_lanelet.predecessor) 135 | 136 | if lanelet.adj_right is not None: 137 | adj_right_lanelet = laneletNetwork.find_lanelet_by_id(lanelet.adj_right) 138 | 139 | newLanelet = lanelet.refPLane.convertToLanelet( 140 | ref="left", 141 | refDistance=[0.0, adj_right_lanelet.calc_width_at_end()] 142 | ) 143 | 144 | lanelet.left_vertices = newLanelet.left_vertices 145 | lanelet.center_vertices = newLanelet.center_vertices 146 | lanelet.right_vertices = newLanelet.right_vertices 147 | 148 | lanelet.predecessor.extend(adj_right_lanelet.predecessor) 149 | 150 | 151 | # Assign an integer id to each lanelet 152 | def convert_to_new_id(old_lanelet_id): 153 | if old_lanelet_id in convert_to_new_id.id_assign: 154 | new_lanelet_id = convert_to_new_id.id_assign[old_lanelet_id] 155 | else: 156 | new_lanelet_id = convert_to_new_id.lanelet_id 157 | convert_to_new_id.id_assign[old_lanelet_id] = new_lanelet_id 158 | 159 | convert_to_new_id.lanelet_id += 1 160 | return new_lanelet_id 161 | 162 | convert_to_new_id.id_assign = {} 163 | convert_to_new_id.lanelet_id = 100 164 | 165 | for lanelet in laneletNetwork.lanelets: 166 | lanelet.description = lanelet.lanelet_id 167 | lanelet.lanelet_id = convert_to_new_id(lanelet.lanelet_id) 168 | 169 | lanelet.predecessor = [convert_to_new_id(x) for x in lanelet.predecessor] 170 | lanelet.successor = [convert_to_new_id(x) for x in lanelet.successor] 171 | lanelet.adj_left = None if lanelet.adj_left is None else convert_to_new_id(lanelet.adj_left) 172 | lanelet.adj_right = None if lanelet.adj_right is None else convert_to_new_id(lanelet.adj_right) 173 | 174 | return laneletNetwork 175 | 176 | def exportCommonRoadScenario(self, dt=0.1, benchmark_id=None, filterTypes=None): 177 | """ Export a full CommonRoad scenario """ 178 | 179 | scenario = Scenario( 180 | dt=dt, 181 | benchmark_id=benchmark_id if benchmark_id is not None else "none" 182 | ) 183 | 184 | scenario.add_objects(self.exportLaneletNetwork( 185 | filterTypes=filterTypes if isinstance(filterTypes, list) else ['driving', 'onRamp', 'offRamp', 'exit', 'entry'] 186 | )) 187 | 188 | return scenario 189 | 190 | ############################################################################################## 191 | ## Helper functions 192 | 193 | @staticmethod 194 | def createReferenceBorder(planView, laneOffsets): 195 | """ Create the first (most inner) border line for a road, includes the lane Offsets """ 196 | 197 | firstLaneBorder = Border() 198 | 199 | # Set reference to plan view 200 | firstLaneBorder.reference = planView 201 | firstLaneBorder.refOffset = 0.0 202 | 203 | # Lane offfsets will be coeffs 204 | if any(laneOffsets): 205 | for laneOffset in laneOffsets: 206 | firstLaneBorder.coeffsOffsets.append(laneOffset.sPos) 207 | firstLaneBorder.coeffs.append(laneOffset.coeffs) 208 | else: 209 | firstLaneBorder.coeffsOffsets.append(0.0) 210 | firstLaneBorder.coeffs.append([0.0]) 211 | 212 | return firstLaneBorder 213 | 214 | @staticmethod 215 | def laneSectionToPLanes(laneSection, referenceBorder): 216 | """ Convert a whole lane section into a list of planes """ 217 | 218 | newPLanes = [] 219 | 220 | # Length of this lane section 221 | laneSectionStart = laneSection.sPos 222 | 223 | for side in ["right", "left"]: 224 | 225 | # lanes loaded by opendriveparser are aleady sorted by id 226 | # coeffsFactor decides if border is left or right of the reference line 227 | if side == "right": 228 | lanes = laneSection.rightLanes 229 | coeffsFactor = -1.0 230 | 231 | else: 232 | lanes = laneSection.leftLanes 233 | coeffsFactor = 1.0 234 | 235 | # Most inner border 236 | laneBorders = [referenceBorder] 237 | prevInnerNeighbours = [] 238 | 239 | for lane in lanes: 240 | 241 | if abs(lane.id) > 1: 242 | 243 | if lane.id > 0: 244 | innerLaneId = lane.id - 1 245 | outerLaneId = lane.id + 1 246 | else: 247 | innerLaneId = lane.id + 1 248 | outerLaneId = lane.id - 1 249 | 250 | innerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, innerLaneId, -1) 251 | innerNeighbourSameDirection = True 252 | 253 | outerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, outerLaneId, -1) 254 | else: 255 | # Skip lane id 0 256 | 257 | if lane.id == 1: 258 | innerLaneId = -1 259 | outerLaneId = 2 260 | else: 261 | innerLaneId = 1 262 | outerLaneId = -2 263 | 264 | innerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, innerLaneId, -1) 265 | innerNeighbourSameDirection = False 266 | 267 | outerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, outerLaneId, -1) 268 | 269 | innerNeighbours = [] 270 | 271 | # Create outer lane border 272 | newPLaneBorder = Border() 273 | newPLaneBorder.reference = laneBorders[-1] 274 | 275 | if len(laneBorders) == 1: 276 | newPLaneBorder.refOffset = laneSectionStart 277 | else: 278 | # Offset from reference border is already included in first created outer lane border 279 | newPLaneBorder.refOffset = 0.0 280 | 281 | for width in lane.widths: 282 | newPLaneBorder.coeffsOffsets.append(width.sOffset) 283 | newPLaneBorder.coeffs.append([x * coeffsFactor for x in width.coeffs]) 284 | 285 | laneBorders.append(newPLaneBorder) 286 | 287 | # Create new lane for each width segment 288 | newPLanesList = [] 289 | 290 | for width in lane.widths: 291 | 292 | newPLane = PLane( 293 | id=encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, lane.id, width.idx), 294 | type=lane.type 295 | ) 296 | 297 | if allCloseToZero(width.coeffs): 298 | newPLane.isNotExistent = True 299 | 300 | newPLane.innerNeighbours = prevInnerNeighbours 301 | 302 | newPLane.length = width.length 303 | 304 | newPLane.innerBorder = laneBorders[-2] 305 | newPLane.innerBorderOffset = width.sOffset + laneBorders[-1].refOffset 306 | 307 | newPLane.outerBorder = laneBorders[-1] 308 | newPLane.outerBorderOffset = width.sOffset 309 | 310 | newPLanesList.append(newPLane) 311 | innerNeighbours.append(newPLane) 312 | 313 | # Organize everything in a pLaneGroup if it is more than one element 314 | if len(newPLanesList) >= 1: 315 | newPLaneGroup = PLaneGroup( 316 | id=encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, lane.id, -1), 317 | innerNeighbour=innerNeighbourId, 318 | innerNeighbourSameDirection=innerNeighbourSameDirection, 319 | outerNeighbour=outerNeighbourId, 320 | reverse=True if lane.id > 0 else False 321 | ) 322 | 323 | for newPLane in newPLanesList: 324 | newPLaneGroup.append(newPLane) 325 | 326 | newPLanes.append(newPLaneGroup) 327 | 328 | else: 329 | newPLanes.append(newPLanesList[0]) 330 | 331 | prevInnerNeighbours = innerNeighbours 332 | 333 | return newPLanes 334 | 335 | @staticmethod 336 | def createLinkIndex(openDrive): 337 | """ Step through all junctions and each single lane to build up a index """ 338 | 339 | def add_to_index(linkIndex, pLaneId, successorId, reverse=False): 340 | if reverse: 341 | linkIndex.addLink(successorId, pLaneId) 342 | else: 343 | linkIndex.addLink(pLaneId, successorId) 344 | 345 | linkIndex = LinkIndex() 346 | 347 | # Extract link information from road lanes 348 | for road in openDrive.roads: 349 | for laneSection in road.lanes.laneSections: 350 | for lane in laneSection.allLanes: 351 | pLaneId = encode_road_section_lane_width_id(road.id, laneSection.idx, lane.id, -1) 352 | 353 | # Not the last lane section? > Next lane section in same road 354 | if laneSection.idx < road.lanes.getLastLaneSectionIdx(): 355 | 356 | successorId = encode_road_section_lane_width_id(road.id, laneSection.idx + 1, lane.link.successorId, -1) 357 | 358 | add_to_index(linkIndex, pLaneId, successorId, lane.id >= 0) 359 | 360 | # Last lane section! > Next road in first lane section 361 | else: 362 | 363 | # Try to get next road 364 | if road.link.successor is not None and road.link.successor.elementType != "junction": 365 | 366 | nextRoad = openDrive.getRoad(road.link.successor.elementId) 367 | 368 | if nextRoad is not None: 369 | 370 | if road.link.successor.contactPoint == "start": 371 | successorId = encode_road_section_lane_width_id(nextRoad.id, 0, lane.link.successorId, -1) 372 | add_to_index(linkIndex, pLaneId, successorId, lane.id >= 0) 373 | 374 | else: # contact point = end 375 | successorId = encode_road_section_lane_width_id(nextRoad.id, nextRoad.lanes.getLastLaneSectionIdx(), lane.link.successorId, -1) 376 | add_to_index(linkIndex, pLaneId, successorId, lane.id >= 0) 377 | 378 | 379 | # Not first lane section? > Previous lane section in same road 380 | if laneSection.idx > 0: 381 | predecessorId = encode_road_section_lane_width_id(road.id, laneSection.idx - 1, lane.link.predecessorId, -1) 382 | 383 | add_to_index(linkIndex, predecessorId, pLaneId, lane.id >= 0) 384 | 385 | # First lane section! > Previous road 386 | else: 387 | 388 | # Try to get previous road 389 | if road.link.predecessor is not None and road.link.predecessor.elementType != "junction": 390 | 391 | prevRoad = openDrive.getRoad(road.link.predecessor.elementId) 392 | 393 | if prevRoad is not None: 394 | 395 | if road.link.predecessor.contactPoint == "start": 396 | predecessorId = encode_road_section_lane_width_id(prevRoad.id, 0, lane.link.predecessorId, -1) 397 | add_to_index(linkIndex, predecessorId, pLaneId, lane.id >= 0) 398 | 399 | else: # contact point = end 400 | predecessorId = encode_road_section_lane_width_id(prevRoad.id, prevRoad.lanes.getLastLaneSectionIdx(), lane.link.predecessorId, -1) 401 | add_to_index(linkIndex, predecessorId, pLaneId, lane.id >= 0) 402 | 403 | # Add junctions 404 | for road in openDrive.roads: 405 | 406 | # Add junction links to end of road 407 | if road.link.successor is not None and road.link.successor.elementType == "junction": 408 | 409 | junction = openDrive.getJunction(road.link.successor.elementId) 410 | 411 | if junction is not None: 412 | 413 | for connection in junction.connections: 414 | 415 | roadA = openDrive.getRoad(connection.incomingRoad) 416 | roadAcp = "end" 417 | roadB = openDrive.getRoad(connection.connectingRoad) 418 | roadBcp = connection.contactPoint 419 | 420 | if roadA.id != road.id: 421 | roadA, roadB = [roadB, roadA] 422 | 423 | for laneLink in connection.laneLinks: 424 | 425 | if roadAcp == "start": 426 | pLaneId = encode_road_section_lane_width_id(roadA.id, 0, laneLink.fromId, -1) 427 | else: 428 | successorId = encode_road_section_lane_width_id(roadA.id, roadA.lanes.getLastLaneSectionIdx(), laneLink.fromId, -1) 429 | 430 | if roadBcp == "start": 431 | pLaneId = encode_road_section_lane_width_id(roadB.id, 0, laneLink.toId, -1) 432 | else: 433 | successorId = encode_road_section_lane_width_id(roadB.id, roadB.lanes.getLastLaneSectionIdx(), laneLink.toId, -1) 434 | 435 | add_to_index(linkIndex, pLaneId, successorId, laneLink.fromId < 0) 436 | 437 | # Add junction links to start of road 438 | if road.link.predecessor is not None and road.link.predecessor.elementType == "junction": 439 | 440 | junction = openDrive.getJunction(road.link.predecessor.elementId) 441 | 442 | if junction is not None: 443 | 444 | for connection in junction.connections: 445 | 446 | roadA = openDrive.getRoad(connection.incomingRoad) 447 | roadAcp = "start" 448 | roadB = openDrive.getRoad(connection.connectingRoad) 449 | roadBcp = connection.contactPoint 450 | 451 | if roadA.id != road.id: 452 | roadA, roadB = [roadB, roadA] 453 | 454 | for laneLink in connection.laneLinks: 455 | 456 | if roadAcp == "start": 457 | pLaneId = encode_road_section_lane_width_id(roadA.id, 0, laneLink.fromId, -1) 458 | else: 459 | predecessorId = encode_road_section_lane_width_id(roadA.id, roadA.lanes.getLastLaneSectionIdx(), laneLink.fromId, -1) 460 | 461 | if roadBcp == "start": 462 | pLaneId = encode_road_section_lane_width_id(roadB.id, 0, laneLink.toId, -1) 463 | else: 464 | predecessorId = encode_road_section_lane_width_id(roadB.id, roadB.lanes.getLastLaneSectionIdx(), laneLink.toId, -1) 465 | 466 | add_to_index(linkIndex, predecessorId, pLaneId, laneLink.fromId < 0) 467 | 468 | # for junction in openDrive.junctions: 469 | # for connection in junction.connections: 470 | 471 | # incomingRoad = openDrive.getRoad(connection.incomingRoad) 472 | # connectingRoad = openDrive.getRoad(connection.connectingRoad) 473 | 474 | # if incomingRoad is not None and connectingRoad is not None: 475 | 476 | # for laneLink in connection.laneLinks: 477 | 478 | # pLaneId = encode_road_section_lane_width_id(incomingRoad.id, incomingRoad.lanes.getLastLaneSectionIdx(), laneLink.fromId, -1) 479 | 480 | # if connection.contactPoint == "end": 481 | # successorId = encode_road_section_lane_width_id(connectingRoad.id, 0, laneLink.toId, -1) 482 | # else: 483 | # successorId = encode_road_section_lane_width_id(connectingRoad.id, connectingRoad.lanes.getLastLaneSectionIdx(), laneLink.toId, -1) 484 | 485 | # add_to_index(linkIndex, pLaneId, successorId, lane.id >= 0) 486 | 487 | 488 | return linkIndex 489 | 490 | 491 | class LinkIndex(object): 492 | """ Overall index of all links in the file, save everything as successors, predecessors can be found via a reverse search """ 493 | 494 | def __init__(self): 495 | self._successors = {} 496 | 497 | def addLink(self, pLaneId, successor): 498 | if pLaneId not in self._successors: 499 | self._successors[pLaneId] = [] 500 | 501 | if successor not in self._successors[pLaneId]: 502 | self._successors[pLaneId].append(successor) 503 | 504 | def remove(self, pLaneId): 505 | # Delete key 506 | if pLaneId in self._successors: 507 | del self._successors[pLaneId] 508 | 509 | # Delete all occurances in successor lists 510 | for successorsId, successors in self._successors.items(): 511 | if pLaneId in successors: 512 | self._successors[successorsId].remove(pLaneId) 513 | 514 | def getSuccessors(self, pLaneId): 515 | if pLaneId not in self._successors: 516 | return [] 517 | 518 | return self._successors[pLaneId] 519 | 520 | def getPredecessors(self, pLaneId): 521 | predecessors = [] 522 | 523 | for successorsPLaneId, successors in self._successors.items(): 524 | if pLaneId not in successors: 525 | continue 526 | 527 | if successorsPLaneId in predecessors: 528 | continue 529 | 530 | predecessors.append(successorsPLaneId) 531 | 532 | return predecessors 533 | 534 | def __str__(self): 535 | retstr = "Link Index:\n" 536 | 537 | for pre, succs in self._successors.items(): 538 | retstr += "\t{:15} > {}\n".format(pre, ", ".join(succs)) 539 | 540 | return retstr 541 | --------------------------------------------------------------------------------