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 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_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 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 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 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_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 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)
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 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