def printTarget():
print('printing')
pub = rospy.Publisher('colorcells2', GridCells, queue_size=1)
global mapRes
global startX
global startY
global gridWidth
global gridHeight
global stuff
global origPosX
global origPosY
global closest
print(closest)
grid = GridCells()
grid.header.frame_id = 'map'
grid.cell_width = mapRes
grid.cell_height = mapRes
global xFinal
global yFinal
xFinal = (closest[0]) * mapRes + origPosX
yFinal = (closest[1]) * mapRes + origPosY
t = Point()
t.y = xFinal
t.x = yFinal
t.z = 0
grid.cells.append(t)
while 1:
pub.publish(grid)
def publishCells(grid, num):
global ckd
global bud
global front
global xOffset
global yOffset
# print "publishing"
k = 0
cells = GridCells()
cells.header.frame_id = "map"
cells.cell_width = 0.2 # edit for grid size .3 for simple map
cells.cell_height = 0.2 # edit for grid size
for square in grid: # height should be set to hieght of grid
# print k # used for debugging
point = Point()
point.x = square.x * cells.cell_width + cells.cell_width + xOffset # edit for grid size
point.y = square.y * cells.cell_height + cells.cell_width + yOffset # edit for grid size
point.z = 0
cells.cells.append(point)
# print cells # used for debugging
if num == 100:
pub.publish(cells)
if num == 1:
ckd.publish(cells)
if num == 2:
front.publish(cells)
if num == 4:
bud.publish(cells)
def publishCells(grid):
global pub
print "publishing"
# resolution and offset of the map
k = 0
cells = GridCells()
cells.header.frame_id = 'map'
cells.cell_width = resolution
cells.cell_height = resolution
for i in range(1, height): #height should be set to hieght of grid
k = k + 1
for j in range(1, width): #width should be set to width of grid
k = k + 1
#print k # used for debugging
if (grid[k] == 100):
point = Point()
point.x = (j * resolution) + offsetX + (
1.5 * resolution) # added secondary offset
point.y = (i * resolution) + offsetY - (
.5 * resolution) # added secondary offset ... Magic ?
point.z = 0
cells.cells.append(point)
pub.publish(cells)
def point_to_grid_cells(self,list):
msg=GridCells()
msg.header.frame_id='map'
msg.cell_width=self.metadata.resolution
msg.cell_height=self.metadata.resolution
msg.cells=[self.grid_to_world(loc) for loc in list]
return msg
def map_callback(ret):
rospy.loginfo("Map Callback")
#print ret
map_data = OccupancyMap(ret)
centroids = map_data.findFrontiers()
# Generate all grid cells info
gridCells = GridCells()
gridCells.cell_width = ret.info.resolution
gridCells.cell_height = ret.info.resolution
gridCells.header.frame_id = 'map'
gridCells.header.stamp = rospy.Time.now()
#Generate all of the point data
for centroid in centroids:
point = Point()
cell = transform_grid_cells_to_map_meters((centroid[0], centroid[1]), ret.info)
point.x = cell[0]
point.y = cell[1]
point.z = 0
gridCells.cells.append(point)
# Publish this data
global frontierPub
rospy.loginfo("Publishing frontier")
frontierPub.publish(gridCells)
def calc_grid(self):
if None in [
self.pos, self.map_pos, self.map, self.costmap,
self.measurements
] or len(self.map.data) <= 0:
return
if self.costmap[self.map_pos.x][self.map_pos.y] >= 100:
print("position in wall")
return
if not self._calc_lock.acquire(False):
return
if self.dists is None or self.dists[self.map_pos.x][
self.map_pos.y] <= -1:
self.calc_dists()
grid = GridCells()
grid.header.stamp = rospy.Time.now()
grid.header.frame_id = '/map'
grid.cell_width = grid.cell_height = self.map.info.resolution
grid.cells = list(
Point(
x * self.map.info.resolution +
self.map.info.origin.position.x, y * self.map.info.resolution +
self.map.info.origin.position.y, 0)
for x, y in itertools.product(xrange(len(self.costmap)),
xrange(len(self.costmap[0])))
if self.dists[x][y] > -1 and self.measurements[x][y] <= -1)
self.grid = grid
self._calc_lock.release()
self.send()
def convertMapToPoints(self):
points = GridCells()
pts = 0
points.header.frame_id = "/my_frame"
points.header.stamp = rospy.Time.now()
points.cell_height = self.cellHeight
points.cell_width = self.cellWidth
map = self.map
# map[0,0] = 1
# map[0, self.height-1]=1
# map[self.width-1,0]=1
# map[self.width-1, self.height-1]=1
for i in range(len(map)):
for j in range(len(map[0])):
if map[i, j]>0:
pts += 1
m = Point()
m.x = i * self.cellWidth
m.y = j * self.cellHeight
points.cells.append(m)
return points
def publishCells(nodes, publisher):
"""
Publish Cells
Publishes a list of nodes as GridCells to RViz
Uses the given publisher
"""
global offsetY
global offsetX
print "publishing frontiers"
# resolution and offset of the map
k=0
cells = GridCells()
cells.header.frame_id = 'map'
cells.cell_width = resolution
cells.cell_height = resolution
for node in nodes:
point=Point()
point.x = (node.x * resolution) + offsetX + (0.72 / resolution)
point.y = (node.y * resolution) + offsetY + (0.643 / resolution)
point.z = 0
cells.cells.append(point)
publisher.publish(cells)
def publishChecked(grid):
global ckd
print "publishing"
k=0
cells = GridCells()
cells.header.frame_id = 'map'
cells.cell_width = 0.3 # edit for grid size .3 for simple map
cells.cell_height = 0.3 # edit for grid size
for i in range(1,10): #height should be set to hieght of grid
for j in range(1,9): #height should be set to hieght of grid
#print k # used for debugging
if (grid[k] == 0):
point=Point()
point.x=j*.3+.32 # edit for grid size
point.y=i*.3-.15 # edit for grid size
point.z=0
cells.cells.append(point)
k=k+1
k=k+1
if (grid[k] == 0):
point=Point()
point.x=j*.3+.62 # edit for grid size
point.y=i*.3-.15 # edit for grid size
point.z=0
cells.cells.append(point)
#print cells # used for debugging
ckd.publish(cells)
def publishCells(grid):
global wallpub
print "publishing"
k = 0
cells = GridCells()
cells.header.frame_id = "map"
cells.cell_width = 0.3 # edit for grid size .3 for simple map
cells.cell_height = 0.3 # edit for grid size
for i in range(1, height): # height should be set to hieght of grid
for j in range(1, width): # height should be set to hieght of grid
# print k # used for debugging
if grid[k] == 100:
point = Point()
point.x = j * 0.3 + 0.32 # edit for grid size
point.y = i * 0.3 - 0.15 # edit for grid size
point.z = 0
cells.cells.append(point)
k = k + 1
k = k + 1
if grid[k] == 100:
point = Point()
point.x = j * 0.3 + 0.62 # edit for grid size
point.y = i * 0.3 - 0.15 # edit for grid size
point.z = 0
cells.cells.append(point)
# print cells # used for debugging
wallpub.publish(cells)
def publishFrontier(grid): global pubfrontier # print "publishing frontier" # resolution and offset of the map k=0 cells = GridCells() cells.header.frame_id = 'map' res = resolution * 2 cells.cell_width = res cells.cell_height = res for i in range(1,height/2): #height should be set to hieght of grid k=k+1 for j in range(1,width/2): #width should be set to width of grid k=k+1 #print k # used for debugging if (grid[k] == 100): point=Point() point.x=(j*res)+offsetX + (1.5 * res) # added secondary offset # point.x=(j*res) + (.5 * res) point.y=(i*res)+offsetY - (.5 * res) # added secondary offset ... Magic ? # point.y=(i*res) + (.5 * res) point.z=0 cells.cells.append(point) pubfrontier.publish(cells)
def publishGridCells(listOfPoints, topicNum):
# create new grid cells object
gridCells = GridCells()
# set the list of points, frame, and cell size
gridCells.cells = listOfPoints
gridCells.header.frame_id = "/map"
gridCells.cell_width = mapRes
gridCells.cell_height = mapRes
# publish the grid to the correct topic
if topicNum == 1:
gridCellPub.publish(gridCells)
elif topicNum == 2:
exploredCellsPub.publish(gridCells)
elif topicNum == 3:
frontierCellsPub.publish(gridCells)
elif topicNum == 4:
wayPointsPub.publish(gridCells)
elif topicNum == 5:
pathPub.publish(gridCells)
elif topicNum == 6:
goalCellPub.publish(gridCells)
def waveFront(start, graph, depthLimit):
openSet = Queue()
found = []
frontier = []
openSet.put(start)
resolution = globalMap.info.resolution
cells = GridCells()
cells.header.frame_id = 'map'
cells.cell_width = resolution
cells.cell_height = resolution
while not openSet.empty() and not rospy.is_shutdown():
current = openSet.queue[0]
# found an unknown cell
if graph.cellValue(current) == -1:
found.append(current)
publishCells(frontier, opB_pub, globalMap)
else:
for n in graph.getNeighbors(current):
if graph.cellValue(n) == -1 and current not in frontier and graph.cellValue(current) < 50:
frontier.append(current)
# n.g - how far away fro start the cell is
if n not in found and n not in openSet.queue and n.g < depthLimit:
openSet.put(n)
found.append(openSet.get())
# sub_status = rospy.Subscriber('/move_base/status', GoalStatusArray, findWantedNode, queue_size = 10)
return frontier
def map_to_cells(my_map):
resolution = my_map.info.resolution
gc = GridCells()
gc.cell_width = resolution
gc.cell_height = resolution
gc.header.frame_id = "map"
width = my_map.info.width
height = my_map.info.height
xOrigin = my_map.info.origin.position.x
yOrigin = my_map.info.origin.position.y
for i in range(0, width, 1):
for j in range(0, height, 1):
index = point_to_index((i, j), my_map)
if my_map.data[index] == 0:
gcPoint = Point()
point = convert_location((i, j), my_map)
gcPoint.x = point[0]
gcPoint.y = point[1]
gcPoint.z = 0
gc.cells.append(gcPoint)
return gc
def publishCells(grid):
global pub
global cellMsg
print "publishing"
# resolution and offset of the map
k = 0
cells = GridCells()
cells.header.frame_id = 'map'
# Resolution is set to 0.05m/cell, multiplying by 10 increases this to .5m/cell
# This is approximately equal to the widest point on the turtlebot plus a little
cells.cell_width = resolution
cells.cell_height = resolution
for i in range(0, height): #height should be set to hieght of grid
for j in range(0, width): #width should be set to width of grid
#print k # used for debugging
if (grid[k] == 100):
point = Point()
point.x = (j * resolution) + offsetX + (
0.5 * resolution) # added secondary offset
point.y = (i * resolution) + offsetY + (
0.5 * resolution) # added secondary offset ... Magic ?
point.z = 0
cells.cells.append(point)
k = k + 1
#print "row: ", i+1
#cells.cells = numpy.reshape(grid.data, (grid.info.width,grid.info.height))
cellMsg = cells
pub.publish(cells)
def publishPointListAsGridCells(points, publisher):
gridCells = GridCells() # make an empty grid cells
gridCells.cell_height = G_GridResolution
gridCells.cell_width = G_GridResolution
gridCells.header.frame_id = "map"
gridCells.cells = points
publisher.publish(gridCells)
def drawNodes(self, nodes, topic):
publisher = None
if topic == "frontier":
publisher = self._grid_frontier_pub
elif topic == "explored":
publisher = self._grid_explored_pub
elif topic == "path":
publisher = self._grid_path_pub
elif topic == "wall":
publisher = self._grid_wall_pub
else:
return
cells = GridCells()
cells.header.frame_id = self._map.header.frame_id
cells.cell_width = self._map.info.resolution
cells.cell_height = self._map.info.resolution
cell_points = []
for node in nodes:
(xc, yc) = self.mapCoordsToWorld(node.x, node.y)
cell_x = xc
cell_y = yc
cell_points.append(Point(cell_x, cell_y, 0))
cells.cells = cell_points
publisher.publish(cells)
rospy.sleep(0.01)
def to_grid_cells(self):
msg=GridCells()
msg.header.frame_id='map'
msg.cell_width=self.metadata.resolution
msg.cell_height=self.metadata.resolution
msg.cells=[self.grid_to_world(loc) for loc,val in np.ndenumerate(self.data) if val > 0]
return msg
def getGridUpdate(grid):
global gridIndices
# Initialize variables
cells = GridCells()
cells.header.frame_id = 'map'
cells.cell_width = resolution
cells.cell_height = resolution
k = 0
for i in range(0, height):
for j in range(0, width):
if (grid[k] < 50):
indexChecklist = getNearbyIndices(k)
for index in indexChecklist:
if index not in wallIndices:
wallIndices.append(index)
k = k + 1
gridIndices = 1
wallPoints = getPointsFromIndicies(wallIndices)
#print("Length of wallPoints: ",len(wallIndices))
for point in wallPoints:
outPoint = Point()
outPoint.x = point[0]
outPoint.y = point[1]
outPoint.z = 0
cells.cells.append(outPoint)
#print(point)
mapPub.publish(cells)
def setStart(msg):
global start, pub_start, mapInfo, mapData
#define a point object that will represent our start point on the grid
point = msg.pose.pose.position #this needs to be adjuseted depending on how the gridcells object is using the point object.
#set the starting point for the search to the gridpoint defined by the user mouse click.
start = globalToGrid(point, mapInfo)
#convert the point to a grid position
point.x = round(point.x/mapInfo.resolution) * mapInfo.resolution
point.y = round(point.y/mapInfo.resolution) * mapInfo.resolution
#define a new gridcells object
gridCells = GridCells()
#start construction our message
gridCells.header = msg.header
gridCells.cell_width = mapInfo.resolution
gridCells.cell_height = mapInfo.resolution
cells = [point]
gridCells.cells = cells
pub_start.publish(gridCells)
print "startpoint set"
def og_pub_gridcells(points, z, pub, og):
"""
Brief: Publishes list of points (i, j) as gridcells
Inputs:
points [list(tuple(i, j))]: List of grid coordinates
z [float]: Height to print at
pub [rospy.Publisher]: Publisher
og [OccupancyGrid]: Reference grid
Return: None
"""
# Creaty empty grid cells object
grid_cells = GridCells()
grid_cells.header.frame_id = og.header.frame_id
grid_cells.cell_width = og.info.resolution
grid_cells.cell_height = og.info.resolution
# Convert points to grid cell array
cells = []
for point in points:
i = point[0]
j = point[1]
x, y = og_ij_to_xy(i, j, og)
cells.append(Point(x, y, z))
grid_cells.cells = cells
# Publish on given publisher
pub.publish(grid_cells)
def publishCells(grid, num):
global frontierPub
global clearPub
global wallPub
global xOffset
global yOffset
#print "publishing"
k=0
cells = GridCells()
cells.header.frame_id = 'map'
cells.cell_width = 0.2 # edit for grid size .3 for simple map
cells.cell_height = 0.2 # edit for grid size
for square in grid: #height should be set to hieght of grid
#print k # used for debugging
point=Point()
point.x=square.x*cells.cell_width+cells.cell_width+xOffset # edit for grid size
point.y=square.y*cells.cell_height+cells.cell_width+yOffset # edit for grid size
point.z=0
cells.cells.append(point)
#print cells # used for debugging
if(num == 0):
frontierPub.publish(cells)
elif(num == 1):
clearPub.publish(cells)
elif(num == 2):
wallPub.publish(cells)
def publishGridCells(listOfPoints, topicNum):
# create new grid cells object
gridCells = GridCells()
# set the list of points, frame, and cell size
gridCells.cells = listOfPoints
gridCells.header.frame_id = "/map"
gridCells.cell_width = mapRes
gridCells.cell_height = mapRes
# publish the grid to the correct topic
if topicNum == 7:
frontierPub.publish(gridCells)
else:
time.sleep(0.5)
if topicNum == 8:
smallFrontPub0.publish(gridCells)
elif topicNum == 9:
smallFrontPub1.publish(gridCells)
elif topicNum == 10:
smallFrontPub2.publish(gridCells)
elif topicNum == 11:
smallFrontPub3.publish(gridCells)
elif topicNum == 12:
smallFrontPub4.publish(gridCells)
def publishGridCells(listOfPoints, topicNum):
# create new grid cells object
gridCells = GridCells()
# set the list of points, frame, and cell size
gridCells.cells = listOfPoints
gridCells.header.frame_id = "/map"
gridCells.cell_width = mapRes
gridCells.cell_height = mapRes
# publish the grid to the correct topic
if topicNum == 7:
frontierPub.publish(gridCells)
else:
time.sleep(0.5)
if topicNum == 8:
smallFrontPub0.publish(gridCells)
elif topicNum == 9:
smallFrontPub1.publish(gridCells)
elif topicNum == 10:
smallFrontPub2.publish(gridCells)
elif topicNum == 11:
smallFrontPub3.publish(gridCells)
elif topicNum == 12:
smallFrontPub4.publish(gridCells)
def publishClosedCellsReduce(map2D):
global resolution
global scale
global reducedHeight
global reducedWidth
global x0
global x1
global y0
global y1
gridCells = GridCells()
gridCells.header.frame_id = "/map"
gridCells.header.stamp = rospy.Time.now()
gridCells.cell_width = resolution*scale
gridCells.cell_height = resolution*scale
xyscale = 1.0/(resolution*scale)
pointList = []
for x in range(reducedWidth/scale):
for y in range(reducedHeight/scale):
if(map2D[y][x] > obsThresh):
p = Point()
p.x = float((x+x0/scale)/xyscale)+1/(2*xyscale) + originx
p.y = float((y+y0/scale)/xyscale)+1/(2*xyscale) + originy
p.z = 0
pointList.append(p)
gridCells.cells = pointList
closedPub.publish(gridCells)
def publishGridCellList(lst,typ):
global resolution
global scale
gridCells = GridCells()
gridCells.header.frame_id = "/map"
gridCells.header.stamp = rospy.Time.now()
gridCells.cell_width = resolution*scale
gridCells.cell_height = resolution*scale
xyscale = 1.0/(resolution*scale)
pntList=[]
for pnt in lst:
p = Point()
# p.x= float(pnt.x/xyscale)+1/(2*xyscale)
# p.y= float(pnt.y/xyscale)+1/(2*xyscale)
p.x = float((pnt.x+x0/scale)/xyscale)+1/(2*xyscale) + originx
p.y = float((pnt.y+y0/scale)/xyscale)+1/(2*xyscale) + originy
p.z=0
pntList.append(p)
gridCells.cells=pntList
if(typ==0):
openPub.publish(gridCells)
if(typ==1):
closedPub.publish(gridCells)
if(typ==2):
pathVizPub.publish(gridCells)
if(typ==3):
astarVizPub.publish(gridCells)
def newGoalCallback(msg): global endpos global regen_map global goal_pub global has_goal global curmap global has_map if (has_map): #extract point from message point = msg.pose.position print 'Got new gloal position, regenerating map' #round point values to nearest integer endpos = point; print "x: ", endpos.x print "y: ", endpos.y #send rounded goal point as a GridCells message to /lab3/astar/goal end = GridCells() end.cell_width = curmap.info.resolution end.cell_height = curmap.info.resolution end.cells = [mapToWorldPos(curmap, worldToMapCell(curmap, endpos))] end.header.frame_id = 'map' goal_pub.publish(end) #trigger A* path regeneration regen_map = 1 #indicate that we have received a goal position has_goal = 1
def check_occupancy(current, debug): #check whether the cell is occupied
FRONTIER_PUBLISH = rospy.Publisher('/Frontier', GridCells) #Frontier Grid Cells Publisher
neighbors_all = find_all_neighbors(current)
#print neighbors_all
neighbors_empty = []
Frontier = []
for neighbor_index, neighbor in enumerate(neighbors_all):
#cell_index = ( neighbor.y - (RESOLUTION/2) ) * WIDTH
#if MAP_DATA[int(cell_index)] < 50: #EMPTY CELL
for elem in PtArr_Empty:
if numpy.allclose([elem.x], [neighbor.position.x]) and numpy.allclose([elem.y], [neighbor.position.y]):
#if elem.x == neighbor.position.x and elem.y == neighbor.position.y:
#print "index:", cell_index
#print int(cell_index)
Frontier.append(Point(neighbor.position.x, neighbor.position.y, 0))
neighbors_empty.append(neighbor.position)
# print "adding neighbor: ", neighbor_index
#if elem.x == neighbor.position.x:
# print "element_x: ", elem.x
#print "element_y: ", elem.y
#print PtArr_Empty
Frontier_Empty = GridCells()
Frontier_Empty.header = HEADER
Frontier_Empty.cell_width = RESOLUTION
Frontier_Empty.cell_height = RESOLUTION
Frontier_Empty.cells = Frontier
if debug:
count = 0
while count < 1500:
FRONTIER_PUBLISH.publish(Frontier_Empty)
count = count+1
return neighbors_empty
def path_to_gridcell(tuple_list, my_map):
"""
converts a list of tuples (path) to a gridcells object
:param tuple_list: a list of tuples representing the points in a path
:return: GridCells object
"""
resolution = my_map.info.resolution
gc = GridCells()
gc.cell_width = resolution
gc.cell_height = resolution
gc.header.frame_id = "map"
xOrigin = my_map.info.origin.position.x
yOrigin = my_map.info.origin.position.y
for i in tuple_list:
gcPoint = Point()
point = convert_location((i[0], i[1]), my_map)
gcPoint.x = point[0]
gcPoint.y = point[1]
gcPoint.z = .1
gc.cells.append(gcPoint)
return gc
def mapCallback(OccupancyGrid):
print 'Got Occupancy Map'
global expandGrid
global ocGridMeta
global ocGrid
expandGrid = GridCells()
ocGridMeta = OccupancyGrid.info
ocGrid = [[0 for _ in range(OccupancyGrid.info.height)] for _ in range(OccupancyGrid.info.width)] #Generate the ocgrid as a 2-d array
for row in range(OccupancyGrid.info.height):
for col in range(OccupancyGrid.info.width):
i = ( row * OccupancyGrid.info.width) + col
if( OccupancyGrid.data[i] >= 30): #if there is a 60% propabability of obsticle
ocGrid[col][row] = 1 #flag
expandObsticals(col, row) #expand the discovered obstical
elif OccupancyGrid.data[i] == -1: #if the cell is unknown
ocGrid[col][row] = 0 #flag
expandGrid.cell_height = ocGridMeta.resolution
expandGrid.cell_width = ocGridMeta.resolution
expandGrid.header.frame_id = 'map'
ExpandPub.publish(expandGrid)
global position
global startNode
x = position.x
y = position.y
x = int(round((x - ocGridMeta.origin.position.x)/ocGridMeta.resolution))
y = int(round((y - ocGridMeta.origin.position.y)/ocGridMeta.resolution))
print "Got a start Position"
startNode = node(x, y,None,0,0)
if goalNode != None:
aStar(startNode, goalNode)
print 'import done'
def MakeGridCellsFromList(cellList):
gridCells = GridCells()
gridCells.cell_width = 1
gridCells.cell_height = 1
gridCells.cells = cellList
gridCells.header.frame_id = 'map'
return gridCells
def calc_cspace(self, mapdata):
"""
Calculates the C-Space, i.e., makes the obstacles in the map thicker.
:return [OccupancyGrid] The C-Space.
"""
rospy.loginfo("Calculating C-Space")
# Go through each cell in the occupancy grid
# Inflate the obstacles where necessary
padding = 2
newmap = deepcopy(mapdata)
# for each cell in mapdata
# if (cell is walkable) and (cell has an obstacle within padding)
# add padding in newmap at current cell
# input (x,y,pad)
# sx = min(max(0, x-pad), width)
# sy = min(max(0, y-pad), height)
# ex = max(min(width, x+pad), 0)
# ey = max(min(height, y+pad), 0)
# for each xx in range(sx,ex)
# for each yy in range(sy,ey)
# if cell(xx,yx) has obstacle
# return True
# return False
new_data = []
for item in mapdata.data:
new_data.append(item)
for h in range(mapdata.info.height - 1):
for w in range(mapdata.info.width - 1):
neighbors = ExpandMap.neighbors_of_8(mapdata, w, h)
if len(neighbors) < 8:
new_data[ExpandMap.grid_to_index(mapdata, w, h)] = 100
#Create a GridCells message and publish it
expanded_cells = GridCells()
expanded_cells.header.frame_id = "map"
expanded_cells.cell_width = mapdata.info.resolution
expanded_cells.cell_height = mapdata.info.resolution
expanded_cells.cells = []
for h in range(mapdata.info.height):
for w in range(mapdata.info.width):
index = ExpandMap.grid_to_index(mapdata, w, h)
if new_data[index] == 100:
msg_Point = Point()
world_point = PathPlanner.grid_to_world(mapdata, w, h)
msg_Point.x = world_point.x
msg_Point.y = world_point.y
msg_Point.z = 0
expanded_cells.cells.append(msg_Point)
self.expanded_cells.publish(expanded_cells)
## Return the C-space
#print("END")
#print(mapdata)
newmap.data = new_data
return newmap
def MakeGridCellsFromList (cellList): gridCells = GridCells() gridCells.cell_width = .2 gridCells.cell_height = .2 gridCells.cells = cellList gridCells.header.frame_id = 'map' return gridCells
def merge(self, other): overlap_counter = 0 self.f_ids.sort() other.f_ids.sort() # merge the cells for new_cell in other.cells: if new_cell not in self.cells: self.add_cell(new_cell.x, new_cell.y) else: overlap_counter += 1 # add the sizes self.size += other.size - overlap_counter # when in debug mode, merge the gridcells and publish if DEBUG: for gridcell in other.gridcells.cells: if gridcell not in self.gridcells.cells: self.gridcells.cells.append(gridcell) # copy over the publisher if self.f_ids[0] > other.f_ids[0]: dummy_cells = GridCells() dummy_cells.header.frame_id = 'map' dummy_cells.cell_width = resolution dummy_cells.cell_height = resolution self.publisher.publish(dummy_cells) self.publisher = other.publisher self.publish() # merge the frontier ids for f_id in other.f_ids: if f_id not in self.f_ids: self.f_ids.append(f_id) self.f_ids.sort()
def rvizObstacles(w_map):
global obstacles_pub
obstacles_GC = GridCells()
obstacles_GC.cell_width = w_map.info.resolution
obstacles_GC.cell_height = w_map.info.resolution
obstacles_GC.cells = []
obstacle_pts = []
expanded_pts = set([])
e_o_i = set([])
for index, node in enumerate(w_map.data):
if node > 50:
obstacle_pts.append(index)
for index in obstacle_pts:
for i in range(-4, 5):
for j in range(-4, 5):
point = i_to_p(index, w_map.info.width)
point.x += i
point.y += j
e_o_i.add(p_to_i(point, w_map.info.width))
expanded_pts.add(lab_4.gridToWorld(point, w_map))
obstacles_GC.cells = list(expanded_pts)
obstacles_GC.header.frame_id = 'map'
obstacles_pub.publish(obstacles_GC)
return list(e_o_i)
def publishCells(grid):
global wallIndices
# Initialize variables
k = 0
cells = GridCells()
cells.header.frame_id = 'map'
cells.cell_width = resolution
cells.cell_height = resolution
for i in range(0, height): #height should be set to hieght of grid
for j in range(0, width): #width should be set to width of grid
# grid values are probability percentages of occupancy. The following condition gives all the cells where
# occupancy probability is more than 50%
if (grid[k] > 50):
point = Point()
# 0.5*resolution moves the point to the center of the grid cell
point.x = (j * resolution) + offsetX + (0.5 * resolution)
point.y = (i * resolution) + offsetY + (0.5 * resolution)
point.z = 0
cells.cells.append(point)
index = i * width + j
# Store a list of Wall Indices - for checking neighbors
if index not in wallIndices:
wallIndices.append(index)
k = k + 1
# Display walls in rviz
mapPub.publish(cells)
def publishWaypoints(points):
global pubway
print "Publishing Waypoints"
k = 0
cells = GridCells()
cells.header.frame_id = 'map'
cells.cell_width = resolution
cells.cell_height = resolution
waypoints = Path()
waypoints.header.frame_id = 'map'
for i in range(0, len(points)):
point = Point()
point.x = (points[i].x * resolution) + offsetX + (.5 * resolution)
point.y = (points[i].y * resolution) + offsetY + (.5 * resolution)
point.z = 0
cells.cells.append(point)
temp = PoseStamped()
temp.pose.position.x = point.x
temp.pose.position.y = point.y
#temp.pose.orientation.z = 0
waypoints.poses.append(temp)
pubway.publish(waypoints)
def make_gridcells(data):
obs = GridCells()
obs.cell_width = 1
obs.cell_height = 1
obs.header.frame_id = 'map'
obs.cells = data
return obs
def newStartCallback(msg): global startpos global regen_map global start_pub global has_start global curmap global has_map if (has_map): #extract point from message point = msg.pose.pose.position print 'Got new starting position, regenerating path' #round point values to nearest integer startpos = point; print "x: ", startpos.x print "y: ", startpos.y #send rounded start point as a GridCells message to /lab3/astar/start start = GridCells() start.cell_width = curmap.info.resolution start.cell_height = curmap.info.resolution start.cells = [mapToWorldPos(curmap, worldToMapCell(curmap, startpos))] start.header.frame_id = 'map' start_pub.publish(start) #trigger A* path regeneration regen_map = 1 #indicate that we have receive a start position has_start = 2
def GCfromList(List):
# from referenced Source and previous labs
gridCells = GridCells()
gridCells.cell_width = 1
gridCells.cell_height = 1
gridCells.cells = List
gridCells.header.frame_id = 'map'
return gridCells
def publishGrid (nodeList, rez, where):
pub = rospy.Publisher(where, GridCells, queue_size=1)
gridCell = GridCells()
gridCell.cell_width = rez
gridCell.cell_height = rez
gridCell.header.frame_id = "map"
gridCell.cells = nodeList
pub.publish(gridCell)
def publishTwist(position):
pub = rospy.Publisher('~gridCellData', GridCells)
cells = GridCells()
cells.cell_width = 1
cells.cell_height = 1
for i in range(0, len(position) - 1):
cells.cells.append([position[i][0], position[i][1], 0])
pub.publish(cells)
def pubPath(path):
trail = GridCells()
trail.header.frame_id = "map"
trail.cell_width = mapper.resolution
trail.cell_height = mapper.resolution
trail.cells = path
trailPublisher.publish(trail)
def toGridCell(self, resolution, origin): grid = GridCells() grid.header.frame_id = "map" grid.cell_width = resolution grid.cell_height = resolution grid.cells.append(self.toRosPoint(resolution,origin)) return grid
def publishGrid(nodeList):
pub = rospy.Publisher('/path', GridCells, queue_size=1)
gridCell = GridCells()
gridCell.cell_width = mapRez
gridCell.cell_height = mapRez
gridCell.header.frame_id = "map"
gridCell.cells = nodeList
pub.publish(gridCell)
def publishGrid(nodeList, where):
pub = rospy.Publisher(where, GridCells, queue_size=1)
gridCell = GridCells()
gridCell.cell_width = rmap.rez
gridCell.cell_height = rmap.rez
gridCell.header.frame_id = "map"
gridCell.cells = nodeList
pub.publish(gridCell)
def publishGrid (nodeList):
pub = rospy.Publisher('/path', GridCells, queue_size=1)
gridCell = GridCells()
gridCell.cell_width = mapRez
gridCell.cell_height = mapRez
gridCell.header.frame_id = "map"
gridCell.cells = nodeList
pub.publish(gridCell)
def publishCells(grid):
#begin test:
testGrid = grid
# grid = expandObsticals(testGrid)
#end test:
global pub
row = ""
global navigable_gridpos
navigable_gridpos = []
unnavigable_gridpos = []
# resolution and offset of the map
k = 0
cells = GridCells()
cells.header.frame_id = 'map'
cells.cell_width = resolution
cells.cell_height = resolution
for i in range(0, height): # height should be set to hieght of grid
for j in range(0, width): # width should be set to width of grid
#print grid[] # used for debugging
if (grid[k] < 50):
row = row + " "
navigable_gridpos.append((j, i))
if (grid[k] == 100):
unnavigable_gridpos.append((j, i))
if (i > 1):
point = getPoint((j, i - 1))
cells.cells.append(point)
unnavigable_gridpos.append(point)
if (j > 1):
point = getPoint((j - 1, i))
cells.cells.append(point)
unnavigable_gridpos.append(point)
if (width > j):
point = getPoint((j + 1, i))
cells.cells.append(point)
unnavigable_gridpos.append(point)
if (height > i):
point = getPoint((j, i + 1))
cells.cells.append(point)
unnavigable_gridpos.append(point)
# row = row + " " + str (grid[k]);
# point = getPoint((j, i))
# cells.cells.append(point)
k = k + 1
print "\n" + row
row = ""
pub.publish(cells)
def Visualize_Path(path): #Function to publish path
print "visualizing path"
Path = rospy.Publisher('/Path', GridCells)
Path_GridCells = GridCells()
Path_GridCells.header = HEADER
Path_GridCells.cell_width = RESOLUTION
Path_GridCells.cell_height = RESOLUTION
Path_GridCells.cells = path
Path.publish(Path_GridCells)
def aSTAR(start,goal):
global currentPoint
global mapData
global cells_met
cells_met = GridCells();
cells_met.header.frame_id = 'map'
cells_met.cell_width = 0.3 #change based off grid size
cells_met.cell_height = 0.3 #change based off grid size
startPos = start.pose.position
goalPos = goal.position
#publishObjectCells(mapData)
while(not doneFlag and not rospy.is_shutdown()):
#lowest
#direction
costFront=9001
costLeft=9001
costRight=9001
costBack=9001 #derp I capitalized C by accident##############################################################################################################
hugh = heuristic(startPos,goalPos)
readCurrentPos()
mrRogers(currentPoint)
if(not cellOccupied(front)):#im adding not cause I think I did logic wrong
costFront=distanceFormula(currentPoint,front)#seems to fix it but still i think there is a problem with if it is occupied nvm(its capitalization)
costFront+=heuristic(front,goalPos)
if(not cellOccupied(back)):
costBack=distanceFormula(currentPoint,back)
costBack+=heuristic(back,goalPos)
if(not cellOccupied(left)):
costLeft=distanceFormula(currentPoint,left)
costLeft+=heuristic(left,goalPos)
if(not cellOccupied(right)):
costRight=distanceFormula(currentPoint,right)
costRight+=heuristic(right,goalPos)
if(costFront<costLeft):
lowest=costFront
direction= "front";
else:
lowest=costLeft
direction="left"
if(lowest>costBack):
lowest=costBack
direction="back"
if(lowest>costRight):
lowest=costRight
direction="right"
if(direction=="front"):
goFront()
elif(direction=="right"):
goRight()
elif(direction=="back"):
goBack()
elif(direction=="left"):
goLeft()
print "DONE"
def publish_path(self):
# publish path topic
cs = GridCells()
cs.header.frame_id = 'map'
cs.cell_width = self.map_res
cs.cell_height = self.map_res
cs.cells = self.path_cells
self.path_pub.publish(cs)
def publishFrontierCells(frontier, pub):
cells = GridCells()
cells.header.frame_id = 'map'
cells.cell_width = 0.05
cells.cell_height = 0.05
for i in range(0, len(frontier)):
point = frontier[i]
cells.cells.append(point)
pub.publish(cells)
def rvizPath(cell_list, worldMap):
global path_pub
path_GC = GridCells()
path_GC.cell_width = worldMap.info.resolution
path_GC.cell_height = worldMap.info.resolution
path_GC.cells = []
for cell in cell_list:
path_GC.cells.append(gridToWorld(cell, worldMap))
path_GC.header.frame_id = 'map'
path_pub.publish(path_GC)
def publish_expanded(self):
# publish expanded topic
cs = GridCells()
cs.header.frame_id = 'map'
cs.cell_width = self.map_res
cs.cell_height = self.map_res
cs.cells = self.exp_cells
self.exp_pub.publish(cs)
def publish_frontier(self):
# pubish frontier topic
cs = GridCells()
cs.header.frame_id = 'map'
cs.cell_width = self.map_res
cs.cell_height = self.map_res
cs.cells = self.front_cells
self.front_pub.publish(cs)
def rvizFrontier(cell_list, w_map):
global frontier_pub
frontier_GC = GridCells()
frontier_GC.cell_width = w_map.info.resolution
frontier_GC.cell_height = w_map.info.resolution
frontier_GC.cells = []
for cell in cell_list:
frontier_GC.cells.append(lab_4.gridToWorld(i_to_p(cell, w_map.info.width), w_map))
frontier_GC.header.frame_id = 'map'
frontier_pub.publish(frontier_GC)
def rvizUnexplored(cell_list, w_map):
global unexplored_pub
unexplored_GC = GridCells()
unexplored_GC.cell_width = w_map.info.resolution
unexplored_GC.cell_height = w_map.info.resolution
unexplored_GC.cells = []
for cell in cell_list:
unexplored_GC.cells.append(lab_4.gridToWorld(i_to_p(cell, w_map.info.width), w_map))
unexplored_GC.header.frame_id = 'map'
unexplored_pub.publish(unexplored_GC)
def createGridCellsMessage():
gridCells = GridCells()
gridCells.header.seq = 0
gridCells.header.stamp = rospy.Time.now()
gridCells.header.frame_id = "map"
gridCells.cell_width = originalGridMessage.info.resolution
gridCells.cell_height = originalGridMessage.info.resolution
gridCells.cells = []
return gridCells
