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 __init__(self):
#set up the position of therobot
self._odom_list = tf.TransformListener()
self._roll = 0
self._pitch = 0
self._yaw = 0
#current robot orientation
self._current = Pose()
#robot's position
self._robot = WayPoint(-1000000, -1000000)
#the current goal as a waypoint
self._goalWay = None
#the current
self._CurrGoal = None
#update robot's position
rospy.Timer(rospy.Duration(0.1), self.timerCallback)
#size of the robot's width/length
self._robotSize = .1
#the current occupancy grid
self._currmap = None
#the current path as a set of poses
self._currPath = None
#A boolean, wait until the robot's position is set
self.RobotPoseInit = False
#only make one base mao
self.UpdateOriginalMapOnce = True
#list of all waypoints in the base map
self._waypointlist = []
#list of all occupancy grids through time
self._allMaps = []
#list of occupied grids
self._OccGrids = GridCells()
#is there a path that the robot is navigating to?
self.IsPath = False
self.front = GridCells()
self.Unocc = GridCells()
self.frontierList = []
self.isCurrFrontGoal = False
self.frontGoal = WayPoint(-10000,-10000)
#subscribe to the map's occupancy grid.
#set up the visualization for the grid and path
rospy.Subscriber('/map', OccupancyGrid, self.getMapInfo, queue_size=1)
self._showOccupied = rospy.Publisher('/nav_msgs/GridCellsOcc', GridCells, None, queue_size=1)
self._ShowStart = rospy.Publisher('/nav_msgs/GridCellsStart', GridCells, None, queue_size=1)
self._ShowEnd = rospy.Publisher('/nav_msgs/GridCellsEnd', GridCells, None, queue_size=1)
self._ShowPathGrid = rospy.Publisher('/nav_msgs/GridCellsPath', GridCells, None, queue_size=1)
self._ShowPathPath = rospy.Publisher('/Aplan', Path, None, queue_size=1)
self._ShowFront = rospy.Publisher('/nav_msgs/GridCellsFrontier', GridCells, None, queue_size=1)
self._goToFront = rospy.Publisher('/move_base_simple/goal', PoseStamped, None, queue_size=10)
self._Showunocc = self._ShowPathGrid = rospy.Publisher('/nav_msgs/GridCellsChecked', GridCells, None, queue_size=1)
#print("here")
rospy.Timer(rospy.Duration(1), self.CreateMapOccupancy) #will be useful for D*
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 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 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 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 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 publishTarget():
data = GridCells()
point = minimumFrontier(150, 80)
for i in range(point.x - 1, point.x + 2):
for j in range(point.y - 1, point.y + 2):
data.cells.append(addScaledPoint(j, i))
publishCells(targetcentroidpub, data)
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 make_gridcells(data):
obs = GridCells()
obs.cell_width = 1
obs.cell_height = 1
obs.header.frame_id = 'map'
obs.cells = data
return obs
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 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 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(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 __init__(self, name=NODE_NAME):
# Get the ~private namespace parameters from command line or launch
# file.
# initializes ROS node
rospy.init_node(name, anonymous=True, log_level=rospy.DEBUG)
rate = float(rospy.get_param('~rate', '1.0'))
topic = rospy.get_param('~topic', name)
rospy.loginfo("rate = {}".format(rate))
rospy.loginfo("topic = {}".format(topic))
# create the server
rospy.logdebug(dir(ParamSrv))
self.server = ParamSrv(GridCells, self.update_cb)
self.gen = ParameterGenerator()
gen.add("lower", str_t, 0, "The data", 0)
# Create a publisher for our custom message.
self.pub = rospy.Publisher("/turtlebot/profiler",
GridCells,
queue_size=2)
# Initialize message variables.
self.msg = GridCells()
rospy.logdebug(self.msg)
# Main while loop.
while not rospy.is_shutdown():
# Publish our custom message.
self.pub.publish(msg)
# Sleep for a while before publishing new messages. Division is so
# rate != period.
if rate:
rospy.sleep(1 / rate)
else:
rospy.sleep(1.0)
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 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 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 publishNavFrontier():
global reshapedMap
data = GridCells()
executioncount = 0
frontiercount = 0
for i in range(0, height - 1):
for k in range(0, width - 1):
#print("Tried a frontier at")
#print(i)
#print(k)
executioncount = executioncount + 1
#print(executioncount)
if getFrontier(i, k):
data.cells.append(addScaledPoint(k, i))
reshapedMap[i, k] = 20
print(reshapedMap[i, k])
allFrontiers.append(xy(i, k))
print("Found a frontier")
frontiercount = frontiercount + 1
print(frontiercount)
print(allFrontiers.__len__())
publishCells(navfrontierpub, data)
print("Published frontiers")
print("Total frontier count is")
print(frontiercount)
print allFrontiers.__len__()
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 publishExpandedWalls():
data = GridCells()
for i in range(0, height):
for j in range(0, width):
if getWall(i, j) and not getTrueWall(i, j):
data.cells.append(addScaledPoint(j, i))
publishCells(exwallpub, data)
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 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 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 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 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 MakeGridCellsFromList(cellList):
gridCells = GridCells()
gridCells.cell_width = 1
gridCells.cell_height = 1
gridCells.cells = cellList
gridCells.header.frame_id = 'map'
return gridCells
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 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 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
