def plan_path(costs, start, end): pathlist = [] loc = start oldloc = start while loc != end: adjlst = val.adjacent(costs, loc) minadjv = costs[loc] for adj in adjlst: if minadjv > costs[adj]: minadjv = costs[adj] loc = adj # print loc if loc == oldloc: break pathlist.append(loc) return pathlist
def find_object(grid):
'''
Finds all contiguous sections of ones and returns as a list of lists
'''
objec_locs = set()
objects = []
l, w = grid.shape
for i in range(l):
for j in range(w):
if (i,j) not in objec_locs:
if grid[i,j] == 1:
newobj = []
frontier = {(i,j)}
while len(frontier) != 0:
exp = frontier.pop()
objec_locs = objec_locs | {exp}
adjlst = val.adjacent(grid, exp)
newobj.append(list(exp))
for adj in adjlst:
if grid[adj] == 1 and adj not in objec_locs:
frontier = frontier | {adj}
# print frontier
objects.append(newobj)
return objects
def explorer(objs_des):
# print 'objs_des'+str(objs_des)
currentPos = zumy_pos
# print objs_des
# print np.sum(objs_des, axis=0)
com_x = np.floor(np.sum(objs_des, axis=0)[0]/len(objs_des))
com_y = np.floor(np.sum(objs_des, axis=0)[1]/len(objs_des))
distance = -1
for i in range(len(objs_des)):
if np.linalg.norm(objs_des[i]-[com_x,com_y])>distance:
distance = np.linalg.norm(objs_des[i]-[com_x,com_y])
radius = distance + 2
omega = np.array([[0],[0],[1]])
theta = -np.pi/4 #assuming exploration in cw direction
n= 8 #number of sides
# om, th = eqf.find_omega_theta(zumyRBT[0:3,0:3])
length = 2*radius*np.sin(np.pi/n)
# print 'octagon'+str(com_x)+str(com_y)+str(radius)+str(length)
# translation = np.array([[currentPos[0]+length*np.cos(th)],[currentPos[1]+length*np.sin(th)],[0]])
# since translations are relative to spatial frame, right?
#translation = np.hstack((currentPos-corner[idx],[0])).T
octagon_points = np.empty((8,2),float)
octagon_points[0,0] = com_x+length/2
octagon_points[0,1] = com_y-radius
octagon_points[1,0] = com_x-length/2
octagon_points[1,1] = com_y-radius
octagon_points[2,0] = com_x-radius
octagon_points[2,1] = com_y-length/2
octagon_points[3,0] = com_x-radius
octagon_points[3,1] = com_y+length/2
octagon_points[4,0] = com_x-length/2
octagon_points[4,1] = com_y+radius
octagon_points[5,0] = com_x+length/2
octagon_points[5,1] = com_y+radius
octagon_points[6,0] = com_x+radius
octagon_points[6,1] = com_y+length/2
octagon_points[7,0] = com_x+radius
octagon_points[7,1] = com_y-length/2
oops = False
print 'old' + str(octagon_points)
new_pts = []
for i in range(octagon_points.shape[0]):
point = (octagon_points[i][0], octagon_points[i][1])
if not grid_check(point, grid):
for adj_pnt in val.adjacent(grid, point):
if grid_check(adj_pnt, grid):
new_pts.append([adj_pnt[0], adj_pnt[1]])
break
# dist = 100
# for j in range(3):
# for m in range(3):
# if np.linalg.norm([octagon_points[i][0]-j,octagon_points[i][1]-m]) < dist:
# for k in range(len(objects[0])):
# print 'ugh' + str(int(octagon_points[i][0]-j))
# if int(octagon_points[i][0]-j) == objects[0][k][0] and int(octagon_points[i][1]-m) == objects[0][k][1]:
# print 'okay cool'
# print int(octagon_points[i][0]-j)
# oops = True
# break
# if oops == False:
# # print 'huh'
# dist = np.linalg.norm([octagon_points[i][0]-j,octagon_points[i][1]-m])
# # print 'dist' + str(dist)
# new_lst = [octagon_points[i][0]-j,octagon_points[i][1]-m]
# # print 'new' + str(new)
# if dist != 100 and oops == False:
# octagon_points[i] = np.array(new_lst)
# else:
# print 'gotta delete'
# toDelete.append(i)
else:
new_pts.append([point[0], point[1]])
def edist(p1, p2):
return np.linalg.norm((p1[0]-p2[0], p1[1]-p2[1]))
dist = 10000
best = 0
for i in range(len(new_pts)):
ndist = edist(zumy_pos, new_pts[i])
if ndist < dist:
best = i
dist = ndist
new_pts = new_pts[best:] + new_pts[:best]
if edist(new_pts[0], new_pts[-1]) < edist(new_pts[0], new_pts[1]):
new_pts = [new_pts[0]] + new_pts[1:][::-1]
# print toDelete
# for i in toDelete:
# octagon_points_new = np.delete(octagon_points,i,0)
# print 'before'+str(octagon_points_new)
print new_pts
points = np.asarray([zumy_pos] + new_pts).astype(int)
print 'octagon'+str(points)
# pub_nav.publish(startGoal(zumy_pos, octagon_points[0]))
# print zumy_pos
return points, (com_x, com_y)
