def responder(name):
global path
global start
global goal
global grid
global need_next
last = None
current = None
# initalize the publishers
directions = rospy.Publisher('/'+ zumy_name + "/next_cell", startGoal, queue_size=10)
next_dest = rospy.Publisher('/'+ zumy_name + "/nav_complete", doneWith, queue_size=10)
rospy.init_node('navigator', anonymous=True)
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
#publish on all of my publishing topics
if not stop:
if not updated or dirty:
# start a new path
path = pth.gen_path(start, goal, grid)
print path, start, goal
last = 0
current = 0
elif need_next:
# send the next location in the path
last = current
current += 1
need_next = False
# keep resending the old directions
if current == len(path):
next_dest.publish(doneWith(True, goal[0], goal[1]))
else:
print path[current]
directions.publish(path[last][0], path[last][1], path[current][0], path[current][1])
r.sleep()
rospy.signal_shutdown("should be dead")
def responder(name):
global path, start, goal, grid, need_next, updated, current, stop
current += 1
last = None
# initalize the publishers
directions = rospy.Publisher('/'+ zumy_name + "/next_cell", startGoal, queue_size=10)
next_dest = rospy.Publisher('/'+ zumy_name + "/nav_complete", doneWith, queue_size=10)
rospy.init_node('navigator', anonymous=True)
r = rospy.Rate(3) # 10hz
while not rospy.is_shutdown():
#publish on all of my publishing topics
# print updated
# print grid
# if zumy is not None:
# print "grid zumy: " + str((zumy[0], zumy[1]))
if grid is not None and updated:
# start a new path
path = pth.gen_path(start, goal, grid)
print 'path'+str(path)
# print 'path, start, goal from explorer: ' + str((path, start, goal))
# print path, start, goal
last = 0
current = 0
updated = False
stop = False
elif not stop:
if need_next:
# send the next location in the path
# print current
last = current
current += 1
need_next = False
# keep resending the old directions
if current >= len(path):
# print "current"
# print current
if current == len(path) and cent is not None:
print "turn_directive"
# plt.imshow(grid, interpolation='nearest')
# plt.pause(.001)
# plt.show(block=False)
directions.publish(startGoal(-2, -2, cent[0], cent[1]))
else:
print "done"
directions.publish(startGoal(-3, -3, 3, 3))
next_dest.publish(doneWith(True, goal[0], goal[1]))
else:
print "current: " + str(path[current])
gzum = grid.astype(float)
for pnt in path[current:]:
gzum[pnt] = .75
gzum[zumy] = .5
plt.imshow(gzum, interpolation='nearest', origin ='lower')
plt.pause(.001)
plt.show(block=False)
# print 'path[current], current dest'
# print "path current: " + str(path[current])
# directions.publish(path[last][0], path[last][1], 3, 3)
directions.publish(path[last][0], path[last][1], path[current][0], path[current][1])
# directions.publish(3,3,1,1)
r.sleep()
rospy.signal_shutdown("should be dead")