import epf
import time
# 1.5 * 1.3
####################
f = epf.EuclidField(
(75, 65), # width x height
(20, 60), # goal
[ # obstacles
(50, 50), (37, 32), (32, 32)
])
src = (50, 32)
####################
start = time.time()
print("starting")
path = pf.find_path(f, src, f.dst, 100)
next_point = pf.find_path_sample(f, src, f.dst, 100, 0.1)
end = time.time()
print(f'Delta time: {end - start}')
print(f)
im = pf.field_to_image(f)
pf.draw_path(im, path, next_point)
im.save('out.png')
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
f = epf.EuclidField(
(50, 50), # width x height
(7, 14), # goal
[ # obstacles
(9, 5),
(10, 4),
(10, 5),
(10, 6),
(11, 5),
(9, 15),
(10, 14),
(10, 15),
(10, 16),
(11, 15),
(39, 10),
(40, 9),
(40, 10),
(40, 11),
(41, 10),
]
)
src = (46, 5)
####################
path = pf.find_path(f, src, f.dst, 100)
im = pf.field_to_image(f)
pf.draw_path(im, path)
im.save('out.png')
import epf
####################
f = epf.EuclidField(
(50, 50), # width x height
(7, 14), # goal
[ # obstacles
(9, 5),
(10, 4),
(10, 5),
(10, 6),
(11, 5),
(9, 15),
(10, 14),
(10, 15),
(10, 16),
(11, 15),
(39, 10),
(40, 9),
(40, 10),
(40, 11),
(41, 10),
])
src = (46, 5)
####################
path = pf.find_path(f, src, f.dst, 100)
im = pf.field_to_image(f)
pf.draw_path(im, path)
im.save('out.png')
