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