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 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