def setUp(self): '''Build an sdf file from a known set of data''' osmDict = {} osmDict = getOsmFile([-75.93, 40.61, -75.90, 40.62], 'map.osm') osmRoads = Osm2Dict(-75.93, 40.61, osmDict) (roadPointWidthMap, modelPoseMap, buildingLocationMap) = osmRoads.getMapDetails() #Initialize the getSdf class sdfFile = GetSDF() #Set up the spherical coordinates sdfFile.addSphericalCoords(osmRoads.getLat(), osmRoads.getLon()) #add Required models sdfFile.includeModel("sun") for model in modelPoseMap.keys(): points = modelPoseMap[model]['points'] sdfFile.addModel(modelPoseMap[model]['mainModel'], model, [points[0, 0], points[1, 0], points[2, 0]]) for building in buildingLocationMap.keys(): sdfFile.addBuilding(buildingLocationMap[building]['mean'], buildingLocationMap[building]['points'], building, buildingLocationMap[building]['color']) #Include the roads in the map in sdf file for road in roadPointWidthMap.keys(): sdfFile.addRoad(road) sdfFile.setRoadWidth(roadPointWidthMap[road]['width'], road) points = roadPointWidthMap[road]['points'] for point in range(len(points[0, :])): sdfFile.addRoadPoint([points[0, point], points[1, point], points[2, point]], road) #output sdf File sdfFile.writeToFile('outFile.sdf')
def setUp(self): '''Build an sdf file from a known set of data''' osmDict = {} osmDict = getOsmFile([-75.93, 40.61, -75.90, 40.62], 'map.osm') osmRoads = Osm2Dict(-75.93, 40.61, osmDict) (roadPointWidthMap, modelPoseMap, buildingLocationMap) = osmRoads.getMapDetails() #Initialize the getSdf class sdfFile = GetSDF() #Set up the spherical coordinates sdfFile.addSphericalCoords([-75.93, 40.61, -75.90, 40.62]) #add Required models sdfFile.includeModel("sun") for model in modelPoseMap.keys(): points = modelPoseMap[model]['points'] sdfFile.addModel(modelPoseMap[model]['mainModel'], model, [points[0, 0], points[1, 0], points[2, 0]]) for building in buildingLocationMap.keys(): sdfFile.addBuilding(buildingLocationMap[building]['mean'], buildingLocationMap[building]['points'], building, buildingLocationMap[building]['color']) #Include the roads in the map in sdf file for road in roadPointWidthMap.keys(): sdfFile.addRoad(road) sdfFile.setRoadWidth(roadPointWidthMap[road]['width'], road) points = roadPointWidthMap[road]['points'] for point in range(len(points[0, :])): sdfFile.addRoadPoint( [points[0, point], points[1, point], points[2, point]], road) #output sdf File sdfFile.writeToFile('outFile.sdf')
##plt.plot(x, y, 'b+') #plt.show() # lanes.saveImage(size, lanePointsA, lanePointsB) # if idx == len(roadPointWidthMap.keys())-1: # lanes.showImage() for point in range(len(x)): sdfFile.addRoadPoint([x[point], y[point], 0], road) #sdfFile.addRoadDebug([x[point], y[point], 0], road) print ('|') print ('|-----------------------------------') print ('| Generating the SDF world file...') sdfFile.writeToFile(args.outFile) # if args.imageFile: print ('| Generating Image File...') print ('|-----------------------------------') print ('|') size = osmRoads.getMapSize() # args.imageFile = args.directory + args.imageFile lanes.makeImage(size, 5, roadLaneSegments, centerLaneSegments) print ('| Lat Center = '+ str(osmRoads.getLat())) print ('| Lon Center = '+ str(osmRoads.getLon())) #plt.show()
#!/usr/bin/env python from dict2sdf import GetSDF from osm2dict import Osm2Dict osmRoads = Osm2Dict() roadPointWidthMap = osmRoads.getRoadCoords() sdfFile = GetSDF() sdfFile.addModule("sun") sdfFile.addModule("ground_plane") sdfFile.addSphericalCoords(osmRoads.getLat(), osmRoads.getLon()) for road in roadPointWidthMap.keys(): sdfFile.addRoad( road ) sdfFile.setRoadWidth( roadPointWidthMap[road]['width'], road) points = roadPointWidthMap[ road]['points'] for point in range(len(points[0,:])): sdfFile.addRoadPoint([points[0,point], points[1,point], points[2,point]], road) sdfFile.writeToFile('outFile2.sdf')
#### Debug # lanes.saveImage(size, lanePointsA, lanePointsB) # if idx == len(roadPointWidthMap.keys())-1: # lanes.showImage() for point in range(len(x)): sdfFile.addRoadPoint([x[point], y[point], 0], road) #sdfFile.addRoadDebug([x[point], y[point], 0], road) print ('|') print ('|-----------------------------------') print ('| Generating the SDF world file...') sdfFile.writeToFile(args.outFile) # if args.imageFile: print ('| Generating Image File...') print ('|-----------------------------------') print ('|') size = osmRoads.getMapSize() # args.imageFile = args.directory + args.imageFile lanes.makeImage(size, 5, roadLaneSegments, centerLaneSegments, laneSegmentWidths, args.name + ".png") print ('| Lat Center = '+ str(osmRoads.getLat())) print ('| Lon Center = '+ str(osmRoads.getLon())) if args.stage: stage = StageWorld([443, 700], [1300, 4800], [-45.12, 14.4334], [4.2,444.355])
def parse_osm_(self): if not self.args: self.parse_args_() flags = [] if self.args.buildings: flags.append('b') if self.args.models: flags.append('m') if self.args.roads: flags.append('r') if not (self.args.roads or self.args.models or self.args.buildings) or self.args.displayAll: flags.append('a') if not os.path.exists(self.args.directory): os.makedirs(self.args.directory) self.args.osmFile = self.args.directory + '/' + self.args.osmFile self.args.outFile = self.args.directory + '/' + self.args.outFile osmDictionary = {} if self.args.inputOsmFile: f = open(self.args.inputOsmFile, 'r') root = etree.fromstring(f.read()) f.close() self.args.boundingbox = [ root[0].get('minlon'), root[0].get('minlat'), root[0].get('maxlon'), root[0].get('maxlat') ] else: if not self.arg.boundingbox: print("Can not get osm file from server without bounding box") sys.exit("No bounding box for target area") print("Downloading the osm data ... ") osmDictionary = getOsmFile(self.args.boundingbox, self.args.osmFile, self.args.inputOsmFile) #Initialize Osm Dictionary class osmRoads = Osm2Dict(self.args.boundingbox[0], self.args.boundingbox[1], osmDictionary, flags) print("Extracting the map data for gazebo ...") #Get Road and model details roadPointWidthMap, modelPoseMap, buildingLocationMap = osmRoads.getMapDetails( ) print("Building sdf file ...") #Initialize the getSdf class sdfFile = GetSDF() #Set up the spherical coordinates sdfFile.setOffset(self.args.boundingbox) sdfFile.addGroundPlane(self.args.boundingbox) sdfFile.addSphericalCoords(self.args.boundingbox) #Add Required models for model in modelPoseMap.keys(): points = modelPoseMap[model]['points'] sdfFile.addModel(modelPoseMap[model]['mainModel'], model, [points[0, 0], points[1, 0], points[2, 0]]) for building in buildingLocationMap.keys(): sdfFile.addBuilding(buildingLocationMap[building]['mean'], buildingLocationMap[building]['points'], building, buildingLocationMap[building]['color']) #Include the roads in the map in sdf file for road in roadPointWidthMap.keys(): sdfFile.addRoad(road) sdfFile.setRoadWidth(roadPointWidthMap[road]['width'], road) points = roadPointWidthMap[road]['points'] for point in range(len(points[0, :])): sdfFile.addRoadPoint( [points[0, point], points[1, point], points[2, point]], road) #Output sdf File sdfFile.writeToFile(self.args.outFile)