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')
print ('|')
print ('|-----------------------------------')
print ('| Number of Roads: ' + str(len(roadPointWidthMap.keys())))
print ('|-----------------------------------')
#print ('|')
#fig = plt.figure()
lanes = 0
roadLaneSegments = []
centerLaneSegments = []
#Include the roads in the map in sdf file
for idx, road in enumerate(roadPointWidthMap.keys()):
sdfFile.addRoad(road, roadPointWidthMap[road]['texture'])
sdfFile.setRoadWidth(roadPointWidthMap[road]['width'], road)
points = roadPointWidthMap[road]['points']
print ('| Road' + str(idx+1) + ': ' + road)
xData = points[0, :]
yData = points[1, :]
if len(xData) < 3:
#print ('Cannot apply spline with [' + str(len(xData)) + '] points. At least 3 needed.')
x = []
y = []
lanePoint = []
#!/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')
print ('|-----------------------------------')
print ('| Number of Roads: ' + str(len(roadPointWidthMap.keys())))
print ('|-----------------------------------')
#print ('|')
#fig = plt.figure()
lanes = 0
roadLaneSegments = []
centerLaneSegments = []
laneSegmentWidths = []
#Include the roads in the map in sdf file
for idx, road in enumerate(roadPointWidthMap.keys()):
sdfFile.addRoad(road, roadPointWidthMap[road]['texture'])
sdfFile.setRoadWidth(roadPointWidthMap[road]['width'], road)
points = roadPointWidthMap[road]['points']
print ('| Road' + str(idx+1) + ': ' + road)
laneSegmentWidths.append(roadPointWidthMap[road]['width'])
print ("| -- Width: ", str(roadPointWidthMap[road]['width']))
xData = points[0, :]
yData = points[1, :]
if len(xData) < 3:
#print ('Cannot apply spline with [' + str(len(xData)) + '] points. At least 3 needed.')
x = []
#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(args.outFile)
if TIMER:
toc()
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)