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 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 pub_cells(self, cells, cell_pub, cells_as_points=False):
msg = GridCells()
msg.cell_height = star.map_resolution
msg.cell_width = star.map_resolution
msg.header.frame_id = '/map'
if cells_as_points:
msg.cells = cells
else:
msg.cells = star.cell_coords_to_points( cells )
cell_pub.publish(msg)
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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 MakeGridCellsFromList (cellList): gridCells = GridCells() gridCells.cell_width = .2 gridCells.cell_height = .2 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 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 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 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 MakeGridCellsFromList(cellList):
gridCells = GridCells()
gridCells.cell_width = 1
gridCells.cell_height = 1
gridCells.cells = cellList
gridCells.header.frame_id = 'map'
return gridCells
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 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 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 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 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 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 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 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 MapHandler(self,map):
global botWorld, resolution
self.width = map.info.width
self.height = map.info.height
self.resolution = map.info.resolution
resolution = self.resolution
self.originX = map.info.origin.position.x
self.originY = map.info.origin.position.y
self.cSpaceConverter(map.data)
cell = GridCells()
cell.header.frame_id = "map"
cell.cell_width = self.resolution
celll.cell_height = self.resolution
inaccessible = GridCells()
inaccessible.header.frame_id = "map"
inaccessible.cell_width = self.resolution
inaccessible.cell_height = self.resolution
freespace = []
occupied = []
for position, score in enumerate(self.world):
point = Point()
point.x = (index % self.width)*self.resolution+self.originX
point.y = (index/self.width)*self.resolution+self.originY
point.z = 0
if score is 0 or -1:
freespace.append(point)
if score is 2:
occupied.append(point)
cell.cells = freespace
inaccessible.cells = [occupied]
inaccessiblePublisher.publish(inaccessible)
botWorld = freespace
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 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 calc_cspace(self, mapdata, padding=3):
"""
Calculates the C-Space, i.e., makes the obstacles in the map thicker.
Publishes the list of cells that were added to the original map.
:param mapdata [OccupancyGrid] The map data.
:param padding [int] The number of cells around the obstacles.
:return [[int8]] The C-Space as a list of values from 0 to 100.
"""
rospy.loginfo("Calculating C-Space")
## Goes through each cell in the occupancy grid
## Inflates the obstacles where necessary based on padding argument
self.orig_map = self.map
new_map = []
print "calc_cspace: ", type(mapdata)
total_range = mapdata.info.width * mapdata.info.height
for p in range(0, padding):
copy = list(mapdata.data)
for i in range(0, total_range):
if mapdata.data[i] == 100:
#coordinates are found via indexing through occupancy grid.
#Index to grid is used to convert the index to useable grid coordinates
coordinates = PathPlanner.index_to_grid(mapdata, i)
pos_x = coordinates[0]
pos_y = coordinates[1]
neighbors = PathPlanner.neighbors_of_8(
mapdata, pos_x, pos_y)
#iterates through neighbors list and changes all available surrounding neighbors to obstacles
for n in neighbors:
index_value = PathPlanner.grid_to_index(
mapdata, n[0], n[1])
copy[index_value] = 100
world_pos = PathPlanner.grid_to_world(
mapdata, n[0], n[1])
cell_point = Point()
cell_point.x = world_pos[0]
cell_point.y = world_pos[1]
cell_point.z = 0
new_map.append(cell_point)
mapdata.data = copy
## Creates a GridCells meassage and publish it
grid_cells_message = GridCells()
grid_cells_message.header.frame_id = 'map'
grid_cells_message.cell_width = mapdata.info.resolution
grid_cells_message.cell_height = mapdata.info.resolution
grid_cells_message.cells = new_map
## Returns the C-space
self.cspace.publish(grid_cells_message)
return mapdata.data
def createGridCellsMessage(gridResolution):
gridCells = GridCells()
gridCells.header.seq = 0
gridCells.header.stamp = rospy.Time.now()
gridCells.header.frame_id = "map"
gridCells.cell_width = gridResolution
gridCells.cell_height = gridResolution
gridCells.cells = []
return gridCells
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 publishPoint(point, pub, mapInfo):
gridCells = GridCells()
gridCells = GridCells()
gridCells.header = Header()
gridCells.header.frame_id = "map"
gridCells.cell_width = mapInfo.resolution
gridCells.cell_height = mapInfo.resolution
gridCells.cells = [point]
pub.publish(gridCells)
def createGridCellsMessage(gridResolution):
gridCells = GridCells()
gridCells.header.seq = 0
gridCells.header.stamp = rospy.Time.now()
gridCells.header.frame_id = "map"
gridCells.cell_width = gridResolution
gridCells.cell_height = gridResolution
gridCells.cells = []
return gridCells
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 rvizExpanded(cell_list, w_map):
global expanded_pub
expanded_GC = GridCells()
expanded_GC.cell_width = w_map.info.resolution
expanded_GC.cell_height = w_map.info.resolution
expanded_GC.cells = []
for cell in cell_list:
expanded_GC.cells.append(lab_4.gridToWorld(i_to_p(cell, w_map.info.width), w_map))
expanded_GC.header.frame_id = 'map'
expanded_pub.publish(expanded_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
def showCells(listOfPoints, pub, map): cells = GridCells() cells.header.frame_id = 'map' cells.cell_width = MyGlobals.globalMap.info.resolution cells.cell_height = MyGlobals.globalMap.info.resolution cells.cells = list() for point in listOfPoints: point = convertToPose(point, map).position cells.cells.append(point) pub.publish(cells)
def Frontier_init():
Frontier = rospy.Publisher('/Frontier', GridCells)
#find maximum point in empty space in the x and y drection
Array_EmptyCells = PtArr_Empty
OCCUPIED = False
MAX_BOOL = False
MIN_BOOL = False
while Array_EmptyCells:
MAX_Point = max(Array_EmptyCells)
MIN_Point = min(Array_EmptyCells)
Frontier_Array = []
while True:
#check If the point after the max_point.x is occupied
for elem in PtArr_Occupied:
if (numpy.close(MAX_Point.x + RESOLUTION, elem.x) and numpy.close(MAX_Point.y, elem.y)):
OCCUPIED = True
MAX_BOOL = True
break
for elem in PtArr_Occupied:
if (numpy.close(MIN_Point.x - RESOLUTION, elem.x) and numpy.close(MIN_Point.y, elem.y)):
MIN_BOOL = True
OCCUPIED = True
break
if not OCCUPIED:
if MAX_BOOL:
Frontier_Array.append(MAX_Point) #add it to frontier array
if MIN_BOOL:
Frontier_Array.append(MIN_Point) #add it to frontier array
Frontier_MaxPoint = Max_Point
Frontier_MinPoint = Min_Point
while True:
Frontier_MaxPoint.y = Frontier_MaxPoint.y + RESOLUTION #Next frontier point
for elem in PtArr_Occupied:
if numpy.close(Frontier_MaxPoint.y, elem.y ) and numpy.close(MAX_Point.x, elem.x) :
Frontier_Array.append(Frontier_MaxPoint)
break
for elem in PtArr_Occupied:
if numpy.close(Frontier_MinPoint.y, elem.y ) and numpy.close(Frontier_MinPoint.x, elem.x) :
Frontier_Array.append(Frontier_MinPoint)
break
break
#Remove row from frontier array
for elem in Array_EmptyCells:
if numpy.close(MAX_Point.y, elem.y):
Array_EmptyCells.remove(elem)
gridcells_frontier = GridCells()
gridcells_frontier.header = HEADER
gridcells_frontier.cell_width = RESOLUTION
gridcells_frontier.cell_height = RESOLUTION
gridcells_frontier.cells = Frontier_Array
Frontier.publish(gridcells_frontier)
return
def publishGridList(list_of_cells, mapInfo, pub):
cells = []
for x in list_of_cells:
cells.append(gridToGlobal(x, mapInfo))
gridCells = GridCells()
gridCells.header = Header()
gridCells.header.frame_id = "map"
gridCells.cell_width = mapInfo.resolution
gridCells.cell_height = mapInfo.resolution
gridCells.cells = cells
pub.publish(gridCells)
def Occupancy_Grid_init(): #initialize visual aid for Occupancy Grid
global PtArr_Empty, PtArr_Unknown, PtArr_Occupied
GridCells_Occupied = rospy.Publisher('/GridCellsOccupied', GridCells)
GridCells_Unknown = rospy.Publisher('/GridCellsUnknown', GridCells)
GridCells_Empty = rospy.Publisher('/GridCellsEmpty', GridCells)
PtArr_Occupied = []
PtArr_Unknown = []
PtArr_Empty = []
for i in range(len(MAP_DATA)):
row = round(((i / WIDTH)*RESOLUTION) + (RESOLUTION/2), 4) + ORIGIN_Y
column = round(((i % WIDTH)*RESOLUTION) + (RESOLUTION/2), 4) + ORIGIN_X
if MAP_DATA[i] > 50: #Occupied
PtArr_Occupied.append(Point(column, row, 0))
elif MAP_DATA[i] == -1: #Unknown
PtArr_Unknown.append(Point(column, row, 0))
else: #Empty
PtArr_Empty.append(Point(column, row, 0))
gridcells_Occupied = GridCells()
gridcells_Occupied.header = HEADER
gridcells_Occupied.cell_width = RESOLUTION
gridcells_Occupied.cell_height = RESOLUTION
gridcells_Occupied.cells = PtArr_Occupied
gridcells_Unknown = GridCells()
gridcells_Unknown.header = HEADER
gridcells_Unknown.cell_width = RESOLUTION
gridcells_Unknown.cell_height = RESOLUTION
gridcells_Unknown.cells = PtArr_Unknown
gridcells_Empty = GridCells()
gridcells_Empty.header = HEADER
gridcells_Empty.cell_width = RESOLUTION
gridcells_Empty.cell_height = RESOLUTION
gridcells_Empty.cells = PtArr_Empty
GridCells_Occupied.publish(gridcells_Occupied)
GridCells_Unknown.publish(gridcells_Unknown)
GridCells_Empty.publish(gridcells_Empty)
def publish_frontier():
"""
Publishes the information stored in unexplored_cells to the map
"""
db_print('publishFrontier')
pub_unexplored = rospy.Publisher('/frontier_cells', GridCells, queue_size=1)
# Information all GridCells messages will use
msg = GridCells()
msg.header.frame_id = 'map'
msg.cell_width = CELL_WIDTH
msg.cell_height = CELL_HEIGHT
msg.cells = frontier_cells
pub_unexplored.publish(msg)
def pubMap(publisher, mapInfo, mapData):
cells = []
for i in range(mapInfo.width * mapInfo.height):
if mapData[i] == 100:
gridPoint = indexToGrid(i, mapInfo)
gp = gridToGlobal(gridPoint, mapInfo)
cells.append(gp)
gridCells = GridCells()
gridCells.header = Header()
gridCells.header.frame_id = "map"
gridCells.cell_width = mapInfo.resolution
gridCells.cell_height = mapInfo.resolution
gridCells.cells = cells
publisher.publish(gridCells)
