diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..56e84f2 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "src/aadcUserPython/opendrive/opendrive2lanelets-converter"] + path = src/aadcUserPython/opendrive/opendrive2lanelets-converter + url = https://github.com/stefan-urban/opendrive2lanelets-converter.git diff --git a/src/aadcUserPython/opendrive/openDrive2csv/openDrive2csv.ipynb b/src/aadcUserPython/opendrive/openDrive2csv.ipynb similarity index 99% rename from src/aadcUserPython/opendrive/openDrive2csv/openDrive2csv.ipynb rename to src/aadcUserPython/opendrive/openDrive2csv.ipynb index 0eb0410..0c4c3b7 100644 --- a/src/aadcUserPython/opendrive/openDrive2csv/openDrive2csv.ipynb +++ b/src/aadcUserPython/opendrive/openDrive2csv.ipynb @@ -32,6 +32,9 @@ "metadata": {}, "outputs": [], "source": [ + "import sys\n", + "sys.path.append('opendrive2lanelets-converter')\n", + "\n", "from lxml import etree\n", "from opendriveparser import parse_opendrive\n", "from opendrive2lanelet import Network\n", @@ -3952,7 +3955,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.6.4" + "version": "3.6.3" }, "varInspector": { "cols": { diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/README.md b/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/README.md deleted file mode 100644 index 6a327d6..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/README.md +++ /dev/null @@ -1,33 +0,0 @@ -# OpenDRIVE 2 Lanelets - Converter - -## Requirements - -- Python 3.x -- numpy -- opendriveparser - -## Usage - -```python -from lxml import etree -from opendriveparser import parse_opendrive -from opendrive2lanelet import Network - -fh = open("input_opendrive.xodr", 'r') -openDrive = parse_opendrive(etree.parse(fh).getroot()) -fh.close() - -roadNetwork = Network() -roadNetwork.loadOpenDrive(openDrive) - -scenario = roadNetwork.exportCommonRoadScenario() - -# Write CommonRoad scenario to file -fh = open("output_commonroad_file.xml", "wb") -fh.write(scenario.export_to_string()) -fh.close() -``` - -## Author - -Stefan Urban \ No newline at end of file diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/XML_commonRoad_XSD.xsd b/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/XML_commonRoad_XSD.xsd deleted file mode 100644 index 5d17313..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/XML_commonRoad_XSD.xsd +++ /dev/null @@ -1,264 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/__init__.py b/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/__init__.py deleted file mode 100644 index 07d7f52..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ - -from opendrive2lanelet.network import Network diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/commonroad.py b/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/commonroad.py deleted file mode 100644 index 8cc7c67..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/commonroad.py +++ /dev/null @@ -1,230 +0,0 @@ - -import os -import time - -import numpy as np -from lxml import etree, objectify - -from lxml import etree -from lxml.builder import E - - - -class Scenario(object): - - def __init__(self, dt, benchmark_id=None): - self.dt = dt - self.lanelet_network = LaneletNetwork() - self.benchmark_id = benchmark_id - - def add_objects(self, o): - if type(o) == list: - for oo in o: - self.add_objects(oo) - elif type(o) == LaneletNetwork: - for l in o.lanelets: - self.lanelet_network.add_lanelet(l) - elif type(o) == Lanelet: - self.lanelet_network.add_lanelet(o) - else: - raise ScenarioError - - def export_to_string(self): - - rootElement = E("commonRoad", benchmarkID="a", commonRoadVersion="2017a", date=time.strftime("%Y-%m-%d"), timeStepSize="0.1") - - # Road network - for lanelet in self.lanelet_network.lanelets: - - # Bounds - leftPointsElements = E("leftBound") - - for (x, y) in lanelet.left_vertices: - leftPointsElements.append(E("point", E("x", str(x)), E("y", str(y)))) - - rightPointsElements = E("rightBound") - - for (x, y) in lanelet.right_vertices: - rightPointsElements.append(E("point", E("x", str(x)), E("y", str(y)))) - - laneletElement = E("lanelet", leftPointsElements, rightPointsElements) - laneletElement.set("id", str(int(lanelet.lanelet_id))) - - for predecessor in lanelet.predecessor: - laneletElement.append(E("predecessor", ref=str(predecessor))) - - for successor in lanelet.successor: - laneletElement.append(E("successor", ref=str(successor))) - - if lanelet.adj_left is not None: - laneletElement.append(E("adjacentLeft", ref=str(lanelet.adj_left), drivingDirection=str("same" if lanelet.adj_left_same_direction else "opposite"))) - - if lanelet.adj_right is not None: - laneletElement.append(E("adjacentRight", ref=str(lanelet.adj_right), drivingDirection=str("same" if lanelet.adj_right_same_direction else "opposite"))) - - rootElement.append(laneletElement) - - # Dummy planning problem - planningProblem = E("planningProblem", id=str(0)) - - initialState = E("initialState") - initialState.append(E('position', E('point', E('x', str(0.0)), E('y', str(0.0))))) - initialState.append(E('velocity', E('exact', str(0.0)))) - initialState.append(E('orientation', E('exact', str(0.0)))) - initialState.append(E('yawRate', E('exact', str(0.0)))) - initialState.append(E('slipAngle', E('exact', str(0.0)))) - initialState.append(E('time', E('exact', str(0.0)))) - - planningProblem.append(initialState) - - goalRegion = E("goalRegion") - - goalState = E("state") - goalState.append(E('position', E('point', E('x', str(0.0)), E('y', str(0.0))))) - goalState.append(E('orientation', E('exact', str(0.0)))) - goalState.append(E('time', E('exact', str(0.0)))) - goalState.append(E('velocity', E('exact', str(0.0)))) - goalState.append(E('acceleration', E('exact', str(0.0)))) - - goalRegion.append(goalState) - - planningProblem.append(goalRegion) - - rootElement.append(planningProblem) - - return etree.tostring(rootElement, pretty_print=True) - - @staticmethod - def read_from_string(input_string, dt=0.1): - - # Parse XML using CommonRoad schema - schema = etree.XMLSchema(file=open(os.path.dirname(os.path.abspath(__file__)) + "/XML_commonRoad_XSD.xsd", "rb")) - parser = objectify.makeparser(schema=schema) - - root = objectify.fromstring(input_string, parser=parser) - - # Create scenario - scenario = Scenario( - dt=dt, - benchmark_id=root.attrib['benchmarkID'] - ) - - for lanelet in root.iterchildren('lanelet'): - - left_vertices = [] - right_vertices = [] - - for pt in lanelet.leftBound.getchildren(): - left_vertices.append(np.array([float(pt.x), float(pt.y)])) - - for pt in lanelet.rightBound.getchildren(): - right_vertices.append(np.array([float(pt.x), float(pt.y)])) - - center_vertices = [(l + r) / 2 for (l, r) in zip(left_vertices, right_vertices)] - - scenario.lanelet_network.add_lanelet(Lanelet( - left_vertices=np.array([np.array([x, y]) for x, y in left_vertices]), - center_vertices=np.array([np.array([x, y]) for x, y in center_vertices]), - right_vertices=np.array([np.array([x, y]) for x, y in right_vertices]), - lanelet_id=int(lanelet.attrib['id']), - predecessor=[int(el.attrib['ref']) for el in lanelet.iterchildren(tag='predecessor')], - successor=[int(el.attrib['ref']) for el in lanelet.iterchildren(tag='successor')] - )) - - return scenario - - -class ScenarioError(Exception): - """Base class for exceptions in this module.""" - pass - -class LaneletNetwork(object): - - def __init__(self): - self.lanelets = [] - - def find_lanelet_by_id(self, lanelet_id): - for l in self.lanelets: - if l.lanelet_id == lanelet_id: - return l - raise ScenarioError - - def add_lanelet(self, lanelet): - if type(lanelet) == list: - for l in lanelet: - self.add_lanelet(l) - else: - try: - self.find_lanelet_by_id(lanelet.lanelet_id) - except ScenarioError: - self.lanelets.append(lanelet) - else: - raise Exception("Lanelet with id {} already in network.".format(lanelet.lanelet_id)) - -class Lanelet(object): - - def __init__(self, left_vertices, center_vertices, right_vertices, - lanelet_id, - predecessor=None, successor=None, - adjacent_left=None, adjacent_left_same_direction=None, - adjacent_right=None, adjacent_right_same_direction=None, - speed_limit=None): - if (len(left_vertices) != len(center_vertices) and - len(center_vertices) != len(right_vertices)): - raise ScenarioError - self.left_vertices = left_vertices - self.center_vertices = center_vertices - self.right_vertices = right_vertices - if predecessor is None: - self.predecessor = [] - else: - self.predecessor = predecessor - if successor is None: - self.successor = [] - else: - self.successor = successor - self.adj_left = adjacent_left - self.adj_left_same_direction = adjacent_left_same_direction - self.adj_right = adjacent_right - self.adj_right_same_direction = adjacent_right_same_direction - - self.lanelet_id = lanelet_id - self.speed_limit = speed_limit - self.distance = [0] - for i in range(1, len(self.center_vertices)): - self.distance.append(self.distance[i-1] + - np.linalg.norm(self.center_vertices[i] - - self.center_vertices[i-1])) - self.distance = np.array(self.distance) - self.description = "" - - def calc_width_at_start(self): - return np.linalg.norm(self.left_vertices[0], self.right_vertices[0]) - - def calc_width_at_end(self): - return np.linalg.norm(np.array(self.left_vertices[-1]) - np.array(self.right_vertices[-1])) - - def concatenate(self, lanelet, lanelet_id=-1): - float_tolerance = 1e-6 - if (np.linalg.norm(self.center_vertices[-1] - lanelet.center_vertices[0]) > float_tolerance or - np.linalg.norm(self.left_vertices[-1] - lanelet.left_vertices[0]) > float_tolerance or - np.linalg.norm(self.right_vertices[-1] - lanelet.right_vertices[0]) > float_tolerance): - pass - #return None - left_vertices = np.vstack((self.left_vertices, - lanelet.left_vertices[1:])) - center_vertices = np.vstack((self.center_vertices, - lanelet.center_vertices[1:])) - right_vertices = np.vstack((self.right_vertices, - lanelet.right_vertices[1:])) - return Lanelet(center_vertices=center_vertices, - left_vertices=left_vertices, - right_vertices=right_vertices, - predecessor=self.predecessor.copy(), - successor=lanelet.successor.copy(), - adjacent_left=None, - adjacent_left_same_direction=None, - adjacent_right=None, - adjacent_right_same_direction=None, - lanelet_id=lanelet_id, - speed_limit=None) diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/network.py b/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/network.py deleted file mode 100644 index db95162..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/network.py +++ /dev/null @@ -1,528 +0,0 @@ - -import copy - -import numpy as np - -from opendriveparser.elements.openDrive import OpenDrive - -from opendrive2lanelet.plane_elements.plane import PLane -from opendrive2lanelet.plane_elements.plane_group import PLaneGroup -from opendrive2lanelet.plane_elements.border import Border -from opendrive2lanelet.utils import encode_road_section_lane_width_id, decode_road_section_lane_width_id, allCloseToZero - -from opendrive2lanelet.commonroad import LaneletNetwork, Scenario, ScenarioError - - -class Network(object): - """ Represents a network of parametric lanes """ - - def __init__(self): - self._planes = [] - self._linkIndex = None - - def loadOpenDrive(self, openDrive): - """ Load all elements of an OpenDRIVE network to a parametric lane representation """ - - if not isinstance(openDrive, OpenDrive): - raise TypeError() - - self._linkIndex = self.createLinkIndex(openDrive) - - # Convert all parts of a road to parametric lanes (planes) - for road in openDrive.roads: - - # The reference border is the base line for the whole road - referenceBorder = Network.createReferenceBorder(road.planView, road.lanes.laneOffsets) - - # A lane section is the smallest part that can be converted at once - for laneSection in road.lanes.laneSections: - - pLanes = Network.laneSectionToPLanes(laneSection, referenceBorder) - - self._planes.extend(pLanes) - - def addPLane(self, pLane): - if not isinstance(pLane, PLane): - raise TypeError() - self._planes.append(pLane) - - def exportLaneletNetwork(self, filterTypes=None): - """ Export lanelet as lanelet network """ - - # Convert groups to lanelets - laneletNetwork = LaneletNetwork() - - for pLane in self._planes: - if filterTypes is not None and pLane.type not in filterTypes: - continue - - lanelet = pLane.convertToLanelet() - - lanelet.predecessor = self._linkIndex.getPredecessors(pLane.id) - lanelet.successor = self._linkIndex.getSuccessors(pLane.id) - - lanelet.refPLane = pLane - - laneletNetwork.add_lanelet(lanelet) - - # Prune all not existing references - lanelet_ids = [x.lanelet_id for x in laneletNetwork.lanelets] - - for lanelet in laneletNetwork.lanelets: - for predecessor in lanelet.predecessor: - if predecessor not in lanelet_ids: - lanelet.predecessor.remove(predecessor) - for successor in lanelet.successor: - if successor not in lanelet_ids: - lanelet.successor.remove(successor) - if lanelet.adj_left not in lanelet_ids: - lanelet.adj_left = None - if lanelet.adj_right not in lanelet_ids: - lanelet.adj_right = None - - # Perform lane merges - # Condition for lane merge: - # - Lanelet ends (has no successor or predecessor) - # - Has an adjacent (left or right) with same type - for lanelet in laneletNetwork.lanelets: - - if len(lanelet.successor) == 0: - - if lanelet.adj_left is not None: - adj_left_lanelet = laneletNetwork.find_lanelet_by_id(lanelet.adj_left) - - newLanelet = lanelet.refPLane.convertToLanelet( - ref="right", - refDistance=[adj_left_lanelet.calc_width_at_end(), 0.0] - ) - - lanelet.left_vertices = newLanelet.left_vertices - lanelet.center_vertices = newLanelet.center_vertices - lanelet.right_vertices = newLanelet.right_vertices - - lanelet.successor.extend(adj_left_lanelet.successor) - - if lanelet.adj_right is not None: - adj_right_lanelet = laneletNetwork.find_lanelet_by_id(lanelet.adj_right) - - newLanelet = lanelet.refPLane.convertToLanelet( - ref="left", - refDistance=[-1 * adj_right_lanelet.calc_width_at_end(), 0.0] - ) - - lanelet.left_vertices = newLanelet.left_vertices - lanelet.center_vertices = newLanelet.center_vertices - lanelet.right_vertices = newLanelet.right_vertices - - lanelet.successor.extend(adj_right_lanelet.successor) - - if len(lanelet.predecessor) == 0: - - if lanelet.adj_left is not None: - adj_left_lanelet = laneletNetwork.find_lanelet_by_id(lanelet.adj_left) - - newLanelet = lanelet.refPLane.convertToLanelet( - ref="right", - refDistance=[0.0, -1 * adj_left_lanelet.calc_width_at_end()] - ) - - lanelet.left_vertices = newLanelet.left_vertices - lanelet.center_vertices = newLanelet.center_vertices - lanelet.right_vertices = newLanelet.right_vertices - - lanelet.predecessor.extend(adj_left_lanelet.predecessor) - - if lanelet.adj_right is not None: - adj_right_lanelet = laneletNetwork.find_lanelet_by_id(lanelet.adj_right) - - newLanelet = lanelet.refPLane.convertToLanelet( - ref="left", - refDistance=[0.0, adj_right_lanelet.calc_width_at_end()] - ) - - lanelet.left_vertices = newLanelet.left_vertices - lanelet.center_vertices = newLanelet.center_vertices - lanelet.right_vertices = newLanelet.right_vertices - - lanelet.predecessor.extend(adj_right_lanelet.predecessor) - - - # Assign an integer id to each lanelet - def convert_to_new_id(old_lanelet_id): - if old_lanelet_id in convert_to_new_id.id_assign: - new_lanelet_id = convert_to_new_id.id_assign[old_lanelet_id] - else: - new_lanelet_id = convert_to_new_id.lanelet_id - convert_to_new_id.id_assign[old_lanelet_id] = new_lanelet_id - - convert_to_new_id.lanelet_id += 1 - return new_lanelet_id - - convert_to_new_id.id_assign = {} - convert_to_new_id.lanelet_id = 100 - - for lanelet in laneletNetwork.lanelets: - lanelet.description = lanelet.lanelet_id - lanelet.lanelet_id = convert_to_new_id(lanelet.lanelet_id) - - lanelet.predecessor = [convert_to_new_id(x) for x in lanelet.predecessor] - lanelet.successor = [convert_to_new_id(x) for x in lanelet.successor] - lanelet.adj_left = None if lanelet.adj_left is None else convert_to_new_id(lanelet.adj_left) - lanelet.adj_right = None if lanelet.adj_right is None else convert_to_new_id(lanelet.adj_right) - - return laneletNetwork - - def exportCommonRoadScenario(self, dt=0.1, benchmark_id=None, filterTypes=None): - """ Export a full CommonRoad scenario """ - - scenario = Scenario( - dt=dt, - benchmark_id=benchmark_id if benchmark_id is not None else "none" - ) - - scenario.add_objects(self.exportLaneletNetwork( - filterTypes=filterTypes if isinstance(filterTypes, list) else ['driving', 'onRamp', 'offRamp', 'exit', 'entry'] - )) - - return scenario - - ############################################################################################## - ## Helper functions - - @staticmethod - def createReferenceBorder(planView, laneOffsets): - """ Create the first (most inner) border line for a road, includes the lane Offsets """ - - firstLaneBorder = Border() - - # Set reference to plan view - firstLaneBorder.reference = planView - firstLaneBorder.refOffset = 0.0 - - # Lane offfsets will be coeffs - if any(laneOffsets): - for laneOffset in laneOffsets: - firstLaneBorder.coeffsOffsets.append(laneOffset.sPos) - firstLaneBorder.coeffs.append(laneOffset.coeffs) - else: - firstLaneBorder.coeffsOffsets.append(0.0) - firstLaneBorder.coeffs.append([0.0]) - - return firstLaneBorder - - @staticmethod - def laneSectionToPLanes(laneSection, referenceBorder): - """ Convert a whole lane section into a list of planes """ - - newPLanes = [] - - # Length of this lane section - laneSectionStart = laneSection.sPos - - for side in ["right", "left"]: - - # lanes loaded by opendriveparser are aleady sorted by id - # coeffsFactor decides if border is left or right of the reference line - if side == "right": - lanes = laneSection.rightLanes - coeffsFactor = -1.0 - - else: - lanes = laneSection.leftLanes - coeffsFactor = 1.0 - - # Most inner border - laneBorders = [referenceBorder] - prevInnerNeighbours = [] - - for lane in lanes: - - if abs(lane.id) > 1: - - if lane.id > 0: - innerLaneId = lane.id - 1 - outerLaneId = lane.id + 1 - else: - innerLaneId = lane.id + 1 - outerLaneId = lane.id - 1 - - innerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, innerLaneId, -1) - innerNeighbourSameDirection = True - - outerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, outerLaneId, -1) - else: - # Skip lane id 0 - - if lane.id == 1: - innerLaneId = -1 - outerLaneId = 2 - else: - innerLaneId = 1 - outerLaneId = -2 - - innerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, innerLaneId, -1) - innerNeighbourSameDirection = False - - outerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, outerLaneId, -1) - - newPLaneGroup = PLaneGroup( - id=encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, lane.id, -1), - innerNeighbour=innerNeighbourId, - innerNeighbourSameDirection=innerNeighbourSameDirection, - outerNeighbour=outerNeighbourId - ) - - innerNeighbours = [] - - # Create outer lane border - newPLaneBorder = Border() - newPLaneBorder.reference = laneBorders[-1] - - if len(laneBorders) == 1: - newPLaneBorder.refOffset = laneSectionStart - else: - # Offset from reference border is already included in first created outer lane border - newPLaneBorder.refOffset = 0.0 - - for width in lane.widths: - newPLaneBorder.coeffsOffsets.append(width.sOffset) - newPLaneBorder.coeffs.append([x * coeffsFactor for x in width.coeffs]) - - laneBorders.append(newPLaneBorder) - - # Create new lane for each width segment - for width in lane.widths: - - newPLane = PLane( - id=encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, lane.id, width.idx), - type=lane.type - ) - - if allCloseToZero(width.coeffs): - newPLane.isNotExistent = True - - newPLane.innerNeighbours = prevInnerNeighbours - - newPLane.length = width.length - - newPLane.innerBorder = laneBorders[-2] - newPLane.innerBorderOffset = width.sOffset + laneBorders[-1].refOffset - - newPLane.outerBorder = laneBorders[-1] - newPLane.outerBorderOffset = width.sOffset - - newPLaneGroup.append(newPLane) - innerNeighbours.append(newPLane) - - newPLanes.append(newPLaneGroup) - - prevInnerNeighbours = innerNeighbours - - return newPLanes - - @staticmethod - def createLinkIndex(openDrive): - """ Step through all junctions and each single lane to build up a index """ - - def add_to_index(linkIndex, pLaneId, successorId, reverse=False): - if reverse: - linkIndex.addLink(successorId, pLaneId) - else: - linkIndex.addLink(pLaneId, successorId) - - linkIndex = LinkIndex() - - # Extract link information from road lanes - for road in openDrive.roads: - for laneSection in road.lanes.laneSections: - for lane in laneSection.allLanes: - pLaneId = encode_road_section_lane_width_id(road.id, laneSection.idx, lane.id, -1) - - # Not the last lane section? > Next lane section in same road - if laneSection.idx < road.lanes.getLastLaneSectionIdx(): - - successorId = encode_road_section_lane_width_id(road.id, laneSection.idx + 1, lane.link.successorId, -1) - - add_to_index(linkIndex, pLaneId, successorId, lane.id >= 0) - - # Last lane section! > Next road in first lane section - else: - - # Try to get next road - if road.link.successor is not None and road.link.successor.elementType != "junction": - - nextRoad = openDrive.getRoad(road.link.successor.elementId) - - if nextRoad is not None: - - if road.link.successor.contactPoint == "start": - successorId = encode_road_section_lane_width_id(nextRoad.id, 0, lane.link.successorId, -1) - add_to_index(linkIndex, pLaneId, successorId, lane.id >= 0) - - else: # contact point = end - successorId = encode_road_section_lane_width_id(nextRoad.id, nextRoad.lanes.getLastLaneSectionIdx(), lane.link.successorId, -1) - add_to_index(linkIndex, pLaneId, successorId, lane.id >= 0) - - - # Not first lane section? > Previous lane section in same road - if laneSection.idx > 0: - predecessorId = encode_road_section_lane_width_id(road.id, laneSection.idx - 1, lane.link.predecessorId, -1) - - add_to_index(linkIndex, predecessorId, pLaneId, lane.id >= 0) - - # First lane section! > Previous road - else: - - # Try to get previous road - if road.link.predecessor is not None and road.link.predecessor.elementType != "junction": - - prevRoad = openDrive.getRoad(road.link.predecessor.elementId) - - if prevRoad is not None: - - if road.link.predecessor.contactPoint == "start": - predecessorId = encode_road_section_lane_width_id(prevRoad.id, 0, lane.link.predecessorId, -1) - add_to_index(linkIndex, predecessorId, pLaneId, lane.id >= 0) - - else: # contact point = end - predecessorId = encode_road_section_lane_width_id(prevRoad.id, prevRoad.lanes.getLastLaneSectionIdx(), lane.link.predecessorId, -1) - add_to_index(linkIndex, predecessorId, pLaneId, lane.id >= 0) - - # Add junctions - for road in openDrive.roads: - - # Add junction links to end of road - if road.link.successor is not None and road.link.successor.elementType == "junction": - - junction = openDrive.getJunction(road.link.successor.elementId) - - if junction is not None: - - for connection in junction.connections: - - roadA = openDrive.getRoad(connection.incomingRoad) - roadAcp = "end" - roadB = openDrive.getRoad(connection.connectingRoad) - roadBcp = connection.contactPoint - - if roadA.id != road.id: - roadA, roadB = [roadB, roadA] - - for laneLink in connection.laneLinks: - - if roadAcp == "start": - pLaneId = encode_road_section_lane_width_id(roadA.id, 0, laneLink.fromId, -1) - else: - successorId = encode_road_section_lane_width_id(roadA.id, roadA.lanes.getLastLaneSectionIdx(), laneLink.fromId, -1) - - if roadBcp == "start": - pLaneId = encode_road_section_lane_width_id(roadB.id, 0, laneLink.toId, -1) - else: - successorId = encode_road_section_lane_width_id(roadB.id, roadB.lanes.getLastLaneSectionIdx(), laneLink.toId, -1) - - add_to_index(linkIndex, pLaneId, successorId, laneLink.fromId < 0) - - # Add junction links to start of road - if road.link.predecessor is not None and road.link.predecessor.elementType == "junction": - - junction = openDrive.getJunction(road.link.predecessor.elementId) - - if junction is not None: - - for connection in junction.connections: - - roadA = openDrive.getRoad(connection.incomingRoad) - roadAcp = "start" - roadB = openDrive.getRoad(connection.connectingRoad) - roadBcp = connection.contactPoint - - if roadA.id != road.id: - roadA, roadB = [roadB, roadA] - - for laneLink in connection.laneLinks: - - if roadAcp == "start": - pLaneId = encode_road_section_lane_width_id(roadA.id, 0, laneLink.fromId, -1) - else: - predecessorId = encode_road_section_lane_width_id(roadA.id, roadA.lanes.getLastLaneSectionIdx(), laneLink.fromId, -1) - - if roadBcp == "start": - pLaneId = encode_road_section_lane_width_id(roadB.id, 0, laneLink.toId, -1) - else: - predecessorId = encode_road_section_lane_width_id(roadB.id, roadB.lanes.getLastLaneSectionIdx(), laneLink.toId, -1) - - add_to_index(linkIndex, predecessorId, pLaneId, laneLink.fromId < 0) - - # for junction in openDrive.junctions: - # for connection in junction.connections: - - # incomingRoad = openDrive.getRoad(connection.incomingRoad) - # connectingRoad = openDrive.getRoad(connection.connectingRoad) - - # if incomingRoad is not None and connectingRoad is not None: - - # for laneLink in connection.laneLinks: - - # pLaneId = encode_road_section_lane_width_id(incomingRoad.id, incomingRoad.lanes.getLastLaneSectionIdx(), laneLink.fromId, -1) - - # if connection.contactPoint == "end": - # successorId = encode_road_section_lane_width_id(connectingRoad.id, 0, laneLink.toId, -1) - # else: - # successorId = encode_road_section_lane_width_id(connectingRoad.id, connectingRoad.lanes.getLastLaneSectionIdx(), laneLink.toId, -1) - - # add_to_index(linkIndex, pLaneId, successorId, lane.id >= 0) - - - return linkIndex - - -class LinkIndex(object): - """ Overall index of all links in the file, save everything as successors, predecessors can be found via a reverse search """ - - def __init__(self): - self._successors = {} - - def addLink(self, pLaneId, successor): - if pLaneId not in self._successors: - self._successors[pLaneId] = [] - - if successor not in self._successors[pLaneId]: - self._successors[pLaneId].append(successor) - - def remove(self, pLaneId): - # Delete key - if pLaneId in self._successors: - del self._successors[pLaneId] - - # Delete all occurances in successor lists - for successorsId, successors in self._successors.items(): - if pLaneId in successors: - self._successors[successorsId].remove(pLaneId) - - def getSuccessors(self, pLaneId): - if pLaneId not in self._successors: - return [] - - return self._successors[pLaneId] - - def getPredecessors(self, pLaneId): - predecessors = [] - - for successorsPLaneId, successors in self._successors.items(): - if pLaneId not in successors: - continue - - if successorsPLaneId in predecessors: - continue - - predecessors.append(successorsPLaneId) - - return predecessors - - def __str__(self): - retstr = "Link Index:\n" - - for pre, succs in self._successors.items(): - retstr += "\t{:15} > {}\n".format(pre, ", ".join(succs)) - - return retstr diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/plane_elements/__init__.py b/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/plane_elements/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/plane_elements/border.py b/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/plane_elements/border.py deleted file mode 100644 index d92d5c0..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/plane_elements/border.py +++ /dev/null @@ -1,84 +0,0 @@ - -from functools import lru_cache -import numpy as np - -from opendriveparser.elements.roadPlanView import PlanView - -class Border(object): - """ - A lane border defines a path along a whole lane section - - a lane always used an inner and outer lane border - - the reference can be another lane border or a plan view - """ - - def __init__(self): - - self._refOffset = 0.0 - - self._coeffsOffsets = [] - self._coeffs = [] - - self._reference = None - - def __str__(self): - return str(self._refOffset) - - @property - def reference(self): - return self._reference - - @reference.setter - def reference(self, value): - if not isinstance(value, Border) and not isinstance(value, PlanView): - raise TypeError("Value must be instance of Border or PlanView") - - self._reference = value - - @property - def refOffset(self): - """ The reference offset """ - return self._refOffset - - @refOffset.setter - def refOffset(self, value): - self._refOffset = float(value) - - @property - def coeffs(self): - """ It is assumed the coeffs are added in ascending order! ([0] + [1] * x + [2] * x**2 + ...) """ - return self._coeffs - - @property - def coeffsOffsets(self): - """ Offsets for coeffs """ - return self._coeffsOffsets - - @lru_cache(maxsize=200000) - def calc(self, sPos, addOffset=0.0): - """ Calculate the border """ - - if isinstance(self._reference, PlanView): - # Last reference has to be a reference geometry - refPos, refTang = self._reference.calc(self._refOffset + sPos) - - elif isinstance(self._reference, Border): - # Offset of all inner lanes - refPos, refTang = self._reference.calc(self._refOffset + sPos) - - else: - raise Exception("Reference must be plan view or other lane border.") - - if not self._coeffs or not self._coeffsOffsets: - raise Exception("No entries for width definitions.") - - # Find correct coefficients - widthIdx = next((self._coeffsOffsets.index(n) for n in self._coeffsOffsets[::-1] if n <= sPos), len(self._coeffsOffsets)) - - # Calculate width at sPos - distance = np.polynomial.polynomial.polyval(sPos - self._coeffsOffsets[widthIdx], self._coeffs[widthIdx]) + addOffset - - # New point is in orthogonal direction - ortho = refTang + np.pi / 2 - newPos = refPos + np.array([distance * np.cos(ortho), distance * np.sin(ortho)]) - - return newPos, refTang diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/plane_elements/plane.py b/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/plane_elements/plane.py deleted file mode 100644 index c5a1ef9..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/plane_elements/plane.py +++ /dev/null @@ -1,160 +0,0 @@ - -import numpy as np - -from opendrive2lanelet.plane_elements.border import Border -from opendrive2lanelet.commonroad import Lanelet - -class PLane(object): - """ A lane defines a part of a road along a reference trajectory (plan view), using lane borders and start/stop positions (parametric) """ - - def __init__(self, id, type): - """ Each lane is defined with a starting point, an offset to the reference trajectory and a lane width """ - - self._id = id - self._type = type - self._length = None - self._innerBorder = None - self._innerBorderOffset = None - self._outerBorder = None - self._outerBorderOffset = None - - self._isNotExistent = False - self._innerNeighbours = [] - self._outerNeighbours = [] - - self._successors = [] - self._predecessors = [] - - @property - def id(self): - return self._id - - @property - def type(self): - return self._type - - @property - def length(self): - return self._length - - @length.setter - def length(self, value): - self._length = float(value) - - @property - def innerBorder(self): - return self._innerBorder - - @innerBorder.setter - def innerBorder(self, value): - if not isinstance(value, Border): - raise TypeError("Value must be instance of _LaneBorder.") - - self._innerBorder = value - - def calcInnerBorder(self, sPos, addOffset=0.0): - return self._innerBorder.calc(self._innerBorderOffset + sPos, addOffset=addOffset) - - @property - def outerBorder(self): - return self._outerBorder - - @property - def innerBorderOffset(self): - return self._innerBorderOffset - - @innerBorderOffset.setter - def innerBorderOffset(self, value): - self._innerBorderOffset = float(value) - - @outerBorder.setter - def outerBorder(self, value): - if not isinstance(value, Border): - raise TypeError("Value must be instance of Border.") - - self._outerBorder = value - - def calcOuterBorder(self, sPos, addOffset=0.0): - return self._outerBorder.calc(self._outerBorderOffset + sPos, addOffset=addOffset) - - @property - def outerBorderOffset(self): - return self._outerBorderOffset - - @outerBorderOffset.setter - def outerBorderOffset(self, value): - self._outerBorderOffset = float(value) - - def calcWidth(self, sPos): - innerCoords = self.calcInnerBorder(sPos) - outerCoords = self.calcOuterBorder(sPos) - - return np.linalg.norm(innerCoords[0] - outerCoords[0]) - - def convertToLanelet(self, precision=0.5, ref=None, refDistance=[0.0, 0.0]): - # Define calculation points - # TODO dependent on max error - numSteps = max(2, np.ceil(self._length / float(precision))) - poses = np.linspace(0, self._length, numSteps) - - left_vertices = [] - right_vertices = [] - - for pos in poses: - if ref is None: - left_vertices.append(self.calcInnerBorder(pos)[0]) - right_vertices.append(self.calcOuterBorder(pos)[0]) - else: - x = pos - m = (refDistance[1] - refDistance[0]) / self._length - t = refDistance[0] - - d = m*x + t - - if ref == "left": - left_vertices.append(self.calcInnerBorder(pos)[0]) - right_vertices.append(self.calcOuterBorder(pos, d)[0]) - elif ref == "right": - left_vertices.append(self.calcInnerBorder(pos, d)[0]) - right_vertices.append(self.calcOuterBorder(pos)[0]) - - center_vertices = [(l + r) / 2 for (l, r) in zip(left_vertices, right_vertices)] - - return Lanelet( - left_vertices=np.array([np.array([x, y]) for x, y in left_vertices]), - center_vertices=np.array([np.array([x, y]) for x, y in center_vertices]), - right_vertices=np.array([np.array([x, y]) for x, y in right_vertices]), - lanelet_id=self._id - ) - - @property - def isNotExistent(self): - return self._isNotExistent - - @isNotExistent.setter - def isNotExistent(self, value): - self._isNotExistent = bool(value) - - @property - def innerNeighbours(self): - return self._innerNeighbours - - @innerNeighbours.setter - def innerNeighbours(self, value): - self._innerNeighbours = value - - @property - def outerNeighbours(self): - return self._outerNeighbours - - @outerNeighbours.setter - def outerNeighbours(self, value): - self._outerNeighbours = value - - @property - def successors(self): - return self._successors - - @property - def predecessors(self): - return self._predecessors diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/plane_elements/plane_group.py b/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/plane_elements/plane_group.py deleted file mode 100644 index 0a5478b..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/plane_elements/plane_group.py +++ /dev/null @@ -1,97 +0,0 @@ - -class PLaneGroup(object): - """ A group of pLanes can be converted to a lanelet just like a single pLane """ - - def __init__(self, id=None, pLanes=None, innerNeighbour=None, innerNeighbourSameDirection=True, outerNeighbour=None): - - self._pLanes = [] - self._id = id - self._innerNeighbour = innerNeighbour - self._innerNeighbourSameDirection = innerNeighbourSameDirection - self._outerNeighbour = outerNeighbour - - if pLanes is not None: - - if isinstance(pLanes, list): - self._pLanes.extend(pLanes) - else: - self._pLanes.append(pLanes) - - def append(self, pLane): - self._pLanes.append(pLane) - - @property - def id(self): - if self._id is not None: - return self._id - - raise Exception() - - @property - def type(self): - return self._pLanes[0]._type - - @property - def length(self): - return sum([x.length for x in self._pLanes]) - - def convertToLanelet(self, precision=0.5, ref=None, refDistance=[0.0, 0.0]): - - lanelet = None - y1 = refDistance[0] - x = 0 - - for pLane in self._pLanes: - - x += pLane.length - y2 = (refDistance[1] - refDistance[0]) / self.length * x + refDistance[0] - - # First lanelet - if lanelet is None: - lanelet = pLane.convertToLanelet(precision=precision, ref=ref, refDistance=[y1, y2]) - lanelet.lanelet_id = self.id - continue - - # Append all following lanelets - lanelet = lanelet.concatenate(pLane.convertToLanelet(precision=precision, ref=ref, refDistance=[y1, y2]), self.id) - - y1 = y2 - - if lanelet is None: - raise Exception("Lanelet concatenation problem") - - # Adjacent lanes - if self.innerNeighbour is not None: - lanelet.adj_left = self.innerNeighbour - lanelet.adj_left_same_direction = self.innerNeighbourSameDirection - - if self.outerNeighbour is not None: - lanelet.adj_right = self.outerNeighbour - lanelet.adj_right_same_direction = True - - return lanelet - - @property - def innerNeighbour(self): - return self._innerNeighbour - - @innerNeighbour.setter - def innerNeighbour(self, value): - self._innerNeighbour = value - - @property - def innerNeighbourSameDirection(self): - return self._innerNeighbourSameDirection - - @innerNeighbourSameDirection.setter - def innerNeighbourSameDirection(self, value): - self._innerNeighbourSameDirection = value - - @property - def outerNeighbour(self): - """ Outer neighbour has always the same driving direction """ - return self._outerNeighbour - - @outerNeighbour.setter - def outerNeighbour(self, value): - self._outerNeighbour = value diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/utils.py b/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/utils.py deleted file mode 100644 index c7e2082..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendrive2lanelet/utils.py +++ /dev/null @@ -1,18 +0,0 @@ - -def encode_road_section_lane_width_id(roadId, sectionId, laneId, widthId): - return ".".join([str(roadId), str(sectionId), str(laneId), str(widthId)]) - -def decode_road_section_lane_width_id(encodedString): - - parts = encodedString.split(".") - - if len(parts) != 4: - raise Exception() - - return (int(parts[0]), int(parts[1]), int(parts[2]), int(parts[3])) - -def allCloseToZero(a): - """ Tests if all elements of a are close to zero. """ - - import numpy - return numpy.allclose(a, numpy.zeros(numpy.shape(a))) diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/README.md b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/README.md deleted file mode 100644 index 7132e21..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/README.md +++ /dev/null @@ -1,20 +0,0 @@ -# OpenDRIVE parser - -## Usage - -```python -from lxml import etree -from opendriveparser import parse_opendrive - -fh = open("input_opendrive.xodr", 'r') -openDrive = parse_opendrive(etree.parse(fh).getroot()) -fh.close() - -# Now do stuff with the data -for road in openDrive.roads: - print("Road ID: {}".format(road.id)) -``` - -## Author - -Stefan Urban \ No newline at end of file diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/__init__.py b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/__init__.py deleted file mode 100644 index dac222a..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ - -from opendriveparser.parser import parse_opendrive diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/__init__.py b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/eulerspiral.py b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/eulerspiral.py deleted file mode 100644 index 8bd1cb9..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/eulerspiral.py +++ /dev/null @@ -1,41 +0,0 @@ - -import numpy as np -from scipy.special import fresnel - -class EulerSpiral(object): - - def __init__(self, gamma): - self._gamma = gamma - - @staticmethod - def createFromLengthAndCurvature(length, curvStart, curvEnd): - return EulerSpiral(1 * (curvEnd - curvStart) / length) - - def calc(self, s, x0=0, y0=0, kappa0=0, theta0=0): - - # Start - C0 = x0 + 1j * y0 - - if self._gamma == 0 and kappa0 == 0: - # Straight line - Cs = C0 + np.exp(1j * theta0 * s) - - elif self._gamma == 0 and kappa0 != 0: - # Arc - Cs = C0 + np.exp(1j * theta0) / kappa0 * (np.sin(kappa0 * s) + 1j * (1 - np.cos(kappa0 * s))) - - else: - # Fresnel integrals - Sa, Ca = fresnel((kappa0 + self._gamma * s) / np.sqrt(np.pi * np.abs(self._gamma))) - Sb, Cb = fresnel(kappa0 / np.sqrt(np.pi * np.abs(self._gamma))) - - # Euler Spiral - Cs1 = np.sqrt(np.pi / np.abs(self._gamma)) * np.exp(1j * (theta0 - kappa0**2 / 2 / self._gamma)) - Cs2 = np.sign(self._gamma) * (Ca - Cb) + 1j * Sa - 1j * Sb - - Cs = C0 + Cs1 * Cs2 - - # Tangent at each point - theta = self._gamma * s**2 / 2 + kappa0 * s + theta0 - - return (Cs.real, Cs.imag, theta) diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/junction.py b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/junction.py deleted file mode 100644 index 2a334fc..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/junction.py +++ /dev/null @@ -1,116 +0,0 @@ - -class Junction(object): - # TODO priority - # TODO controller - - def __init__(self): - self._id = None - self._name = None - self._connections = [] - - @property - def id(self): - return self._id - - @id.setter - def id(self, value): - self._id = int(value) - - @property - def name(self): - return self._name - - @name.setter - def name(self, value): - self._name = str(value) - - @property - def connections(self): - return self._connections - - def addConnection(self, connection): - if not isinstance(connection, Connection): - raise TypeError("Has to be of instance Connection") - - self._connections.append(connection) - - -class Connection(object): - - def __init__(self): - self._id = None - self._incomingRoad = None - self._connectingRoad = None - self._contactPoint = None - self._laneLinks = [] - - @property - def id(self): - return self._id - - @id.setter - def id(self, value): - self._id = int(value) - - @property - def incomingRoad(self): - return self._incomingRoad - - @incomingRoad.setter - def incomingRoad(self, value): - self._incomingRoad = int(value) - - @property - def connectingRoad(self): - return self._connectingRoad - - @connectingRoad.setter - def connectingRoad(self, value): - self._connectingRoad = int(value) - - @property - def contactPoint(self): - return self._contactPoint - - @contactPoint.setter - def contactPoint(self, value): - if value not in ["start", "end"]: - raise AttributeError("Contact point can only be start or end.") - - self._contactPoint = value - - @property - def laneLinks(self): - return self._laneLinks - - def addLaneLink(self, laneLink): - if not isinstance(laneLink, LaneLink): - raise TypeError("Has to be of instance LaneLink") - - self._laneLinks.append(laneLink) - - -class LaneLink(object): - - def __init__(self): - self._from = None - self._to = None - - def __str__(self): - return str(self._from) + " > " + str(self._to) - - @property - def fromId(self): - return self._from - - @fromId.setter - def fromId(self, value): - self._from = int(value) - - @property - def toId(self): - return self._to - - @toId.setter - def toId(self, value): - self._to = int(value) diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/openDrive.py b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/openDrive.py deleted file mode 100644 index 3a1192d..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/openDrive.py +++ /dev/null @@ -1,142 +0,0 @@ - -class OpenDrive(object): - - def __init__(self): - self._header = Header() - self._roads = [] - self._controllers = [] - self._junctions = [] - self._junctionGroups = [] - self._stations = [] - - @property - def header(self): - return self._header - - @property - def roads(self): - return self._roads - - def getRoad(self, id): - for road in self._roads: - if road.id == id: - return road - - return None - - @property - def controllers(self): - return self._controllers - - @property - def junctions(self): - return self._junctions - - def getJunction(self, junctionId): - for junction in self._junctions: - if junction.id == junctionId: - return junction - return None - - @property - def junctionGroups(self): - return self._junctionGroups - - @property - def stations(self): - return self._stations - - -class Header(object): - - def __init__(self): - self._revMajor = None - self._revMinor = None - self._name = None - self._version = None - self._date = None - self._north = None - self._south = None - self._east = None - self._west = None - self._vendor = None - - @property - def revMajor(self): - return self._revMajor - - @revMajor.setter - def revMajor(self, value): - self._revMajor = value - - @property - def revMinor(self): - return self._revMinor - - @revMinor.setter - def revMinor(self, value): - self._revMinor = value - - @property - def name(self): - return self._name - - @name.setter - def name(self, value): - self._name = value - - @property - def version(self): - return self._version - - @version.setter - def version(self, value): - self._version = value - - @property - def date(self): - return self._date - - @date.setter - def date(self, value): - self._date = value - - @property - def north(self): - return self._north - - @north.setter - def north(self, value): - self._north = value - - @property - def south(self): - return self._south - - @south.setter - def south(self, value): - self._south = value - - @property - def east(self): - return self._east - - @east.setter - def east(self, value): - self._east = value - - @property - def west(self): - return self._west - - @west.setter - def west(self, value): - self._west = value - - @property - def vendor(self): - return self._vendor - - @vendor.setter - def vendor(self, value): - self._vendor = value diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/road.py b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/road.py deleted file mode 100644 index fc59c5a..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/road.py +++ /dev/null @@ -1,77 +0,0 @@ - -from opendriveparser.elements.roadPlanView import PlanView -from opendriveparser.elements.roadLink import Link -from opendriveparser.elements.roadLanes import Lanes -from opendriveparser.elements.roadElevationProfile import ElevationProfile -from opendriveparser.elements.roadLateralProfile import LateralProfile -from opendriveparser.elements.junction import Junction - -class Road(object): - - def __init__(self): - self._id = None - self._name = None - self._junction = None - self._length = None - - self._header = None # TODO - self._link = Link() - self._types = [] - self._planView = PlanView() - self._elevationProfile = ElevationProfile() - self._lateralProfile = LateralProfile() - self._lanes = Lanes() - - @property - def id(self): - return self._id - - @id.setter - def id(self, value): - self._id = int(value) - - @property - def name(self): - return self._name - - @name.setter - def name(self, value): - self._name = str(value) - - @property - def junction(self): - return self._junction - - @junction.setter - def junction(self, value): - if not isinstance(value, Junction) and value is not None: - raise TypeError("Property must be a Junction or NoneType") - - if value == -1: - value = None - - self._junction = value - - @property - def link(self): - return self._link - - @property - def types(self): - return self._types - - @property - def planView(self): - return self._planView - - @property - def elevationProfile(self): - return self._elevationProfile - - @property - def lateralProfile(self): - return self._lateralProfile - - @property - def lanes(self): - return self._lanes diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadElevationProfile.py b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadElevationProfile.py deleted file mode 100644 index b4240e8..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadElevationProfile.py +++ /dev/null @@ -1,59 +0,0 @@ - -class ElevationProfile(object): - - def __init__(self): - self._elevations = [] - - @property - def elevations(self): - return self._elevations - - -class Elevation(object): - - def __init__(self): - self._sPos = None - self._a = None - self._b = None - self._c = None - self._d = None - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def a(self): - return self._a - - @a.setter - def a(self, value): - self._a = float(value) - - @property - def b(self): - return self._b - - @b.setter - def b(self, value): - self._b = float(value) - - @property - def c(self): - return self._c - - @c.setter - def c(self, value): - self._c = float(value) - - @property - def d(self): - return self._d - - @d.setter - def d(self, value): - self._d = float(value) diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadLanes.py b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadLanes.py deleted file mode 100644 index 0829488..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadLanes.py +++ /dev/null @@ -1,355 +0,0 @@ - -class Lanes(object): - - def __init__(self): - self._laneOffsets = [] - self._laneSections = [] - - @property - def laneOffsets(self): - self._laneOffsets.sort(key=lambda x: x.sPos) - return self._laneOffsets - - @property - def laneSections(self): - self._laneSections.sort(key=lambda x: x.sPos) - return self._laneSections - - def getLaneSection(self, laneSectionIdx): - for laneSection in self.laneSections: - if laneSection.idx == laneSectionIdx: - return laneSection - - return None - - def getLastLaneSectionIdx(self): - - numLaneSections = len(self.laneSections) - - if numLaneSections > 1: - return numLaneSections - 1 - - return 0 - -class LaneOffset(object): - - def __init__(self): - self._sPos = None - self._a = None - self._b = None - self._c = None - self._d = None - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def a(self): - return self._a - - @a.setter - def a(self, value): - self._a = float(value) - - @property - def b(self): - return self._b - - @b.setter - def b(self, value): - self._b = float(value) - - @property - def c(self): - return self._c - - @c.setter - def c(self, value): - self._c = float(value) - - @property - def d(self): - return self._d - - @d.setter - def d(self, value): - self._d = float(value) - - @property - def coeffs(self): - """ Array of coefficients for usage with numpy.polynomial.polynomial.polyval """ - return [self._a, self._b, self._c, self._d] - - -class LaneSection(object): - - def __init__(self, road=None): - self._idx = None - self._sPos = None - self._singleSide = None - self._leftLanes = LeftLanes() - self._centerLanes = CenterLanes() - self._rightLanes = RightLanes() - - self._parentRoad = road - - @property - def idx(self): - return self._idx - - @idx.setter - def idx(self, value): - self._idx = int(value) - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def length(self): - return self._length - - @length.setter - def length(self, value): - self._length = float(value) - - @property - def singleSide(self): - return self._singleSide - - @singleSide.setter - def singleSide(self, value): - if value not in ["true", "false"] and value is not None: - raise AttributeError("Value must be true or false.") - - self._singleSide = (value == "true") - - @property - def leftLanes(self): - """ Get list of sorted lanes always starting in the middle (lane id -1) """ - return self._leftLanes.lanes - - @property - def centerLanes(self): - return self._centerLanes.lanes - - @property - def rightLanes(self): - """ Get list of sorted lanes always starting in the middle (lane id 1) """ - return self._rightLanes.lanes - - @property - def allLanes(self): - """ Attention! lanes are not sorted by id """ - return self._leftLanes.lanes + self._centerLanes.lanes + self._rightLanes.lanes - - def getLane(self, laneId): - for lane in self.allLanes: - if lane.id == laneId: - return lane - - return None - - @property - def parentRoad(self): - return self._parentRoad - - -class LeftLanes(object): - - sort_direction = False - - def __init__(self): - self._lanes = [] - - @property - def lanes(self): - self._lanes.sort(key=lambda x: x.id, reverse=self.sort_direction) - return self._lanes - -class CenterLanes(LeftLanes): - pass - -class RightLanes(LeftLanes): - sort_direction = True - - -class Lane(object): - - laneTypes = [ - "none", "driving", "stop", "shoulder", "biking", "sidewalk", "border", - "restricted", "parking", "bidirectional", "median", "special1", "special2", - "special3", "roadWorks", "tram", "rail", "entry", "exit", "offRamp", "onRamp" - ] - - def __init__(self, parentRoad): - self._parent_road = parentRoad - self._id = None - self._type = None - self._level = None - self._link = LaneLink() - self._widths = [] - self._borders = [] - - @property - def parentRoad(self): - return self._parent_road - - @property - def id(self): - return self._id - - @id.setter - def id(self, value): - self._id = int(value) - - @property - def type(self): - return self._type - - @type.setter - def type(self, value): - if value not in self.laneTypes: - raise Exception() - - self._type = str(value) - - @property - def level(self): - return self._level - - @level.setter - def level(self, value): - if value not in ["true", "false"] and value is not None: - raise AttributeError("Value must be true or false.") - - self._level = (value == "true") - - @property - def link(self): - return self._link - - @property - def widths(self): - self._widths.sort(key=lambda x: x.sOffset) - return self._widths - - def getWidth(self, widthIdx): - for width in self._widths: - if width.idx == widthIdx: - return width - - return None - - def getLastLaneWidthIdx(self): - """ Returns the index of the last width sector of the lane """ - - numWidths = len(self._widths) - - if numWidths > 1: - return numWidths - 1 - - return 0 - - @property - def borders(self): - return self._borders - - -class LaneLink(object): - - def __init__(self): - self._predecessor = None - self._successor = None - - @property - def predecessorId(self): - return self._predecessor - - @predecessorId.setter - def predecessorId(self, value): - self._predecessor = int(value) - - @property - def successorId(self): - return self._successor - - @successorId.setter - def successorId(self, value): - self._successor = int(value) - - -class LaneWidth(object): - - def __init__(self): - self._idx = None - self._sOffset = None - self._a = None - self._b = None - self._c = None - self._d = None - - @property - def idx(self): - return self._idx - - @idx.setter - def idx(self, value): - self._idx = int(value) - - @property - def sOffset(self): - return self._sOffset - - @sOffset.setter - def sOffset(self, value): - self._sOffset = float(value) - - @property - def a(self): - return self._a - - @a.setter - def a(self, value): - self._a = float(value) - - @property - def b(self): - return self._b - - @b.setter - def b(self, value): - self._b = float(value) - - @property - def c(self): - return self._c - - @c.setter - def c(self, value): - self._c = float(value) - - @property - def d(self): - return self._d - - @d.setter - def d(self, value): - self._d = float(value) - - @property - def coeffs(self): - """ Array of coefficients for usage with numpy.polynomial.polynomial.polyval """ - return [self._a, self._b, self._c, self._d] - -class LaneBorder(LaneWidth): - pass diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadLateralProfile.py b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadLateralProfile.py deleted file mode 100644 index cf0dc8b..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadLateralProfile.py +++ /dev/null @@ -1,211 +0,0 @@ - -class LateralProfile(object): - - def __init__(self): - self._superelevations = [] - self._crossfalls = [] - self._shapes = [] - - @property - def superelevations(self): - return self._superelevations - - @superelevations.setter - def superelevations(self, value): - if not isinstance(value, list) or not all(isinstance(x, Superelevation) for x in value): - raise TypeError("Value must be an instance of Superelevation.") - - self._superelevations = value - - @property - def crossfalls(self): - return self._crossfalls - - @crossfalls.setter - def crossfalls(self, value): - if not isinstance(value, list) or not all(isinstance(x, Crossfall) for x in value): - raise TypeError("Value must be an instance of Crossfall.") - - self._crossfalls = value - - @property - def shapes(self): - return self._shapes - - @shapes.setter - def shapes(self, value): - if not isinstance(value, list) or not all(isinstance(x, Shape) for x in value): - raise TypeError("Value must be a list of instances of Shape.") - - self._shapes = value - - -class Superelevation(object): - - def __init__(self): - self._sPos = None - self._a = None - self._b = None - self._c = None - self._d = None - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def a(self): - return self._a - - @a.setter - def a(self, value): - self._a = float(value) - - @property - def b(self): - return self._b - - @b.setter - def b(self, value): - self._b = float(value) - - @property - def c(self): - return self._c - - @c.setter - def c(self, value): - self._c = float(value) - - @property - def d(self): - return self._d - - @d.setter - def d(self, value): - self._d = float(value) - - -class Crossfall(object): - - def __init__(self): - self._side = None - self._sPos = None - self._a = None - self._b = None - self._c = None - self._d = None - - @property - def side(self): - return self._side - - @side.setter - def side(self, value): - if value not in ["left", "right", "both"]: - raise TypeError("Value must be string with content 'left', 'right' or 'both'.") - - self._side = value - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def a(self): - return self._a - - @a.setter - def a(self, value): - self._a = float(value) - - @property - def b(self): - return self._b - - @b.setter - def b(self, value): - self._b = float(value) - - @property - def c(self): - return self._c - - @c.setter - def c(self, value): - self._c = float(value) - - @property - def d(self): - return self._d - - @d.setter - def d(self, value): - self._d = float(value) - - -class Shape(object): - - def __init__(self): - self._sPos = None - self._t = None - self._a = None - self._b = None - self._c = None - self._d = None - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def t(self): - return self._t - - @t.setter - def t(self, value): - self._t = float(value) - - @property - def a(self): - return self._a - - @a.setter - def a(self, value): - self._a = float(value) - - @property - def b(self): - return self._b - - @b.setter - def b(self, value): - self._b = float(value) - - @property - def c(self): - return self._c - - @c.setter - def c(self, value): - self._c = float(value) - - @property - def d(self): - return self._d - - @d.setter - def d(self, value): - self._d = float(value) diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadLink.py b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadLink.py deleted file mode 100644 index ce89503..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadLink.py +++ /dev/null @@ -1,139 +0,0 @@ - -class Link(object): - - def __init__(self): - self._id = None - self._predecessor = None - self._successor = None - self._neighbors = [] - - def __str__(self): - return " > link id " + str(self._id) + " | successor: " + str(self._successor) - - @property - def id(self): - return self._id - - @id.setter - def id(self, value): - self._id = int(value) - - @property - def predecessor(self): - return self._predecessor - - @predecessor.setter - def predecessor(self, value): - if not isinstance(value, Predecessor): - raise TypeError("Value must be Predecessor") - - self._predecessor = value - - @property - def successor(self): - return self._successor - - @successor.setter - def successor(self, value): - if not isinstance(value, Successor): - raise TypeError("Value must be Successor") - - self._successor = value - - @property - def neighbors(self): - return self._neighbors - - @neighbors.setter - def neighbors(self, value): - if not isinstance(value, list) or not all(isinstance(x, Neighbor) for x in value): - raise TypeError("Value must be list of instances of Neighbor.") - - self._neighbors = value - - def addNeighbor(self, value): - if not isinstance(value, Neighbor): - raise TypeError("Value must be Neighbor") - - self._neighbors.append(value) - - -class Predecessor(object): - - def __init__(self): - self._elementType = None - self._elementId = None - self._contactPoint = None - - def __str__(self): - return str(self._elementType) + " with id " + str(self._elementId) + " contact at " + str(self._contactPoint) - - @property - def elementType(self): - return self._elementType - - @elementType.setter - def elementType(self, value): - if value not in ["road", "junction"]: - raise AttributeError("Value must be road or junction") - - self._elementType = value - - @property - def elementId(self): - return self._elementId - - @elementId.setter - def elementId(self, value): - self._elementId = int(value) - - @property - def contactPoint(self): - return self._contactPoint - - @contactPoint.setter - def contactPoint(self, value): - if value not in ["start", "end"] and value is not None: - raise AttributeError("Value must be start or end") - - self._contactPoint = value - -class Successor(Predecessor): - pass - -class Neighbor(object): - - def __init__(self): - self._side = None - self._elementId = None - self._direction = None - - @property - def side(self): - return self._side - - @side.setter - def side(self, value): - if value not in ["left", "right"]: - raise AttributeError("Value must be left or right") - - self._side = value - - @property - def elementId(self): - return self._elementId - - @elementId.setter - def elementId(self, value): - self._elementId = int(value) - - @property - def direction(self): - return self._direction - - @direction.setter - def direction(self, value): - if value not in ["same", "opposite"]: - raise AttributeError("Value must be same or opposite") - - self._direction = value diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadPlanView.py b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadPlanView.py deleted file mode 100644 index d88a59a..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadPlanView.py +++ /dev/null @@ -1,225 +0,0 @@ - -import abc -import numpy as np - -from opendriveparser.elements.eulerspiral import EulerSpiral - - -class PlanView(object): - - def __init__(self): - self._geometries = [] - - def addLine(self, startPosition, heading, length): - self._geometries.append(Line(startPosition, heading, length)) - - def addSpiral(self, startPosition, heading, length, curvStart, curvEnd): - self._geometries.append(Spiral(startPosition, heading, length, curvStart, curvEnd)) - - def addArc(self, startPosition, heading, length, curvature): - self._geometries.append(Arc(startPosition, heading, length, curvature)) - - def addParamPoly3(self, startPosition, heading, length, aU, bU, cU, dU, aV, bV, cV, dV, pRange): - self._geometries.append(ParamPoly3(startPosition, heading, length, aU, bU, cU, dU, aV, bV, cV, dV, pRange)) - - def getLength(self): - """ Get length of whole plan view """ - - length = 0 - - for geometry in self._geometries: - length += geometry.getLength() - - return length - - def calc(self, sPos): - """ Calculate position and tangent at sPos """ - - for geometry in self._geometries: - if geometry.getLength() < sPos and not np.isclose(geometry.getLength(), sPos): - sPos -= geometry.getLength() - continue - return geometry.calcPosition(sPos) - - # TODO - return self._geometries[-1].calcPosition(self._geometries[-1].getLength()) - - # 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())) - -class Geometry(object): - __metaclass__ = abc.ABCMeta - - @abc.abstractmethod - def getStartPosition(self): - """ Returns the overall geometry length """ - return - - @abc.abstractmethod - def getLength(self): - """ Returns the overall geometry length """ - return - - @abc.abstractmethod - def calcPosition(self, s): - """ Calculates the position of the geometry as if the starting point is (0/0) """ - return - -class Line(Geometry): - - def __init__(self, startPosition, heading, length): - self.startPosition = np.array(startPosition) - self.heading = heading - self.length = length - - def getStartPosition(self): - return self.startPosition - - def getLength(self): - return self.length - - def calcPosition(self, s): - pos = self.startPosition + np.array([s * np.cos(self.heading), s * np.sin(self.heading)]) - tangent = self.heading - - return (pos, tangent) - -class Arc(Geometry): - - def __init__(self, startPosition, heading, length, curvature): - self.startPosition = np.array(startPosition) - self.heading = heading - self.length = length - self.curvature = curvature - - def getStartPosition(self): - return self.startPosition - - def getLength(self): - return self.length - - def calcPosition(self, s): - c = self.curvature - hdg = self.heading - np.pi / 2 - - a = 2 / c * np.sin(s * c / 2) - alpha = (np.pi - s * c) / 2 - hdg - - dx = -1 * a * np.cos(alpha) - dy = a * np.sin(alpha) - - pos = self.startPosition + np.array([dx, dy]) - tangent = self.heading + s * self.curvature - - return (pos, tangent) - -class Spiral(Geometry): - - def __init__(self, startPosition, heading, length, curvStart, curvEnd): - self._startPosition = np.array(startPosition) - self._heading = heading - self._length = length - self._curvStart = curvStart - self._curvEnd = curvEnd - - self._spiral = EulerSpiral.createFromLengthAndCurvature(self._length, self._curvStart, self._curvEnd) - - def getStartPosition(self): - return self._startPosition - - def getLength(self): - return self._length - - def calcPosition(self, s): - (x, y, t) = self._spiral.calc(s, self._startPosition[0], self._startPosition[1], self._curvStart, self._heading) - - return (np.array([x, y]), t) - -class Poly3(Geometry): - - def __init__(self, startPosition, heading, length, a, b, c, d): - self._startPosition = np.array(startPosition) - self._heading = heading - self._length = length - self._a = a - self._b = b - self._c = c - self._d = d - - raise NotImplementedError() - - def getStartPosition(self): - return self._startPosition - - def getLength(self): - return self._length - - def calcPosition(self, s): - # TODO untested - - # Calculate new point in s/t coordinate system - coeffs = [self._a, self._b, self._c, self._d] - - t = np.polynomial.polynomial.polyval(s, coeffs) - - # Rotate and translate - srot = s * np.cos(self._heading) - t * np.sin(self._heading) - trot = s * np.sin(self._heading) + t * np.cos(self._heading) - - # Derivate to get heading change - dCoeffs = coeffs[1:] * np.array(np.arange(1, len(coeffs))) - tangent = np.polynomial.polynomial.polyval(s, dCoeffs) - - return (self._startPosition + np.array([srot, trot]), self._heading + tangent) - -class ParamPoly3(Geometry): - - def __init__(self, startPosition, heading, length, aU, bU, cU, dU, aV, bV, cV, dV, pRange): - self._startPosition = np.array(startPosition) - self._heading = heading - self._length = length - - self._aU = aU - self._bU = bU - self._cU = cU - self._dU = dU - self._aV = aV - self._bV = bV - self._cV = cV - self._dV = dV - - if pRange is None: - self._pRange = 1.0 - else: - self._pRange = pRange - - def getStartPosition(self): - return self._startPosition - - def getLength(self): - return self._length - - def calcPosition(self, s): - - # Position - pos = (s / self._length) * self._pRange - - coeffsU = [self._aU, self._bU, self._cU, self._dU] - coeffsV = [self._aV, self._bV, self._cV, self._dV] - - x = np.polynomial.polynomial.polyval(pos, coeffsU) - y = np.polynomial.polynomial.polyval(pos, coeffsV) - - xrot = x * np.cos(self._heading) - y * np.sin(self._heading) - yrot = x * np.sin(self._heading) + y * np.cos(self._heading) - - # Tangent is defined by derivation - dCoeffsU = coeffsU[1:] * np.array(np.arange(1, len(coeffsU))) - dCoeffsV = coeffsV[1:] * np.array(np.arange(1, len(coeffsV))) - - dx = np.polynomial.polynomial.polyval(pos, dCoeffsU) - dy = np.polynomial.polynomial.polyval(pos, dCoeffsV) - - tangent = np.arctan2(dy, dx) - - - return (self._startPosition + np.array([xrot, yrot]), self._heading + tangent) diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadType.py b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadType.py deleted file mode 100644 index 0c8bf30..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/elements/roadType.py +++ /dev/null @@ -1,64 +0,0 @@ - - -class Type(object): - - allowedTypes = ["unknown", "rural", "motorway", "town", "lowSpeed", "pedestrian", "bicycle"] - - def __init__(self): - self._sPos = None - self._type = None - self._speed = None - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def type(self): - return self._type - - @type.setter - def type(self, value): - if value not in self.allowedTypes: - raise AttributeError("Type not allowed.") - - self._type = value - - @property - def speed(self): - return self._speed - - @speed.setter - def speed(self, value): - if not isinstance(value, Speed): - raise TypeError("Value must be instance of Speed.") - - self._speed = value - - -class Speed(object): - - def __init__(self): - self._max = None - self._unit = None - - @property - def max(self): - return self._max - - @max.setter - def max(self, value): - self._max = str(value) - - @property - def unit(self): - return self._unit - - @unit.setter - def unit(self, value): - # TODO validate input - self._unit = str(value) diff --git a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/parser.py b/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/parser.py deleted file mode 100644 index aac8bb8..0000000 --- a/src/aadcUserPython/opendrive/openDrive2csv/opendriveparser/parser.py +++ /dev/null @@ -1,413 +0,0 @@ - -import numpy as np -from lxml import etree - -from opendriveparser.elements.openDrive import OpenDrive -from opendriveparser.elements.road import Road -from opendriveparser.elements.roadLink import Predecessor as RoadLinkPredecessor, Successor as RoadLinkSuccessor, Neighbor as RoadLinkNeighbor -from opendriveparser.elements.roadType import Type as RoadType, Speed as RoadTypeSpeed -from opendriveparser.elements.roadElevationProfile import Elevation as RoadElevationProfileElevation -from opendriveparser.elements.roadLateralProfile import Superelevation as RoadLateralProfileSuperelevation, Crossfall as RoadLateralProfileCrossfall, Shape as RoadLateralProfileShape -from opendriveparser.elements.roadLanes import LaneOffset as RoadLanesLaneOffset, Lane as RoadLaneSectionLane, LaneSection as RoadLanesSection, LaneWidth as RoadLaneSectionLaneWidth, LaneBorder as RoadLaneSectionLaneBorder -from opendriveparser.elements.junction import Junction, Connection as JunctionConnection, LaneLink as JunctionConnectionLaneLink - - -def parse_opendrive(rootNode): - """ Tries to parse XML tree, returns OpenDRIVE object """ - - # Only accept lxml element - if not etree.iselement(rootNode): - raise TypeError("Argument rootNode is not a xml element") - - newOpenDrive = OpenDrive() - - # Header - header = rootNode.find("header") - - if header is not None: - - if header.get('revMajor') is not None: - newOpenDrive.header.revMajor = str(header.get('revMajor')) - - if header.get('revMinor') is not None: - newOpenDrive.header.revMinor = str(header.get('revMinor')) - - if header.get('name') is not None: - newOpenDrive.header.name = str(header.get('name')) - - if header.get('version') is not None: - newOpenDrive.header.version = str(header.get('version')) - - if header.get('date') is not None: - newOpenDrive.header.date = str(header.get('date')) - - if header.get('north') is not None: - newOpenDrive.header.north = str(header.get('north')) - - if header.get('south') is not None: - newOpenDrive.header.south = str(header.get('south')) - - if header.get('east') is not None: - newOpenDrive.header.east = str(header.get('east')) - - if header.get('west') is not None: - newOpenDrive.header.west = str(header.get('west')) - - if header.get('vendor') is not None: - newOpenDrive.header.vendor = str(header.get('vendor')) - - # Reference - if header.find("geoReference") is not None: - pass - # TODO not implemented - - # Junctions - for junction in rootNode.findall("junction"): - - newJunction = Junction() - - newJunction.id = int(junction.get("id")) - newJunction.name = str(junction.get("name")) - - for connection in junction.findall("connection"): - - newConnection = JunctionConnection() - - newConnection.id = connection.get("id") - newConnection.incomingRoad = connection.get("incomingRoad") - newConnection.connectingRoad = connection.get("connectingRoad") - newConnection.contactPoint = connection.get("contactPoint") - - for laneLink in connection.findall("laneLink"): - - newLaneLink = JunctionConnectionLaneLink() - - newLaneLink.fromId = laneLink.get("from") - newLaneLink.toId = laneLink.get("to") - - newConnection.addLaneLink(newLaneLink) - - newJunction.addConnection(newConnection) - - newOpenDrive.junctions.append(newJunction) - - # Load roads - for road in rootNode.findall("road"): - - newRoad = Road() - - newRoad.id = int(road.get("id")) - newRoad.name = road.get("name") - - junctionId = int(road.get("junction")) if road.get("junction") != "-1" else None - - if junctionId: - newRoad.junction = newOpenDrive.getJunction(junctionId) - - # TODO verify road length - newRoad.length = float(road.get("length")) - - # Links - if road.find("link") is not None: - - predecessor = road.find("link").find("predecessor") - - if predecessor is not None: - - newPredecessor = RoadLinkPredecessor() - - newPredecessor.elementType = predecessor.get("elementType") - newPredecessor.elementId = predecessor.get("elementId") - newPredecessor.contactPoint = predecessor.get("contactPoint") - - newRoad.link.predecessor = newPredecessor - - successor = road.find("link").find("successor") - - if successor is not None: - - newSuccessor = RoadLinkSuccessor() - - newSuccessor.elementType = successor.get("elementType") - newSuccessor.elementId = successor.get("elementId") - newSuccessor.contactPoint = successor.get("contactPoint") - - newRoad.link.successor = newSuccessor - - for neighbor in road.find("link").findall("neighbor"): - - newNeighbor = RoadLinkNeighbor() - - newNeighbor.side = neighbor.get("side") - newNeighbor.elementId = neighbor.get("elementId") - newNeighbor.direction = neighbor.get("direction") - - newRoad.link.neighbors.append(newNeighbor) - - # Type - for roadType in road.findall("type"): - - newType = RoadType() - - newType.sPos = roadType.get("s") - newType.type = roadType.get("type") - - if roadType.find("speed"): - - newSpeed = RoadTypeSpeed() - - newSpeed.max = roadType.find("speed").get("max") - newSpeed.unit = roadType.find("speed").get("unit") - - newType.speed = newSpeed - - newRoad.types.append(newType) - - # Plan view - for geometry in road.find("planView").findall("geometry"): - - startCoord = [float(geometry.get("x")), float(geometry.get("y"))] - - if geometry.find("line") is not None: - newRoad.planView.addLine(startCoord, float(geometry.get("hdg")), float(geometry.get("length"))) - - elif geometry.find("spiral") is not None: - newRoad.planView.addSpiral(startCoord, float(geometry.get("hdg")), float(geometry.get("length")), float(geometry.find("spiral").get("curvStart")), float(geometry.find("spiral").get("curvEnd"))) - - elif geometry.find("arc") is not None: - newRoad.planView.addArc(startCoord, float(geometry.get("hdg")), float(geometry.get("length")), float(geometry.find("arc").get("curvature"))) - - elif geometry.find("poly3") is not None: - raise NotImplementedError() - - elif geometry.find("paramPoly3") is not None: - if geometry.find("paramPoly3").get("pRange"): - - if geometry.find("paramPoly3").get("pRange") == "arcLength": - pMax = float(geometry.get("length")) - else: - pMax = None - else: - pMax = None - - newRoad.planView.addParamPoly3( \ - startCoord, \ - float(geometry.get("hdg")), \ - float(geometry.get("length")), \ - float(geometry.find("paramPoly3").get("aU")), \ - float(geometry.find("paramPoly3").get("bU")), \ - float(geometry.find("paramPoly3").get("cU")), \ - float(geometry.find("paramPoly3").get("dU")), \ - float(geometry.find("paramPoly3").get("aV")), \ - float(geometry.find("paramPoly3").get("bV")), \ - float(geometry.find("paramPoly3").get("cV")), \ - float(geometry.find("paramPoly3").get("dV")), \ - pMax \ - ) - - else: - raise Exception("invalid xml") - - # Elevation profile - if road.find("elevationProfile") is not None: - - for elevation in road.find("elevationProfile").findall("elevation"): - - newElevation = RoadElevationProfileElevation() - - newElevation.sPos = elevation.get("s") - newElevation.a = elevation.get("a") - newElevation.b = elevation.get("b") - newElevation.c = elevation.get("c") - newElevation.d = elevation.get("d") - - newRoad.elevationProfile.elevations.append(newElevation) - - # Lateral profile - if road.find("lateralProfile") is not None: - - for superelevation in road.find("lateralProfile").findall("superelevation"): - - newSuperelevation = RoadLateralProfileSuperelevation() - - newSuperelevation.sPos = superelevation.get("s") - newSuperelevation.a = superelevation.get("a") - newSuperelevation.b = superelevation.get("b") - newSuperelevation.c = superelevation.get("c") - newSuperelevation.d = superelevation.get("d") - - newRoad.lateralProfile.superelevations.append(newSuperelevation) - - for crossfall in road.find("lateralProfile").findall("crossfall"): - - newCrossfall = RoadLateralProfileCrossfall() - - newCrossfall.side = crossfall.get("side") - newCrossfall.sPos = crossfall.get("s") - newCrossfall.a = crossfall.get("a") - newCrossfall.b = crossfall.get("b") - newCrossfall.c = crossfall.get("c") - newCrossfall.d = crossfall.get("d") - - newRoad.lateralProfile.crossfalls.append(newCrossfall) - - for shape in road.find("lateralProfile").findall("shape"): - - newShape = RoadLateralProfileShape() - - newShape.sPos = shape.get("s") - newShape.t = shape.get("t") - newShape.a = shape.get("a") - newShape.b = shape.get("b") - newShape.c = shape.get("c") - newShape.d = shape.get("d") - - newRoad.lateralProfile.shapes.append(newShape) - - # Lanes - lanes = road.find("lanes") - - if lanes is None: - raise Exception("Road must have lanes element") - - # Lane offset - for laneOffset in lanes.findall("laneOffset"): - - newLaneOffset = RoadLanesLaneOffset() - - newLaneOffset.sPos = laneOffset.get("s") - newLaneOffset.a = laneOffset.get("a") - newLaneOffset.b = laneOffset.get("b") - newLaneOffset.c = laneOffset.get("c") - newLaneOffset.d = laneOffset.get("d") - - newRoad.lanes.laneOffsets.append(newLaneOffset) - - # Lane sections - for laneSectionIdx, laneSection in enumerate(road.find("lanes").findall("laneSection")): - - newLaneSection = RoadLanesSection(road=newRoad) - - # Manually enumerate lane sections for referencing purposes - newLaneSection.idx = laneSectionIdx - - newLaneSection.sPos = laneSection.get("s") - newLaneSection.singleSide = laneSection.get("singleSide") - - sides = dict( - left=newLaneSection.leftLanes, - center=newLaneSection.centerLanes, - right=newLaneSection.rightLanes - ) - - for sideTag, newSideLanes in sides.items(): - - side = laneSection.find(sideTag) - - # It is possible one side is not present - if side is None: - continue - - for lane in side.findall("lane"): - - newLane = RoadLaneSectionLane( - parentRoad=newRoad - ) - - newLane.id = lane.get("id") - newLane.type = lane.get("type") - - # In some sample files the level is not specified according to the OpenDRIVE spec - newLane.level = "true" if lane.get("level") in [1, '1', 'true'] else "false" - - # Lane Links - if lane.find("link") is not None: - - if lane.find("link").find("predecessor") is not None: - newLane.link.predecessorId = lane.find("link").find("predecessor").get("id") - - if lane.find("link").find("successor") is not None: - newLane.link.successorId = lane.find("link").find("successor").get("id") - - # Width - for widthIdx, width in enumerate(lane.findall("width")): - - newWidth = RoadLaneSectionLaneWidth() - - newWidth.idx = widthIdx - newWidth.sOffset = width.get("sOffset") - newWidth.a = width.get("a") - newWidth.b = width.get("b") - newWidth.c = width.get("c") - newWidth.d = width.get("d") - - newLane.widths.append(newWidth) - - # Border - for borderIdx, border in enumerate(lane.findall("border")): - - newBorder = RoadLaneSectionLaneBorder() - - newBorder.idx = borderIdx - newBorder.sPos = border.get("sOffset") - newBorder.a = border.get("a") - newBorder.b = border.get("b") - newBorder.c = border.get("c") - newBorder.d = border.get("d") - - newLane.borders.append(newBorder) - - # Road Marks - # TODO implementation - - # Material - # TODO implementation - - # Visiblility - # TODO implementation - - # Speed - # TODO implementation - - # Access - # TODO implementation - - # Lane Height - # TODO implementation - - # Rules - # TODO implementation - - newSideLanes.append(newLane) - - newRoad.lanes.laneSections.append(newLaneSection) - - # OpenDRIVE does not provide lane section lengths by itself, calculate them by ourselves - for laneSection in newRoad.lanes.laneSections: - - # Last lane section in road - if laneSection.idx + 1 >= len(newRoad.lanes.laneSections): - laneSection.length = newRoad.planView.getLength() - laneSection.sPos - - # All but the last lane section end at the succeeding one - else: - laneSection.length = newRoad.lanes.laneSections[laneSection.idx + 1].sPos - laneSection.sPos - - # OpenDRIVE does not provide lane width lengths by itself, calculate them by ourselves - for laneSection in newRoad.lanes.laneSections: - for lane in laneSection.allLanes: - widthsPoses = np.array([x.sOffset for x in lane.widths] + [laneSection.length]) - widthsLengths = widthsPoses[1:] - widthsPoses[:-1] - - for widthIdx, width in enumerate(lane.widths): - width.length = widthsLengths[widthIdx] - - # Objects - # TODO implementation - - # Signals - # TODO implementation - - newOpenDrive.roads.append(newRoad) - - return newOpenDrive diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/openDrive_test.ipynb b/src/aadcUserPython/opendrive/openDrive_test.ipynb similarity index 99% rename from src/aadcUserPython/opendrive/opendrive2lanelet/openDrive_test.ipynb rename to src/aadcUserPython/opendrive/openDrive_test.ipynb index 545b194..ccd432f 100644 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/openDrive_test.ipynb +++ b/src/aadcUserPython/opendrive/openDrive_test.ipynb @@ -11,6 +11,9 @@ "metadata": {}, "outputs": [], "source": [ + "import sys\n", + "sys.path.append('opendrive2lanelets-converter')\n", + "\n", "from lxml import etree\n", "from opendriveparser import parse_opendrive\n", "from opendrive2lanelet import Network\n", @@ -582,7 +585,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.6.6" + "version": "3.6.3" }, "varInspector": { "cols": { diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/.gitignore b/src/aadcUserPython/opendrive/opendrive2lanelet/.gitignore deleted file mode 100644 index 4f65315..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/.gitignore +++ /dev/null @@ -1,104 +0,0 @@ -*.xodr -*.xml -venv/ -.vscode/ - -# Byte-compiled / optimized / DLL files -__pycache__/ -*.py[cod] -*$py.class - -# C extensions -*.so - -# Distribution / packaging -.Python -build/ -develop-eggs/ -dist/ -downloads/ -eggs/ -.eggs/ -lib/ -lib64/ -parts/ -sdist/ -var/ -wheels/ -*.egg-info/ -.installed.cfg -*.egg - -# PyInstaller -# Usually these files are written by a python script from a template -# before PyInstaller builds the exe, so as to inject date/other infos into it. -*.manifest -*.spec - -# Installer logs -pip-log.txt -pip-delete-this-directory.txt - -# Unit test / coverage reports -htmlcov/ -.tox/ -.coverage -.coverage.* -.cache -nosetests.xml -coverage.xml -*.cover -.hypothesis/ - -# Translations -*.mo -*.pot - -# Django stuff: -*.log -local_settings.py - -# Flask stuff: -instance/ -.webassets-cache - -# Scrapy stuff: -.scrapy - -# Sphinx documentation -docs/_build/ - -# PyBuilder -target/ - -# Jupyter Notebook -.ipynb_checkpoints - -# pyenv -.python-version - -# celery beat schedule file -celerybeat-schedule - -# SageMath parsed files -*.sage.py - -# Environments -.env -.venv -env/ -venv/ -ENV/ - -# Spyder project settings -.spyderproject -.spyproject - -# Rope project settings -.ropeproject - -# mkdocs documentation -/site - -# mypy -.mypy_cache/ \ No newline at end of file diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/.travis.yml b/src/aadcUserPython/opendrive/opendrive2lanelet/.travis.yml deleted file mode 100644 index c322c66..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/.travis.yml +++ /dev/null @@ -1,12 +0,0 @@ -language: python -sudo: false -python: - - "2.7" - - "3.4" - - "3.5" - - "3.6" -install: - - pip install . - - pip install -r requirements.txt -script: - - python -m unittest diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/README.md b/src/aadcUserPython/opendrive/opendrive2lanelet/README.md deleted file mode 100644 index a555d3c..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/README.md +++ /dev/null @@ -1,73 +0,0 @@ -# OpenDRIVE 2 Lanelets - Converter - -We provide 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, which has been introduced in our [paper](https://mediatum.ub.tum.de/doc/1449005/1449005.pdf): M. Althoff, S. Urban, and M. Koschi, "Automatic Conversion of Road Networks from OpenDRIVE to Lanelets," in Proc. of the IEEE International Conference on Service Operations and Logistics, and Informatics, 2018. - -## Installation - -The following python packages have to be available: -- Python 3.x -- numpy -- scipy -- lxml -- PyQt5 only for GUI - -If needed, the converter libraries can be installed using ```pip```: - -```bash -git clone https://gitlab.com/commonroad/commonroad.gitlab.io.git -cd tools/ opendrive2lanelet && pip install . -``` - -## Example OpenDRIVE Files - -Download example files from: http://opendrive.org/download.html - -## Usage - -### Using our provided GUI - -Additional requirement: PyQt5. Start the GUI with ```python gui.py``` - -![GUI screenshot](gui_screenshot.png "Screenshot of converter GUI") - -### Using the library in your own scripts - -```python -from lxml import etree -from opendriveparser import parse_opendrive -from opendrive2lanelet import Network - -# Import, parse and convert OpenDRIVE file -fh = open("input_opendrive.xodr", 'r') -openDrive = parse_opendrive(etree.parse(fh).getroot()) -fh.close() - -roadNetwork = Network() -roadNetwork.loadOpenDrive(openDrive) - -scenario = roadNetwork.exportCommonRoadScenario() - -# Write CommonRoad scenario to file -fh = open("output_commonroad_file.xml", "wb") -fh.write(scenario.export_to_string()) -fh.close() -``` - - -## Known Problems - -- When trying to use the gui.py under Wayland, the following error occurs: - ``` - This application failed to start because it could not find or load the Qt platform plugin "wayland" in "". - Available platform plugins are: eglfs, linuxfb, minimal, minimalegl, offscreen, vnc, xcb. - Reinstalling the application may fix this problem. - ``` - Set the platform to *xcb* using this command: ```export QT_QPA_PLATFORM="xcb"``` - -## ToDo - -- When CommonRoad distinguishes lane types, the OpenDRIVE types have to be mapped accordingly and added to the lanelet output. - -## Author - -Stefan Urban \ No newline at end of file diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/gui.py b/src/aadcUserPython/opendrive/opendrive2lanelet/gui.py deleted file mode 100644 index e6b494b..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/gui.py +++ /dev/null @@ -1,206 +0,0 @@ - -import os -import signal -import sys - -from lxml import etree - -from PyQt5.QtCore import Qt -from PyQt5.QtWidgets import QApplication, QWidget, QLineEdit, QFileDialog, QMainWindow -from PyQt5.QtWidgets import QPushButton, QMessageBox, QLabel - -from opendriveparser import parse_opendrive -from opendrive2lanelet import Network - -from viewer import MainWindow as ViewerWidget - -class MainWindow(QWidget): - - def __init__(self, argv): - super().__init__() - - self.loadedRoadNetwork = None - - self._initUserInterface() - self.show() - - if len(argv) >= 2: - self.loadOpenDriveFile(argv[1]) - self.viewLaneletNetwork() - - def _initUserInterface(self): - - self.setWindowTitle("OpenDRIVE 2 Lanelets Converter") - - self.setFixedSize(560, 345) - - self.loadButton = QPushButton('Load OpenDRIVE', self) - self.loadButton.setToolTip('Load a OpenDRIVE scenario within a *.xodr file') - self.loadButton.move(10, 10) - self.loadButton.resize(130, 35) - self.loadButton.clicked.connect(self.openOpenDriveFileDialog) - - self.inputOpenDriveFile = QLineEdit(self) - self.inputOpenDriveFile.move(150, 10) - self.inputOpenDriveFile.resize(400, 35) - self.inputOpenDriveFile.setReadOnly(True) - - self.statsText = QLabel(self) - self.statsText.move(10, 55) - self.statsText.resize(540, 235) - self.statsText.setAlignment(Qt.AlignLeft | Qt.AlignTop) - self.statsText.setTextFormat(Qt.RichText) - - self.exportCommonRoadButton = QPushButton('Export as CommonRoad', self) - self.exportCommonRoadButton.move(10, 300) - self.exportCommonRoadButton.resize(170, 35) - self.exportCommonRoadButton.setDisabled(True) - self.exportCommonRoadButton.clicked.connect(self.exportAsCommonRoad) - - self.viewOutputButton = QPushButton('View Road Network', self) - self.viewOutputButton.move(190, 300) - self.viewOutputButton.resize(170, 35) - self.viewOutputButton.setDisabled(True) - self.viewOutputButton.clicked.connect(self.viewLaneletNetwork) - - def resetOutputElements(self): - self.exportCommonRoadButton.setDisabled(True) - self.viewOutputButton.setDisabled(True) - - def openOpenDriveFileDialog(self): - self.resetOutputElements() - - path, _ = QFileDialog.getOpenFileName( - self, - "QFileDialog.getOpenFileName()", - "", - "OpenDRIVE files *.xodr (*.xodr)", - options=QFileDialog.Options() - ) - - if not path: - return - - self.loadOpenDriveFile(path) - - def loadOpenDriveFile(self, path): - - filename = os.path.basename(path) - self.inputOpenDriveFile.setText(filename) - - # Load road network and print some statistics - try: - fh = open(path, 'r') - openDriveXml = parse_opendrive(etree.parse(fh).getroot()) - fh.close() - except (etree.XMLSyntaxError) as e: - errorMsg = 'XML Syntax Error: {}'.format(e) - QMessageBox.warning(self, 'OpenDRIVE error', 'There was an error during the loading of the selected OpenDRIVE file.\n\n{}'.format(errorMsg), QMessageBox.Ok) - return - except (TypeError, AttributeError, ValueError) as e: - errorMsg = 'Value Error: {}'.format(e) - QMessageBox.warning(self, 'OpenDRIVE error', 'There was an error during the loading of the selected OpenDRIVE file.\n\n{}'.format(errorMsg), QMessageBox.Ok) - return - - self.loadedRoadNetwork = Network() - self.loadedRoadNetwork.loadOpenDrive(openDriveXml) - - self.statsText.setText("Name: {}
Version: {}
Date: {}

OpenDRIVE Version {}.{}

Number of roads: {}
Total length of road network: {:.2f} meters".format( - openDriveXml.header.name if openDriveXml.header.name else "unset", - openDriveXml.header.version, - openDriveXml.header.date, - openDriveXml.header.revMajor, - openDriveXml.header.revMinor, - len(openDriveXml.roads), - sum([road.length for road in openDriveXml.roads]) - )) - - self.exportCommonRoadButton.setDisabled(False) - self.viewOutputButton.setDisabled(False) - - def exportAsCommonRoad(self): - - if not self.loadedRoadNetwork: - return - - path, _ = QFileDialog.getSaveFileName( - self, - "QFileDialog.getSaveFileName()", - "", - "CommonRoad files *.xml (*.xml)", - options=QFileDialog.Options() - ) - - if not path: - return - - try: - fh = open(path, "wb") - fh.write(self.loadedRoadNetwork.exportCommonRoadScenario().export_to_string()) - fh.close() - except (IOError) as e: - QMessageBox.critical(self, 'CommonRoad file not created!', 'The CommonRoad file was not exported due to an error.\n\n{}'.format(e), QMessageBox.Ok) - return - - QMessageBox.information(self, 'CommonRoad file created!', 'The CommonRoad file was successfully exported.', QMessageBox.Ok) - - def viewLaneletNetwork(self): - - class ViewerWindow(QMainWindow): - def __init__(self, parent=None): - super(ViewerWindow, self).__init__(parent) - self.viewer = ViewerWidget(self) - - self.setCentralWidget(self.viewer) - - viewer = ViewerWindow(self) - viewer.viewer.openScenario(self.loadedRoadNetwork.exportCommonRoadScenario()) - viewer.show() - - # def viewLaneletNetwork(self): - # import matplotlib.pyplot as plt - # from fvks.visualization.draw_dispatch import draw_object - # from fvks.scenario.lanelet import Lanelet as FvksLanelet - - # def convert_to_fvks_lanelet(ll): - # return FvksLanelet( - # left_vertices=ll.left_vertices, - # center_vertices=ll.center_vertices, - # right_vertices=ll.right_vertices, - # lanelet_id=ll.lanelet_id - # ) - - # scenario = self.loadedRoadNetwork.exportCommonRoadScenario(filterTypes=[ - # 'driving', - # 'onRamp', - # 'offRamp', - # 'stop', - # 'parking', - # 'special1', - # 'special2', - # 'special3', - # 'entry', - # 'exit', - # ]) - - # # Visualization - # fig = plt.figure() - # ax = fig.add_subplot(111) - - # for lanelet in scenario.lanelet_network.lanelets: - # draw_object(convert_to_fvks_lanelet(lanelet), ax=ax) - - # ax.set_aspect('equal', 'datalim') - # plt.axis('off') - - # plt.show() - - -if __name__ == '__main__': - # Make it possible to exit application with ctrl+c on console - signal.signal(signal.SIGINT, signal.SIG_DFL) - - # Startup application - app = QApplication(sys.argv) - ex = MainWindow(sys.argv) - sys.exit(app.exec_()) diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/gui_screenshot.png b/src/aadcUserPython/opendrive/opendrive2lanelet/gui_screenshot.png deleted file mode 100644 index 4d1a64e..0000000 Binary files a/src/aadcUserPython/opendrive/opendrive2lanelet/gui_screenshot.png and /dev/null differ diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/README.md b/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/README.md deleted file mode 100644 index 6a327d6..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/README.md +++ /dev/null @@ -1,33 +0,0 @@ -# OpenDRIVE 2 Lanelets - Converter - -## Requirements - -- Python 3.x -- numpy -- opendriveparser - -## Usage - -```python -from lxml import etree -from opendriveparser import parse_opendrive -from opendrive2lanelet import Network - -fh = open("input_opendrive.xodr", 'r') -openDrive = parse_opendrive(etree.parse(fh).getroot()) -fh.close() - -roadNetwork = Network() -roadNetwork.loadOpenDrive(openDrive) - -scenario = roadNetwork.exportCommonRoadScenario() - -# Write CommonRoad scenario to file -fh = open("output_commonroad_file.xml", "wb") -fh.write(scenario.export_to_string()) -fh.close() -``` - -## Author - -Stefan Urban \ No newline at end of file diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/XML_commonRoad_XSD.xsd b/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/XML_commonRoad_XSD.xsd deleted file mode 100644 index 5d17313..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/XML_commonRoad_XSD.xsd +++ /dev/null @@ -1,264 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/__init__.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/__init__.py deleted file mode 100644 index 07d7f52..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ - -from opendrive2lanelet.network import Network diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/commonroad.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/commonroad.py deleted file mode 100644 index 8cc7c67..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/commonroad.py +++ /dev/null @@ -1,230 +0,0 @@ - -import os -import time - -import numpy as np -from lxml import etree, objectify - -from lxml import etree -from lxml.builder import E - - - -class Scenario(object): - - def __init__(self, dt, benchmark_id=None): - self.dt = dt - self.lanelet_network = LaneletNetwork() - self.benchmark_id = benchmark_id - - def add_objects(self, o): - if type(o) == list: - for oo in o: - self.add_objects(oo) - elif type(o) == LaneletNetwork: - for l in o.lanelets: - self.lanelet_network.add_lanelet(l) - elif type(o) == Lanelet: - self.lanelet_network.add_lanelet(o) - else: - raise ScenarioError - - def export_to_string(self): - - rootElement = E("commonRoad", benchmarkID="a", commonRoadVersion="2017a", date=time.strftime("%Y-%m-%d"), timeStepSize="0.1") - - # Road network - for lanelet in self.lanelet_network.lanelets: - - # Bounds - leftPointsElements = E("leftBound") - - for (x, y) in lanelet.left_vertices: - leftPointsElements.append(E("point", E("x", str(x)), E("y", str(y)))) - - rightPointsElements = E("rightBound") - - for (x, y) in lanelet.right_vertices: - rightPointsElements.append(E("point", E("x", str(x)), E("y", str(y)))) - - laneletElement = E("lanelet", leftPointsElements, rightPointsElements) - laneletElement.set("id", str(int(lanelet.lanelet_id))) - - for predecessor in lanelet.predecessor: - laneletElement.append(E("predecessor", ref=str(predecessor))) - - for successor in lanelet.successor: - laneletElement.append(E("successor", ref=str(successor))) - - if lanelet.adj_left is not None: - laneletElement.append(E("adjacentLeft", ref=str(lanelet.adj_left), drivingDirection=str("same" if lanelet.adj_left_same_direction else "opposite"))) - - if lanelet.adj_right is not None: - laneletElement.append(E("adjacentRight", ref=str(lanelet.adj_right), drivingDirection=str("same" if lanelet.adj_right_same_direction else "opposite"))) - - rootElement.append(laneletElement) - - # Dummy planning problem - planningProblem = E("planningProblem", id=str(0)) - - initialState = E("initialState") - initialState.append(E('position', E('point', E('x', str(0.0)), E('y', str(0.0))))) - initialState.append(E('velocity', E('exact', str(0.0)))) - initialState.append(E('orientation', E('exact', str(0.0)))) - initialState.append(E('yawRate', E('exact', str(0.0)))) - initialState.append(E('slipAngle', E('exact', str(0.0)))) - initialState.append(E('time', E('exact', str(0.0)))) - - planningProblem.append(initialState) - - goalRegion = E("goalRegion") - - goalState = E("state") - goalState.append(E('position', E('point', E('x', str(0.0)), E('y', str(0.0))))) - goalState.append(E('orientation', E('exact', str(0.0)))) - goalState.append(E('time', E('exact', str(0.0)))) - goalState.append(E('velocity', E('exact', str(0.0)))) - goalState.append(E('acceleration', E('exact', str(0.0)))) - - goalRegion.append(goalState) - - planningProblem.append(goalRegion) - - rootElement.append(planningProblem) - - return etree.tostring(rootElement, pretty_print=True) - - @staticmethod - def read_from_string(input_string, dt=0.1): - - # Parse XML using CommonRoad schema - schema = etree.XMLSchema(file=open(os.path.dirname(os.path.abspath(__file__)) + "/XML_commonRoad_XSD.xsd", "rb")) - parser = objectify.makeparser(schema=schema) - - root = objectify.fromstring(input_string, parser=parser) - - # Create scenario - scenario = Scenario( - dt=dt, - benchmark_id=root.attrib['benchmarkID'] - ) - - for lanelet in root.iterchildren('lanelet'): - - left_vertices = [] - right_vertices = [] - - for pt in lanelet.leftBound.getchildren(): - left_vertices.append(np.array([float(pt.x), float(pt.y)])) - - for pt in lanelet.rightBound.getchildren(): - right_vertices.append(np.array([float(pt.x), float(pt.y)])) - - center_vertices = [(l + r) / 2 for (l, r) in zip(left_vertices, right_vertices)] - - scenario.lanelet_network.add_lanelet(Lanelet( - left_vertices=np.array([np.array([x, y]) for x, y in left_vertices]), - center_vertices=np.array([np.array([x, y]) for x, y in center_vertices]), - right_vertices=np.array([np.array([x, y]) for x, y in right_vertices]), - lanelet_id=int(lanelet.attrib['id']), - predecessor=[int(el.attrib['ref']) for el in lanelet.iterchildren(tag='predecessor')], - successor=[int(el.attrib['ref']) for el in lanelet.iterchildren(tag='successor')] - )) - - return scenario - - -class ScenarioError(Exception): - """Base class for exceptions in this module.""" - pass - -class LaneletNetwork(object): - - def __init__(self): - self.lanelets = [] - - def find_lanelet_by_id(self, lanelet_id): - for l in self.lanelets: - if l.lanelet_id == lanelet_id: - return l - raise ScenarioError - - def add_lanelet(self, lanelet): - if type(lanelet) == list: - for l in lanelet: - self.add_lanelet(l) - else: - try: - self.find_lanelet_by_id(lanelet.lanelet_id) - except ScenarioError: - self.lanelets.append(lanelet) - else: - raise Exception("Lanelet with id {} already in network.".format(lanelet.lanelet_id)) - -class Lanelet(object): - - def __init__(self, left_vertices, center_vertices, right_vertices, - lanelet_id, - predecessor=None, successor=None, - adjacent_left=None, adjacent_left_same_direction=None, - adjacent_right=None, adjacent_right_same_direction=None, - speed_limit=None): - if (len(left_vertices) != len(center_vertices) and - len(center_vertices) != len(right_vertices)): - raise ScenarioError - self.left_vertices = left_vertices - self.center_vertices = center_vertices - self.right_vertices = right_vertices - if predecessor is None: - self.predecessor = [] - else: - self.predecessor = predecessor - if successor is None: - self.successor = [] - else: - self.successor = successor - self.adj_left = adjacent_left - self.adj_left_same_direction = adjacent_left_same_direction - self.adj_right = adjacent_right - self.adj_right_same_direction = adjacent_right_same_direction - - self.lanelet_id = lanelet_id - self.speed_limit = speed_limit - self.distance = [0] - for i in range(1, len(self.center_vertices)): - self.distance.append(self.distance[i-1] + - np.linalg.norm(self.center_vertices[i] - - self.center_vertices[i-1])) - self.distance = np.array(self.distance) - self.description = "" - - def calc_width_at_start(self): - return np.linalg.norm(self.left_vertices[0], self.right_vertices[0]) - - def calc_width_at_end(self): - return np.linalg.norm(np.array(self.left_vertices[-1]) - np.array(self.right_vertices[-1])) - - def concatenate(self, lanelet, lanelet_id=-1): - float_tolerance = 1e-6 - if (np.linalg.norm(self.center_vertices[-1] - lanelet.center_vertices[0]) > float_tolerance or - np.linalg.norm(self.left_vertices[-1] - lanelet.left_vertices[0]) > float_tolerance or - np.linalg.norm(self.right_vertices[-1] - lanelet.right_vertices[0]) > float_tolerance): - pass - #return None - left_vertices = np.vstack((self.left_vertices, - lanelet.left_vertices[1:])) - center_vertices = np.vstack((self.center_vertices, - lanelet.center_vertices[1:])) - right_vertices = np.vstack((self.right_vertices, - lanelet.right_vertices[1:])) - return Lanelet(center_vertices=center_vertices, - left_vertices=left_vertices, - right_vertices=right_vertices, - predecessor=self.predecessor.copy(), - successor=lanelet.successor.copy(), - adjacent_left=None, - adjacent_left_same_direction=None, - adjacent_right=None, - adjacent_right_same_direction=None, - lanelet_id=lanelet_id, - speed_limit=None) diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/network.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/network.py deleted file mode 100644 index db95162..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/network.py +++ /dev/null @@ -1,528 +0,0 @@ - -import copy - -import numpy as np - -from opendriveparser.elements.openDrive import OpenDrive - -from opendrive2lanelet.plane_elements.plane import PLane -from opendrive2lanelet.plane_elements.plane_group import PLaneGroup -from opendrive2lanelet.plane_elements.border import Border -from opendrive2lanelet.utils import encode_road_section_lane_width_id, decode_road_section_lane_width_id, allCloseToZero - -from opendrive2lanelet.commonroad import LaneletNetwork, Scenario, ScenarioError - - -class Network(object): - """ Represents a network of parametric lanes """ - - def __init__(self): - self._planes = [] - self._linkIndex = None - - def loadOpenDrive(self, openDrive): - """ Load all elements of an OpenDRIVE network to a parametric lane representation """ - - if not isinstance(openDrive, OpenDrive): - raise TypeError() - - self._linkIndex = self.createLinkIndex(openDrive) - - # Convert all parts of a road to parametric lanes (planes) - for road in openDrive.roads: - - # The reference border is the base line for the whole road - referenceBorder = Network.createReferenceBorder(road.planView, road.lanes.laneOffsets) - - # A lane section is the smallest part that can be converted at once - for laneSection in road.lanes.laneSections: - - pLanes = Network.laneSectionToPLanes(laneSection, referenceBorder) - - self._planes.extend(pLanes) - - def addPLane(self, pLane): - if not isinstance(pLane, PLane): - raise TypeError() - self._planes.append(pLane) - - def exportLaneletNetwork(self, filterTypes=None): - """ Export lanelet as lanelet network """ - - # Convert groups to lanelets - laneletNetwork = LaneletNetwork() - - for pLane in self._planes: - if filterTypes is not None and pLane.type not in filterTypes: - continue - - lanelet = pLane.convertToLanelet() - - lanelet.predecessor = self._linkIndex.getPredecessors(pLane.id) - lanelet.successor = self._linkIndex.getSuccessors(pLane.id) - - lanelet.refPLane = pLane - - laneletNetwork.add_lanelet(lanelet) - - # Prune all not existing references - lanelet_ids = [x.lanelet_id for x in laneletNetwork.lanelets] - - for lanelet in laneletNetwork.lanelets: - for predecessor in lanelet.predecessor: - if predecessor not in lanelet_ids: - lanelet.predecessor.remove(predecessor) - for successor in lanelet.successor: - if successor not in lanelet_ids: - lanelet.successor.remove(successor) - if lanelet.adj_left not in lanelet_ids: - lanelet.adj_left = None - if lanelet.adj_right not in lanelet_ids: - lanelet.adj_right = None - - # Perform lane merges - # Condition for lane merge: - # - Lanelet ends (has no successor or predecessor) - # - Has an adjacent (left or right) with same type - for lanelet in laneletNetwork.lanelets: - - if len(lanelet.successor) == 0: - - if lanelet.adj_left is not None: - adj_left_lanelet = laneletNetwork.find_lanelet_by_id(lanelet.adj_left) - - newLanelet = lanelet.refPLane.convertToLanelet( - ref="right", - refDistance=[adj_left_lanelet.calc_width_at_end(), 0.0] - ) - - lanelet.left_vertices = newLanelet.left_vertices - lanelet.center_vertices = newLanelet.center_vertices - lanelet.right_vertices = newLanelet.right_vertices - - lanelet.successor.extend(adj_left_lanelet.successor) - - if lanelet.adj_right is not None: - adj_right_lanelet = laneletNetwork.find_lanelet_by_id(lanelet.adj_right) - - newLanelet = lanelet.refPLane.convertToLanelet( - ref="left", - refDistance=[-1 * adj_right_lanelet.calc_width_at_end(), 0.0] - ) - - lanelet.left_vertices = newLanelet.left_vertices - lanelet.center_vertices = newLanelet.center_vertices - lanelet.right_vertices = newLanelet.right_vertices - - lanelet.successor.extend(adj_right_lanelet.successor) - - if len(lanelet.predecessor) == 0: - - if lanelet.adj_left is not None: - adj_left_lanelet = laneletNetwork.find_lanelet_by_id(lanelet.adj_left) - - newLanelet = lanelet.refPLane.convertToLanelet( - ref="right", - refDistance=[0.0, -1 * adj_left_lanelet.calc_width_at_end()] - ) - - lanelet.left_vertices = newLanelet.left_vertices - lanelet.center_vertices = newLanelet.center_vertices - lanelet.right_vertices = newLanelet.right_vertices - - lanelet.predecessor.extend(adj_left_lanelet.predecessor) - - if lanelet.adj_right is not None: - adj_right_lanelet = laneletNetwork.find_lanelet_by_id(lanelet.adj_right) - - newLanelet = lanelet.refPLane.convertToLanelet( - ref="left", - refDistance=[0.0, adj_right_lanelet.calc_width_at_end()] - ) - - lanelet.left_vertices = newLanelet.left_vertices - lanelet.center_vertices = newLanelet.center_vertices - lanelet.right_vertices = newLanelet.right_vertices - - lanelet.predecessor.extend(adj_right_lanelet.predecessor) - - - # Assign an integer id to each lanelet - def convert_to_new_id(old_lanelet_id): - if old_lanelet_id in convert_to_new_id.id_assign: - new_lanelet_id = convert_to_new_id.id_assign[old_lanelet_id] - else: - new_lanelet_id = convert_to_new_id.lanelet_id - convert_to_new_id.id_assign[old_lanelet_id] = new_lanelet_id - - convert_to_new_id.lanelet_id += 1 - return new_lanelet_id - - convert_to_new_id.id_assign = {} - convert_to_new_id.lanelet_id = 100 - - for lanelet in laneletNetwork.lanelets: - lanelet.description = lanelet.lanelet_id - lanelet.lanelet_id = convert_to_new_id(lanelet.lanelet_id) - - lanelet.predecessor = [convert_to_new_id(x) for x in lanelet.predecessor] - lanelet.successor = [convert_to_new_id(x) for x in lanelet.successor] - lanelet.adj_left = None if lanelet.adj_left is None else convert_to_new_id(lanelet.adj_left) - lanelet.adj_right = None if lanelet.adj_right is None else convert_to_new_id(lanelet.adj_right) - - return laneletNetwork - - def exportCommonRoadScenario(self, dt=0.1, benchmark_id=None, filterTypes=None): - """ Export a full CommonRoad scenario """ - - scenario = Scenario( - dt=dt, - benchmark_id=benchmark_id if benchmark_id is not None else "none" - ) - - scenario.add_objects(self.exportLaneletNetwork( - filterTypes=filterTypes if isinstance(filterTypes, list) else ['driving', 'onRamp', 'offRamp', 'exit', 'entry'] - )) - - return scenario - - ############################################################################################## - ## Helper functions - - @staticmethod - def createReferenceBorder(planView, laneOffsets): - """ Create the first (most inner) border line for a road, includes the lane Offsets """ - - firstLaneBorder = Border() - - # Set reference to plan view - firstLaneBorder.reference = planView - firstLaneBorder.refOffset = 0.0 - - # Lane offfsets will be coeffs - if any(laneOffsets): - for laneOffset in laneOffsets: - firstLaneBorder.coeffsOffsets.append(laneOffset.sPos) - firstLaneBorder.coeffs.append(laneOffset.coeffs) - else: - firstLaneBorder.coeffsOffsets.append(0.0) - firstLaneBorder.coeffs.append([0.0]) - - return firstLaneBorder - - @staticmethod - def laneSectionToPLanes(laneSection, referenceBorder): - """ Convert a whole lane section into a list of planes """ - - newPLanes = [] - - # Length of this lane section - laneSectionStart = laneSection.sPos - - for side in ["right", "left"]: - - # lanes loaded by opendriveparser are aleady sorted by id - # coeffsFactor decides if border is left or right of the reference line - if side == "right": - lanes = laneSection.rightLanes - coeffsFactor = -1.0 - - else: - lanes = laneSection.leftLanes - coeffsFactor = 1.0 - - # Most inner border - laneBorders = [referenceBorder] - prevInnerNeighbours = [] - - for lane in lanes: - - if abs(lane.id) > 1: - - if lane.id > 0: - innerLaneId = lane.id - 1 - outerLaneId = lane.id + 1 - else: - innerLaneId = lane.id + 1 - outerLaneId = lane.id - 1 - - innerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, innerLaneId, -1) - innerNeighbourSameDirection = True - - outerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, outerLaneId, -1) - else: - # Skip lane id 0 - - if lane.id == 1: - innerLaneId = -1 - outerLaneId = 2 - else: - innerLaneId = 1 - outerLaneId = -2 - - innerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, innerLaneId, -1) - innerNeighbourSameDirection = False - - outerNeighbourId = encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, outerLaneId, -1) - - newPLaneGroup = PLaneGroup( - id=encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, lane.id, -1), - innerNeighbour=innerNeighbourId, - innerNeighbourSameDirection=innerNeighbourSameDirection, - outerNeighbour=outerNeighbourId - ) - - innerNeighbours = [] - - # Create outer lane border - newPLaneBorder = Border() - newPLaneBorder.reference = laneBorders[-1] - - if len(laneBorders) == 1: - newPLaneBorder.refOffset = laneSectionStart - else: - # Offset from reference border is already included in first created outer lane border - newPLaneBorder.refOffset = 0.0 - - for width in lane.widths: - newPLaneBorder.coeffsOffsets.append(width.sOffset) - newPLaneBorder.coeffs.append([x * coeffsFactor for x in width.coeffs]) - - laneBorders.append(newPLaneBorder) - - # Create new lane for each width segment - for width in lane.widths: - - newPLane = PLane( - id=encode_road_section_lane_width_id(laneSection.parentRoad.id, laneSection.idx, lane.id, width.idx), - type=lane.type - ) - - if allCloseToZero(width.coeffs): - newPLane.isNotExistent = True - - newPLane.innerNeighbours = prevInnerNeighbours - - newPLane.length = width.length - - newPLane.innerBorder = laneBorders[-2] - newPLane.innerBorderOffset = width.sOffset + laneBorders[-1].refOffset - - newPLane.outerBorder = laneBorders[-1] - newPLane.outerBorderOffset = width.sOffset - - newPLaneGroup.append(newPLane) - innerNeighbours.append(newPLane) - - newPLanes.append(newPLaneGroup) - - prevInnerNeighbours = innerNeighbours - - return newPLanes - - @staticmethod - def createLinkIndex(openDrive): - """ Step through all junctions and each single lane to build up a index """ - - def add_to_index(linkIndex, pLaneId, successorId, reverse=False): - if reverse: - linkIndex.addLink(successorId, pLaneId) - else: - linkIndex.addLink(pLaneId, successorId) - - linkIndex = LinkIndex() - - # Extract link information from road lanes - for road in openDrive.roads: - for laneSection in road.lanes.laneSections: - for lane in laneSection.allLanes: - pLaneId = encode_road_section_lane_width_id(road.id, laneSection.idx, lane.id, -1) - - # Not the last lane section? > Next lane section in same road - if laneSection.idx < road.lanes.getLastLaneSectionIdx(): - - successorId = encode_road_section_lane_width_id(road.id, laneSection.idx + 1, lane.link.successorId, -1) - - add_to_index(linkIndex, pLaneId, successorId, lane.id >= 0) - - # Last lane section! > Next road in first lane section - else: - - # Try to get next road - if road.link.successor is not None and road.link.successor.elementType != "junction": - - nextRoad = openDrive.getRoad(road.link.successor.elementId) - - if nextRoad is not None: - - if road.link.successor.contactPoint == "start": - successorId = encode_road_section_lane_width_id(nextRoad.id, 0, lane.link.successorId, -1) - add_to_index(linkIndex, pLaneId, successorId, lane.id >= 0) - - else: # contact point = end - successorId = encode_road_section_lane_width_id(nextRoad.id, nextRoad.lanes.getLastLaneSectionIdx(), lane.link.successorId, -1) - add_to_index(linkIndex, pLaneId, successorId, lane.id >= 0) - - - # Not first lane section? > Previous lane section in same road - if laneSection.idx > 0: - predecessorId = encode_road_section_lane_width_id(road.id, laneSection.idx - 1, lane.link.predecessorId, -1) - - add_to_index(linkIndex, predecessorId, pLaneId, lane.id >= 0) - - # First lane section! > Previous road - else: - - # Try to get previous road - if road.link.predecessor is not None and road.link.predecessor.elementType != "junction": - - prevRoad = openDrive.getRoad(road.link.predecessor.elementId) - - if prevRoad is not None: - - if road.link.predecessor.contactPoint == "start": - predecessorId = encode_road_section_lane_width_id(prevRoad.id, 0, lane.link.predecessorId, -1) - add_to_index(linkIndex, predecessorId, pLaneId, lane.id >= 0) - - else: # contact point = end - predecessorId = encode_road_section_lane_width_id(prevRoad.id, prevRoad.lanes.getLastLaneSectionIdx(), lane.link.predecessorId, -1) - add_to_index(linkIndex, predecessorId, pLaneId, lane.id >= 0) - - # Add junctions - for road in openDrive.roads: - - # Add junction links to end of road - if road.link.successor is not None and road.link.successor.elementType == "junction": - - junction = openDrive.getJunction(road.link.successor.elementId) - - if junction is not None: - - for connection in junction.connections: - - roadA = openDrive.getRoad(connection.incomingRoad) - roadAcp = "end" - roadB = openDrive.getRoad(connection.connectingRoad) - roadBcp = connection.contactPoint - - if roadA.id != road.id: - roadA, roadB = [roadB, roadA] - - for laneLink in connection.laneLinks: - - if roadAcp == "start": - pLaneId = encode_road_section_lane_width_id(roadA.id, 0, laneLink.fromId, -1) - else: - successorId = encode_road_section_lane_width_id(roadA.id, roadA.lanes.getLastLaneSectionIdx(), laneLink.fromId, -1) - - if roadBcp == "start": - pLaneId = encode_road_section_lane_width_id(roadB.id, 0, laneLink.toId, -1) - else: - successorId = encode_road_section_lane_width_id(roadB.id, roadB.lanes.getLastLaneSectionIdx(), laneLink.toId, -1) - - add_to_index(linkIndex, pLaneId, successorId, laneLink.fromId < 0) - - # Add junction links to start of road - if road.link.predecessor is not None and road.link.predecessor.elementType == "junction": - - junction = openDrive.getJunction(road.link.predecessor.elementId) - - if junction is not None: - - for connection in junction.connections: - - roadA = openDrive.getRoad(connection.incomingRoad) - roadAcp = "start" - roadB = openDrive.getRoad(connection.connectingRoad) - roadBcp = connection.contactPoint - - if roadA.id != road.id: - roadA, roadB = [roadB, roadA] - - for laneLink in connection.laneLinks: - - if roadAcp == "start": - pLaneId = encode_road_section_lane_width_id(roadA.id, 0, laneLink.fromId, -1) - else: - predecessorId = encode_road_section_lane_width_id(roadA.id, roadA.lanes.getLastLaneSectionIdx(), laneLink.fromId, -1) - - if roadBcp == "start": - pLaneId = encode_road_section_lane_width_id(roadB.id, 0, laneLink.toId, -1) - else: - predecessorId = encode_road_section_lane_width_id(roadB.id, roadB.lanes.getLastLaneSectionIdx(), laneLink.toId, -1) - - add_to_index(linkIndex, predecessorId, pLaneId, laneLink.fromId < 0) - - # for junction in openDrive.junctions: - # for connection in junction.connections: - - # incomingRoad = openDrive.getRoad(connection.incomingRoad) - # connectingRoad = openDrive.getRoad(connection.connectingRoad) - - # if incomingRoad is not None and connectingRoad is not None: - - # for laneLink in connection.laneLinks: - - # pLaneId = encode_road_section_lane_width_id(incomingRoad.id, incomingRoad.lanes.getLastLaneSectionIdx(), laneLink.fromId, -1) - - # if connection.contactPoint == "end": - # successorId = encode_road_section_lane_width_id(connectingRoad.id, 0, laneLink.toId, -1) - # else: - # successorId = encode_road_section_lane_width_id(connectingRoad.id, connectingRoad.lanes.getLastLaneSectionIdx(), laneLink.toId, -1) - - # add_to_index(linkIndex, pLaneId, successorId, lane.id >= 0) - - - return linkIndex - - -class LinkIndex(object): - """ Overall index of all links in the file, save everything as successors, predecessors can be found via a reverse search """ - - def __init__(self): - self._successors = {} - - def addLink(self, pLaneId, successor): - if pLaneId not in self._successors: - self._successors[pLaneId] = [] - - if successor not in self._successors[pLaneId]: - self._successors[pLaneId].append(successor) - - def remove(self, pLaneId): - # Delete key - if pLaneId in self._successors: - del self._successors[pLaneId] - - # Delete all occurances in successor lists - for successorsId, successors in self._successors.items(): - if pLaneId in successors: - self._successors[successorsId].remove(pLaneId) - - def getSuccessors(self, pLaneId): - if pLaneId not in self._successors: - return [] - - return self._successors[pLaneId] - - def getPredecessors(self, pLaneId): - predecessors = [] - - for successorsPLaneId, successors in self._successors.items(): - if pLaneId not in successors: - continue - - if successorsPLaneId in predecessors: - continue - - predecessors.append(successorsPLaneId) - - return predecessors - - def __str__(self): - retstr = "Link Index:\n" - - for pre, succs in self._successors.items(): - retstr += "\t{:15} > {}\n".format(pre, ", ".join(succs)) - - return retstr diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/plane_elements/__init__.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/plane_elements/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/plane_elements/border.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/plane_elements/border.py deleted file mode 100644 index d92d5c0..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/plane_elements/border.py +++ /dev/null @@ -1,84 +0,0 @@ - -from functools import lru_cache -import numpy as np - -from opendriveparser.elements.roadPlanView import PlanView - -class Border(object): - """ - A lane border defines a path along a whole lane section - - a lane always used an inner and outer lane border - - the reference can be another lane border or a plan view - """ - - def __init__(self): - - self._refOffset = 0.0 - - self._coeffsOffsets = [] - self._coeffs = [] - - self._reference = None - - def __str__(self): - return str(self._refOffset) - - @property - def reference(self): - return self._reference - - @reference.setter - def reference(self, value): - if not isinstance(value, Border) and not isinstance(value, PlanView): - raise TypeError("Value must be instance of Border or PlanView") - - self._reference = value - - @property - def refOffset(self): - """ The reference offset """ - return self._refOffset - - @refOffset.setter - def refOffset(self, value): - self._refOffset = float(value) - - @property - def coeffs(self): - """ It is assumed the coeffs are added in ascending order! ([0] + [1] * x + [2] * x**2 + ...) """ - return self._coeffs - - @property - def coeffsOffsets(self): - """ Offsets for coeffs """ - return self._coeffsOffsets - - @lru_cache(maxsize=200000) - def calc(self, sPos, addOffset=0.0): - """ Calculate the border """ - - if isinstance(self._reference, PlanView): - # Last reference has to be a reference geometry - refPos, refTang = self._reference.calc(self._refOffset + sPos) - - elif isinstance(self._reference, Border): - # Offset of all inner lanes - refPos, refTang = self._reference.calc(self._refOffset + sPos) - - else: - raise Exception("Reference must be plan view or other lane border.") - - if not self._coeffs or not self._coeffsOffsets: - raise Exception("No entries for width definitions.") - - # Find correct coefficients - widthIdx = next((self._coeffsOffsets.index(n) for n in self._coeffsOffsets[::-1] if n <= sPos), len(self._coeffsOffsets)) - - # Calculate width at sPos - distance = np.polynomial.polynomial.polyval(sPos - self._coeffsOffsets[widthIdx], self._coeffs[widthIdx]) + addOffset - - # New point is in orthogonal direction - ortho = refTang + np.pi / 2 - newPos = refPos + np.array([distance * np.cos(ortho), distance * np.sin(ortho)]) - - return newPos, refTang diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/plane_elements/plane.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/plane_elements/plane.py deleted file mode 100644 index c5a1ef9..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/plane_elements/plane.py +++ /dev/null @@ -1,160 +0,0 @@ - -import numpy as np - -from opendrive2lanelet.plane_elements.border import Border -from opendrive2lanelet.commonroad import Lanelet - -class PLane(object): - """ A lane defines a part of a road along a reference trajectory (plan view), using lane borders and start/stop positions (parametric) """ - - def __init__(self, id, type): - """ Each lane is defined with a starting point, an offset to the reference trajectory and a lane width """ - - self._id = id - self._type = type - self._length = None - self._innerBorder = None - self._innerBorderOffset = None - self._outerBorder = None - self._outerBorderOffset = None - - self._isNotExistent = False - self._innerNeighbours = [] - self._outerNeighbours = [] - - self._successors = [] - self._predecessors = [] - - @property - def id(self): - return self._id - - @property - def type(self): - return self._type - - @property - def length(self): - return self._length - - @length.setter - def length(self, value): - self._length = float(value) - - @property - def innerBorder(self): - return self._innerBorder - - @innerBorder.setter - def innerBorder(self, value): - if not isinstance(value, Border): - raise TypeError("Value must be instance of _LaneBorder.") - - self._innerBorder = value - - def calcInnerBorder(self, sPos, addOffset=0.0): - return self._innerBorder.calc(self._innerBorderOffset + sPos, addOffset=addOffset) - - @property - def outerBorder(self): - return self._outerBorder - - @property - def innerBorderOffset(self): - return self._innerBorderOffset - - @innerBorderOffset.setter - def innerBorderOffset(self, value): - self._innerBorderOffset = float(value) - - @outerBorder.setter - def outerBorder(self, value): - if not isinstance(value, Border): - raise TypeError("Value must be instance of Border.") - - self._outerBorder = value - - def calcOuterBorder(self, sPos, addOffset=0.0): - return self._outerBorder.calc(self._outerBorderOffset + sPos, addOffset=addOffset) - - @property - def outerBorderOffset(self): - return self._outerBorderOffset - - @outerBorderOffset.setter - def outerBorderOffset(self, value): - self._outerBorderOffset = float(value) - - def calcWidth(self, sPos): - innerCoords = self.calcInnerBorder(sPos) - outerCoords = self.calcOuterBorder(sPos) - - return np.linalg.norm(innerCoords[0] - outerCoords[0]) - - def convertToLanelet(self, precision=0.5, ref=None, refDistance=[0.0, 0.0]): - # Define calculation points - # TODO dependent on max error - numSteps = max(2, np.ceil(self._length / float(precision))) - poses = np.linspace(0, self._length, numSteps) - - left_vertices = [] - right_vertices = [] - - for pos in poses: - if ref is None: - left_vertices.append(self.calcInnerBorder(pos)[0]) - right_vertices.append(self.calcOuterBorder(pos)[0]) - else: - x = pos - m = (refDistance[1] - refDistance[0]) / self._length - t = refDistance[0] - - d = m*x + t - - if ref == "left": - left_vertices.append(self.calcInnerBorder(pos)[0]) - right_vertices.append(self.calcOuterBorder(pos, d)[0]) - elif ref == "right": - left_vertices.append(self.calcInnerBorder(pos, d)[0]) - right_vertices.append(self.calcOuterBorder(pos)[0]) - - center_vertices = [(l + r) / 2 for (l, r) in zip(left_vertices, right_vertices)] - - return Lanelet( - left_vertices=np.array([np.array([x, y]) for x, y in left_vertices]), - center_vertices=np.array([np.array([x, y]) for x, y in center_vertices]), - right_vertices=np.array([np.array([x, y]) for x, y in right_vertices]), - lanelet_id=self._id - ) - - @property - def isNotExistent(self): - return self._isNotExistent - - @isNotExistent.setter - def isNotExistent(self, value): - self._isNotExistent = bool(value) - - @property - def innerNeighbours(self): - return self._innerNeighbours - - @innerNeighbours.setter - def innerNeighbours(self, value): - self._innerNeighbours = value - - @property - def outerNeighbours(self): - return self._outerNeighbours - - @outerNeighbours.setter - def outerNeighbours(self, value): - self._outerNeighbours = value - - @property - def successors(self): - return self._successors - - @property - def predecessors(self): - return self._predecessors diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/plane_elements/plane_group.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/plane_elements/plane_group.py deleted file mode 100644 index 0a5478b..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/plane_elements/plane_group.py +++ /dev/null @@ -1,97 +0,0 @@ - -class PLaneGroup(object): - """ A group of pLanes can be converted to a lanelet just like a single pLane """ - - def __init__(self, id=None, pLanes=None, innerNeighbour=None, innerNeighbourSameDirection=True, outerNeighbour=None): - - self._pLanes = [] - self._id = id - self._innerNeighbour = innerNeighbour - self._innerNeighbourSameDirection = innerNeighbourSameDirection - self._outerNeighbour = outerNeighbour - - if pLanes is not None: - - if isinstance(pLanes, list): - self._pLanes.extend(pLanes) - else: - self._pLanes.append(pLanes) - - def append(self, pLane): - self._pLanes.append(pLane) - - @property - def id(self): - if self._id is not None: - return self._id - - raise Exception() - - @property - def type(self): - return self._pLanes[0]._type - - @property - def length(self): - return sum([x.length for x in self._pLanes]) - - def convertToLanelet(self, precision=0.5, ref=None, refDistance=[0.0, 0.0]): - - lanelet = None - y1 = refDistance[0] - x = 0 - - for pLane in self._pLanes: - - x += pLane.length - y2 = (refDistance[1] - refDistance[0]) / self.length * x + refDistance[0] - - # First lanelet - if lanelet is None: - lanelet = pLane.convertToLanelet(precision=precision, ref=ref, refDistance=[y1, y2]) - lanelet.lanelet_id = self.id - continue - - # Append all following lanelets - lanelet = lanelet.concatenate(pLane.convertToLanelet(precision=precision, ref=ref, refDistance=[y1, y2]), self.id) - - y1 = y2 - - if lanelet is None: - raise Exception("Lanelet concatenation problem") - - # Adjacent lanes - if self.innerNeighbour is not None: - lanelet.adj_left = self.innerNeighbour - lanelet.adj_left_same_direction = self.innerNeighbourSameDirection - - if self.outerNeighbour is not None: - lanelet.adj_right = self.outerNeighbour - lanelet.adj_right_same_direction = True - - return lanelet - - @property - def innerNeighbour(self): - return self._innerNeighbour - - @innerNeighbour.setter - def innerNeighbour(self, value): - self._innerNeighbour = value - - @property - def innerNeighbourSameDirection(self): - return self._innerNeighbourSameDirection - - @innerNeighbourSameDirection.setter - def innerNeighbourSameDirection(self, value): - self._innerNeighbourSameDirection = value - - @property - def outerNeighbour(self): - """ Outer neighbour has always the same driving direction """ - return self._outerNeighbour - - @outerNeighbour.setter - def outerNeighbour(self, value): - self._outerNeighbour = value diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/utils.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/utils.py deleted file mode 100644 index c7e2082..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendrive2lanelet/utils.py +++ /dev/null @@ -1,18 +0,0 @@ - -def encode_road_section_lane_width_id(roadId, sectionId, laneId, widthId): - return ".".join([str(roadId), str(sectionId), str(laneId), str(widthId)]) - -def decode_road_section_lane_width_id(encodedString): - - parts = encodedString.split(".") - - if len(parts) != 4: - raise Exception() - - return (int(parts[0]), int(parts[1]), int(parts[2]), int(parts[3])) - -def allCloseToZero(a): - """ Tests if all elements of a are close to zero. """ - - import numpy - return numpy.allclose(a, numpy.zeros(numpy.shape(a))) diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/README.md b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/README.md deleted file mode 100644 index 7132e21..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/README.md +++ /dev/null @@ -1,20 +0,0 @@ -# OpenDRIVE parser - -## Usage - -```python -from lxml import etree -from opendriveparser import parse_opendrive - -fh = open("input_opendrive.xodr", 'r') -openDrive = parse_opendrive(etree.parse(fh).getroot()) -fh.close() - -# Now do stuff with the data -for road in openDrive.roads: - print("Road ID: {}".format(road.id)) -``` - -## Author - -Stefan Urban \ No newline at end of file diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/__init__.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/__init__.py deleted file mode 100644 index dac222a..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ - -from opendriveparser.parser import parse_opendrive diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/__init__.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/eulerspiral.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/eulerspiral.py deleted file mode 100644 index 8bd1cb9..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/eulerspiral.py +++ /dev/null @@ -1,41 +0,0 @@ - -import numpy as np -from scipy.special import fresnel - -class EulerSpiral(object): - - def __init__(self, gamma): - self._gamma = gamma - - @staticmethod - def createFromLengthAndCurvature(length, curvStart, curvEnd): - return EulerSpiral(1 * (curvEnd - curvStart) / length) - - def calc(self, s, x0=0, y0=0, kappa0=0, theta0=0): - - # Start - C0 = x0 + 1j * y0 - - if self._gamma == 0 and kappa0 == 0: - # Straight line - Cs = C0 + np.exp(1j * theta0 * s) - - elif self._gamma == 0 and kappa0 != 0: - # Arc - Cs = C0 + np.exp(1j * theta0) / kappa0 * (np.sin(kappa0 * s) + 1j * (1 - np.cos(kappa0 * s))) - - else: - # Fresnel integrals - Sa, Ca = fresnel((kappa0 + self._gamma * s) / np.sqrt(np.pi * np.abs(self._gamma))) - Sb, Cb = fresnel(kappa0 / np.sqrt(np.pi * np.abs(self._gamma))) - - # Euler Spiral - Cs1 = np.sqrt(np.pi / np.abs(self._gamma)) * np.exp(1j * (theta0 - kappa0**2 / 2 / self._gamma)) - Cs2 = np.sign(self._gamma) * (Ca - Cb) + 1j * Sa - 1j * Sb - - Cs = C0 + Cs1 * Cs2 - - # Tangent at each point - theta = self._gamma * s**2 / 2 + kappa0 * s + theta0 - - return (Cs.real, Cs.imag, theta) diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/junction.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/junction.py deleted file mode 100644 index 2a334fc..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/junction.py +++ /dev/null @@ -1,116 +0,0 @@ - -class Junction(object): - # TODO priority - # TODO controller - - def __init__(self): - self._id = None - self._name = None - self._connections = [] - - @property - def id(self): - return self._id - - @id.setter - def id(self, value): - self._id = int(value) - - @property - def name(self): - return self._name - - @name.setter - def name(self, value): - self._name = str(value) - - @property - def connections(self): - return self._connections - - def addConnection(self, connection): - if not isinstance(connection, Connection): - raise TypeError("Has to be of instance Connection") - - self._connections.append(connection) - - -class Connection(object): - - def __init__(self): - self._id = None - self._incomingRoad = None - self._connectingRoad = None - self._contactPoint = None - self._laneLinks = [] - - @property - def id(self): - return self._id - - @id.setter - def id(self, value): - self._id = int(value) - - @property - def incomingRoad(self): - return self._incomingRoad - - @incomingRoad.setter - def incomingRoad(self, value): - self._incomingRoad = int(value) - - @property - def connectingRoad(self): - return self._connectingRoad - - @connectingRoad.setter - def connectingRoad(self, value): - self._connectingRoad = int(value) - - @property - def contactPoint(self): - return self._contactPoint - - @contactPoint.setter - def contactPoint(self, value): - if value not in ["start", "end"]: - raise AttributeError("Contact point can only be start or end.") - - self._contactPoint = value - - @property - def laneLinks(self): - return self._laneLinks - - def addLaneLink(self, laneLink): - if not isinstance(laneLink, LaneLink): - raise TypeError("Has to be of instance LaneLink") - - self._laneLinks.append(laneLink) - - -class LaneLink(object): - - def __init__(self): - self._from = None - self._to = None - - def __str__(self): - return str(self._from) + " > " + str(self._to) - - @property - def fromId(self): - return self._from - - @fromId.setter - def fromId(self, value): - self._from = int(value) - - @property - def toId(self): - return self._to - - @toId.setter - def toId(self, value): - self._to = int(value) diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/openDrive.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/openDrive.py deleted file mode 100644 index 3a1192d..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/openDrive.py +++ /dev/null @@ -1,142 +0,0 @@ - -class OpenDrive(object): - - def __init__(self): - self._header = Header() - self._roads = [] - self._controllers = [] - self._junctions = [] - self._junctionGroups = [] - self._stations = [] - - @property - def header(self): - return self._header - - @property - def roads(self): - return self._roads - - def getRoad(self, id): - for road in self._roads: - if road.id == id: - return road - - return None - - @property - def controllers(self): - return self._controllers - - @property - def junctions(self): - return self._junctions - - def getJunction(self, junctionId): - for junction in self._junctions: - if junction.id == junctionId: - return junction - return None - - @property - def junctionGroups(self): - return self._junctionGroups - - @property - def stations(self): - return self._stations - - -class Header(object): - - def __init__(self): - self._revMajor = None - self._revMinor = None - self._name = None - self._version = None - self._date = None - self._north = None - self._south = None - self._east = None - self._west = None - self._vendor = None - - @property - def revMajor(self): - return self._revMajor - - @revMajor.setter - def revMajor(self, value): - self._revMajor = value - - @property - def revMinor(self): - return self._revMinor - - @revMinor.setter - def revMinor(self, value): - self._revMinor = value - - @property - def name(self): - return self._name - - @name.setter - def name(self, value): - self._name = value - - @property - def version(self): - return self._version - - @version.setter - def version(self, value): - self._version = value - - @property - def date(self): - return self._date - - @date.setter - def date(self, value): - self._date = value - - @property - def north(self): - return self._north - - @north.setter - def north(self, value): - self._north = value - - @property - def south(self): - return self._south - - @south.setter - def south(self, value): - self._south = value - - @property - def east(self): - return self._east - - @east.setter - def east(self, value): - self._east = value - - @property - def west(self): - return self._west - - @west.setter - def west(self, value): - self._west = value - - @property - def vendor(self): - return self._vendor - - @vendor.setter - def vendor(self, value): - self._vendor = value diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/road.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/road.py deleted file mode 100644 index fc59c5a..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/road.py +++ /dev/null @@ -1,77 +0,0 @@ - -from opendriveparser.elements.roadPlanView import PlanView -from opendriveparser.elements.roadLink import Link -from opendriveparser.elements.roadLanes import Lanes -from opendriveparser.elements.roadElevationProfile import ElevationProfile -from opendriveparser.elements.roadLateralProfile import LateralProfile -from opendriveparser.elements.junction import Junction - -class Road(object): - - def __init__(self): - self._id = None - self._name = None - self._junction = None - self._length = None - - self._header = None # TODO - self._link = Link() - self._types = [] - self._planView = PlanView() - self._elevationProfile = ElevationProfile() - self._lateralProfile = LateralProfile() - self._lanes = Lanes() - - @property - def id(self): - return self._id - - @id.setter - def id(self, value): - self._id = int(value) - - @property - def name(self): - return self._name - - @name.setter - def name(self, value): - self._name = str(value) - - @property - def junction(self): - return self._junction - - @junction.setter - def junction(self, value): - if not isinstance(value, Junction) and value is not None: - raise TypeError("Property must be a Junction or NoneType") - - if value == -1: - value = None - - self._junction = value - - @property - def link(self): - return self._link - - @property - def types(self): - return self._types - - @property - def planView(self): - return self._planView - - @property - def elevationProfile(self): - return self._elevationProfile - - @property - def lateralProfile(self): - return self._lateralProfile - - @property - def lanes(self): - return self._lanes diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadElevationProfile.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadElevationProfile.py deleted file mode 100644 index b4240e8..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadElevationProfile.py +++ /dev/null @@ -1,59 +0,0 @@ - -class ElevationProfile(object): - - def __init__(self): - self._elevations = [] - - @property - def elevations(self): - return self._elevations - - -class Elevation(object): - - def __init__(self): - self._sPos = None - self._a = None - self._b = None - self._c = None - self._d = None - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def a(self): - return self._a - - @a.setter - def a(self, value): - self._a = float(value) - - @property - def b(self): - return self._b - - @b.setter - def b(self, value): - self._b = float(value) - - @property - def c(self): - return self._c - - @c.setter - def c(self, value): - self._c = float(value) - - @property - def d(self): - return self._d - - @d.setter - def d(self, value): - self._d = float(value) diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadLanes.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadLanes.py deleted file mode 100644 index 0829488..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadLanes.py +++ /dev/null @@ -1,355 +0,0 @@ - -class Lanes(object): - - def __init__(self): - self._laneOffsets = [] - self._laneSections = [] - - @property - def laneOffsets(self): - self._laneOffsets.sort(key=lambda x: x.sPos) - return self._laneOffsets - - @property - def laneSections(self): - self._laneSections.sort(key=lambda x: x.sPos) - return self._laneSections - - def getLaneSection(self, laneSectionIdx): - for laneSection in self.laneSections: - if laneSection.idx == laneSectionIdx: - return laneSection - - return None - - def getLastLaneSectionIdx(self): - - numLaneSections = len(self.laneSections) - - if numLaneSections > 1: - return numLaneSections - 1 - - return 0 - -class LaneOffset(object): - - def __init__(self): - self._sPos = None - self._a = None - self._b = None - self._c = None - self._d = None - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def a(self): - return self._a - - @a.setter - def a(self, value): - self._a = float(value) - - @property - def b(self): - return self._b - - @b.setter - def b(self, value): - self._b = float(value) - - @property - def c(self): - return self._c - - @c.setter - def c(self, value): - self._c = float(value) - - @property - def d(self): - return self._d - - @d.setter - def d(self, value): - self._d = float(value) - - @property - def coeffs(self): - """ Array of coefficients for usage with numpy.polynomial.polynomial.polyval """ - return [self._a, self._b, self._c, self._d] - - -class LaneSection(object): - - def __init__(self, road=None): - self._idx = None - self._sPos = None - self._singleSide = None - self._leftLanes = LeftLanes() - self._centerLanes = CenterLanes() - self._rightLanes = RightLanes() - - self._parentRoad = road - - @property - def idx(self): - return self._idx - - @idx.setter - def idx(self, value): - self._idx = int(value) - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def length(self): - return self._length - - @length.setter - def length(self, value): - self._length = float(value) - - @property - def singleSide(self): - return self._singleSide - - @singleSide.setter - def singleSide(self, value): - if value not in ["true", "false"] and value is not None: - raise AttributeError("Value must be true or false.") - - self._singleSide = (value == "true") - - @property - def leftLanes(self): - """ Get list of sorted lanes always starting in the middle (lane id -1) """ - return self._leftLanes.lanes - - @property - def centerLanes(self): - return self._centerLanes.lanes - - @property - def rightLanes(self): - """ Get list of sorted lanes always starting in the middle (lane id 1) """ - return self._rightLanes.lanes - - @property - def allLanes(self): - """ Attention! lanes are not sorted by id """ - return self._leftLanes.lanes + self._centerLanes.lanes + self._rightLanes.lanes - - def getLane(self, laneId): - for lane in self.allLanes: - if lane.id == laneId: - return lane - - return None - - @property - def parentRoad(self): - return self._parentRoad - - -class LeftLanes(object): - - sort_direction = False - - def __init__(self): - self._lanes = [] - - @property - def lanes(self): - self._lanes.sort(key=lambda x: x.id, reverse=self.sort_direction) - return self._lanes - -class CenterLanes(LeftLanes): - pass - -class RightLanes(LeftLanes): - sort_direction = True - - -class Lane(object): - - laneTypes = [ - "none", "driving", "stop", "shoulder", "biking", "sidewalk", "border", - "restricted", "parking", "bidirectional", "median", "special1", "special2", - "special3", "roadWorks", "tram", "rail", "entry", "exit", "offRamp", "onRamp" - ] - - def __init__(self, parentRoad): - self._parent_road = parentRoad - self._id = None - self._type = None - self._level = None - self._link = LaneLink() - self._widths = [] - self._borders = [] - - @property - def parentRoad(self): - return self._parent_road - - @property - def id(self): - return self._id - - @id.setter - def id(self, value): - self._id = int(value) - - @property - def type(self): - return self._type - - @type.setter - def type(self, value): - if value not in self.laneTypes: - raise Exception() - - self._type = str(value) - - @property - def level(self): - return self._level - - @level.setter - def level(self, value): - if value not in ["true", "false"] and value is not None: - raise AttributeError("Value must be true or false.") - - self._level = (value == "true") - - @property - def link(self): - return self._link - - @property - def widths(self): - self._widths.sort(key=lambda x: x.sOffset) - return self._widths - - def getWidth(self, widthIdx): - for width in self._widths: - if width.idx == widthIdx: - return width - - return None - - def getLastLaneWidthIdx(self): - """ Returns the index of the last width sector of the lane """ - - numWidths = len(self._widths) - - if numWidths > 1: - return numWidths - 1 - - return 0 - - @property - def borders(self): - return self._borders - - -class LaneLink(object): - - def __init__(self): - self._predecessor = None - self._successor = None - - @property - def predecessorId(self): - return self._predecessor - - @predecessorId.setter - def predecessorId(self, value): - self._predecessor = int(value) - - @property - def successorId(self): - return self._successor - - @successorId.setter - def successorId(self, value): - self._successor = int(value) - - -class LaneWidth(object): - - def __init__(self): - self._idx = None - self._sOffset = None - self._a = None - self._b = None - self._c = None - self._d = None - - @property - def idx(self): - return self._idx - - @idx.setter - def idx(self, value): - self._idx = int(value) - - @property - def sOffset(self): - return self._sOffset - - @sOffset.setter - def sOffset(self, value): - self._sOffset = float(value) - - @property - def a(self): - return self._a - - @a.setter - def a(self, value): - self._a = float(value) - - @property - def b(self): - return self._b - - @b.setter - def b(self, value): - self._b = float(value) - - @property - def c(self): - return self._c - - @c.setter - def c(self, value): - self._c = float(value) - - @property - def d(self): - return self._d - - @d.setter - def d(self, value): - self._d = float(value) - - @property - def coeffs(self): - """ Array of coefficients for usage with numpy.polynomial.polynomial.polyval """ - return [self._a, self._b, self._c, self._d] - -class LaneBorder(LaneWidth): - pass diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadLateralProfile.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadLateralProfile.py deleted file mode 100644 index cf0dc8b..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadLateralProfile.py +++ /dev/null @@ -1,211 +0,0 @@ - -class LateralProfile(object): - - def __init__(self): - self._superelevations = [] - self._crossfalls = [] - self._shapes = [] - - @property - def superelevations(self): - return self._superelevations - - @superelevations.setter - def superelevations(self, value): - if not isinstance(value, list) or not all(isinstance(x, Superelevation) for x in value): - raise TypeError("Value must be an instance of Superelevation.") - - self._superelevations = value - - @property - def crossfalls(self): - return self._crossfalls - - @crossfalls.setter - def crossfalls(self, value): - if not isinstance(value, list) or not all(isinstance(x, Crossfall) for x in value): - raise TypeError("Value must be an instance of Crossfall.") - - self._crossfalls = value - - @property - def shapes(self): - return self._shapes - - @shapes.setter - def shapes(self, value): - if not isinstance(value, list) or not all(isinstance(x, Shape) for x in value): - raise TypeError("Value must be a list of instances of Shape.") - - self._shapes = value - - -class Superelevation(object): - - def __init__(self): - self._sPos = None - self._a = None - self._b = None - self._c = None - self._d = None - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def a(self): - return self._a - - @a.setter - def a(self, value): - self._a = float(value) - - @property - def b(self): - return self._b - - @b.setter - def b(self, value): - self._b = float(value) - - @property - def c(self): - return self._c - - @c.setter - def c(self, value): - self._c = float(value) - - @property - def d(self): - return self._d - - @d.setter - def d(self, value): - self._d = float(value) - - -class Crossfall(object): - - def __init__(self): - self._side = None - self._sPos = None - self._a = None - self._b = None - self._c = None - self._d = None - - @property - def side(self): - return self._side - - @side.setter - def side(self, value): - if value not in ["left", "right", "both"]: - raise TypeError("Value must be string with content 'left', 'right' or 'both'.") - - self._side = value - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def a(self): - return self._a - - @a.setter - def a(self, value): - self._a = float(value) - - @property - def b(self): - return self._b - - @b.setter - def b(self, value): - self._b = float(value) - - @property - def c(self): - return self._c - - @c.setter - def c(self, value): - self._c = float(value) - - @property - def d(self): - return self._d - - @d.setter - def d(self, value): - self._d = float(value) - - -class Shape(object): - - def __init__(self): - self._sPos = None - self._t = None - self._a = None - self._b = None - self._c = None - self._d = None - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def t(self): - return self._t - - @t.setter - def t(self, value): - self._t = float(value) - - @property - def a(self): - return self._a - - @a.setter - def a(self, value): - self._a = float(value) - - @property - def b(self): - return self._b - - @b.setter - def b(self, value): - self._b = float(value) - - @property - def c(self): - return self._c - - @c.setter - def c(self, value): - self._c = float(value) - - @property - def d(self): - return self._d - - @d.setter - def d(self, value): - self._d = float(value) diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadLink.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadLink.py deleted file mode 100644 index ce89503..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadLink.py +++ /dev/null @@ -1,139 +0,0 @@ - -class Link(object): - - def __init__(self): - self._id = None - self._predecessor = None - self._successor = None - self._neighbors = [] - - def __str__(self): - return " > link id " + str(self._id) + " | successor: " + str(self._successor) - - @property - def id(self): - return self._id - - @id.setter - def id(self, value): - self._id = int(value) - - @property - def predecessor(self): - return self._predecessor - - @predecessor.setter - def predecessor(self, value): - if not isinstance(value, Predecessor): - raise TypeError("Value must be Predecessor") - - self._predecessor = value - - @property - def successor(self): - return self._successor - - @successor.setter - def successor(self, value): - if not isinstance(value, Successor): - raise TypeError("Value must be Successor") - - self._successor = value - - @property - def neighbors(self): - return self._neighbors - - @neighbors.setter - def neighbors(self, value): - if not isinstance(value, list) or not all(isinstance(x, Neighbor) for x in value): - raise TypeError("Value must be list of instances of Neighbor.") - - self._neighbors = value - - def addNeighbor(self, value): - if not isinstance(value, Neighbor): - raise TypeError("Value must be Neighbor") - - self._neighbors.append(value) - - -class Predecessor(object): - - def __init__(self): - self._elementType = None - self._elementId = None - self._contactPoint = None - - def __str__(self): - return str(self._elementType) + " with id " + str(self._elementId) + " contact at " + str(self._contactPoint) - - @property - def elementType(self): - return self._elementType - - @elementType.setter - def elementType(self, value): - if value not in ["road", "junction"]: - raise AttributeError("Value must be road or junction") - - self._elementType = value - - @property - def elementId(self): - return self._elementId - - @elementId.setter - def elementId(self, value): - self._elementId = int(value) - - @property - def contactPoint(self): - return self._contactPoint - - @contactPoint.setter - def contactPoint(self, value): - if value not in ["start", "end"] and value is not None: - raise AttributeError("Value must be start or end") - - self._contactPoint = value - -class Successor(Predecessor): - pass - -class Neighbor(object): - - def __init__(self): - self._side = None - self._elementId = None - self._direction = None - - @property - def side(self): - return self._side - - @side.setter - def side(self, value): - if value not in ["left", "right"]: - raise AttributeError("Value must be left or right") - - self._side = value - - @property - def elementId(self): - return self._elementId - - @elementId.setter - def elementId(self, value): - self._elementId = int(value) - - @property - def direction(self): - return self._direction - - @direction.setter - def direction(self, value): - if value not in ["same", "opposite"]: - raise AttributeError("Value must be same or opposite") - - self._direction = value diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadPlanView.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadPlanView.py deleted file mode 100644 index d88a59a..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadPlanView.py +++ /dev/null @@ -1,225 +0,0 @@ - -import abc -import numpy as np - -from opendriveparser.elements.eulerspiral import EulerSpiral - - -class PlanView(object): - - def __init__(self): - self._geometries = [] - - def addLine(self, startPosition, heading, length): - self._geometries.append(Line(startPosition, heading, length)) - - def addSpiral(self, startPosition, heading, length, curvStart, curvEnd): - self._geometries.append(Spiral(startPosition, heading, length, curvStart, curvEnd)) - - def addArc(self, startPosition, heading, length, curvature): - self._geometries.append(Arc(startPosition, heading, length, curvature)) - - def addParamPoly3(self, startPosition, heading, length, aU, bU, cU, dU, aV, bV, cV, dV, pRange): - self._geometries.append(ParamPoly3(startPosition, heading, length, aU, bU, cU, dU, aV, bV, cV, dV, pRange)) - - def getLength(self): - """ Get length of whole plan view """ - - length = 0 - - for geometry in self._geometries: - length += geometry.getLength() - - return length - - def calc(self, sPos): - """ Calculate position and tangent at sPos """ - - for geometry in self._geometries: - if geometry.getLength() < sPos and not np.isclose(geometry.getLength(), sPos): - sPos -= geometry.getLength() - continue - return geometry.calcPosition(sPos) - - # TODO - return self._geometries[-1].calcPosition(self._geometries[-1].getLength()) - - # 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())) - -class Geometry(object): - __metaclass__ = abc.ABCMeta - - @abc.abstractmethod - def getStartPosition(self): - """ Returns the overall geometry length """ - return - - @abc.abstractmethod - def getLength(self): - """ Returns the overall geometry length """ - return - - @abc.abstractmethod - def calcPosition(self, s): - """ Calculates the position of the geometry as if the starting point is (0/0) """ - return - -class Line(Geometry): - - def __init__(self, startPosition, heading, length): - self.startPosition = np.array(startPosition) - self.heading = heading - self.length = length - - def getStartPosition(self): - return self.startPosition - - def getLength(self): - return self.length - - def calcPosition(self, s): - pos = self.startPosition + np.array([s * np.cos(self.heading), s * np.sin(self.heading)]) - tangent = self.heading - - return (pos, tangent) - -class Arc(Geometry): - - def __init__(self, startPosition, heading, length, curvature): - self.startPosition = np.array(startPosition) - self.heading = heading - self.length = length - self.curvature = curvature - - def getStartPosition(self): - return self.startPosition - - def getLength(self): - return self.length - - def calcPosition(self, s): - c = self.curvature - hdg = self.heading - np.pi / 2 - - a = 2 / c * np.sin(s * c / 2) - alpha = (np.pi - s * c) / 2 - hdg - - dx = -1 * a * np.cos(alpha) - dy = a * np.sin(alpha) - - pos = self.startPosition + np.array([dx, dy]) - tangent = self.heading + s * self.curvature - - return (pos, tangent) - -class Spiral(Geometry): - - def __init__(self, startPosition, heading, length, curvStart, curvEnd): - self._startPosition = np.array(startPosition) - self._heading = heading - self._length = length - self._curvStart = curvStart - self._curvEnd = curvEnd - - self._spiral = EulerSpiral.createFromLengthAndCurvature(self._length, self._curvStart, self._curvEnd) - - def getStartPosition(self): - return self._startPosition - - def getLength(self): - return self._length - - def calcPosition(self, s): - (x, y, t) = self._spiral.calc(s, self._startPosition[0], self._startPosition[1], self._curvStart, self._heading) - - return (np.array([x, y]), t) - -class Poly3(Geometry): - - def __init__(self, startPosition, heading, length, a, b, c, d): - self._startPosition = np.array(startPosition) - self._heading = heading - self._length = length - self._a = a - self._b = b - self._c = c - self._d = d - - raise NotImplementedError() - - def getStartPosition(self): - return self._startPosition - - def getLength(self): - return self._length - - def calcPosition(self, s): - # TODO untested - - # Calculate new point in s/t coordinate system - coeffs = [self._a, self._b, self._c, self._d] - - t = np.polynomial.polynomial.polyval(s, coeffs) - - # Rotate and translate - srot = s * np.cos(self._heading) - t * np.sin(self._heading) - trot = s * np.sin(self._heading) + t * np.cos(self._heading) - - # Derivate to get heading change - dCoeffs = coeffs[1:] * np.array(np.arange(1, len(coeffs))) - tangent = np.polynomial.polynomial.polyval(s, dCoeffs) - - return (self._startPosition + np.array([srot, trot]), self._heading + tangent) - -class ParamPoly3(Geometry): - - def __init__(self, startPosition, heading, length, aU, bU, cU, dU, aV, bV, cV, dV, pRange): - self._startPosition = np.array(startPosition) - self._heading = heading - self._length = length - - self._aU = aU - self._bU = bU - self._cU = cU - self._dU = dU - self._aV = aV - self._bV = bV - self._cV = cV - self._dV = dV - - if pRange is None: - self._pRange = 1.0 - else: - self._pRange = pRange - - def getStartPosition(self): - return self._startPosition - - def getLength(self): - return self._length - - def calcPosition(self, s): - - # Position - pos = (s / self._length) * self._pRange - - coeffsU = [self._aU, self._bU, self._cU, self._dU] - coeffsV = [self._aV, self._bV, self._cV, self._dV] - - x = np.polynomial.polynomial.polyval(pos, coeffsU) - y = np.polynomial.polynomial.polyval(pos, coeffsV) - - xrot = x * np.cos(self._heading) - y * np.sin(self._heading) - yrot = x * np.sin(self._heading) + y * np.cos(self._heading) - - # Tangent is defined by derivation - dCoeffsU = coeffsU[1:] * np.array(np.arange(1, len(coeffsU))) - dCoeffsV = coeffsV[1:] * np.array(np.arange(1, len(coeffsV))) - - dx = np.polynomial.polynomial.polyval(pos, dCoeffsU) - dy = np.polynomial.polynomial.polyval(pos, dCoeffsV) - - tangent = np.arctan2(dy, dx) - - - return (self._startPosition + np.array([xrot, yrot]), self._heading + tangent) diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadType.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadType.py deleted file mode 100644 index 0c8bf30..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/elements/roadType.py +++ /dev/null @@ -1,64 +0,0 @@ - - -class Type(object): - - allowedTypes = ["unknown", "rural", "motorway", "town", "lowSpeed", "pedestrian", "bicycle"] - - def __init__(self): - self._sPos = None - self._type = None - self._speed = None - - @property - def sPos(self): - return self._sPos - - @sPos.setter - def sPos(self, value): - self._sPos = float(value) - - @property - def type(self): - return self._type - - @type.setter - def type(self, value): - if value not in self.allowedTypes: - raise AttributeError("Type not allowed.") - - self._type = value - - @property - def speed(self): - return self._speed - - @speed.setter - def speed(self, value): - if not isinstance(value, Speed): - raise TypeError("Value must be instance of Speed.") - - self._speed = value - - -class Speed(object): - - def __init__(self): - self._max = None - self._unit = None - - @property - def max(self): - return self._max - - @max.setter - def max(self, value): - self._max = str(value) - - @property - def unit(self): - return self._unit - - @unit.setter - def unit(self, value): - # TODO validate input - self._unit = str(value) diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/parser.py b/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/parser.py deleted file mode 100644 index aac8bb8..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/opendriveparser/parser.py +++ /dev/null @@ -1,413 +0,0 @@ - -import numpy as np -from lxml import etree - -from opendriveparser.elements.openDrive import OpenDrive -from opendriveparser.elements.road import Road -from opendriveparser.elements.roadLink import Predecessor as RoadLinkPredecessor, Successor as RoadLinkSuccessor, Neighbor as RoadLinkNeighbor -from opendriveparser.elements.roadType import Type as RoadType, Speed as RoadTypeSpeed -from opendriveparser.elements.roadElevationProfile import Elevation as RoadElevationProfileElevation -from opendriveparser.elements.roadLateralProfile import Superelevation as RoadLateralProfileSuperelevation, Crossfall as RoadLateralProfileCrossfall, Shape as RoadLateralProfileShape -from opendriveparser.elements.roadLanes import LaneOffset as RoadLanesLaneOffset, Lane as RoadLaneSectionLane, LaneSection as RoadLanesSection, LaneWidth as RoadLaneSectionLaneWidth, LaneBorder as RoadLaneSectionLaneBorder -from opendriveparser.elements.junction import Junction, Connection as JunctionConnection, LaneLink as JunctionConnectionLaneLink - - -def parse_opendrive(rootNode): - """ Tries to parse XML tree, returns OpenDRIVE object """ - - # Only accept lxml element - if not etree.iselement(rootNode): - raise TypeError("Argument rootNode is not a xml element") - - newOpenDrive = OpenDrive() - - # Header - header = rootNode.find("header") - - if header is not None: - - if header.get('revMajor') is not None: - newOpenDrive.header.revMajor = str(header.get('revMajor')) - - if header.get('revMinor') is not None: - newOpenDrive.header.revMinor = str(header.get('revMinor')) - - if header.get('name') is not None: - newOpenDrive.header.name = str(header.get('name')) - - if header.get('version') is not None: - newOpenDrive.header.version = str(header.get('version')) - - if header.get('date') is not None: - newOpenDrive.header.date = str(header.get('date')) - - if header.get('north') is not None: - newOpenDrive.header.north = str(header.get('north')) - - if header.get('south') is not None: - newOpenDrive.header.south = str(header.get('south')) - - if header.get('east') is not None: - newOpenDrive.header.east = str(header.get('east')) - - if header.get('west') is not None: - newOpenDrive.header.west = str(header.get('west')) - - if header.get('vendor') is not None: - newOpenDrive.header.vendor = str(header.get('vendor')) - - # Reference - if header.find("geoReference") is not None: - pass - # TODO not implemented - - # Junctions - for junction in rootNode.findall("junction"): - - newJunction = Junction() - - newJunction.id = int(junction.get("id")) - newJunction.name = str(junction.get("name")) - - for connection in junction.findall("connection"): - - newConnection = JunctionConnection() - - newConnection.id = connection.get("id") - newConnection.incomingRoad = connection.get("incomingRoad") - newConnection.connectingRoad = connection.get("connectingRoad") - newConnection.contactPoint = connection.get("contactPoint") - - for laneLink in connection.findall("laneLink"): - - newLaneLink = JunctionConnectionLaneLink() - - newLaneLink.fromId = laneLink.get("from") - newLaneLink.toId = laneLink.get("to") - - newConnection.addLaneLink(newLaneLink) - - newJunction.addConnection(newConnection) - - newOpenDrive.junctions.append(newJunction) - - # Load roads - for road in rootNode.findall("road"): - - newRoad = Road() - - newRoad.id = int(road.get("id")) - newRoad.name = road.get("name") - - junctionId = int(road.get("junction")) if road.get("junction") != "-1" else None - - if junctionId: - newRoad.junction = newOpenDrive.getJunction(junctionId) - - # TODO verify road length - newRoad.length = float(road.get("length")) - - # Links - if road.find("link") is not None: - - predecessor = road.find("link").find("predecessor") - - if predecessor is not None: - - newPredecessor = RoadLinkPredecessor() - - newPredecessor.elementType = predecessor.get("elementType") - newPredecessor.elementId = predecessor.get("elementId") - newPredecessor.contactPoint = predecessor.get("contactPoint") - - newRoad.link.predecessor = newPredecessor - - successor = road.find("link").find("successor") - - if successor is not None: - - newSuccessor = RoadLinkSuccessor() - - newSuccessor.elementType = successor.get("elementType") - newSuccessor.elementId = successor.get("elementId") - newSuccessor.contactPoint = successor.get("contactPoint") - - newRoad.link.successor = newSuccessor - - for neighbor in road.find("link").findall("neighbor"): - - newNeighbor = RoadLinkNeighbor() - - newNeighbor.side = neighbor.get("side") - newNeighbor.elementId = neighbor.get("elementId") - newNeighbor.direction = neighbor.get("direction") - - newRoad.link.neighbors.append(newNeighbor) - - # Type - for roadType in road.findall("type"): - - newType = RoadType() - - newType.sPos = roadType.get("s") - newType.type = roadType.get("type") - - if roadType.find("speed"): - - newSpeed = RoadTypeSpeed() - - newSpeed.max = roadType.find("speed").get("max") - newSpeed.unit = roadType.find("speed").get("unit") - - newType.speed = newSpeed - - newRoad.types.append(newType) - - # Plan view - for geometry in road.find("planView").findall("geometry"): - - startCoord = [float(geometry.get("x")), float(geometry.get("y"))] - - if geometry.find("line") is not None: - newRoad.planView.addLine(startCoord, float(geometry.get("hdg")), float(geometry.get("length"))) - - elif geometry.find("spiral") is not None: - newRoad.planView.addSpiral(startCoord, float(geometry.get("hdg")), float(geometry.get("length")), float(geometry.find("spiral").get("curvStart")), float(geometry.find("spiral").get("curvEnd"))) - - elif geometry.find("arc") is not None: - newRoad.planView.addArc(startCoord, float(geometry.get("hdg")), float(geometry.get("length")), float(geometry.find("arc").get("curvature"))) - - elif geometry.find("poly3") is not None: - raise NotImplementedError() - - elif geometry.find("paramPoly3") is not None: - if geometry.find("paramPoly3").get("pRange"): - - if geometry.find("paramPoly3").get("pRange") == "arcLength": - pMax = float(geometry.get("length")) - else: - pMax = None - else: - pMax = None - - newRoad.planView.addParamPoly3( \ - startCoord, \ - float(geometry.get("hdg")), \ - float(geometry.get("length")), \ - float(geometry.find("paramPoly3").get("aU")), \ - float(geometry.find("paramPoly3").get("bU")), \ - float(geometry.find("paramPoly3").get("cU")), \ - float(geometry.find("paramPoly3").get("dU")), \ - float(geometry.find("paramPoly3").get("aV")), \ - float(geometry.find("paramPoly3").get("bV")), \ - float(geometry.find("paramPoly3").get("cV")), \ - float(geometry.find("paramPoly3").get("dV")), \ - pMax \ - ) - - else: - raise Exception("invalid xml") - - # Elevation profile - if road.find("elevationProfile") is not None: - - for elevation in road.find("elevationProfile").findall("elevation"): - - newElevation = RoadElevationProfileElevation() - - newElevation.sPos = elevation.get("s") - newElevation.a = elevation.get("a") - newElevation.b = elevation.get("b") - newElevation.c = elevation.get("c") - newElevation.d = elevation.get("d") - - newRoad.elevationProfile.elevations.append(newElevation) - - # Lateral profile - if road.find("lateralProfile") is not None: - - for superelevation in road.find("lateralProfile").findall("superelevation"): - - newSuperelevation = RoadLateralProfileSuperelevation() - - newSuperelevation.sPos = superelevation.get("s") - newSuperelevation.a = superelevation.get("a") - newSuperelevation.b = superelevation.get("b") - newSuperelevation.c = superelevation.get("c") - newSuperelevation.d = superelevation.get("d") - - newRoad.lateralProfile.superelevations.append(newSuperelevation) - - for crossfall in road.find("lateralProfile").findall("crossfall"): - - newCrossfall = RoadLateralProfileCrossfall() - - newCrossfall.side = crossfall.get("side") - newCrossfall.sPos = crossfall.get("s") - newCrossfall.a = crossfall.get("a") - newCrossfall.b = crossfall.get("b") - newCrossfall.c = crossfall.get("c") - newCrossfall.d = crossfall.get("d") - - newRoad.lateralProfile.crossfalls.append(newCrossfall) - - for shape in road.find("lateralProfile").findall("shape"): - - newShape = RoadLateralProfileShape() - - newShape.sPos = shape.get("s") - newShape.t = shape.get("t") - newShape.a = shape.get("a") - newShape.b = shape.get("b") - newShape.c = shape.get("c") - newShape.d = shape.get("d") - - newRoad.lateralProfile.shapes.append(newShape) - - # Lanes - lanes = road.find("lanes") - - if lanes is None: - raise Exception("Road must have lanes element") - - # Lane offset - for laneOffset in lanes.findall("laneOffset"): - - newLaneOffset = RoadLanesLaneOffset() - - newLaneOffset.sPos = laneOffset.get("s") - newLaneOffset.a = laneOffset.get("a") - newLaneOffset.b = laneOffset.get("b") - newLaneOffset.c = laneOffset.get("c") - newLaneOffset.d = laneOffset.get("d") - - newRoad.lanes.laneOffsets.append(newLaneOffset) - - # Lane sections - for laneSectionIdx, laneSection in enumerate(road.find("lanes").findall("laneSection")): - - newLaneSection = RoadLanesSection(road=newRoad) - - # Manually enumerate lane sections for referencing purposes - newLaneSection.idx = laneSectionIdx - - newLaneSection.sPos = laneSection.get("s") - newLaneSection.singleSide = laneSection.get("singleSide") - - sides = dict( - left=newLaneSection.leftLanes, - center=newLaneSection.centerLanes, - right=newLaneSection.rightLanes - ) - - for sideTag, newSideLanes in sides.items(): - - side = laneSection.find(sideTag) - - # It is possible one side is not present - if side is None: - continue - - for lane in side.findall("lane"): - - newLane = RoadLaneSectionLane( - parentRoad=newRoad - ) - - newLane.id = lane.get("id") - newLane.type = lane.get("type") - - # In some sample files the level is not specified according to the OpenDRIVE spec - newLane.level = "true" if lane.get("level") in [1, '1', 'true'] else "false" - - # Lane Links - if lane.find("link") is not None: - - if lane.find("link").find("predecessor") is not None: - newLane.link.predecessorId = lane.find("link").find("predecessor").get("id") - - if lane.find("link").find("successor") is not None: - newLane.link.successorId = lane.find("link").find("successor").get("id") - - # Width - for widthIdx, width in enumerate(lane.findall("width")): - - newWidth = RoadLaneSectionLaneWidth() - - newWidth.idx = widthIdx - newWidth.sOffset = width.get("sOffset") - newWidth.a = width.get("a") - newWidth.b = width.get("b") - newWidth.c = width.get("c") - newWidth.d = width.get("d") - - newLane.widths.append(newWidth) - - # Border - for borderIdx, border in enumerate(lane.findall("border")): - - newBorder = RoadLaneSectionLaneBorder() - - newBorder.idx = borderIdx - newBorder.sPos = border.get("sOffset") - newBorder.a = border.get("a") - newBorder.b = border.get("b") - newBorder.c = border.get("c") - newBorder.d = border.get("d") - - newLane.borders.append(newBorder) - - # Road Marks - # TODO implementation - - # Material - # TODO implementation - - # Visiblility - # TODO implementation - - # Speed - # TODO implementation - - # Access - # TODO implementation - - # Lane Height - # TODO implementation - - # Rules - # TODO implementation - - newSideLanes.append(newLane) - - newRoad.lanes.laneSections.append(newLaneSection) - - # OpenDRIVE does not provide lane section lengths by itself, calculate them by ourselves - for laneSection in newRoad.lanes.laneSections: - - # Last lane section in road - if laneSection.idx + 1 >= len(newRoad.lanes.laneSections): - laneSection.length = newRoad.planView.getLength() - laneSection.sPos - - # All but the last lane section end at the succeeding one - else: - laneSection.length = newRoad.lanes.laneSections[laneSection.idx + 1].sPos - laneSection.sPos - - # OpenDRIVE does not provide lane width lengths by itself, calculate them by ourselves - for laneSection in newRoad.lanes.laneSections: - for lane in laneSection.allLanes: - widthsPoses = np.array([x.sOffset for x in lane.widths] + [laneSection.length]) - widthsLengths = widthsPoses[1:] - widthsPoses[:-1] - - for widthIdx, width in enumerate(lane.widths): - width.length = widthsLengths[widthIdx] - - # Objects - # TODO implementation - - # Signals - # TODO implementation - - newOpenDrive.roads.append(newRoad) - - return newOpenDrive diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/requirements.txt b/src/aadcUserPython/opendrive/opendrive2lanelet/requirements.txt deleted file mode 100644 index 67b3830..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/requirements.txt +++ /dev/null @@ -1,5 +0,0 @@ -numpy -scipy -lxml -matplotlib -PyQt5 diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/setup.py b/src/aadcUserPython/opendrive/opendrive2lanelet/setup.py deleted file mode 100644 index c48fbac..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/setup.py +++ /dev/null @@ -1,17 +0,0 @@ -#!/usr/bin/env python - -from setuptools import setup, find_packages - -setup(name='opendrive2lanelets', - version='1.0', - description='Parser and converter from OpenDRIVE to lanelets', - author='Stefan Urban', - author_email='stefan.urban@tum.de', - url='https://gitlab.lrz.de/koschi/converter', - packages=find_packages(), - install_requires=[ - "numpy", - "lxml", - "scipy", - ] - ) diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/test/convert.py b/src/aadcUserPython/opendrive/opendrive2lanelet/test/convert.py deleted file mode 100644 index 9939876..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/test/convert.py +++ /dev/null @@ -1,17 +0,0 @@ -from lxml import etree -from opendriveparser import parse_opendrive -from opendrive2lanelet import Network - - -fh = open("test/opendrive-1.xodr", 'r') -openDrive = parse_opendrive(etree.parse(fh).getroot()) -fh.close() - -roadNetwork = Network() -roadNetwork.loadOpenDrive(openDrive) - -scenario = roadNetwork.exportCommonRoadScenario() - -fh = open("test/opendrive-1.xml", "wb") -fh.write(scenario.export_to_string()) -fh.close() diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/test/opendrive-1-rn.dump b/src/aadcUserPython/opendrive/opendrive2lanelet/test/opendrive-1-rn.dump deleted file mode 100644 index c1ec0fe..0000000 Binary files a/src/aadcUserPython/opendrive/opendrive2lanelet/test/opendrive-1-rn.dump and /dev/null differ diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/test/opendrive-1-sc.dump b/src/aadcUserPython/opendrive/opendrive2lanelet/test/opendrive-1-sc.dump deleted file mode 100644 index 9530b4f..0000000 Binary files a/src/aadcUserPython/opendrive/opendrive2lanelet/test/opendrive-1-sc.dump and /dev/null differ diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/test/opendrive-1.py b/src/aadcUserPython/opendrive/opendrive2lanelet/test/opendrive-1.py deleted file mode 100644 index 94181ce..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/test/opendrive-1.py +++ /dev/null @@ -1,41 +0,0 @@ - -import pickle -import os -import unittest - -from lxml import etree -from opendriveparser import parse_opendrive -from opendrive2lanelet import Network - -class OpenDrive1Test(unittest.TestCase): - - def setUp(self): - - # Write CommonRoad scenario to file - fh = open(os.path.dirname(os.path.realpath(__file__)) + "/opendrive-1.xodr", 'r') - openDrive = parse_opendrive(etree.parse(fh).getroot()) - fh.close() - - self.roadNetwork = Network() - self.roadNetwork.loadOpenDrive(openDrive) - - self.scenario = self.roadNetwork.exportCommonRoadScenario() - - #pickle.dump(self.roadNetwork, open(os.path.dirname(os.path.realpath(__file__)) + "opendrive-1-rn.dump", "wb")) - #pickle.dump(self.scenario, open(os.path.dirname(os.path.realpath(__file__)) + "opendrive-1-sc.dump", "wb")) - - - def test_planes(self): - - with open(os.path.dirname(os.path.realpath(__file__)) + "/opendrive-1-rn.dump", "rb") as fh: - self.assertEqual(self.roadNetwork, pickle.load(fh)) - - def test_lanelets(self): - - with open(os.path.dirname(os.path.realpath(__file__)) + "/opendrive-1-sc.dump", "rb") as fh: - self.assertEqual(self.scenario, pickle.load(fh)) - - - -if __name__ == '__main__': - unittest.main() diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/viewer.py b/src/aadcUserPython/opendrive/opendrive2lanelet/viewer.py deleted file mode 100644 index 65979ca..0000000 --- a/src/aadcUserPython/opendrive/opendrive2lanelet/viewer.py +++ /dev/null @@ -1,281 +0,0 @@ - -import math -import signal -import sys -import os - -from lxml import etree - -import numpy as np - -import matplotlib -matplotlib.use('Qt5Agg') - -from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas -from matplotlib.figure import Figure -from matplotlib.path import Path -from matplotlib.patches import PathPatch, Polygon - -from PyQt5.QtCore import Qt, pyqtSlot -from PyQt5.QtWidgets import QApplication, QWidget, QTableWidget, QPushButton, QLineEdit, QFileDialog, QLabel, QMessageBox -from PyQt5.QtWidgets import QSizePolicy, QHBoxLayout, QVBoxLayout, QTableWidgetItem, QAbstractItemView - -from opendrive2lanelet.commonroad import Lanelet, LaneletNetwork, Scenario, ScenarioError - - -class Canvas(FigureCanvas): - """Ultimately, this is a QWidget """ - - def __init__(self, parent=None, width=5, height=5, dpi=100): - - self.ax = None - - self.fig = Figure(figsize=(width, height), dpi=dpi) - - super(Canvas, self).__init__(self.fig) - self.setParent(parent) - self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - - self.clear_axes() - - def clear_axes(self): - if self.ax is not None: - self.ax.clear() - else: - - self.ax = self.fig.add_subplot(111) - self.ax.set_aspect('equal', 'datalim') - self.ax.set_axis_off() - - self.draw() - - def get_axes(self): - return self.ax - - def update_plot(self): - self.draw() - -class MainWindow(QWidget): - - def __init__(self, parent=None, path=None): - super().__init__(parent) - - self.current_scenario = None - self.selected_lanelet_id = None - - self._initUserInterface() - self.show() - - if path is not None: - self.openPath(path) - - def _initUserInterface(self): - - self.setWindowTitle("CommonRoad XML Viewer") - - self.setMinimumSize(1000, 600) - - self.testButton = QPushButton('test', self) - self.testButton.clicked.connect(self.testCmd) - - self.loadButton = QPushButton('Load CommonRoad', self) - self.loadButton.setToolTip('Load a CommonRoad scenario within a *.xml file') - self.loadButton.clicked.connect(self.openCommonRoadFile) - - self.inputCommonRoadFile = QLineEdit(self) - self.inputCommonRoadFile.setReadOnly(True) - - hbox = QHBoxLayout() - hbox.addWidget(self.loadButton) - hbox.addWidget(self.inputCommonRoadFile) - - self.dynamic = Canvas(self, width=5, height=10, dpi=100) - - vbox = QVBoxLayout() - vbox.addLayout(hbox, 0) - vbox.addWidget(self.dynamic, 1) - - vbox.setAlignment(Qt.AlignTop) - - self.laneletsList = QTableWidget(self) - self.laneletsList.setSelectionBehavior(QAbstractItemView.SelectRows) - self.laneletsList.clicked.connect(self.onClickLanelet) - - hbox2 = QHBoxLayout(self) - hbox2.addLayout(vbox, 1) - hbox2.addWidget(self.laneletsList, 0) - - self.setLayout(hbox2) - - def openCommonRoadFile(self): - - path, _ = QFileDialog.getOpenFileName( - self, - "QFileDialog.getOpenFileName()", - "", - "CommonRoad scenario files *.xml (*.xml)", - options=QFileDialog.Options() - ) - - if not path: - return - - self.openPath(path) - - def openPath(self, path): - - filename = os.path.basename(path) - self.inputCommonRoadFile.setText(filename) - - try: - fh = open(path, 'r') - data = fh.read() - fh.close() - - scenario = Scenario.read_from_string(data) - except etree.XMLSyntaxError as e: - errorMsg = 'Syntax Error: {}'.format(e) - QMessageBox.warning(self, 'CommonRoad XML error', 'There was an error during the loading of the selected CommonRoad file.\n\n{}'.format(errorMsg), QMessageBox.Ok) - return - except Exception as e: - errorMsg = 'Unknown Error: {}'.format(e) - QMessageBox.warning(self, 'CommonRoad XML error', 'There was an error during the loading of the selected CommonRoad file.\n\n{}'.format(errorMsg), QMessageBox.Ok) - return - - self.openScenario(scenario) - - def openScenario(self, scenario): - self.current_scenario = scenario - self.update_plot() - - @pyqtSlot() - def onClickLanelet(self): - self.dynamic.clear_axes() - - selectedLanelets = self.laneletsList.selectedItems() - - if not selectedLanelets: - self.selected_lanelet_id = None - return - - self.selected_lanelet_id = int(selectedLanelets[0].text()) - self.update_plot() - - def update_plot(self): - - if self.current_scenario is None: - self.dynamic.clear_axes() - return - - try: - selected_lanelet = self.current_scenario.lanelet_network.find_lanelet_by_id(self.selected_lanelet_id) - - except ScenarioError: - selected_lanelet = None - - ax = self.dynamic.get_axes() - - xlim1 = float('Inf') - xlim2 = -float('Inf') - - ylim1 = float('Inf') - ylim2 = -float('Inf') - - for lanelet in self.current_scenario.lanelet_network.lanelets: - - # Selected lanelet - if selected_lanelet is None: - color = 'gray' - alpha = 0.3 - zorder = 0 - label = None - elif lanelet.lanelet_id == selected_lanelet.lanelet_id: - color = 'red' - alpha = 0.7 - zorder = 10 - label = '{} selected'.format(lanelet.lanelet_id) - elif lanelet.lanelet_id in selected_lanelet.predecessor: - color = 'blue' - alpha = 0.5 - zorder = 5 - label = '{} predecessor of {}'.format(lanelet.lanelet_id, selected_lanelet.lanelet_id) - elif lanelet.lanelet_id in selected_lanelet.successor: - color = 'green' - alpha = 0.5 - zorder = 5 - label = '{} successor of {}'.format(lanelet.lanelet_id, selected_lanelet.lanelet_id) - elif lanelet.lanelet_id == selected_lanelet.adj_left: - color = 'yellow' - alpha = 0.5 - zorder = 5 - label = '{} adj left of {} ({})'.format(lanelet.lanelet_id, selected_lanelet.lanelet_id, "same" if selected_lanelet.adj_left_same_direction else "opposite") - elif lanelet.lanelet_id == selected_lanelet.adj_right: - color = 'orange' - alpha = 0.5 - zorder = 5 - label = '{} adj right of {} ({})'.format(lanelet.lanelet_id, selected_lanelet.lanelet_id, "same" if selected_lanelet.adj_right_same_direction else "opposite") - else: - color = 'gray' - alpha = 0.3 - zorder = 0 - label = None - - - verts = [] - codes = [Path.MOVETO] - - # TODO efficiency - for x, y in np.vstack([lanelet.left_vertices, lanelet.right_vertices[::-1]]): - verts.append([x, y]) - codes.append(Path.LINETO) - - xlim1 = min(xlim1, x) - xlim2 = max(xlim2, x) - - ylim1 = min(ylim1, y) - ylim2 = max(ylim2, y) - - verts.append(verts[0]) - codes[-1] = Path.CLOSEPOLY - - path = Path(verts, codes) - - ax.add_patch(PathPatch(path, facecolor=color, edgecolor="black", lw=0.0, alpha=alpha, zorder=zorder, label=label)) - - ax.plot([x for x, y in lanelet.left_vertices], [y for x, y in lanelet.left_vertices], color="black", lw=0.1) - ax.plot([x for x, y in lanelet.right_vertices], [y for x, y in lanelet.right_vertices], color="black", lw=0.1) - - handles, labels = self.dynamic.get_axes().get_legend_handles_labels() - self.dynamic.get_axes().legend(handles, labels) - - self.dynamic.get_axes().set_xlim([xlim1, xlim2]) - self.dynamic.get_axes().set_ylim([ylim1, ylim2]) - - self.dynamic.update_plot() - - - self.laneletsList.setRowCount(len(self.current_scenario.lanelet_network.lanelets)) - self.laneletsList.setColumnCount(2) - - for idx, lanelet in enumerate(self.current_scenario.lanelet_network.lanelets): - self.laneletsList.setItem(idx - 1, 0, QTableWidgetItem("{}".format(lanelet.lanelet_id))) - self.laneletsList.setItem(idx - 1, 1, QTableWidgetItem("{}".format(lanelet.description))) - - def testCmd(self): - self.dynamic.fig.savefig("foo.pdf", bbox_inches='tight') - - - -if __name__ == '__main__': - # Make it possible to exit application with ctrl+c on console - signal.signal(signal.SIGINT, signal.SIG_DFL) - - # Startup application - app = QApplication(sys.argv) - - if len(sys.argv) >= 2: - ex = MainWindow(path=sys.argv[1]) - else: - ex = MainWindow() - - sys.exit(app.exec_()) diff --git a/src/aadcUserPython/opendrive/opendrive2lanelets-converter b/src/aadcUserPython/opendrive/opendrive2lanelets-converter new file mode 160000 index 0000000..8f547c3 --- /dev/null +++ b/src/aadcUserPython/opendrive/opendrive2lanelets-converter @@ -0,0 +1 @@ +Subproject commit 8f547c35cf6d2abf74ecd9818cf52527dede91cc diff --git a/src/aadcUserPython/opendrive/openDrive2csv/testEventMap.png b/src/aadcUserPython/opendrive/testEventMap.png similarity index 100% rename from src/aadcUserPython/opendrive/openDrive2csv/testEventMap.png rename to src/aadcUserPython/opendrive/testEventMap.png diff --git a/src/aadcUserPython/opendrive/openDrive2csv/testevent_map_bitmap.png b/src/aadcUserPython/opendrive/testevent_map_bitmap.png similarity index 100% rename from src/aadcUserPython/opendrive/openDrive2csv/testevent_map_bitmap.png rename to src/aadcUserPython/opendrive/testevent_map_bitmap.png diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/trajectoryArray.npz b/src/aadcUserPython/opendrive/trajectoryArray.npz similarity index 100% rename from src/aadcUserPython/opendrive/opendrive2lanelet/trajectoryArray.npz rename to src/aadcUserPython/opendrive/trajectoryArray.npz diff --git a/src/aadcUserPython/opendrive/opendrive2lanelet/asdf.png b/src/aadcUserPython/opendrive/visualization1.png similarity index 100% rename from src/aadcUserPython/opendrive/opendrive2lanelet/asdf.png rename to src/aadcUserPython/opendrive/visualization1.png diff --git a/src/aadcUserPython/opendrive/openDrive2csv/asdf.png b/src/aadcUserPython/opendrive/visualization2.png similarity index 100% rename from src/aadcUserPython/opendrive/openDrive2csv/asdf.png rename to src/aadcUserPython/opendrive/visualization2.png