def test_SimpleCurve(self):
roads = []
roads.append(
self.curveRoadBuilder.create(0,
angleBetweenEndpoints=np.pi / 2,
curvature=0.05,
isLeftTurnLane=True))
odrName = "test_LeftTurnLaneCurve"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
xmlPath = f"output/test_LeftTurnLaneCurve.xodr"
odr.write_xml(xmlPath)
roads = []
roads.append(
self.curveRoadBuilder.create(0,
angleBetweenEndpoints=np.pi / 2,
curvature=0.05,
isRightTurnLane=True))
odrName = "test_RightTurnLaneCurve"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
xmlPath = f"output/test_RightTurnLaneCurve.xodr"
odr.write_xml(xmlPath)
def test_DifferentCPs(self):
# same cps
roads = []
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(0,
length=20,
junction=-1,
n_lanes_left=2,
n_lanes_right=1))
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(1,
length=10,
junction=-1,
n_lanes_left=1,
n_lanes_right=2))
roads[0].addExtendedSuccessor(roads[1], 0, pyodrx.ContactPoint.start)
roads[1].addExtendedPredecessor(roads[0], 0, pyodrx.ContactPoint.start)
odrName = "test_DifferentCPs1"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
xmlPath = f"output/test_DifferentCPs1.xodr"
odr.write_xml(xmlPath)
# same cps 2
roads = []
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(0,
length=20,
junction=-1,
n_lanes_left=2,
n_lanes_right=1))
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(1,
length=10,
junction=-1,
n_lanes_left=2,
n_lanes_right=1))
roads[0].addExtendedSuccessor(roads[1], 0, pyodrx.ContactPoint.start)
roads[1].addExtendedPredecessor(roads[0], 0, pyodrx.ContactPoint.end)
odrName = "test_DifferentCPs2"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
xmlPath = f"output/test_DifferentCPs2.xodr"
odr.write_xml(xmlPath)
def test_RightLane(self):
# test scenario for connection road
roads = []
roads.append(pyodrx.create_straight_road(0, 10))
# roads.append(self.roadBuilder.createSimpleCurve(1, np.pi/4, True, curvature = 0.2))
# roads.append(pyodrx.create_straight_road(2, 10))
# roads[0].add_successor(pyodrx.ElementType.junction,1)
# roads[1].add_predecessor(pyodrx.ElementType.road,0,pyodrx.ContactPoint.end)
# # roads[1].add_predecessor(pyodrx.ElementType.road,0,pyodrx.ContactPoint.start)
# roads[1].add_successor(pyodrx.ElementType.road,2,pyodrx.ContactPoint.start)
# roads[2].add_predecessor(pyodrx.ElementType.junction,1, pyodrx.ContactPoint.end)
odrName = "test_connectionRoad"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
self.laneBuilder.addRightTurnLaneUS(roads[0], 3)
# self.laneBuilder.addRightLaneUS(roads[1])
odr.resetAndReadjust(byPredecessor=True)
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
xmlPath = f"output/test-RightLane.xodr"
odr.write_xml(xmlPath)
def test_normalRoadLinks(self):
roads = []
roads.append(self.straightRoadBuilder.create(0))
roads.append(
self.roadBuilder.curveBuilder.createSimple(1,
np.pi / 4,
False,
curvature=0.2))
roads.append(self.straightRoadBuilder.create(2))
RoadLinker.createExtendedPredSuc(predRoad=roads[0],
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[1],
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=roads[1],
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[2],
sucCP=pyodrx.ContactPoint.start)
odrName = "test_connectionRoad"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
# self.laneBuilder.addRightTurnLaneUS(roads[0], 3)
# self.laneBuilder.addRightLaneUS(roads[1])
# odr.resetAndReadjust(byPredecessor=True)
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
def test_DifferentLaneConfigurations(self):
roads = []
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(0,
10,
n_lanes_left=1,
n_lanes_right=1))
connectionRoad = self.straightRoadBuilder.createWithDifferentLanes(
1, 10, n_lanes_left=2, n_lanes_right=2)
roads.append(connectionRoad)
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(2,
10,
n_lanes_left=1,
n_lanes_right=2))
roads[0].addExtendedSuccessor(roads[1], 0, pyodrx.ContactPoint.start)
roads[1].addExtendedPredecessor(roads[0], 0, pyodrx.ContactPoint.end)
roads[1].addExtendedSuccessor(roads[2], 0, pyodrx.ContactPoint.start)
roads[2].addExtendedPredecessor(roads[1], 0, pyodrx.ContactPoint.end)
self.laneBuilder.createLanesForConnectionRoad(connectionRoad, roads[0],
roads[2])
odrName = "test_DifferentLaneConfigurations"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
xmlPath = f"output/test_DifferentLaneConfigurations.xodr"
odr.write_xml(xmlPath)
def test_getMaxCurvatureMaxRoadWidth(self):
roads = []
numberofLanes = 1
laneOffset = 3
angle = np.pi * .75
maxCurve = AngleCurvatureMap.getMaxCurvatureAgainstMaxRoadWidth(
angle, numberofLanes * laneOffset)
curve = maxCurve * 1.1
print(f"max curve {maxCurve}, current curve {curve}")
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(
0,
length=10,
junction=-1,
n_lanes_left=numberofLanes,
n_lanes_right=numberofLanes))
roads.append(
self.curveRoadBuilder.createSimpleCurveWithLongArc(
1,
angleBetweenEndpoints=angle,
curvature=curve,
isJunction=True,
n_lanes=numberofLanes))
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(
2,
length=10,
junction=-1,
n_lanes_left=numberofLanes,
n_lanes_right=numberofLanes))
RoadLinker.createExtendedPredSuc(predRoad=roads[0],
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[1],
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=roads[1],
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[2],
sucCP=pyodrx.ContactPoint.start)
odrName = "test_createSingleLaneConnectionRoad"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
# extensions.printRoadPositions(odr)
road = roads[1]
print(f"{road.id} has length {road.planview.get_total_length()}")
for geom in road.planview._adjusted_geometries:
print(geom.length)
extensions.view_road(odr, os.path.join('..', self.esminipath))
def test_addMedianIslandsTo3Sections(self):
road = self.straightRoadBuilder.create(1,
n_lanes_left=1,
n_lanes_right=1,
length=20,
force3Section=False)
try:
self.laneBuilder.addMedianIslandsTo2Of3Sections(
road, 20, skipEndpoint=pyodrx.ContactPoint.start, width=3)
assert False
except:
assert True
road = self.straightRoadBuilder.create(1,
n_lanes_left=1,
n_lanes_right=1,
length=20,
force3Section=True)
self.laneBuilder.addMedianIslandsTo2Of3Sections(
road, 20, skipEndpoint=pyodrx.ContactPoint.start, width=3)
assert len(road.lanes.lanesections[0].leftlanes) == 1
assert len(road.lanes.lanesections[0].rightlanes) == 1
assert len(road.lanes.lanesections[1].leftlanes) == 2
assert len(road.lanes.lanesections[1].rightlanes) == 2
assert len(road.lanes.lanesections[2].leftlanes) == 2
assert len(road.lanes.lanesections[2].rightlanes) == 2
road = self.straightRoadBuilder.create(1,
n_lanes_left=1,
n_lanes_right=1,
length=20,
force3Section=True)
self.laneBuilder.addMedianIslandsTo2Of3Sections(
road, 20, skipEndpoint=pyodrx.ContactPoint.end, width=3)
assert len(road.lanes.lanesections[0].leftlanes) == 2
assert len(road.lanes.lanesections[0].rightlanes) == 2
assert len(road.lanes.lanesections[1].leftlanes) == 2
assert len(road.lanes.lanesections[1].rightlanes) == 2
assert len(road.lanes.lanesections[2].leftlanes) == 1
assert len(road.lanes.lanesections[2].rightlanes) == 1
odrName = "test_DifferentLaneConfigurations"
odr = extensions.createOdrByPredecessor(odrName, [road], [])
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
xmlPath = f"output/test_addMedianIslandsTo3Sections.xodr"
odr.write_xml(xmlPath)
def test_addMedianIslandsToAllSections(self):
roads = []
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(0,
10,
n_lanes_left=1,
n_lanes_right=1))
self.laneBuilder.addMedianIslandsToAllSections(
roads[0], self.configuration.get('default_lane_width'))
odrName = "test_DifferentLaneConfigurations"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
xmlPath = f"output/test_addMedianIslandsToAllSections.xodr"
odr.write_xml(xmlPath)
def test_getLanePosition(self):
roads = []
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(0,
length=10,
junction=-1,
n_lanes_left=2,
n_lanes_right=2))
roads.append(
self.straightRoadBuilder.createWithRightTurnLanesOnLeft(
1,
length=10,
n_lanes=2,
junction=1,
isLeftTurnLane=True,
isRightTurnLane=True,
numberOfRightTurnLanesOnLeft=2,
mergeLaneOnTheOppositeSideForInternalTurn=False))
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(2,
length=10,
junction=-1,
n_lanes_left=5,
n_lanes_right=3))
self.roadLinker.linkConsecutiveRoadsWithNoBranches(roads)
roads[1].updatePredecessorOffset(-2)
odrName = "test_getBorderDistanceOfLane"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
extensions.printRoadPositions(odr)
print(roads[0].getLanePosition(0, pyodrx.ContactPoint.end))
print(roads[0].getLanePosition(1, pyodrx.ContactPoint.end))
print(roads[0].getLanePosition(2, pyodrx.ContactPoint.end))
positionLeftMost = roads[0].getLanePosition(2, pyodrx.ContactPoint.end)
assert positionLeftMost[0] == 10.0
assert positionLeftMost[1] == 6.0
assert positionLeftMost[2] == 0
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
def test_StartEnd(self):
roads = []
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(0,
10,
n_lanes_left=1,
n_lanes_right=1))
connectionRoad = self.curveBuilder.create(1,
np.pi / 4,
isJunction=True,
curvature=0.1,
n_lanes=1)
roads.append(connectionRoad)
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(2,
10,
n_lanes_left=1,
n_lanes_right=1))
roads[0].addExtendedSuccessor(roads[1], 0, pyodrx.ContactPoint.start)
roads[1].addExtendedPredecessor(roads[0], 0, pyodrx.ContactPoint.start)
roads[1].addExtendedSuccessor(roads[2], 0, pyodrx.ContactPoint.start)
roads[2].addExtendedPredecessor(roads[1], 0, pyodrx.ContactPoint.end)
self.laneBuilder.createLanesForConnectionRoad(connectionRoad, roads[0],
roads[2])
odrName = "test_StartEnd"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
print(
math.degrees(
self.roadLinker.getAngleBetweenStraightRoads(
roads[0], roads[2])))
xmlPath = f"output/test_StartEnd.xodr"
odr.write_xml(xmlPath)
def test_getBorderDistanceOfLane(self):
roads = []
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(0,
length=10,
junction=-1,
n_lanes_left=1,
n_lanes_right=1))
roads.append(
self.straightRoadBuilder.createWithRightTurnLanesOnLeft(
1,
length=10,
n_lanes=1,
junction=1,
isLeftTurnLane=True,
isRightTurnLane=True,
numberOfRightTurnLanesOnLeft=2,
mergeLaneOnTheOppositeSideForInternalTurn=False))
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(2,
length=10,
junction=-1,
n_lanes_left=4,
n_lanes_right=2))
self.roadLinker.linkConsecutiveRoadsWithNoBranches(roads)
assert roads[0].getBorderDistanceOfLane(1,
pyodrx.ContactPoint.start) == 3
assert roads[0].getBorderDistanceOfLane(-1,
pyodrx.ContactPoint.start) == 3
assert roads[1].getBorderDistanceOfLane(1,
pyodrx.ContactPoint.start) == 3
assert roads[1].getBorderDistanceOfLane(2,
pyodrx.ContactPoint.end) == 6
assert roads[1].getBorderDistanceOfLane(3,
pyodrx.ContactPoint.end) == 9
assert roads[1].getBorderDistanceOfLane(4,
pyodrx.ContactPoint.end) == 12
assert roads[1].getBorderDistanceOfLane(-1,
pyodrx.ContactPoint.start) == 3
roads[1].updatePredecessorOffset(-1)
odrName = "test_getBorderDistanceOfLane"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
xmlPath = f"output/test_getBorderDistanceOfLane.xodr"
odr.write_xml(xmlPath)
roads = []
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(0,
length=10,
junction=-1,
n_lanes_left=2,
n_lanes_right=2))
roads.append(
self.straightRoadBuilder.createWithRightTurnLanesOnLeft(
1,
length=10,
n_lanes=2,
junction=1,
isLeftTurnLane=True,
isRightTurnLane=True,
numberOfRightTurnLanesOnLeft=2,
mergeLaneOnTheOppositeSideForInternalTurn=False))
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(2,
length=10,
junction=-1,
n_lanes_left=5,
n_lanes_right=3))
self.roadLinker.linkConsecutiveRoadsWithNoBranches(roads)
roads[1].updatePredecessorOffset(-2)
odrName = "test_getBorderDistanceOfLane"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
xmlPath = f"output/test_getBorderDistanceOfLane.xodr"
odr.write_xml(xmlPath)
def buildSimpleRoundAbout(self,
odrId=0,
numRoads=4,
radius=10,
cp1=pyodrx.ContactPoint.start,
direction=CircularDirection.COUNTERCLOCK_WISE):
"""In a simple roundabout, there is a circle inside the junction, the connection roads reside in the circle.
Args:
numRoads (int, optional): [description]. Defaults to 4.
radius : in meters.
cp1: contact point on the first road.
"""
anglePerRoad = (np.pi * 2) / numRoads
roads = []
roads.append(
self.straightRoadBuilder.create(0, length=self.straightRoadLen))
nextRoadId = 1
roadsCreated = 1
connectionSeres = [
] # holds all the connection road series so that we can create internal connections later.
while roadsCreated < numRoads:
previousRoadId = nextRoadId - 1
newConnectionId = nextRoadId
# 1. create a new connection road series and increase nextRoadId
newConnectionSeries = self.roadBuilder.createRoundAboutConnection(
newConnectionId, anglePerRoad, radius)
connectionSeres.append(newConnectionSeries)
nextRoadId += newConnectionSeries.length()
newRoadId = nextRoadId
nextRoadId += 1
# 2. create a road
newRoad = self.straightRoadBuilder.create(
newRoadId, length=self.straightRoadLen)
# 3 add new roads
roads += newConnectionSeries.getAll()
roads.append(newRoad)
roads[previousRoadId].addExtendedSuccessor(
newConnectionSeries.getFirst(), 0, pyodrx.ContactPoint.start)
if newConnectionSeries.getFirst().id == 1:
newConnectionSeries.getFirst().addExtendedPredecessor(
roads[previousRoadId], 0, cp1)
# TODO this is a hack. It will not eventually work because outgoing roads' ends will come to join other junctions.
else:
newConnectionSeries.getFirst().addExtendedPredecessor(
roads[previousRoadId], 0, pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(
predRoad=newConnectionSeries.getLast(),
predCp=pyodrx.ContactPoint.end,
sucRoad=newRoad,
sucCP=pyodrx.ContactPoint.start)
# 6 get next action
roadsCreated += 1
pass
lastRoad = roads[-1]
# 3. create connections and junction
junction = self.createJunctionForASeriesOfRoads(roads)
# print(f"number of roads created {len(roads)}")
odrName = 'Simple-Roundabout-' + str(numRoads) + '_L2_' + str(odrId)
odr = extensions.createOdrByPredecessor(odrName, roads, [junction])
# The last connection and resetting odr
finalConnectionSeries = self.roadBuilder.createRoundAboutConnection(
nextRoadId, anglePerRoad, radius)
connectionSeres.append(finalConnectionSeries)
roads += finalConnectionSeries.getAll()
RoadLinker.createExtendedPredSuc(
predRoad=lastRoad,
predCp=pyodrx.ContactPoint.start,
sucRoad=finalConnectionSeries.getFirst(),
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(
predRoad=finalConnectionSeries.getLast(),
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[0],
sucCP=cp1)
odr.updateRoads(roads)
odr.resetAndReadjust(byPredecessor=True)
# Last step, link connection series by curves
self.createInternalConnectionsForConnectionSeres(
roads, connectionSeres, junction)
odr.updateRoads(roads)
odr.resetAndReadjust(byPredecessor=True)
return odr
def test_createSingleLaneConnectionRoad(self):
roads = []
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(0,
length=10,
junction=-1,
n_lanes_left=2,
n_lanes_right=2))
roads.append(
self.curveRoadBuilder.createSimple(1,
np.pi / 3,
isJunction=True,
n_lanes=2))
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(2,
length=10,
junction=-1,
n_lanes_left=2,
n_lanes_right=2))
RoadLinker.createExtendedPredSuc(predRoad=roads[0],
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[1],
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=roads[1],
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[2],
sucCP=pyodrx.ContactPoint.start)
odrName = "test_createSingleLaneConnectionRoad"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
# extensions.view_road(odr, os.path.join('..', self.configuration.get("esminipath")))
newConnection = self.connectionBuilder.createSingleLaneConnectionRoad(
3, roads[0], roads[2], -2, -2, pyodrx.ContactPoint.end,
pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=roads[0],
predCp=pyodrx.ContactPoint.end,
sucRoad=newConnection,
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=newConnection,
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[2],
sucCP=pyodrx.ContactPoint.start)
newConnection.updatePredecessorOffset(-1)
roads.append(newConnection)
odr.add_road(newConnection)
newConnection = self.connectionBuilder.createSingleLaneConnectionRoad(
4, roads[2], roads[0], 2, 2, pyodrx.ContactPoint.start,
pyodrx.ContactPoint.end)
RoadLinker.createExtendedPredSuc(predRoad=roads[2],
predCp=pyodrx.ContactPoint.start,
sucRoad=newConnection,
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=newConnection,
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[0],
sucCP=pyodrx.ContactPoint.end)
newConnection.updatePredecessorOffset(1)
roads.append(newConnection)
odr.add_road(newConnection)
odr.resetAndReadjust(byPredecessor=True)
extensions.printRoadPositions(odr)
extensions.view_road(odr, os.path.join('..', self.esminipath))
xmlPath = f"output/test_createSingleLaneConnectionRoad.xodr"
odr.write_xml(xmlPath)
def test_getCurvatureForAngleAndLength(self):
numberofLanes = 5
laneOffset = 3
# angle = np.pi * .75
# angle = np.pi * (5/6)
# length = 15
# maxCurve = AngleCurvatureMap.getMaxCurvatureMaxRoadWidth(angle, numberofLanes * laneOffset)
# curve = AngleCurvatureMap.getCurvatureForAngleBetweenRoadAndLength(angle, length, StandardCurveTypes.Simple)
# # curve = 0.066666667
# print(f"max curve {maxCurve}, current curve {curve}")
# roads = []
# roads.append(self.straightRoadBuilder.createWithDifferentLanes(0, length=10, junction=-1, n_lanes_left=numberofLanes, n_lanes_right=numberofLanes))
# # roads.append(self.curveRoadBuilder.createSimpleCurveWithLongArc(1, angleBetweenEndpoints = angle, curvature=curve, isJunction=True, n_lanes=numberofLanes))
# roads.append(self.curveRoadBuilder.createSimple(1, angleBetweenEndpoints = angle, curvature=curve, isJunction=True, n_lanes=numberofLanes))
# roads.append(self.straightRoadBuilder.createWithDifferentLanes(2, length=10, junction=-1, n_lanes_left=numberofLanes, n_lanes_right=numberofLanes))
# RoadLinker.createExtendedPredSuc(predRoad=roads[0], predCp=pyodrx.ContactPoint.end, sucRoad=roads[1], sucCP=pyodrx.ContactPoint.start)
# RoadLinker.createExtendedPredSuc(predRoad=roads[1], predCp=pyodrx.ContactPoint.end, sucRoad=roads[2], sucCP=pyodrx.ContactPoint.start)
# odrName = "test_createSingleLaneConnectionRoad"
# odr = extensions.createOdrByPredecessor(odrName, roads, [])
# # extensions.printRoadPositions(odr)
# road = roads[1]
# print(f"{road.id} has length {road.planview.get_total_length()}")
# # for geom in road.planview._adjusted_geometries:
# # print(geom.length)
# assert round(road.planview.get_total_length(), 0) == length
# extensions.view_road(odr, os.path.join('..', self.esminipath))
angle = 0.5
while angle < np.pi:
length = 100
while length > 0:
curve = AngleCurvatureMap.getCurvatureForAngleBetweenRoadAndLength(
angle, length, StandardCurveTypes.Simple)
roads = []
curve = AngleCurvatureMap.getCurvatureForAngleBetweenRoadAndLength(
angle, length, StandardCurveTypes.Simple)
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(
0,
length=10,
junction=-1,
n_lanes_left=numberofLanes,
n_lanes_right=numberofLanes))
roads.append(
self.curveRoadBuilder.createSimple(
1,
angleBetweenEndpoints=angle,
curvature=curve,
isJunction=True,
n_lanes=numberofLanes))
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(
2,
length=10,
junction=-1,
n_lanes_left=numberofLanes,
n_lanes_right=numberofLanes))
RoadLinker.createExtendedPredSuc(
predRoad=roads[0],
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[1],
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(
predRoad=roads[1],
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[2],
sucCP=pyodrx.ContactPoint.start)
odrName = "test_createSingleLaneConnectionRoad"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
road = roads[1]
assert round(road.planview.get_total_length(), 0) == length
curve = AngleCurvatureMap.getCurvatureForAngleBetweenRoadAndLength(
angle, length, StandardCurveTypes.Simple)
roads = []
curve = AngleCurvatureMap.getCurvatureForAngleBetweenRoadAndLength(
angle, length, StandardCurveTypes.Simple)
roads.append(
self.curveRoadBuilder.createSimpleCurveWithLongArc(
1,
angleBetweenEndpoints=angle,
curvature=curve,
isJunction=True,
n_lanes=numberofLanes))
roads.append(
self.curveRoadBuilder.createSimple(
1,
angleBetweenEndpoints=angle,
curvature=curve,
isJunction=True,
n_lanes=numberofLanes))
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(
2,
length=10,
junction=-1,
n_lanes_left=numberofLanes,
n_lanes_right=numberofLanes))
RoadLinker.createExtendedPredSuc(
predRoad=roads[0],
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[1],
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(
predRoad=roads[1],
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[2],
sucCP=pyodrx.ContactPoint.start)
odrName = "test_createSingleLaneConnectionRoad"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
road = roads[1]
assert round(road.planview.get_total_length(), 0) == length
length -= 10
angle += 0.25
def harvestUS(self, n_lanes_left=2, n_lanes_right=2, show=False):
# incoming lanes in a junction are right lanes if end point is connected, left lanes if start point is connected.
# 5x5x5x5 for 2, 2
print(f"harvestUS with {n_lanes_left} and {n_lanes_right}")
odrs = []
# now iterate through lanes and set types.
leftCombinations = self.getLaneTurnCombinations(n_lanes_left)
rightCombinations = self.getLaneTurnCombinations(n_lanes_right)
# for each combinations on left and right, create a new road
for leftComb in leftCombinations:
for rightComb in rightCombinations:
road = self.straightRoadBuilder.createWithDifferentLanes(self.lastId, length=self.straightRoadLen, n_lanes_left=n_lanes_left, n_lanes_right=n_lanes_right)
# right lanes, change last lane secion
# left lanes, change first lane section.
laneSectionForLeft = road.getFirstLaneSection()
laneSectionForRight = road.getLastLaneSection()
self.applyTurnCombinationOnLanes(laneSectionForLeft.leftlanes, leftComb)
self.applyTurnCombinationOnLanes(laneSectionForRight.rightlanes, rightComb)
name = f"straightRoad-{self.lastId}"
self.lastId += 1
odr = extensions.createOdrByPredecessor(name, roads=[road], junctions=[])
# 1. save the xml file
fname = odr.name
xmlPath = self.getOutputPath(fname)
odr.write_xml(xmlPath)
# 2. save image
if self.saveImage is True:
extensions.saveRoadImageFromFile(xmlPath, self.esminiPath)
if show:
extensions.view_road(odr, os.path.join('..', self.esminiPath))
odrs.append(odr)
# handle cases where we dont need right lanes
if n_lanes_right == 0:
for leftComb in leftCombinations:
road = self.straightRoadBuilder.createWithDifferentLanes(self.lastId, length=self.straightRoadLen, n_lanes_left=n_lanes_left, n_lanes_right=n_lanes_right)
# right lanes, change last lane secion
# left lanes, change first lane section.
laneSectionForLeft = road.getFirstLaneSection()
self.applyTurnCombinationOnLanes(laneSectionForLeft.leftlanes, leftComb)
name = f"straightRoad-{self.lastId}"
self.lastId += 1
odr = extensions.createOdrByPredecessor(name, roads=[road], junctions=[])
# 1. save the xml file
fname = odr.name
xmlPath = self.getOutputPath(fname)
odr.write_xml(xmlPath)
# 2. save image
if self.saveImage is True:
extensions.saveRoadImageFromFile(xmlPath, self.esminiPath)
if show:
extensions.view_road(odr, os.path.join('..', self.esminiPath))
odrs.append(odr)
# handle cases where we dont need left lanes
if n_lanes_left == 0:
for rightComb in rightCombinations:
road = self.straightRoadBuilder.createWithDifferentLanes(self.lastId, length=self.straightRoadLen, n_lanes_left=n_lanes_left, n_lanes_right=n_lanes_right)
# right lanes, change last lane secion
# left lanes, change first lane section.
laneSectionForRight = road.getLastLaneSection()
self.applyTurnCombinationOnLanes(laneSectionForRight.rightlanes, rightComb)
name = f"straightRoad-{self.lastId}"
self.lastId += 1
odr = extensions.createOdrByPredecessor(name, roads=[road], junctions=[])
# 1. save the xml file
fname = odr.name
xmlPath = self.getOutputPath(fname)
odr.write_xml(xmlPath)
# 2. save image
if self.saveImage is True:
extensions.saveRoadImageFromFile(xmlPath, self.esminiPath)
if show:
extensions.view_road(odr, os.path.join('..', self.esminiPath))
odrs.append(odr)
return odrs
def createWithRandomLaneConfigurations(self,
straightRoadsPath,
id,
maxNumberOfRoadsPerJunction,
firstRoadId=0,
maxLanePerSide=2,
minLanePerSide=0,
internalConnections=True,
cp1=pyodrx.ContactPoint.end,
randomState=None,
internalLinkStrategy = LaneConfigurationStrategies.SPLIT_ANY,
uTurnLanes=1,
equalAngles=False,
getAsOdr=True):
"""All the incoming roads, except for the first, will have their start endpoint connected to the junction.
Args:
straightRoadsPath ([type]): [description]
odrId ([type]): [description]
maxNumberOfRoadsPerJunction ([type]): [description]
maxLanePerSide (int, optional): [description]. Defaults to 2.
minLanePerSide (int, optional): [description]. Defaults to 0.
internalConnections (bool, optional): [description]. Defaults to True.
cp1 ([type], optional): [description]. Defaults to pyodrx.ContactPoint.start.
randomState ([type], optional): [description]. Defaults to None.
Raises:
Exception: [description]
Returns:
[type]: [description]
"""
if maxNumberOfRoadsPerJunction < 2:
raise Exception("Please add more than 1 roads")
if uTurnLanes > 1:
raise Exception("U-turn from more than one lanes is not implemented")
harvestedStraightRoads = []
# if restrictedLanes:
# harvestedStraightRoads = []
# else:
# harvestedStraightRoads = extensions.getObjectsFromDill(straightRoadsPath)
if randomState is not None:
np.random.set_state(randomState)
incidentContactPoints = []
outsideRoads = [] # all the incoming/outgoing roads in this junction
geoConnectionRoads = [] # connections roads which are for geometric positions, having no lanes
laneConnectionRoads = [] # connection roads that have lanes.
roads = []
if cp1 == pyodrx.ContactPoint.end:
roads.append(self.createRandomStraightRoad(0, maxLanePerSide=maxLanePerSide, minLanePerSide=minLanePerSide, skipEndpoint=pyodrx.ContactPoint.start)) # first road
else:
roads.append(self.createRandomStraightRoad(0, maxLanePerSide=maxLanePerSide, minLanePerSide=minLanePerSide, skipEndpoint=pyodrx.ContactPoint.end)) # first road
# if restrictedLanes:
# if cp1 == pyodrx.ContactPoint.end:
# roads.append(self.createRandomStraightRoad(0, maxLanePerSide=maxLanePerSide, minLanePerSide=minLanePerSide, skipEndpoint=pyodrx.ContactPoint.start)) # first road
# else:
# roads.append(self.createRandomStraightRoad(0, maxLanePerSide=maxLanePerSide, minLanePerSide=minLanePerSide, skipEndpoint=pyodrx.ContactPoint.end)) # first road
# else:
# roads.append(self.getRandomHarvestedStraightRoad(0, harvestedStraightRoads, maxLanePerSide, minLanePerSide)) # first road
roads[0].id = firstRoadId
outsideRoads.append(roads[0])
incidentContactPoints.append(cp1)
availableAngle = 1.8 * np.pi # 360 degrees
if equalAngles:
availableAngle = np.pi * 2
maxAnglePerConnection = availableAngle / maxNumberOfRoadsPerJunction
action = self.actionAfterDrawingOne(roads, availableAngle, maxNumberOfRoadsPerJunction)
nextRoadId = firstRoadId + 1
otherContactPoints = pyodrx.ContactPoint.start
nIncidentAdded = 1
while (action != "end"):
logging.debug(f"{self.name}: availableAngle {math.degrees(availableAngle)}, number of roads: {len(roads) / 2}")
# 0. road id generation
previousRoadId = nextRoadId - 1
prevIncidentRoad = roads[-1]
newConnectionId = nextRoadId
nextRoadId += 1
newRoadId = nextRoadId
nextRoadId += 1
prevCp = otherContactPoints
if len(roads) == 1: # first road
prevCp = cp1
# 1. create a road
# newRoad = self.getRandomHarvestedStraightRoad(newRoadId, harvestedStraightRoads, maxLanePerSide, minLanePerSide)
newRoad = self.createRandomStraightRoad(newRoadId, maxLanePerSide, minLanePerSide, skipEndpoint=pyodrx.ContactPoint.end)
# if restrictedLanes:
# newRoad = self.createRandomStraightRoad(newRoadId, maxLanePerSide, minLanePerSide, skipEndpoint=pyodrx.ContactPoint.end)
# else:
# newRoad = self.getRandomHarvestedStraightRoad(newRoadId, harvestedStraightRoads, maxLanePerSide, minLanePerSide)
outsideRoads.append(newRoad)
incidentContactPoints.append(otherContactPoints)
# 2. create a new connection road
prevLanes, nextLanes = self.laneBuilder.getClockwiseAdjacentLanes(prevIncidentRoad, prevCp, newRoad, otherContactPoints)
# maxLaneWidth = ((len(prevLanes) + len(nextLanes)) * self.laneWidth) / 2
maxLaneWidth = max(len(prevLanes), len(nextLanes)) * self.laneWidth
if len(prevLanes) == 0 or len(nextLanes) == 0:
maxLaneWidth = ((len(prevLanes) + len(nextLanes)) * self.laneWidth) / 2
availableAngle -= (maxNumberOfRoadsPerJunction - nIncidentAdded - 1) * self.minAngle
# print(f"Before connection road: minAngle: remaining roads:{maxNumberOfRoadsPerJunction - nIncidentAdded - 1}, {math.degrees(self.minAngle)}, available Angle: {math.degrees(availableAngle)}")
newConnection, availableAngle = self.createGeoConnectionRoad(action, newConnectionId, availableAngle, maxAnglePerConnection, maxLaneWidth=maxLaneWidth, equalAngles=equalAngles)
geoConnectionRoads.append(newConnection)
availableAngle += (maxNumberOfRoadsPerJunction - nIncidentAdded - 1) * self.minAngle
# print(f"after connection road: minAngle: {math.degrees(self.minAngle)}, available Angle: {math.degrees(availableAngle)}")
# 5 add new roads
roads.append(newConnection)
roads.append(newRoad)
RoadLinker.createExtendedPredSuc(predRoad=prevIncidentRoad, predCp=prevCp, sucRoad=newConnection, sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=newConnection, predCp=pyodrx.ContactPoint.end, sucRoad=newRoad, sucCP=otherContactPoints)
self.laneBuilder.createLanesForConnectionRoad(newConnection, prevIncidentRoad, newRoad)
# 6 get next action
action = self.actionAfterDrawingOne(roads, availableAngle, maxNumberOfRoadsPerJunction)
nIncidentAdded += 1
pass
# 3.0 fix outgoing lane numbers
self.fixNumOutgoingLanes(outsideRoads, cp1)
# 3. create connections and junction
# TODO this is not correct anymore.
# junction = self.createJunctionForASeriesOfRoads(roads)
junction = pyodrx.Junction("singleConnectionsJunction", id)
# junction = pyodrx.Junction("singleConnectionsJunction", firstRoadId)
odrName = 'Draw_Rmax' + str(maxNumberOfRoadsPerJunction) + '_L2_' + str(id)
odr = extensions.createOdrByPredecessor(odrName, roads, [junction])
logging.debug(f"{self.name}: roads before internal connections {len(roads)}")
# Permanent connection roads
if internalConnections:
internalConnections = self.connectionBuilder.createSingleLaneConnectionRoads(nextRoadId, outsideRoads, cp1, internalLinkStrategy)
nextRoadId += len(internalConnections)
roads += internalConnections
odr.updateRoads(roads)
# remove lanes from connection roads which are used for geometric positioning of roads
for geoRoad in geoConnectionRoads:
geoRoad.clearLanes()
# TODO create the junction
self.addInternalConnectionsToJunction(junction, internalConnections)
# U-Turns
if uTurnLanes == 1:
uTurnConnections = self.connectionBuilder.createUTurnConnectionRoads(nextRoadId, outsideRoads, cp1)
nextRoadId += 1
roads += uTurnConnections
odr.updateRoads(roads)
self.addInternalConnectionsToJunction(junction, internalConnections)
logging.debug(f"{self.name}: roads after internal connections {len(roads)}")
odr.resetAndReadjust(byPredecessor=True)
if getAsOdr:
return odr
intersection = Intersection(id, outsideRoads, incidentContactPoints, geoConnectionRoads=geoConnectionRoads, odr=odr)
return intersection
def ThreeWayJunctionWithAngle(
self,
odrId,
angleBetweenRoads=np.pi / 4,
maxLanePerSide=2,
minLanePerSide=0,
internalConnections=True,
cp1=pyodrx.ContactPoint.end,
randomState=None,
internalLinkStrategy=LaneConfigurationStrategies.SPLIT_ANY,
uTurnLanes=1):
if angleBetweenRoads < np.pi / 9 or angleBetweenRoads > np.pi / 2:
raise Exception("Come up with a better angle")
outsideRoad = []
roads = []
if cp1 == pyodrx.ContactPoint.end:
firstRoad = self.createRandomStraightRoad(
0,
maxLanePerSide=maxLanePerSide,
minLanePerSide=minLanePerSide,
skipEndpoint=pyodrx.ContactPoint.start) # first road
else:
firstRoad = self.createRandomStraightRoad(
0,
maxLanePerSide=maxLanePerSide,
minLanePerSide=minLanePerSide,
skipEndpoint=pyodrx.ContactPoint.end) # first road
roads.append(firstRoad)
outsideRoad.append(firstRoad)
# second road
secondRoad = self.createRandomStraightRoad(
2,
maxLanePerSide=maxLanePerSide,
minLanePerSide=minLanePerSide,
skipEndpoint=pyodrx.ContactPoint.end)
outsideRoad.append(secondRoad)
prevLanes, nextLanes = self.laneBuilder.getClockwiseAdjacentLanes(
firstRoad=roads[0],
firstCp=cp1,
secondRoad=secondRoad,
secondCP=pyodrx.ContactPoint.start)
maxLaneWidth = self.getMaxLaneWidth(prevLanes, nextLanes)
secondConnectionRoad = self.createConnectionRoadWithAngle(
roadId=1,
angleBetweenRoads=angleBetweenRoads,
maxLaneWidth=maxLaneWidth)
# firstRoad.addExtendedSuccessor(secondConnectionRoad, 0, pyodrx.ContactPoint.start)
# secondConnectionRoad.addExtendedPredecessor(firstRoad, 0, cp1)
RoadLinker.createExtendedPredSuc(predRoad=firstRoad,
predCp=cp1,
sucRoad=secondConnectionRoad,
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=secondConnectionRoad,
predCp=pyodrx.ContactPoint.end,
sucRoad=secondRoad,
sucCP=pyodrx.ContactPoint.start)
roads.append(secondConnectionRoad)
roads.append(secondRoad)
odrName = 'ThreeWay' + 'givenAngle' + str(odrId)
odr = extensions.createOdrByPredecessor(odrName, roads, [])
# third road
thirdRoad = self.createRandomStraightRoad(
4,
maxLanePerSide=maxLanePerSide,
minLanePerSide=minLanePerSide,
skipEndpoint=pyodrx.ContactPoint.end)
outsideRoad.append(thirdRoad)
roadLen = roads[1].length()
thirdConnectionRoad = self.straightRoadBuilder.create(roadId=3,
length=roadLen)
RoadLinker.createExtendedPredSuc(predRoad=firstRoad,
predCp=cp1,
sucRoad=thirdConnectionRoad,
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=thirdConnectionRoad,
predCp=pyodrx.ContactPoint.end,
sucRoad=thirdRoad,
sucCP=pyodrx.ContactPoint.start)
roads.append(thirdConnectionRoad)
roads.append(thirdRoad)
odr.add_road(thirdConnectionRoad)
odr.add_road(thirdRoad)
odr.resetAndReadjust(byPredecessor=True)
# last connection
connectioRoadSecondAndThird = self.createConnectionFor2Roads(
nextRoadId=5,
road1=secondRoad,
road2=thirdRoad,
junction=None,
cp1=pyodrx.ContactPoint.start,
cp2=pyodrx.ContactPoint.start)
roads.append(connectioRoadSecondAndThird)
odr.add_road(connectioRoadSecondAndThird)
self.fixNumOutgoingLanes(outsideRoad, cp1)
if internalConnections:
internalConnections = self.connectionBuilder.createSingleLaneConnectionRoads(
6, outsideRoad, cp1, internalLinkStrategy)
roads += internalConnections
odr.updateRoads(roads)
odr.resetAndReadjust(byPredecessor=True)
return odr
def drawLikeAPainter2L(self, odrId, maxNumberOfRoadsPerJunction, save=True, internalConnections=True, cp1=pyodrx.ContactPoint.end):
if maxNumberOfRoadsPerJunction < 3:
raise Exception("drawLikeAPainter is not for the weak. Please add more than 3 roads")
maxLaneWidth = self.laneWidth
roads = []
roads.append(self.straightRoadBuilder.create(0, length=self.straightRoadLen * 4)) # first road
availableAngle = 1.8 * np.pi # 360 degrees
maxAnglePerConnection = availableAngle / (maxNumberOfRoadsPerJunction - 1)
action = self.actionAfterDrawingOne(roads, availableAngle, maxNumberOfRoadsPerJunction)
nextRoadId = 1
while (action != "end"):
logging.debug(f"{self.name}: availableAngle {math.degrees(availableAngle)}, number of roads: {len(roads) / 2}")
previousRoadId = nextRoadId - 1
newConnectionId = nextRoadId
nextRoadId += 1
newRoadId = nextRoadId
nextRoadId += 1
# 1. create a road
newRoad = self.straightRoadBuilder.create(newRoadId, length=self.straightRoadLen)
# 2. create a new connection road
newConnection, availableAngle = self.createGeoConnectionRoad(action, newConnectionId, availableAngle, maxAnglePerConnection, maxLaneWidth=maxLaneWidth)
# 5 add new roads and increase road id
roads.append(newConnection)
roads.append(newRoad)
# roads[previousRoadId].add_successor(pyodrx.ElementType.junction, newConnection.id)
roads[previousRoadId].addExtendedSuccessor(newConnection, 0, pyodrx.ContactPoint.start)
if newConnection.id == 1:
# TODO this is a hack. It will not eventually work because outgoing roads' ends will come to join other junctions.
newConnection.addExtendedPredecessor(roads[previousRoadId], 0 , cp1)
else:
newConnection.addExtendedPredecessor(roads[previousRoadId], 0 , pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=newConnection, predCp=pyodrx.ContactPoint.end, sucRoad=newRoad, sucCP=pyodrx.ContactPoint.start)
# 6 get next action
action = self.actionAfterDrawingOne(roads, availableAngle, maxNumberOfRoadsPerJunction)
pass
# 3. create connections and junction
junction = self.createJunctionForASeriesOfRoads(roads, odrId)
odrName = 'Draw_Rmax' + str(maxNumberOfRoadsPerJunction) + '_L2_' + str(odrId)
odr = extensions.createOdrByPredecessor(odrName, roads, [junction])
# The last connection and resetting odr
lastConnection = self.createLastConnectionForLastAndFirstRoad(nextRoadId, roads, junction, cp1=cp1)
roads.append(lastConnection)
odr.add_road(lastConnection)
logging.debug(f"{self.name}: roads before internal connections {len(roads)}")
if internalConnections:
self.createInternalConnectionsForMissingSequentialRoads(roads, junction, cp1=cp1)
odr.updateRoads(roads)
logging.debug(f"{self.name}: roads after internal connections {len(roads)}")
odr.resetAndReadjust(byPredecessor=True)
return odr
def test_getLength(self):
angleBetweenEndpoints = 1.5708
curvature = 0.333333
curveType = StandardCurveTypes.Simple
currentLength = AngleCurvatureMap.getLength(angleBetweenEndpoints,
curvature, curveType)
print(f"test length: {currentLength}")
numberofLanes = 5
laneOffset = 3
angle = angleBetweenEndpoints
length = currentLength
maxCurve = AngleCurvatureMap.getCurvatureForAngleBetweenRoadAndLength(
angle, numberofLanes * laneOffset, curveType=curveType)
curve = AngleCurvatureMap.getCurvatureForAngleBetweenRoadAndLength(
angle, length, curveType=curveType)
# curve = 0.066666667
print(f"max curve {maxCurve}, current curve {curve}")
roads = []
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(
0,
length=10,
junction=-1,
n_lanes_left=numberofLanes,
n_lanes_right=numberofLanes))
# roads.append(self.curveRoadBuilder.createSimpleCurveWithLongArc(1, angleBetweenEndpoints = angle, curvature=curve, isJunction=True, n_lanes=numberofLanes))
roads.append(
self.curveRoadBuilder.createSimple(1,
angleBetweenEndpoints=angle,
curvature=curve,
isJunction=True,
n_lanes=numberofLanes))
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(
2,
length=10,
junction=-1,
n_lanes_left=numberofLanes,
n_lanes_right=numberofLanes))
RoadLinker.createExtendedPredSuc(predRoad=roads[0],
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[1],
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=roads[1],
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[2],
sucCP=pyodrx.ContactPoint.start)
odrName = "test_createSingleLaneConnectionRoad"
odr = extensions.createOdrByPredecessor(odrName, roads, [])
# extensions.printRoadPositions(odr)
road = roads[1]
print(f"{road.id} has length {road.planview.get_total_length()}")
# for geom in road.planview._adjusted_geometries:
# print(geom.length)
assert round(road.planview.get_total_length(), 0) == round(length, 0)