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]))
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]))
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)
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
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)
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])
##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)
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]))
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])
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,