def expandObstacle(inputMap):
robotRadius = .25#amount to expand obstacles by
expandedObsVal = 0.05#how sure we are that things near obstacles are also obstacles
newGrid = OccupancyGrid()
newGrid.data = list(inputMap.data)
newGrid.header = inputMap.header
newGrid.info = inputMap.info
inputArray = inputMap.data
gridResolution = newGrid.info.resolution
gridWidth = newGrid.info.width
n = 0
#print "expanding obstacles"
numExpansions = ceil(robotRadius/gridResolution)
#for each cell, increase the value of nearby cells proportional to this cell's value
for n in range(len(inputArray)):
if inputArray[n] > 0:
#determine L bound
if n%gridWidth < numExpansions:
L = n%gridWidth
else:
L = numExpansions
#determine Upper bound
if n/gridWidth < numExpansions:
D = trunc(n/gridWidth)
else:
D = numExpansions
#determine R bound
if gridWidth - n%gridWidth - 1 < numExpansions:
R = gridWidth - n%gridWidth - 1
else:
R = numExpansions
#determine depth
if len(inputArray) - n <= numExpansions * gridWidth:
U = trunc((len(inputArray) - n - 1)/gridWidth)
else:
U = numExpansions
n2 = 0
n3 = 0
for n2 in range(int(U+D+1)):
for n3 in range(int(L+R+1)):
#print "girdwidth %s, U %s, L %s, D %s, R %s"%(gridWidth,U,L,D,R)
tempLoc = int(n + gridWidth * (U - n2) + (n3 - L))
#print "n = %s, temploc = %s (%s, %s)"%(n, tempLoc, n3, n2)
newGrid.data[tempLoc] += inputArray[n] * expandedObsVal
if newGrid.data[tempLoc] > 100:
newGrid.data[tempLoc] = 100
return newGrid
def combineMaps2(localMap, globalMap):
#print "combining maps"
localMapWidth = localMap.info.width
globalMapWidth = globalMap.info.width
newGrid = OccupancyGrid()
newGrid.header = globalMap.header
newGrid.info = globalMap.info
newGrid.data = globalMap.data
#localMap.info.origin.position.y += localMapWidth * localMap.info.resolution / 2
#print "local map origin, global = %s\n%s"%(localMap.info.origin.position,globalMap.info.origin.position)
#print "local map resolution = %s"%(localMap.info.resolution)
#determine the relative location of the maps
if localMap.info.origin.position.x < globalMap.info.origin.position.x:
tempX = 0
else:
tempX = (localMap.info.origin.position.x - globalMap.info.origin.position.x)/globalMap.info.resolution
if localMap.info.origin.position.y < globalMap.info.origin.position.y:
tempY = 0
else:
tempY = (localMap.info.origin.position.y - globalMap.info.origin.position.y)/globalMap.info.resolution
n = tempY * globalMapWidth + tempX
#print "x,y = %s,%s"%(tempX,tempY)
#calculate upper bound
if localMap.info.origin.position.y + localMapWidth*localMap.info.resolution > globalMap.info.origin.position.y + globalMapWidth*globalMap.info.resolution:
U = localMap.info.origin.position.y/localMap.info.resolution + localMapWidth - globalMap.info.origin.position.y/globalMap.info.resolution - globalMapWidth
else:
U = 0
#calculate left bound
if localMap.info.origin.position.x < globalMap.info.origin.position.x:
L = globalMap.info.origin.position.x/globalMap.info.resolution - localMap.info.origin.position.x/localMap.info.resolution
else:
L = 0
#calculate right bound
if localMap.info.origin.position.x + localMapWidth*localMap.info.resolution > globalMap.info.origin.position.x + globalMapWidth*globalMap.info.resolution:
R = localMap.info.origin.position.x/localMap.info.resolution + localMapWidth - globalMap.info.origin.position.x/globalMap.info.resolution - globalMapWidth
else:
R = 0
#calculate lower bound
if localMap.info.origin.position.y < globalMap.info.origin.position.y:
D = globalMap.info.origin.position.y/globalMap.info.resolution - localMap.info.origin.position.y/localMap.info.resolution
else:
D = 0
#print "n,GWidth,LWitdh,g data = %s,%s,%s,%s"%(n, globalMapWidth, localMapWidth,len(globalMap.data))
n2 = 0
n3 = 0
#make stuff ints
#round to nearest number
n = int(n+0.5)
U = int(U+0.5)
L = int(L+0.5)
R = int(R+0.5)
D = int(D+0.5)
#print "U,L,R,D = %s,%s,%s,%s"%(U,L,R,D)
#replace the use the value of the local map wherever the two maps overlap
for n2 in range(localMapWidth - U - D):
for n3 in range(localMapWidth - L - R):
tempGLoc = n + n2*globalMapWidth + n3
tempLLoc = L + n2*localMapWidth + n3 + D * localMapWidth
#if newGrid.data[tempGLoc] < localMap.data[tempLLoc]:
#print "tempG,tempL = %s,%s"%(tempGLoc,tempLLoc)
newGrid.data[tempGLoc] = localMap.data[tempLLoc]
#print "done combining maps"
return newGrid
