コード例 #1
0
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
コード例 #2
-2
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