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 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 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 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 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 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 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 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 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 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 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 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 createGridcells(mapdata, listOfP):
"""
Create a GridCellss message typy given a list of points in the grid coordinate
"""
new_gridcells = GridCells()
new_gridcells.header = mapdata.header
new_gridcells.cell_width = mapdata.info.resolution
new_gridcells.cell_height = mapdata.info.resolution
new_gridcells.cells = []
for p in listOfP:
new_gridcells.cells.append(
PathPlanner.grid_to_world(mapdata, p[0], p[1]))
return new_gridcells
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 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 expandWalls(walls):
global seqNum
global offSetX
global offSetY
global resolution
global expandedWallsPub
expansionList = []
scaledExpansionList = []
expandedWallList = []
for i in walls:
if ((i[0] + 1), i[1]) not in expansionList:
if ((i[0] + 1), i[1]) not in walls:
expansionList.append(((i[0] + 1), i[1]))
if ((i[0] - 1), i[1]) not in expansionList:
if ((i[0] - 1), i[1]) not in walls:
expansionList.append(((i[0] - 1), i[1]))
if (i[0], (i[1] + 1)) not in expansionList:
if (i[0], (i[1] + 1)) not in walls:
expansionList.append((i[0], (i[1] + 1)))
if (i[0], (i[1] - 1)) not in expansionList:
if (i[0], (i[1] - 1)) not in walls:
expansionList.append((i[0], (i[1] - 1)))
for i in walls:
expandedWallList.append((i[0], i[1]))
for i in expansionList:
expandedWallList.append((i[0], i[1]))
for i in expansionList:
newX = round((i[0] + 0.5) * resolution + offSetX, 5)
newY = round((i[1] + 0.5) * resolution + offSetY, 5)
scaledExpansionList.append((newX, newY))
head = Header()
head.seq = seqNum
seqNum += 1
head.stamp = rospy.get_rostime()
head.frame_id = "map"
points = pointList(scaledExpansionList) #get the points
gridCells = GridCells()
gridCells.header = head
gridCells.cell_width = resolution
gridCells.cell_height = resolution
gridCells.cells = points
expandedWallsPub.publish(gridCells)
return expandedWallList
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)
def Expanded_GridCell(cell): #Expand Cell visually on rviz when debug is activated
EXPANDED_PUBLISHER = rospy.Publisher('/Expanded', GridCells) #Expanded Grid Cells Publisher
cell_row = cell.x
cell_column = cell.y
Expanded = []
Expanded.append(Point(cell_row, cell_column, 0))
Expanded_GridCell = GridCells()
Expanded_GridCell.header = HEADER
Expanded_GridCell.cell_width = RESOLUTION
Expanded_GridCell.cell_height = RESOLUTION
Expanded_GridCell.cells = Expanded
count = 0
while count < 1500:
EXPANDED_PUBLISHER.publish(Expanded_GridCell)
count = count+1
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)
def readOdom(msg):
global start, pub_start, mapInfo, mapData, pose, yaw, odom_list
pose = msg.pose.pose
point = msg.pose.pose.position
quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
yaw = euler[2]
start = globalToGrid(point, mapInfo)
#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 = [gridToGlobal(start, mapInfo)]
gridCells.cells = cells
def find_centroid(): #returns centroid of frontier
Frontier_Centroid = rospy.Publisher('/FrontierCentroid', GridCells)
x_c = y_c = count = 0
for elem in PtArr_Empty:
x_c = elem.x + x_c
y_c = elem.y + y_c
count = count + 1
x_c = round_to_nearest_cell(x_c/count, RESOLUTION) + (RESOLUTION/2)
y_c = round_to_nearest_cell(y_c/count, RESOLUTION) + (RESOLUTION/2)
Centroid = Point(x_c, y_c, 0)
gridcells_centroid = GridCells()
gridcells_centroid.header = HEADER
gridcells_centroid.cell_width = RESOLUTION
gridcells_centroid.cell_height = RESOLUTION
gridcells_centroid.cells = [Centroid]
Frontier_Centroid.publish(gridcells_centroid)
Centroid_Pose = Pose()
Centroid_Pose.position = Centroid
Centroid_Pose.orientation.w = 0
return Centroid_Pose
def make_grid_cell(self):
resolution = self.og.info.resolution
width = self.og.info.width
height = self.og.info.height
offsetX = self.og.info.origin.position.x
offsetY = self.og.info.origin.position.y
# resolution and offset of the map
k = 0
cells = GridCells()
cells.header = self.og.header
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
if self.onedmap[k] < self.threshold:
cells.cells.append(getPoint((j, i), resolution, offsetX, offsetY))
k += 1
return cells
def setGoal(msg):
global goal, pub_goal, goalPoint
point = msg.pose.position
#set the goal point for the search to the gridpoint defined by the user mouse click.
goal = globalToGrid(point, mapInfo)
goalPoint = gridToGlobal(goal, mapInfo)
#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 = [goalPoint]
gridCells.cells = cells
pub_goal.publish(gridCells)
print "goal set at"
print goal
def setGoal(msg):
global goal, pub_goal, goalPoint
point = msg.pose.position
#set the goal point for the search to the gridpoint defined by the user mouse click.
goal = globalToGrid(point, mapInfo)
goalPoint = gridToGlobal(goal, mapInfo)
#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 = [goalPoint]
gridCells.cells = cells
pub_goal.publish(gridCells)
print "goal set at"
print goal
def publishGridCells(path):
global seqNum
global offSetX
global offSetY
global resolution
global gridCellsPub
head = Header()
head.seq = seqNum
seqNum += 1
head.stamp = rospy.get_rostime()
head.frame_id = "map"
points = pointList(path) #get the points
gridCells = GridCells()
#fill the message with the necessary data
gridCells.header = head
gridCells.cell_width = resolution
gridCells.cell_height = resolution
gridCells.cells = points
gridCellsPub.publish(gridCells)
def readOdom(msg):
global start, pub_start, mapInfo, mapData, pose, yaw, odom_list
pose = msg.pose.pose
point = msg.pose.pose.position
quaternion = (
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
yaw = euler[2]
start = globalToGrid(point, mapInfo)
#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 = [gridToGlobal(start, mapInfo)]
gridCells.cells = cells
def publishGridCells(path):#takes a list of cells and publishes them to a given topic
global seqNum
global resolution
global gridCellsPub
#create header:
head = Header()
head.seq = seqNum
seqNum += 1
head.stamp = rospy.get_rostime()
head.frame_id = "map"
points = pointList(path)#get the points
gridCells = GridCells()#create the message
#fill the message with the necessary data
gridCells.header = head
gridCells.cell_width = resolution
gridCells.cell_height = resolution
gridCells.cells = points
print "Points: "
print points
gridCellsPub.publish(gridCells)
def __init__(self):
rospy.loginfo("Starting FrontierExploration node...")
rospy.init_node("frontier_exploration")
# store OccupancyGrid
self.mapdata = PathPlanner.request_map()
self.cspace = self.calc_cspace(self.mapdata, 3)
self.unexplored_frontier = []
self.px = 0.0
self.py = 0.0
self.pth = 0.0
self.start_location = 0
# rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, self.update_odometry)
rospy.Subscriber('odom', Odometry, self.update_odometry)
# rospy.Subscriber('/path_planner/cspace', OccupancyGrid, self.update_cspace)
self.goal_pub = rospy.Publisher('/move_base_simple/goal',
PoseStamped,
queue_size=10)
self.frontier_goal_pub = rospy.Publisher(
'/move_base_simple/frontier_goal', PoseStamped, queue_size=10)
self.goal_viz_pub = rospy.Publisher('/goal_location',
GridCells,
queue_size=10)
self.frontier_pub = rospy.Publisher('/explore_frontier',
GridCells,
queue_size=10)
rospy.loginfo("FrontierExploration node started.")
rospy.sleep(3)
# loop until map is mostly revealed
while (len(self.unexplored_frontier)
== 0) or (len(self.unexplored_frontier) > 10):
# store frontier as list of points
self.mapdata = PathPlanner.request_map()
self.cspace = self.calc_cspace(self.mapdata, 3)
self.unexplored_frontier = []
self.update_frontier()
frontier_msg = GridCells()
frontier_cells = []
for e in range(len(self.unexplored_frontier)):
x = e % self.cspace.info.width
y = e / self.cspace.info.height
p = Point()
p.x = x
p.y = y
frontier_cells.append(p)
frontier_msg.header = self.cspace.header
frontier_msg.header.stamp = rospy.get_rostime()
frontier_msg.cell_width = self.cspace.info.resolution
frontier_msg.cell_height = self.cspace.info.resolution
frontier_msg.cells = frontier_cells
self.frontier_pub.publish(frontier_msg)
nearest = self.find_nearest_frontier()
rospy.loginfo("Going to frontier: " + str(nearest))
self.create_frontier_goal_msg(nearest)
rospy.sleep(20)
# save map
self.save_map()
# return to start
msg = PoseStamped()
msg.pose = self.start_location
msg.header.frame_id = 'map'
self.goal_pub.publish(msg)
def a_star(self, mapdata, start, goal):
"""
Generate a path as a list of tuple from Start to goal using A*
"""
print "Inside A star"
rospy.loginfo("Generate path from (%d,%d) to (%d,%d)" %
(start[0], start[1], goal[0], goal[1]))
if not PathPlanner.is_cell_walkable(mapdata, goal[0], goal[1]):
rospy.logerr("not walkable goal")
return []
#calculated from goal
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0
while not frontier.empty():
frontier_msg = GridCells()
frontier_cells = []
for e in frontier.elements:
frontier_cells.append(
PathPlanner.grid_to_world(mapdata, e[1][0], e[1][1]))
frontier_msg.header = mapdata.header
frontier_msg.header.stamp = rospy.get_rostime()
frontier_msg.cell_width = mapdata.info.resolution
frontier_msg.cell_height = mapdata.info.resolution
frontier_msg.cells = frontier_cells
expanded_msg = GridCells()
expanded_cells = []
for e in cost_so_far:
expanded_cells.append(
PathPlanner.grid_to_world(mapdata, e[0], e[1]))
expanded_msg.header = mapdata.header
expanded_msg.header.stamp = rospy.get_rostime()
expanded_msg.cell_width = mapdata.info.resolution
expanded_msg.cell_height = mapdata.info.resolution
expanded_msg.cells = expanded_cells
self.expanded_pub.publish(expanded_msg)
rospy.sleep(0.01)
current = frontier.get()
#creates path
if current == goal:
entry = goal
listOfCoord = []
while entry != None:
listOfCoord.append(entry)
entry = came_from[entry]
listOfCoord.reverse()
self.expanded_pub.publish(
PathPlanner.createGridcells(mapdata, listOfCoord))
return listOfCoord
for next in PathPlanner.neighbors_of_8(mapdata, current[0],
current[1]):
new_cost = cost_so_far[
current] + 1 #assume cost to move each unit is 1
if next not in cost_so_far or new_cost < cost_so_far[next]:
cost_so_far[next] = new_cost
priority = new_cost + PathPlanner.euclidean_distance(
next[0], next[1], goal[0], goal[1])
frontier.put(next, priority)
came_from[next] = current
return []
