def test_ParamPoly(self):
tangentX = np.array([9.389829642616592, -7.596531772501544])
tangentY = np.array([0.0, 5.5192033616035365])
t = np.array([0, 1])
x = np.array([0, 17.8605173461395])
y = np.array([0, -5.803233839653106])
hermiteX = CubicHermiteSpline(t, x, tangentX)
hermiteY = CubicHermiteSpline(t, y, tangentY)
xCoeffs = hermiteX.c.flatten()
yCoeffs = hermiteY.c.flatten()
# scipy coefficient and open drive coefficents have opposite order.
myRoad = self.roadBuilder.curveBuilder.createParamPoly3(
0,
isJunction=False,
au=xCoeffs[3],
bu=xCoeffs[2],
cu=xCoeffs[1],
du=xCoeffs[0],
av=yCoeffs[3],
bv=yCoeffs[2],
cv=yCoeffs[1],
dv=yCoeffs[0])
odr = pyodrx.OpenDrive("test")
odr.add_road(myRoad)
odr.adjust_roads_and_lanes()
extensions.printRoadPositions(odr)
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
def test_cubicParams(self):
roads = []
roads.append(pyodrx.create_straight_road(0, 10))
roads.append(self.roadBuilder.createParamPoly3(1, isJunction=True))
roads.append(pyodrx.create_straight_road(2, 1))
roads[0].add_successor(pyodrx.ElementType.junction, 1)
roads[1].add_predecessor(pyodrx.ElementType.road, 0,
pyodrx.ContactPoint.end)
roads[1].add_successor(pyodrx.ElementType.road, 2,
pyodrx.ContactPoint.start)
roads[2].add_predecessor(pyodrx.ElementType.junction, 1)
con1 = pyodrx.Connection(0, 1, pyodrx.ContactPoint.start)
con1.add_lanelink(-1, -1)
junction = pyodrx.Junction('test', 1)
junction.add_connection(con1)
odr = pyodrx.OpenDrive('test')
for r in roads:
odr.add_road(r)
odr.add_junction(junction)
odr.adjust_roads_and_lanes()
extensions.view_road(odr,
os.path.join('..', 'F:\\myProjects\\av\\esmini'))
def test_object_roadside():
road = pyodrx.create_straight_road(0)
odr = pyodrx.OpenDrive('myroad')
odr.add_road(road)
odr.adjust_roads_and_lanes()
object1 = pyodrx.Object(s=0,
t=0,
dynamic=pyodrx.Dynamic.no,
orientation=pyodrx.Orientation.positive,
zOffset=0.00,
height=1.0,
Type=pyodrx.ObjectType.pole)
road.add_object_roadside(object1, 50, tOffset=0.85)
pyodrx.prettyprint(road.get_element())
id=4,
left_lanes=0,
right_lanes=1,
road_type=1))
# add some connections to non junction roads
roads[0].add_successor(pyodrx.ElementType.junction, 1)
roads[1].add_successor(pyodrx.ElementType.junction, 1)
roads[2].add_predecessor(pyodrx.ElementType.junction, 1)
# add connections to the first connecting road
roads[3].add_predecessor(pyodrx.ElementType.road, 0, pyodrx.ContactPoint.end)
roads[3].add_successor(pyodrx.ElementType.road, 2, pyodrx.ContactPoint.start)
# add connections to the second connecting road with an offset
roads[4].add_predecessor(pyodrx.ElementType.road, 1, pyodrx.ContactPoint.end)
roads[4].add_successor(pyodrx.ElementType.road,
2,
pyodrx.ContactPoint.start,
lane_offset=-2)
junction = pyodrx.create_junction(roads[3:], 1, roads[0:3])
# create the opendrive
odr = pyodrx.OpenDrive('myroad')
for r in roads:
odr.add_road(r)
odr.adjust_roads_and_lanes()
odr.add_junction(junction)
pyodrx.run_road(odr, os.path.join('..', '..', 'esmini'))
def test_create_lane_links_normalroad1():
planview = []
lanec = []
lanel = []
laner = []
lanesec = []
lanes = []
rm = pyodrx.RoadMark(pyodrx.RoadMarkType.solid,0.2,rule=pyodrx.MarkRule.no_passing)
geom= []
geom.append(pyodrx.Line(50))
geom.append(pyodrx.Arc(0.01,angle=np.pi/2))
geom.append(pyodrx.Line(50))
# create planviews
for i in range(len(geom)):
planview.append(pyodrx.PlanView())
planview[i].add_geometry(geom[i])
# create centerlanes
for i in range(len(geom)):
lanec.append(pyodrx.Lane(a=3))
lanel.append(pyodrx.Lane(a=3))
laner.append(pyodrx.Lane(a=3))
#add roadmarks
for i in range(len(geom)):
lanec[i].add_roadmark(rm)
lanel[i].add_roadmark(rm)
laner[i].add_roadmark(rm)
# create lanesections
for i in range(len(geom)):
lanesec.append(pyodrx.LaneSection(0,lanec[i]))
lanesec[i].add_right_lane(lanel[i])
lanesec[i].add_left_lane(laner[i])
#create lanes
for i in range(len(geom)):
lanes.append(pyodrx.Lanes())
lanes[i].add_lanesection(lanesec[i])
#create roads
road1 = pyodrx.Road(1,planview[0],lanes[0])
road1.add_successor(pyodrx.ElementType.road,2, pyodrx.ContactPoint.start)
road2 = pyodrx.Road(2,planview[1],lanes[1])
road2.add_predecessor(pyodrx.ElementType.road,1, pyodrx.ContactPoint.end)
road2.add_successor(pyodrx.ElementType.road,3, pyodrx.ContactPoint.start)
road3 = pyodrx.Road(3,planview[2],lanes[2])
road3.add_predecessor(pyodrx.ElementType.road,2, pyodrx.ContactPoint.end)
# create the opendrive and add roads
odr = pyodrx.OpenDrive('myroad')
odr.add_road(road1)
odr.add_road(road2)
odr.add_road(road3)
odr.adjust_roads_and_lanes()
assert road1.lanes.lanesections[0].rightlanes[0].links.get_predecessor_id() == None
assert int(road1.lanes.lanesections[0].rightlanes[0].links.get_successor_id() ) == -1
assert road1.lanes.lanesections[0].leftlanes[0].links.get_predecessor_id() == None
assert int(road1.lanes.lanesections[0].leftlanes[0].links.get_successor_id() ) == 1
assert int(road2.lanes.lanesections[0].rightlanes[0].links.get_predecessor_id() ) == -1
assert int(road2.lanes.lanesections[0].rightlanes[0].links.get_successor_id() ) == -1
assert int(road2.lanes.lanesections[0].leftlanes[0].links.get_predecessor_id() ) == 1
assert int(road2.lanes.lanesections[0].leftlanes[0].links.get_successor_id() ) == 1
assert int(road3.lanes.lanesections[0].rightlanes[0].links.get_predecessor_id() ) == -1
assert road3.lanes.lanesections[0].rightlanes[0].links.get_successor_id() == None
assert int(road3.lanes.lanesections[0].leftlanes[0].links.get_predecessor_id() ) == 1
assert road3.lanes.lanesections[0].leftlanes[0].links.get_successor_id() == None
def merge2R2L(self, odrs, save=True):
# 1 find connectionRoad in the first, it's predecessor is first road, successor is the second road.
connectionRoadsFirst = extensions.getConnectionRoads(
odrs[0].roads, odrs[0].junctions[0])
connectionRoadFirst = connectionRoadsFirst[0].shallowCopy()
connectionRoadsSecond = extensions.getConnectionRoads(
odrs[1].roads, odrs[1].junctions[0])
connectionRoadSecond = connectionRoadsSecond[0].shallowCopy()
if self.canMerge(connectionRoadFirst, connectionRoadSecond) is False:
raise IncompatibleRoadsException(
"incompatible junctions to merge.")
roadFirstPred = extensions.getRoadFromRoadDic(
odrs[0].roads,
connectionRoadFirst.predecessor.element_id).shallowCopy()
roadFirstSuc = extensions.getRoadFromRoadDic(
odrs[0].roads,
connectionRoadFirst.successor.element_id).shallowCopy()
roadSecondPred = extensions.getRoadFromRoadDic(
odrs[1].roads,
connectionRoadSecond.predecessor.element_id).shallowCopy()
roadSecondSuc = extensions.getRoadFromRoadDic(
odrs[1].roads,
connectionRoadSecond.successor.element_id).shallowCopy()
# case 1: remove roadSecondPred & rebuild links for roads in the first odr
roadFirstPred.clearAllLinks()
roadFirstPred.updateSuccessor(pyodrx.ElementType.junction,
connectionRoadFirst.id)
connectionRoadFirst.clearAllLinks()
connectionRoadFirst.updatePredecessor(
pyodrx.ElementType.road, roadFirstPred.id, pyodrx.ContactPoint.end
) # interestingly, this becomes the start point after merging.
connectionRoadFirst.updateSuccessor(pyodrx.ElementType.road,
roadFirstSuc.id,
pyodrx.ContactPoint.start)
roadFirstSuc.clearAllLinks()
roadSecondSuc.updatePredecessor(pyodrx.ElementType.junction,
connectionRoadFirst.id)
roads = []
roads.append(roadFirstPred)
roads.append(connectionRoadFirst)
roads.append(roadFirstSuc)
roads.append(connectionRoadSecond)
roads.append(roadSecondSuc)
# regenerate ids for connectionRoadSecond and roadSecondSuc
connectionRoadSecond.id = 10000 + roadFirstSuc.id # good number for debugging puporses.
roadSecondSuc.id = connectionRoadSecond.id + 1
# fix links for roadFirstSuc, connectionRoadSecond, roadSecondSuc
self.regenAndMergeWithConnectionRoad(roadFirstSuc,
connectionRoadSecond,
roadSecondSuc)
# create new junction
junction = self.createJunctionFor2Connections(roadFirstPred,
connectionRoadFirst,
roadFirstSuc,
connectionRoadSecond)
# newOdr = self.mergeByRoad(self, commonRoads, ords)
self.lastId += 1
# create the opendrive
odr = pyodrx.OpenDrive("3R_2L_" + str(self.lastId))
for r in roads:
odr.add_road(r)
odr.add_junction(junction)
print(f"starting adjustment. May freeze!!!!!!!!!!!!!")
# odr.adjust_roads_and_lanes()
# print(f"total number of roads added {len(roads)}")
# print(f"total number of roads in odr {len(odr.roads)}")
if save:
odr.write_xml(self.getOutputPath(odr.name))
return odr
