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