def createLastConnectionForLastAndFirstRoad(self,
nextRoadId,
roads,
junction,
cp1=pyodrx.ContactPoint.end,
cp2=pyodrx.ContactPoint.start):
lastConnectionId = nextRoadId
lastConnection = self.roadBuilder.getConnectionRoadBetween(
lastConnectionId, roads[-1], roads[0], cp2, cp1)
RoadLinker.createExtendedPredSuc(predRoad=roads[-1],
predCp=cp2,
sucRoad=lastConnection,
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=lastConnection,
predCp=pyodrx.ContactPoint.end,
sucRoad=roads[0],
sucCP=cp1)
connectionL = pyodrx.Connection(roads[-1].id, lastConnectionId,
pyodrx.ContactPoint.start)
connectionL.add_lanelink(-1, -1)
junction.add_connection(connectionL)
# roads.append(lastConnection) # dangerous. do not add the road
return lastConnection
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 setUp(self):
self.configuration = Configuration()
self.roadBuilder = junctions.RoadBuilder()
self.straightRoadBuilder = StraightRoadBuilder()
self.roadLinker = RoadLinker()
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 setUp(self):
self.configuration = Configuration()
self.esminiPath = self.configuration.get("esminipath")
self.roadBuilder = RoadBuilder()
self.laneBuilder = LaneBuilder()
self.laneLinker = LaneLinker()
self.straightRoadBuilder = StraightRoadBuilder()
self.curveBuilder = CurveRoadBuilder()
self.roadLinker = RoadLinker()
def createConnectionFor2Roads(self,
nextRoadId,
road1,
road2,
junction,
cp1,
cp2,
n_lanes=1,
lane_offset=3,
laneSides=LaneSides.BOTH):
"""Does not modify predecessor or successor of the given roads.
Args:
junction: the junction object to add links.
Returns:
[type]: connection road with first road as the predecessor and second road as the successor
"""
newConnectionId = nextRoadId
newConnectionRoad = self.roadBuilder.getConnectionRoadBetween(
newConnectionId,
road1,
road2,
cp1,
cp2,
isJunction=True,
n_lanes=n_lanes,
lane_offset=lane_offset,
laneSides=laneSides)
RoadLinker.createExtendedPredSuc(predRoad=road1,
predCp=cp1,
sucRoad=newConnectionRoad,
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=newConnectionRoad,
predCp=pyodrx.ContactPoint.end,
sucRoad=road2,
sucCP=cp2)
if junction is not None:
if laneSides == LaneSides.LEFT or laneSides == LaneSides.BOTH:
connectionL = pyodrx.Connection(road2.id, newConnectionId,
pyodrx.ContactPoint.end)
connectionL.add_lanelink(-1, -1)
junction.add_connection(connectionL)
else:
connectionL = pyodrx.Connection(road1.id, newConnectionId,
pyodrx.ContactPoint.start)
connectionL.add_lanelink(1, 1)
junction.add_connection(connectionL)
return newConnectionRoad
def createLanesForConnectionRoad(self, connectionRoad: ExtendedRoad,
predRoad: ExtendedRoad,
sucRoad: ExtendedRoad,
strategy = LaneConfigurationStrategies.MERGE_EDGE,
countryCode=extensions.CountryCodes.US):
"""Assumes start of connection road is connected to predRoad and end to sucRoad and connection road's lanes are not connected to either of the roads.
It can connect roads with two different lane configurations.
Args:
connectionRoad (ExtendedRoad):
predRoad (ExtendedRoad): Extended predecessor road of connectionRoad. That means connection road's start is connected to predRoad
sucRoad (ExtendedRoad): Extended successor road of connectionRoad. That means connection road's end is connected to sucRoad
strategy ([type], optional): [description]. Defaults to LaneConfigurationStrategies.MERGE_EDGE.
"""
try:
cp1, cp1Con = RoadLinker.getContactPoints(predRoad, connectionRoad)
cp2, cp2Con = RoadLinker.getContactPoints(sucRoad, connectionRoad)
laneSection1 = predRoad.getLaneSectionByCP(cp1)
laneSection2 = sucRoad.getLaneSectionByCP(cp2)
connectionRoad.clearLanes()
leftConnections, rightConnections = LaneConfiguration.getLaneLinks(laneSection1, laneSection2, (cp1 == cp2), strategy)
# now we need to workout the number of straight lanes, merge lanes, and turn lanes on each side.
# switch lane sides if cp1 and cp1Con are the same, because the lane orientation is reversed
if cp1 == cp1Con:
leftNumStandard, leftNumMerge, leftNumTurn = LaneConfiguration.getNumberDifferentLanes(rightConnections)
rightNumStandard, rightNumMerge, rightNumTurn = LaneConfiguration.getNumberDifferentLanes(leftConnections)
else:
leftNumStandard, leftNumMerge, leftNumTurn = LaneConfiguration.getNumberDifferentLanes(leftConnections)
rightNumStandard, rightNumMerge, rightNumTurn = LaneConfiguration.getNumberDifferentLanes(rightConnections)
connectionRoad.lanes = self.getLanes(n_lanes_left=leftNumStandard, n_lanes_right=rightNumStandard,
roadLength=connectionRoad.length(),
numLeftTurnsOnLeft=leftNumTurn, numLeftMergeOnLeft=leftNumMerge,
numRightTurnsOnRight= rightNumTurn, numRightMergeOnRight=rightNumMerge)
except Exception as e:
raise e
def combine(odrList: List[ExtendedOpenDrive], name):
"""Does not readjust
Returns:
[type]: [description]
"""
laneLinker = LaneLinker()
roadLinker = RoadLinker()
combinedOdr = ExtendedOpenDrive(name, laneLinker=laneLinker)
for odr in odrList:
roads = list(odr.roads.values())
junctions = odr.junctions
combinedOdr.addFirstRoad(roads[0])
for r in roads:
combinedOdr.add_road(r)
for junction in junctions:
combinedOdr.add_junction(junction)
return combinedOdr
def createOdrByPredecessor(name, roads, junctions):
laneLinker = LaneLinker()
roadLinker = RoadLinker()
odr = ExtendedOpenDrive(name, laneLinker=laneLinker)
for r in roads:
odr.add_road(r)
for junction in junctions:
odr.add_junction(junction)
roadLinker.adjustLaneOffsetsForOdr(odr)
logging.info(
f"moreHelpers: createOdrByPredecessor: starting adjustment. May freeze!!!!!!!!!!!!!"
)
odr.adjust_roads_and_lanesByPredecessor()
return odr
def setUp(self):
self.configuration = Configuration()
self.esminipath = self.configuration.get("esminipath")
self.roadBuilder = junctions.RoadBuilder()
self.straightRoadBuilder = StraightRoadBuilder()
self.roadLinker = RoadLinker()
self.connectionBuilder = ConnectionBuilder()
self.curveRoadBuilder = CurveRoadBuilder()
def createInternalConnectionsForMissingSequentialRoads(self, roads, junction, cp1 = pyodrx.ContactPoint.start, rebuildLanes=False):
"""Does not add connection to any junction. When are junction has all the roads connected to at least one connection road in a sequential manner, you can use
this method to connect roads which are not already connected.
"""
# for first road:
# fromId = 0
# toId = roads[2].id
# nextRoadId = roads[-1].id + 1
# countOldRoads = len(roads)
# while(toId < countOldRoads):
# connectionRoad = self.createConnectionFor2Roads(nextRoadId, roads[fromId], roads[toId], junction, cp1=cp1)
# roads.append(connectionRoad)
# toId += 2
# nextRoadId += 1
newConnectionRoads = []
connectionRoads = self.getConnectionRoadsForSequentialRoads(roads)
fromIndex = 0
countOldRoads = len(roads)
nextRoadId = roads[-1].id + 1
while fromIndex < countOldRoads:
toIndex = fromIndex + 4
while toIndex < countOldRoads:
if toIndex == fromIndex:
toIndex += 2
continue
if not RoadLinker.areRoadsConnected(roads[fromIndex], roads[toIndex], connectionRoads):
if fromIndex == 0:
connectionRoad = self.createConnectionFor2Roads(nextRoadId, roads[fromIndex], roads[toIndex], junction, cp1=cp1, cp2=pyodrx.ContactPoint.start)
else:
connectionRoad = self.createConnectionFor2Roads(nextRoadId, roads[fromIndex], roads[toIndex], junction, cp1=pyodrx.ContactPoint.start, cp2=pyodrx.ContactPoint.start)
roads.append(connectionRoad)
newConnectionRoads.append(connectionRoad)
if rebuildLanes:
self.laneBuilder.createLanesForConnectionRoad(connectionRoad, roads[fromIndex], roads[toIndex])
toIndex += 2
nextRoadId += 1
pass
fromIndex += 2
return newConnectionRoads
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)
class test_ExtendedRoad(unittest.TestCase):
def setUp(self):
self.configuration = Configuration()
self.roadBuilder = junctions.RoadBuilder()
self.straightRoadBuilder = StraightRoadBuilder()
self.roadLinker = RoadLinker()
def test_getArcAngle(self):
for _ in range(1, 10):
for i in range(1, 10):
inputAngle = (np.pi * i) / 10
road = self.roadBuilder.createRandomCurve(0, inputAngle)
if road.curveType == StandardCurveTypes.S:
continue
outputAngle = road.getArcAngle()
deviation = abs(inputAngle - outputAngle) * 100 / inputAngle
print(
f"curveType: {road.curveType} inputAngle: {math.degrees(inputAngle)} outputAngle: {math.degrees(outputAngle)} deviation: {deviation}"
)
assert deviation < 50.0
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 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_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)
class test_RoadLiner(unittest.TestCase):
def setUp(self):
self.configuration = Configuration()
self.esminiPath = self.configuration.get("esminipath")
self.roadBuilder = RoadBuilder()
self.laneBuilder = LaneBuilder()
self.laneLinker = LaneLinker()
self.straightRoadBuilder = StraightRoadBuilder()
self.curveBuilder = CurveRoadBuilder()
self.roadLinker = RoadLinker()
def test_getAngleBetweenStraightRoads(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.2,
n_lanes=1)
roads.append(connectionRoad)
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(2,
10,
n_lanes_left=1,
n_lanes_right=2))
connectionRoad2 = self.curveBuilder.create(3,
np.pi / 2,
isJunction=True,
curvature=0.1,
n_lanes=1)
roads.append(connectionRoad2)
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(4,
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.end)
roads[1].addExtendedSuccessor(roads[2], 0, pyodrx.ContactPoint.start)
roads[2].addExtendedPredecessor(roads[1], 0, pyodrx.ContactPoint.end)
roads[2].addExtendedSuccessor(roads[3], 0, pyodrx.ContactPoint.start)
roads[3].addExtendedPredecessor(roads[2], 0, pyodrx.ContactPoint.start)
roads[3].addExtendedSuccessor(roads[4], 0, pyodrx.ContactPoint.start)
roads[4].addExtendedPredecessor(roads[3], 0, pyodrx.ContactPoint.end)
self.laneBuilder.createLanesForConnectionRoad(connectionRoad, roads[0],
roads[2])
self.laneBuilder.createLanesForConnectionRoad(connectionRoad2,
roads[2], roads[4])
odrName = "test_DifferentLaneConfigurations"
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])))
print(
math.degrees(
self.roadLinker.getAngleBetweenStraightRoads(
roads[0], roads[4])))
print(
math.degrees(
self.roadLinker.getAngleBetweenStraightRoads(
roads[2], roads[4])))
# xmlPath = f"output/test_DifferentLaneConfigurations.xodr"
# odr.write_xml(xmlPath)
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_fails(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.2,
n_lanes=1)
roads.append(connectionRoad)
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(2,
10,
n_lanes_left=1,
n_lanes_right=2))
connectionRoad2 = self.curveBuilder.create(3,
np.pi / 2,
isJunction=True,
curvature=0.1,
n_lanes=1)
roads.append(connectionRoad2)
roads.append(
self.straightRoadBuilder.createWithDifferentLanes(4,
10,
n_lanes_left=1,
n_lanes_right=3))
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)
roads[2].addExtendedSuccessor(roads[3], 0, pyodrx.ContactPoint.start)
roads[3].addExtendedPredecessor(roads[2], 0, pyodrx.ContactPoint.start)
roads[3].addExtendedSuccessor(roads[4], 0, pyodrx.ContactPoint.start)
roads[4].addExtendedPredecessor(roads[3], 0, pyodrx.ContactPoint.end)
self.laneBuilder.createLanesForConnectionRoad(connectionRoad, roads[0],
roads[2])
self.laneBuilder.createLanesForConnectionRoad(connectionRoad2,
roads[2], roads[4])
odrName = "test_DifferentLaneConfigurations"
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])))
print(
math.degrees(
self.roadLinker.getAngleBetweenStraightRoads(
roads[0], roads[4])))
print(
math.degrees(
self.roadLinker.getAngleBetweenStraightRoads(
roads[2], roads[4])))
def _create_links_connecting_road(self,
connecting: ExtendedRoad,
road: ExtendedRoad,
ignoreMismatch=True):
""" _create_links_connecting_road will create lane links between a connecting road and a normal road
Parameters
----------
connecting (Road): a road of type connecting road (not -1)
road (Road): a that connects to the connecting road
"""
linktype, sign, connecting_lanesec = self._get_related_lanesection(
connecting, road)
_, _, road_lanesection_id = self._get_related_lanesection(
road, connecting)
# invert lanes if contact points are the same
try:
roadCp, conCp = RoadLinker.getContactPoints(road, connecting)
except:
# lane linking not possible because they are not neighbours
return
if roadCp == conCp:
logging.debug(
f"{self.name}: switching lane sides for {connecting.id} and {road.id}"
)
if connecting_lanesec != None:
laneSectionForConnection = connecting.lanes.lanesections[
connecting_lanesec]
laneSectionForRoad = road.lanes.lanesections[road_lanesection_id]
if laneSectionForConnection.leftlanes:
# do left lanes
connectionLanes = laneSectionForConnection.leftlanes
roadLanes = laneSectionForRoad.leftlanes
if roadCp == conCp:
roadLanes = laneSectionForRoad.rightlanes
if len(connectionLanes) == len(roadLanes):
for i in range(len(roadLanes)):
linkid = roadLanes[i].lane_id
connectionLanes[i].add_link(linktype, linkid)
elif ignoreMismatch:
# raise NotImplementedError()
# logging.warn(f"number of left lanes are not the same for {connecting.id} and {road.id}")
self.connectMinLanesOnOneSide(connectionLanes, roadLanes,
linktype, None)
else:
raise NotSameAmountOfLanesError(
'Connecting road ', connecting.id, ' and road ',
road.id, 'do not have the same number of left lanes.')
if laneSectionForConnection.rightlanes:
# do right lanes
connectionLanes = laneSectionForConnection.rightlanes
roadLanes = laneSectionForRoad.rightlanes
if roadCp == conCp:
roadLanes = laneSectionForRoad.leftlanes
if len(connectionLanes) == len(roadLanes):
for i in range(len(roadLanes)):
linkid = roadLanes[i].lane_id
connectionLanes[i].add_link(linktype, linkid)
elif ignoreMismatch:
# raise NotImplementedError()
# logging.warn(f"number of left lanes are not the same for {connecting.id} and {road.id}")
self.connectMinLanesOnOneSide(connectionLanes, roadLanes,
linktype, None)
else:
raise NotSameAmountOfLanesError(
'Connecting road ', connecting.id, ' and road ',
road.id, 'do not have the same number of right lanes.')
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 createSingleLaneConnectionRoad(self, newRoadId, incomingRoad,
outgoingRoad, incomingLaneId,
outgoingLaneId, incomingCp, outgoingCp):
"""Warining: uses default lane width. Works only after roads has been adjusted.
Args:
incomingRoad ([type]): [description]
outgoingRoad ([type]): [description]
incomingLaneId ([type]): [description]
outgoingLaneId ([type]): [description]
incomingCp ([type]): [description]
outgoingCp ([type]): [description]
"""
laneSides = None
if self.countryCode == CountryCodes.US:
laneSides = LaneSides.RIGHT
if self.countryCode == CountryCodes.UK:
laneSides = LaneSides.LEFT
incomingBoundaryId = incomingLaneId - 1
if incomingLaneId < 0:
incomingBoundaryId = incomingLaneId + 1
outgoingBoundaryId = outgoingLaneId - 1
if outgoingLaneId < 0:
outgoingBoundaryId = outgoingLaneId + 1
# TODO, get lane widths from road and create an equation.
width = self.config.get("default_lane_width")
x1, y1, h1 = incomingRoad.getLanePosition(incomingBoundaryId,
incomingCp)
x2, y2, h2 = outgoingRoad.getLanePosition(outgoingBoundaryId,
outgoingCp)
# special case for U turns from -1 to 1 or 1 to -1
if x1 == x2 and y1 == y2:
# x1 = 0.9 * x1
# y1 = 0.9 * y1
width -= self.uTurnFirstLaneShift
logging.debug(f"{self.name}: createSingleLaneConnectionRoad: start: ",
x1, y1, h1)
logging.debug(f"{self.name}: createSingleLaneConnectionRoad: end: ",
x2, y2, h2)
xCoeffs, yCoeffs = Geometry.getCoeffsForParamPoly(
x1,
y1,
h1,
x2,
y2,
h2,
incomingCp,
outgoingCp,
vShiftForSamePoint=self.uTurnFirstLaneShift)
# scipy coefficient and open drive coefficents have opposite order.
newConnection = self.curveBuilder.createParamPoly3(newRoadId,
isJunction=True,
au=xCoeffs[3],
bu=xCoeffs[2],
cu=xCoeffs[1],
du=xCoeffs[0],
av=yCoeffs[3],
bv=yCoeffs[2],
cv=yCoeffs[1],
dv=yCoeffs[0],
n_lanes=1,
lane_offset=width,
laneSides=laneSides)
newConnection.predecessorOffset = incomingBoundaryId
newConnection.isSingleLaneConnection = True
RoadLinker.createExtendedPredSuc(predRoad=incomingRoad,
predCp=incomingCp,
sucRoad=newConnection,
sucCP=pyodrx.ContactPoint.start)
RoadLinker.createExtendedPredSuc(predRoad=newConnection,
predCp=pyodrx.ContactPoint.end,
sucRoad=outgoingRoad,
sucCP=outgoingCp)
return newConnection
def setUp(self):
self.configuration = Configuration()
self.curveRoadBuilder = CurveRoadBuilder()
self.roadLinker = RoadLinker()
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 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 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 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 _create_links_roads(self,
pre_road: ExtendedRoad,
suc_road: ExtendedRoad,
ignoreMismatch=True):
""" _create_links_roads takes two roads and connect the lanes with links, if they have the same amount.
Parameters
----------
pre_road (Road): the predecessor road
suc_road (Road): the successor road
"""
# invert lanes if contact points are the same
try:
roadCp, conCp = RoadLinker.getContactPoints(pre_road, suc_road)
except:
# lane linking not possible because they are not neighbours
return
if roadCp == conCp:
logging.debug(
f"{self.name}: switching lane sides for {pre_road.id} and {suc_road.id}"
)
pre_linktype, pre_sign, pre_connecting_lanesec = self._get_related_lanesection(
pre_road, suc_road)
suc_linktype, suc_sign, suc_connecting_lanesec = self._get_related_lanesection(
suc_road, pre_road)
preLaneSection = pre_road.lanes.lanesections[pre_connecting_lanesec]
# TODO it may be wrong. shouldn't it be suc_connecting_lanesec
# sucLaneSection = suc_road.lanes.lanesections[-1]
sucLaneSection = suc_road.lanes.lanesections[suc_connecting_lanesec]
# left
preLanes = preLaneSection.leftlanes
sucLanes = sucLaneSection.leftlanes
if roadCp == conCp:
sucLanes = sucLaneSection.rightlanes
if len(preLanes) == len(sucLanes):
for i in range(len(preLanes)):
preLanes[i].add_link(pre_linktype, sucLanes[i].lane_id)
sucLanes[i].add_link(suc_linktype, preLanes[i].lane_id)
elif ignoreMismatch:
# logging.warn(f"number of left lanes are not the same for {pre_road.id} and {suc_road.id}")
self.connectMinLanesOnOneSide(preLanes, sucLanes, pre_linktype,
suc_linktype)
else:
raise NotSameAmountOfLanesError(
'Road ' + str(pre_road.id) + ' and road ' + str(suc_road.id) +
' does not have the same number of right lanes.')
#right
preLanes = preLaneSection.rightlanes
sucLanes = sucLaneSection.rightlanes
if roadCp == conCp:
sucLanes = sucLaneSection.leftlanes
if len(preLanes) == len(sucLanes):
for i in range(len(preLanes)):
preLanes[i].add_link(pre_linktype, sucLanes[i].lane_id)
sucLanes[i].add_link(suc_linktype, preLanes[i].lane_id)
elif ignoreMismatch:
# logging.warn(f"number of left lanes are not the same for {pre_road.id} and {suc_road.id}")
self.connectMinLanesOnOneSide(preLanes, sucLanes, pre_linktype,
suc_linktype)
else:
raise NotSameAmountOfLanesError(
'Road ' + str(pre_road.id) + ' and road ' + str(suc_road.id) +
' does not have the same number of right lanes.')