Пример #1
0
def get_randomforest_pred(image, model):
    pose = get_pose(image)
    pose = clip_pose(pose)
    angles = get_ang(pose)
    prediction = model.predict_proba([angles])[0]
    i = prediction.argmax()
    return (int(i), float(prediction[i]))
Пример #2
0
def get_logreg_pred(image, classifier):
    pose = get_pose(image)
    pose = clip_pose(pose)
    angles = get_ang(pose)
    prediction = classifier.predict_proba((np.array(angles)).reshape(1, -1))
    i = prediction[0].argmax()
    return (int(i), float(prediction[0][i]))
Пример #3
0
def update():
    global motion_state
    if motion_state['motion_done'] == True:
        return (0, 0)
    (x_goal, y_goal) = motion_state['goal_pos']
    (x_robot, y_robot, heading_robot) = pose.get_pose()
    (distance_goal, heading_goal) = math2.topolar(x_goal - x_robot, y_goal - y_robot)
    heading_goal = math2.normalize_angle(heading_goal)

    if distance_goal < MOTION_CAPTURE_DISTANCE:
        # you are at your destination 
        motion_state['motion_done'] = True
        tv = 0
        rv = 0
    elif distance_goal > MOTION_RELEASE_DISTANCE:
        motion_state['motion_done'] = False
        
    if motion_state['motion_done'] == False:
        # Drive towards goal position
        tv = distance_goal * MOTION_TV_GAIN + MOTION_TV_MIN
        tv = velocity.clamp(tv, motion_state['tv_max'])
    
        # Rotate towards goal position
        heading_error = math2.smallest_angle_diff(heading_robot, heading_goal)
        rv = MOTION_RV_GAIN * heading_error
        rv = velocity.clamp(rv, MOTION_RV_MAX)

        if motion_state['rotate_only']:
            tv = 0
            if abs(heading_error) < MOTION_RELEASE_ANGLE:
                motion_state['rotate_only'] = False
        else:
            if abs(heading_error) > MOTION_CAPTURE_ANGLE:
                motion_state['rotate_only'] = True
    return (tv, rv)
Пример #4
0
    def initialize(self):
        cap = cv2.VideoCapture(self.source)
        if not cap.isOpened():
            cap.open()

        # read a few frames to make sure the camera is self-calibrated
        for i in range(10):
            cap.read()

        # let user position camera
        print("Position the camera, then hit space.")
        while cv2.waitKey(20) != ord(' '):
            ret, img = cap.read()
            cv2.imshow(self.window_name, img)

        # find the board grid
        ret, img = cap.read()
        self.lines = find_grid.find_grid(img, self.board_size)
        self.grid = find_grid.get_grid_intersections(self.lines,
                                                     self.board_size)
        self.corners = util.get_board_corners(self.grid)
        rvec, tvec, inliners, t = pose.get_pose(self.corners, self.board_size,
                                                self.cam_mtx, self.distortion)
        self.offsets = pose.compute_offsets(self.grid, self.board_size, t,
                                            rvec, tvec, self.cam_mtx,
                                            self.distortion)

        self.corners = np.int32(self.corners)
        self.offsets = np.int32(self.offsets)
        board_mask = util.get_board_mask(img.shape[:2], self.corners)
        self.cap = select_frames.FrameSelector(cap)
        self.cap.set_roi(None, board_mask)
        self.cap.initialize()

        self.finder = find_stones.StoneFinder(self.board_size, self.lines,
                                              self.grid, self.black,
                                              self.white, self.offsets)
        self.finder.set_last_gray_image(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY))

        if self.debug:
            self.finder.draw_stone_masks(img)

            for i, j in util.square(self.board_size):
                pt1, pt2 = tuple(self.grid[i,j,::-1].ravel()), \
                           tuple(self.offsets[i,j,::-1].ravel())
                cv2.line(img, pt1, pt2, (0, 255, 0), 2)
            cv2.imshow('pose', img)
            cv2.waitKey(0)
            cv2.destroyWindow('pose')

        return True
Пример #5
0
def update():
    global motion_state
    if motion_state['motion_done'] == True:
        return (0, 0)
    (x_goal, y_goal) = motion_state['goal_pos']
    (x_robot, y_robot, heading_robot) = pose.get_pose()
    (distance_goal, heading_goal) = math2.topolar(x_goal - x_robot,
                                                  y_goal - y_robot)
    heading_goal = math2.normalize_angle(heading_goal)

    if distance_goal < MOTION_CAPTURE_DISTANCE:
        # you are at your destination
        motion_state['motion_done'] = True
        tv = 0
        rv = 0
    elif distance_goal > MOTION_RELEASE_DISTANCE:
        motion_state['motion_done'] = False

    if motion_state['motion_done'] == False:
        # Drive towards goal position
        tv = distance_goal * MOTION_TV_GAIN + MOTION_TV_MIN
        tv = velocity.clamp(tv, motion_state['tv_max'])

        # Rotate towards goal position
        heading_error = math2.smallest_angle_diff(heading_robot, heading_goal)
        rv = MOTION_RV_GAIN * heading_error
        rv = velocity.clamp(rv, MOTION_RV_MAX)

        if motion_state['rotate_only']:
            tv = 0
            if abs(heading_error) < MOTION_RELEASE_ANGLE:
                motion_state['rotate_only'] = False
        else:
            if abs(heading_error) > MOTION_CAPTURE_ANGLE:
                motion_state['rotate_only'] = True
    return (tv, rv)
Пример #6
0
def spring():
    at_tree_odo = None
    tree_pose = None
    followers = 0
    have_seen_leader = False
    beh.init(0.22, 40, 0.5, 0.1)
    motion.init_rv(1000, MOTION_RV, MOTION_CAPTURE_DISTANCE,
                   MOTION_RELEASE_DISTANCE, MOTION_CAPTURE_ANGLE,
                   MOTION_RELEASE_ANGLE)
    state = STATE_IDLE

    while True:
        # run the system updates
        new_nbrs = beh.update()
        nbr_list = neighbors.get_neighbors()
        if new_nbrs:
            print state

        beh_out = beh.BEH_INACTIVE

        # set colors, because why not do it at the top
        color_counts = [0, 0, 0]
        for i in range(3):
            color_counts[i] = min([5, (state - 5 * i)])
        if state == STATE_IDLE:
            leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
        elif state == STATE_SUCCESS:
            leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
        else:
            leds.set_pattern(color_counts, 'count', LED_BRIGHTNESS)

        if rone.button_get_value('g'):
            tree_pose = None
            followers = 0
            have_seen_leader = False
            state = STATE_IDLE

        # this is the main finite-state machine
        if not state in [
                STATE_IDLE, STATE_LEADER, STATE_SUCCESS, STATE_FOLLOW
        ]:
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state in [STATE_LEADER, STATE_SUCCESS]:
                    start_time = sys.time()
                    state = STATE_FOLLOW

        if state == STATE_IDLE:

            if rone.button_get_value('r'):
                pose.set_pose(0, 0, 0)
                state = STATE_WANDER
            elif rone.button_get_value('b'):
                state = STATE_QUEEN

        elif state == STATE_QUEEN:
            pass

        elif state == STATE_WANDER:
            ##            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            nav = hba.find_nav_tower_nbr(NAV_ID)
            beh_out = beh.avoid_nbr(nav, MOTION_TV)
            queen = None
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_QUEEN:
                    queen = nbr

            if bump_front():
                if queen != None:
                    state = STATE_SUCCESS
                else:
                    tree_pose = pose.get_pose()
                    motion.set_goal((0.0, 0.0), MOTION_TV)
                    at_tree_odo = pose.get_odometer()
                    state = STATE_RETURN
            elif nav == None:
                motion.set_goal((0.0, 0.0), MOTION_TV)
                state = STATE_RETURN

        elif state == STATE_RETURN:
            ##            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            queen = None
            recruiter = None
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_QUEEN:
                    queen = nbr
                elif nbr_state == STATE_RECRUIT:
                    recruiter = nbr
            if queen != None:
                if (recruiter == None) and (tree_pose != None) and \
                close_to_nbr(queen):
                    start_time = sys.time()
                    dist_traveled = pose.get_odometer() - at_tree_odo
                    at_queen_odo = pose.get_odometer()
                    state = STATE_RECRUIT
                elif not closer_to_nbr(queen):
                    beh_out = beh.follow_nbr(queen, MOTION_TV)
                else:
                    start_time = sys.time()
                    state = STATE_FOLLOW
            else:
                (tv, rv) = motion.update()
                beh_out = beh.tvrv(tv, rv)

        elif state == STATE_RECRUIT:
            new_followers = 0
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_RECRUIT:
                    if neighbors.get_nbr_id(nbr) > rone.get_id():
                        state = STATE_FOLLOW
                elif nbr_state in [STATE_FOLLOW, STATE_QUEEN]:
                    new_followers += 1
            if new_followers > followers:
                print 'reset timer'
                start_time = sys.time()
            followers = max([followers, new_followers])
            if followers == 4 or sys.time() > start_time + WAIT_TIME:
                tree_pos = (tree_pose[0], tree_pose[1])
                motion.set_goal(tree_pos, MOTION_TV)
                state = STATE_LEADER

        elif state == STATE_FOLLOW:
            recruiter = None
            leader = None
            success = False
            new_followers = 1
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_RECRUIT:
                    recruiter = nbr
                elif nbr_state == STATE_LEADER:
                    leader = nbr
                elif nbr_state == STATE_SUCCESS:
                    leader = nbr
                    success = True
                elif nbr_state in [STATE_FOLLOW, STATE_WANDER]:
                    new_followers += 1
            if success:
                have_seen_leader = True
            if new_followers > followers:
                start_time = sys.time()
            followers = max([followers, new_followers])

            if recruiter == None:
                if leader == None:
                    if have_seen_leader:
                        beh_out = beh.tvrv(MOTION_TV, 0)
                    elif followers == 5 or sys.time() > start_time + WAIT_TIME:
                        followers = 0
                        state = STATE_WANDER
                else:
                    if success and bump() and close_to_nbr(leader):
                        state = STATE_SUCCESS
                    beh_out = beh.follow_nbr(leader, MOTION_TV)

        elif state == STATE_LEADER:
            ##            (tv, rv) = motion.update()
            ##            beh_out = beh.tvrv(tv, rv)
            ##            if motion.is_done():
            ##                state = STATE_SUCCESS
            beh_out = beh.tvrv(-MOTION_TV, 0)

            if bump() or (pose.get_odometer() - at_queen_odo) > dist_traveled:
                state = STATE_SUCCESS

        elif state == STATE_SUCCESS:
            pass

        # end of the FSM

        if state not in [
                STATE_IDLE, STATE_RECRUIT, STATE_LEADER, STATE_SUCCESS,
                STATE_QUEEN
        ]:
            bump_beh_out = beh.bump_beh(MOTION_TV)
            beh_out = beh.subsume([beh_out, bump_beh_out])

        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        msg = [0, 0, 0]
        msg[MSG_IDX_STATE] = state
        hba.set_msg(msg[0], msg[1], msg[2])
Пример #7
0
##img = cv2.imread('tests/calibrate/1.jpg')
##corners = np.array([[132, 318], [177, 506], [387, 408], [307, 218]],
##                   dtype=np.int32)
##board_size = 7

img = cv2.imread('tests/photos/1.jpg')
corners = np.array([[75, 251], [164, 542], [445, 383], [275, 51]],
                   dtype=np.int32)
board_size = 9

data = np.load('camera_params.npz')
mtx = data['mtx']
#dist = data['dist']
dist = None

rvec, tvec, inliers, t = pose.get_pose(corners, board_size, mtx, dist, True)
img = pose.draw_pose(img, board_size, corners, t, rvec, tvec, mtx, dist)

lines = find_grid.find_grid(img, board_size, corners)
grid = find_grid.get_grid_intersections(lines, board_size)
offsets = pose.compute_offsets(grid, board_size, t, rvec, tvec, mtx, dist)

finder = find_stones.StoneFinder(board_size, lines, grid,
                                 np.zeros((0, 2), dtype=np.int32),
                                 np.zeros((0, 2), dtype=np.int32), offsets)
finder.draw_stone_masks(img)

for i, j in util.square(board_size):
    pt1, pt2 = tuple(grid[i, j, ::-1].ravel()), tuple(offsets[i,
                                                              j, ::-1].ravel())
    cv2.line(img, pt1, pt2, (0, 255, 0), 2)
Пример #8
0
def get_catboost_pred(image, classifier):
    pose = get_pose(image)
    angles = get_ang(pose)
    prediction = classifier.predict_proba(angles)
    i = prediction.argmax()
    return (int(i), float(prediction[i]))
Пример #9
0
def spring():
    at_tree_odo = None
    tree_pose = None
    followers = 0
    have_seen_leader = False
    beh.init(0.22, 40, 0.5, 0.1)
    motion.init_rv(1000, MOTION_RV, MOTION_CAPTURE_DISTANCE, MOTION_RELEASE_DISTANCE
            , MOTION_CAPTURE_ANGLE, MOTION_RELEASE_ANGLE)
    state = STATE_IDLE

    while True:
        # run the system updates
        new_nbrs = beh.update()
        nbr_list = neighbors.get_neighbors()
        if new_nbrs:
            print state
            
        beh_out = beh.BEH_INACTIVE

        # set colors, because why not do it at the top
        color_counts = [0, 0, 0]
        for i in range(3):
            color_counts[i] = min([5, (state - 5 * i)])
        if state == STATE_IDLE:
            leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
        elif state == STATE_SUCCESS:
            leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
        else:
            leds.set_pattern(color_counts, 'count', LED_BRIGHTNESS)

        if rone.button_get_value('g'):
            tree_pose = None
            followers = 0
            have_seen_leader = False
            state = STATE_IDLE
            
        # this is the main finite-state machine
        if not state in [STATE_IDLE, STATE_LEADER, STATE_SUCCESS, STATE_FOLLOW]:
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state in [STATE_LEADER, STATE_SUCCESS]:
                    start_time = sys.time()
                    state = STATE_FOLLOW
                    
        if state == STATE_IDLE:

            if rone.button_get_value('r'):
                pose.set_pose(0, 0, 0)
                state = STATE_WANDER
            elif rone.button_get_value('b'):
                state = STATE_QUEEN

        elif state == STATE_QUEEN:
            pass

        elif state == STATE_WANDER:
            ##            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            nav = hba.find_nav_tower_nbr(NAV_ID)
            beh_out = beh.avoid_nbr(nav, MOTION_TV)
            queen = None
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_QUEEN:
                    queen = nbr

            if bump_front():
                if queen != None:
                    state = STATE_SUCCESS
                else:
                    tree_pose = pose.get_pose()
                    motion.set_goal((0.0, 0.0), MOTION_TV)
                    at_tree_odo = pose.get_odometer()
                    state = STATE_RETURN
            elif nav == None:
                motion.set_goal((0.0, 0.0), MOTION_TV)
                state = STATE_RETURN

        elif state == STATE_RETURN:
            ##            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            queen = None
            recruiter = None
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_QUEEN:
                    queen = nbr
                elif nbr_state == STATE_RECRUIT:
                    recruiter = nbr
            if queen != None:
                if (recruiter == None) and (tree_pose != None) and \
                close_to_nbr(queen):
                    start_time = sys.time()
                    dist_traveled = pose.get_odometer() - at_tree_odo
                    at_queen_odo = pose.get_odometer()
                    state = STATE_RECRUIT
                elif not closer_to_nbr(queen):
                    beh_out = beh.follow_nbr(queen, MOTION_TV)
                else:
                    start_time = sys.time()
                    state = STATE_FOLLOW
            else:
                (tv, rv) = motion.update()
                beh_out = beh.tvrv(tv, rv)

        elif state == STATE_RECRUIT:
            new_followers = 0
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr,new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_RECRUIT:
                    if neighbors.get_nbr_id(nbr) > rone.get_id():
                        state = STATE_FOLLOW
                elif nbr_state in [STATE_FOLLOW, STATE_QUEEN]:
                    new_followers += 1
            if new_followers > followers:
                print 'reset timer'
                start_time = sys.time()
            followers = max([followers, new_followers])
            if followers == 4 or sys.time() > start_time + WAIT_TIME:
                tree_pos = (tree_pose[0], tree_pose[1])
                motion.set_goal(tree_pos, MOTION_TV)
                state = STATE_LEADER

        elif state == STATE_FOLLOW: 
            recruiter = None
            leader = None
            success = False
            new_followers = 1
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr,new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_RECRUIT:
                    recruiter = nbr
                elif nbr_state == STATE_LEADER:
                    leader = nbr
                elif nbr_state == STATE_SUCCESS:
                    leader = nbr
                    success = True
                elif nbr_state in [STATE_FOLLOW, STATE_WANDER]:
                    new_followers += 1
            if success:
                have_seen_leader = True
            if new_followers > followers:
                start_time = sys.time()
            followers = max([followers, new_followers])

            if recruiter == None:
                if leader == None:
                    if have_seen_leader:
                        beh_out = beh.tvrv(MOTION_TV, 0)
                    elif followers == 5 or sys.time() > start_time + WAIT_TIME:
                        followers = 0
                        state = STATE_WANDER
                else:
                    if success and bump() and close_to_nbr(leader):
                        state = STATE_SUCCESS
                    beh_out = beh.follow_nbr(leader, MOTION_TV)
            
        elif state == STATE_LEADER:
##            (tv, rv) = motion.update()
##            beh_out = beh.tvrv(tv, rv)
##            if motion.is_done():
##                state = STATE_SUCCESS
            beh_out = beh.tvrv(-MOTION_TV, 0)

            if bump() or (pose.get_odometer() - at_queen_odo) > dist_traveled:
                state = STATE_SUCCESS

        elif state == STATE_SUCCESS:
            pass
        
        # end of the FSM
        
        if state not in [STATE_IDLE, STATE_RECRUIT, STATE_LEADER, STATE_SUCCESS, STATE_QUEEN]:
            bump_beh_out = beh.bump_beh(MOTION_TV)
            beh_out = beh.subsume([beh_out, bump_beh_out])

        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        msg = [0, 0, 0]
        msg[MSG_IDX_STATE] = state
        hba.set_msg(msg[0], msg[1], msg[2])
Пример #10
0
white = np.zeros((0, 2), dtype='int32')
black = np.zeros((0, 2), dtype='int32')
stone_size = 25

file = open('tests/game1.txt', 'r')
prefix = 'tests/'
#file = open('c:/sam/tests/game1.txt', 'r')
#prefix='c:/sam/tests/'
img = cv2.imread(prefix + file.readline().strip(), cv2.IMREAD_COLOR)

lines = find_grid.find_grid(img, BOARDSIZE, corners)
grid = find_grid.get_grid_intersections(lines, BOARDSIZE)
grid = np.int32(grid)

mtx, dist = pose.get_fake_camera(img)
rvec, tvec, inliers, t = pose.get_pose(corners, BOARDSIZE, mtx, dist)
offsets = pose.compute_offsets(grid, BOARDSIZE, t, rvec, tvec, mtx, dist)

##offsets = grid.copy()
##offsets[:,:,0] = offsets[:,:,0] - 2

#print('shape of image', img.shape)
#print('grid')
#print(grid)

finder = find_stones.StoneFinder(BOARDSIZE,
                                 img.shape,
                                 lines,
                                 grid,
                                 white,
                                 black,