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")
plt.show(block=False)
if reached and points[idx, 0] == desti[0] and points[idx, 1] == desti[1]:
# print desti
idx += 1
reached = False
if idx < len(points):
# print 'we reached this shit'
pub_nav.publish(startGoalCentroid(zumy_pos[0],zumy_pos[1], points[idx,0], points[idx,1], cent[0], cent[1]))
else:
pub_nav.publish(startGoalCentroid(zumy_pos[0],zumy_pos[1], points[idx,0], points[idx,1], cent[0], cent[1]))
elif explore > 0:
# print done
print "explore done: " + str(target)
done = True
# print 'we did not reach this shit'
pub_fin.publish(doneWith(True, target[0], target[1]))
else:
# print done
print "explore done: " + str(target)
done = True
# print 'we did not reach this shit'
pub_fin.publish(doneWith(True))
# stateUpdate()
except Exception as e:
print 'insufficient data: ' + str(e)
rate.sleep()
rospy.signal_shutdown("should be dead")
homogeneous = eqf.compute_gab(rb_cur,rb_des)
print 'h**o'+str(homogeneous)
trans = homogeneous[0:3,3]
# (omega,theta) = eqf.find_omega_theta(homogeneous[0:2,0:2])
# try:
# (trans, rot) = listener.lookupTransform(rw_des, rw_cur, rospy.Time(0))
# except:
# print "not enough"
# continue
twist = [[0,0,0], [0,0,0]]
unfound = True
print 'trans'+str(trans)
if trans[0]**2+ trans[1]**2+ trans[2]**2 < .01:
pub.publish(Twist(Vector3(0,0,0), Vector3(0,0,0)))
# print 'twist'+str(twist[0][0]**2+ twist[0][1]**2+ twist[0][2]**2)
pub_state.publish(doneWith(True, rw_des.translation.x, rw_des.translation.y))
unfound = False
elif trans[0] < 0:
if abs(trans[1]) > .3:
if trans[1] < 0:
twist[1] = [0,0,-.2]
else:
twist[1] = [0,0,.2]
else:
if abs(trans[0]) > .3:
if trans[0] < 0:
twist[1] = [0,0,.2]
else:
twist[1] = [0,0,-.2]
else:
twist[0] = [.2,0,0]
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")
ang = np.arctan(trans[1]/trans[0])
if trans[1] < 0 and trans[0] < 0:
ang = -np.pi + ang
elif trans[1] > 0 and trans[0] < 0:
ang = np.pi + ang
# print 'ang angnew ' + str(ang) + str(np.arctan(trans[1]/trans[0]))
ang_buff.append(ang)
ang = np.median(np.asarray(list(ang_buff)))
print "angle: " + str(ang) + " x: " + str(trans[0]) + "y: " + str(trans[1])
# print homogeneous, rb_cur, rb_des
if trans[0]**2+ trans[1]**2 < .0006 or done:
print done
pub.publish(Twist(Vector3(0,0,0), Vector3(0,0,0)))
# print 'twist'+str(twist[0][0]**2+ twist[0][1]**2+ twist[0][2]**2)
pub_state.publish(doneWith(True, round((rw_des.translation.x - length/2)/length), round((rw_des.translation.y - length/2)/length)))
unfound = False
forward_mode = False
print doneWith(True, round((rw_des.translation.x - length/2)/length), round((rw_des.translation.y - length/2)/length))
# print 'hella small'
# print unfound
# print unfound
else:
if abs(ang) < .01 or forward_mode:
# forward_mode = True
if turn:
pub_state.publish(doneWith(True, -2, -2))
else:
if trans[0] < 0:
twist[0] = [-1*trans[0]*.6,0,0]
