def follow_ar_tag():
global listener, status
while not rospy.is_shutdown():
zumy_control.stop()
raw_input('press enter to do path planning')
status = False
while True:
try:
dst_coord = getcoord(goal_artag)
src_coord = getcoord(zumy_artag)
obst_coords = [getcoord(i) for i in obstacle_artags]
print dst_coord, src_coord, obst_coords
if is_arrived():
zumy_control.stop()
break
if status == True:
break
f = epf.EuclidField(
(field_width, field_height), # width x height
dst_coord, # goal
obst_coords # obstacles
)
try:
im = pf.field_to_image(f)
pf.draw_path(im, [src_coord])
except:
pass
#im.show()
im.resize((800, 800)).save('out.png')
point = pf.find_nextstep(f, src_coord)
print 'going to', point
zumy_control.goto(ftrans(point))
#raw_input('press enter for next step')
except tf.Exception:
zumy_control.stop()
continue
while True:
try:
src_coord = getcoord(zumy_artag)
obst_coords = [getcoord(i) for i in obstacle_artags]
print src_coord, obst_coords
if is_home():
pub.publish(Arrive('GameOver'))
status = True
if status==True:
break
f = gpf.EuclidField(
(field_width, field_height), # width x height # goal
obst_coords # obstacles
)
try:
im = pf.field_to_image(f)
pf.draw_path(im, [src_coord])
except:
pass
#im.show()
im.resize((800, 800)).save('out.png')
point = pf.find_nextstep(f, src_coord)
print 'going to', point
zumy_control.goto(ftrans(point))
#raw_input('press enter for next step')
except tf.Exception:
zumy_control.stop()
continue
def follow_ar_tag():
global listener, status
while not rospy.is_shutdown():
zumy_control.stop()
raw_input('press enter to do path planning')
status = False
while True:
try:
dst_coord = getcoord(goal_artag)
src_coord = getcoord(zumy_artag)
obst_coords = [getcoord(i) for i in obstacle_artags]
print dst_coord, src_coord, obst_coords
if is_arrived():
pub.publish(Arrive('GameOver'))
status = True
if status==True:
break
f = epf.EuclidField(
(field_width, field_height), # width x height
dst_coord, # goal
obst_coords
)
point = pf.find_nextstep(f, src_coord)
print 'going to', point
zumy_control.goto(ftrans(point))
#raw_input('press enter for next step')
except tf.Exception:
zumy_control.stop()
continue