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)