def generatePath(cmap, goal, parents, start, width): global path_pub global way_pub global curmap #Create GridCells() message to display path and waypoints in rviz path = GridCells() path.cell_width = curmap.info.resolution path.cell_height = curmap.info.resolution path.header.frame_id = 'map' way = GridCells() way.cell_width = curmap.info.resolution way.cell_height = curmap.info.resolution way.header.frame_id = 'map' waycells = [mapToWorldPos(cmap, goal)] #Create a list of cells starting with the start position cells = [mapToWorldPos(cmap, start)] #trace path from goal back to start current = parents[normalize(goal, width)] lastpt = goal last_ang = math.atan2((current.y-lastpt.y),(current.x-lastpt.x)) while current != start: cells.append(mapToWorldPos(cmap, current)) #if we change travel direction, add a waypoint ang = math.atan2((current.y-lastpt.y),(current.x-lastpt.x)) if (abs(ang-last_ang) > 0.1): waycells.append(mapToWorldPos(cmap, lastpt)) last_ang = ang lastpt = current current = parents[normalize(current, width)] path.cells = cells way.cells = list(reversed(waycells)) path_pub.publish(path) way_pub.publish(way)
def displayFrontier(): rospy.init_node('lab3') pubGrid = rospy.Publisher("/frontier", GridCells, queue_size=1) pubGrid2 = rospy.Publisher("/explored", GridCells, queue_size=1) frontierCells = GridCells() # create a new GridCells message frontierCells.header.frame_id = 'map' frontierCells.cell_width = 0.3000 # resolution value from map.yaml file frontierCells.cell_height = 0.3000 exploredCells = GridCells() # create a new GridCells message exploredCells.header.frame_id = 'map' exploredCells.cell_width = 0.3000 # resolution value from map.yaml file exploredCells.cell_height = 0.3000 for node in frontierDisplay: point = Point() point.x = node.position[0] point.y = node.position[1] point.z = 0 frontierCells.cells.append(point) for node2 in explored: point = Point() point.x = node2.position[0] point.y = node2.position[1] point.z = 0 exploredCells.cells.append(point) pubGrid.publish(frontierCells) pubGrid2.publish(exploredCells)
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 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 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 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 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 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 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 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 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 voronoi_grid_cb(msg_var, data_var): global voronoi_grid_publisher, list_voronoi_grid, header idx = data_var voronoi_grid = np.asarray(msg_var.data).reshape( (len(par.Xaxis), len(par.Xaxis))) list_voronoi_grid[idx] = voronoi_grid voronoi_grid = np.fliplr(voronoi_grid) #np.rot90(voronoi_grid,k=2) voronoi_grid = list(rotated(voronoi_grid)) voronoi_grid = list(rotated(voronoi_grid)) voronoi_grid = list(rotated(voronoi_grid)) voronoi_grid_msg = GridCells() voronoi_grid_msg.header = header voronoi_grid_msg.cell_width = 1.0 voronoi_grid_msg.cell_height = 1.0 voronoi_grid_msg.cells = [] for i in range(par.NumberOfPoints): for j in range(par.NumberOfPoints): if (voronoi_grid[i][j] == 1): temp_point = Point32() temp_point.x = i temp_point.y = j temp_point.z = 0.0 voronoi_grid_msg.cells.append(temp_point) voronoi_grid_publisher[idx].publish(voronoi_grid_msg)
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 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 publishFrontierCell(x, y): global frontier_pub global frontier_cell_list global cell_size global offset msg = GridCells() grid_height = cell_size grid_width = cell_size header = Header() header.frame_id = "map" msg.header = header msg.cell_width = grid_width msg.cell_height = grid_height point = Point() point.x = (x * cell_size) + offset + (cell_size / 2) point.y = (y * cell_size) + (cell_size / 2) frontier_cell_list.append(point) msg.cells = frontier_cell_list frontier_pub.publish(msg)
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 cellPublish(): pub = rospy.Publisher('gridCell', GridCells, queue_size=10) rospy.init_node('cellPublish', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): global seqNum #create header: head = Header() head.seq = seqNum seqNum = seqNum + 1 head.stamp = rospy.get_rostime() head.frame_id = "map" points = pointList()#get the points cells = GridCells()#create the message #fill the message with the necessary data cells.header = head cells.cell_width = 1 cells.cell_height = 1 cells.cells = points pub.publish(cells) rate.sleep()
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 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 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 publishVisitedCell(x, y): global visited_pub global visited_cell_list global cell_size global offsets msg = GridCells() grid_height = cell_size grid_width = cell_size header = Header() header.frame_id = "map" msg.header = header msg.cell_width = grid_width msg.cell_height = grid_height point = Point() point.x = (x * cell_size) + offset + (cell_size / 2) point.y = (y * cell_size) + (cell_size / 2) visited_cell_list.append(point) msg.cells = visited_cell_list visited_pub.publish(msg)
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, 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 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 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 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 MakeGridCellsFromList (cellList): gridCells = GridCells() gridCells.cell_width = .2 gridCells.cell_height = .2 gridCells.cells = cellList gridCells.header.frame_id = 'map' return gridCells
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 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 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 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 publishBad_Map_front(x, y): global bad_map_front_pub global bad_map_frontier_list global cell_size global Xoffset global Yoffset msg = GridCells() grid_height = cell_size grid_width = cell_size header = Header() header.frame_id = "map" msg.header = header msg.cell_width = grid_width msg.cell_height = grid_height point = Point() point.x = (x * cell_size) + Xoffset + (cell_size / 2) point.y = (y * cell_size) + Yoffset + (cell_size / 2) bad_map_frontier_list.append(point) msg.cells = bad_map_frontier_list bad_map_front_pub.publish(msg)
def publishPathCell(x, y): global path_pub global path_cell_list global cell_size global Xoffset global Yoffset msg = GridCells() grid_height = cell_size grid_width = cell_size header = Header() header.frame_id = "map" msg.header = header msg.cell_width = grid_width msg.cell_height = grid_height point = Point() point.x = (x * cell_size) + Xoffset + (cell_size / 2) point.y = (y * cell_size) + Yoffset + (cell_size / 2) path_cell_list.append(point) msg.cells = path_cell_list path_pub.publish(msg)
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 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 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 publishGridCells( cells, topic): #takes a list of cells and publishes them to a given topic global seqNum global resolution pub = rospy.Publisher(topic, GridCells, queue_size=10) #create header: head = Header() head.seq = seqNum seqNum += 1 head.stamp = rospy.get_rostime() head.frame_id = "map" points = pointList(cells) #get the points gridCells = GridCells() #create the message #fill the message with the necessary data gridCells.header = head gridCells.cell_width = 1 gridCells.cell_height = 1 gridCells.cells = points pub.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 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 MakeGridCellsFromList(cellList): gridCells = GridCells() gridCells.cell_width = 1 gridCells.cell_height = 1 gridCells.cells = cellList gridCells.header.frame_id = 'map' return gridCells
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 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 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 make_gridcells(data): obs = GridCells() obs.cell_width = 1 obs.cell_height = 1 obs.header.frame_id = 'map' obs.cells = data return obs
def publishObjectCells(grid): global pubGCell global unknownCell global height global width global occupiedCells k = 0 b = 0 occupiedCells = GridCells() occupiedCells.header.frame_id = 'map' occupiedCells.cell_width = 0.3 #change based off grid size occupiedCells.cell_height = 0.3 #change based off grid size for i in range(1,height): #height should be set to hieght of grid k=k+1 for j in range(1,width): #height should be set to hieght of grid k=k+1 #print k # used for debugging if (grid[k] == 100): point=Point() point.x=j*.3 # edit for grid size point.y=i*.3 # edit for grid size point.z=0 occupiedCells.cells.append(point) pubGCell.publish(occupiedCells) Cells = GridCells() Cells.header.frame_id = 'map' Cells.cell_width = 0.3 #change based off grid size Cells.cell_height = 0.3 #change based off grid size for i in range(1,height): #height should be set to hieght of grid b=b+1 for j in range(1,width): #height should be set to hieght of grid b=b+1 #print b # used for debugging if (grid[b] == 0): point1=Point() point1.x=j*.3 # edit for grid size point1.y=i*.3 # edit for grid size point1.z=0 Cells.cells.append(point1) unknownCell.publish(Cells)
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 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 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 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 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 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 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 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