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")))
class test_ExtendedRoad(unittest.TestCase):
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 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_LaneLinker(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.roadLinker = RoadLinker()
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_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)
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])))
class test_AngleCurvatureMap(unittest.TestCase):
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 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_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_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_StraightRoadHarvester(unittest.TestCase):
def setUp(self):
self.configuration = Configuration()
self.esminiPath = self.configuration.get("esminipath")
self.harvester = StraightRoadHarvester(outputDir="./output", outputPrefix="harvest-straight-", lastId=0, straightRoadLen=10, esminiPath=self.esminiPath)
self.straightRoadBuilder = StraightRoadBuilder()
self.pp = pprint.PrettyPrinter(indent=4)
def test_getPossibleTurnsWRTLaneOnRight(self):
possibleTurns = self.harvester.getPossibleTurnsWRTLaneOnRight(TurnTypes.RIGHT)
assert len(possibleTurns) == 6
possibleTurns = self.harvester.getPossibleTurnsWRTLaneOnRight(TurnTypes.STRAIGHT_RIGHT)
assert len(possibleTurns) == 3
possibleTurns = self.harvester.getPossibleTurnsWRTLaneOnRight(TurnTypes.STRAIGHT)
print(possibleTurns)
assert len(possibleTurns) == 3
possibleTurns = self.harvester.getPossibleTurnsWRTLaneOnRight(TurnTypes.STRAIGHT_LEFT)
print(possibleTurns)
assert len(possibleTurns) == 1
possibleTurns = self.harvester.getPossibleTurnsWRTLaneOnRight(TurnTypes.LEFT)
print(possibleTurns)
assert len(possibleTurns) == 1
possibleTurns = self.harvester.getPossibleTurnsWRTLaneOnRight(TurnTypes.ALL)
print(possibleTurns)
assert len(possibleTurns) == 1
def test_getLaneTurnCombinations(self):
combinations = self.harvester.getLaneTurnCombinations(1)
# self.pp.pprint(combinations)
assert len(combinations) == 6
combinations = self.harvester.getLaneTurnCombinations(2)
self.pp.pprint(combinations)
print(len(combinations))
assert len(combinations) == 15
def test_applyTurnCombinationOnLanes(self):
road = self.straightRoadBuilder.createWithDifferentLanes(0, length=10, n_lanes_left=2, n_lanes_right=2)
leftCombinations = self.harvester.getLaneTurnCombinations(2)
rightCombinations = leftCombinations
laneSectionForLeft = road.getFirstLaneSection()
laneSectionForRight = road.getLastLaneSection()
self.harvester.applyTurnCombinationOnLanes(laneSectionForLeft.leftlanes, leftCombinations[0])
self.harvester.applyTurnCombinationOnLanes(laneSectionForRight.rightlanes, rightCombinations[0])
assert laneSectionForLeft.leftlanes[0].turnType == leftCombinations[0][0]
assert laneSectionForLeft.leftlanes[0].turnType == rightCombinations[0][0]
def test_harvestUS(self):
odrs = self.harvester.harvestUS(2, 2, False)
print(len(odrs))
def test_harvest(self):
self.harvester.harvest(maxLeftLanes=2, maxRightLanes=2, countryCode=CountryCodes.US)
class StraightRoadHarvester:
def __init__(self,
outputDir,
outputPrefix,
lastId=0,
straightRoadBuilder=None,
straightRoadLen = 2,
esminiPath = None,
saveImage = False
):
self.destinationPrefix = os.path.join(outputDir, outputPrefix)
self.configuration = Configuration()
self.lastId = lastId
self.straightRoadLen = straightRoadLen
self.straightRoadBuilder = straightRoadBuilder
if straightRoadBuilder is None:
self.straightRoadBuilder = StraightRoadBuilder()
if esminiPath is None:
self.esminiPath = self.configuration.get("esminipath")
else:
self.esminiPath = esminiPath
self.saveImage = saveImage
if os.path.isdir(self.esminiPath) is False:
logging.warn(f"Esmini path not found {self.esminiPath}. Will break if you try to save images using harvester.")
pass
def getOutputPath(self, fname):
return self.destinationPrefix + fname + '.xodr'
def harvest(self, maxLeftLanes=2, maxRightLanes=2, countryCode=CountryCodes.US, show=False):
"""[summary]
Args:
maxLeftLanes (int, optional): [description]. Defaults to 2.
maxRightLanes (int, optional): [description]. Defaults to 2.
countryCode ([type], optional): [description]. Defaults to CountryCodes.US.
show (bool, optional): [description]. Defaults to False.
Raises:
NotImplementedError: [description]
Returns: dic with keys like "n_lanes_left-n_lanes_right" and list of odrs per key
"""
if countryCode != CountryCodes.US:
raise NotImplementedError("Only US is implemented")
# permutations
# no merge or extension
# each lane can have one of the 5 traffic direction.
odrs = {} # values are a list of odrs for each key
for l in range(maxLeftLanes + 1):
for r in range(maxRightLanes + 1):
if l == 0 and r == 0:
continue
odrs[f"{l}-{r}"] = self.harvestUS(l, r, show) # do not change the key convention
# Save the odrs
objectPath = self.destinationPrefix + f"{countryCode.value}.dill"
with(open(objectPath, "wb")) as f:
dill.dump(odrs, f)
print("Odr objects saved to " + objectPath)
print(f"keys {odrs.keys()}")
return objectPath
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 applyTurnCombinationOnLanes(self, lanes, combination):
for i in range(len(lanes)):
lanes[i].turnType = combination[i]
pass
def getLaneTurnCombinations(self, n):
"""[summary]
Args:
n ([type]): [description]
Returns:
[type]: List of combinations. each combination is an ordered list
"""
# from left to right
if n == 0:
return []
if n == 1:
return [[i] for i in TurnTypes.getAll()]
combinations = []
childrenCombinations = self.getLaneTurnCombinations(n-1)
for childComb in childrenCombinations:
possibleTurns = self.getPossibleTurnsWRTLaneOnRight(childComb[0])
# push into child comb
for possibleTurn in possibleTurns:
childCopy = childComb.copy()
childCopy.insert(0, possibleTurn)
combinations.append(childCopy)
return combinations
def getPossibleTurnsWRTLaneOnRight(self, turnOnRight):
if turnOnRight == TurnTypes.RIGHT:
return TurnTypes.getAll()
if turnOnRight in [TurnTypes.STRAIGHT_RIGHT, TurnTypes.STRAIGHT]:
return (TurnTypes.LEFT, TurnTypes.STRAIGHT_LEFT, TurnTypes.STRAIGHT)
return (TurnTypes.LEFT, ) # for ALL or LEFT types
class test_LaneBuilder(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()
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_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_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_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)