def __init__(self, source, type):
self.type = type
try:
self.video = cv2.VideoCapture(source)
self.rval = True
except cv2.error:
self.rval = False
if type == Mode.HIGH_GOAL:
comms.set_high_goal_state(State.CAMERA_ERROR)
else:
comms.set_gear_state(State.CAMERA_ERROR)
self.source = source
self.live = True if isinstance(source, int) else False
self.__configure()
def __configure(self):
if self.rval:
self.video.set(cv.CV_CAP_PROP_FRAME_WIDTH, config.RESOLUTION_X)
self.video.set(cv.CV_CAP_PROP_FRAME_HEIGHT, config.RESOLUTION_Y)
log.info("Set resolution to %sx%s", config.RESOLUTION_X,
config.RESOLUTION_Y)
# find out if the camera is actually working
if self.video.isOpened():
self.rval, frame = self.video.read()
# run some configuration if everything is good
if self.rval:
log.info("Read from capture successfully")
# run config using v4l2 driver if the platform is linux and the feed is live
if platform.system() == "Linux" and self.live:
try:
log.info("Running Linux config using v4l2ctl")
v4l2ctl.restore_defaults(self.source)
if self.type == Mode.HIGH_GOAL:
for prop in config.CAMERA_SETTINGS_HIGH_GOAL:
v4l2ctl.set(
self.source, prop,
config.CAMERA_SETTINGS_HIGH_GOAL[prop])
else:
for prop in config.CAMERA_SETTINGS_GEARS:
v4l2ctl.set(
self.source, prop,
config.CAMERA_SETTINGS_GEARS[prop])
except AttributeError as e:
log.error('Setting camera properties failed!')
if type == Mode.HIGH_GOAL:
comms.set_high_goal_state(State.CAMERA_ERROR)
else:
comms.set_gear_state(State.CAMERA_ERROR)
print(e)
else:
self.rval = False
log.critical("Problem reading from capture")
if type == Mode.HIGH_GOAL:
comms.set_high_goal_state(State.CAMERA_ERROR)
else:
comms.set_gear_state(State.CAMERA_ERROR)
else:
self.rval = False
log.critical("Problem opening capture")
if type == Mode.HIGH_GOAL:
comms.set_high_goal_state(State.CAMERA_ERROR)
else:
comms.set_gear_state(State.CAMERA_ERROR)
gear_cam_server.set_jpeg(frame)
# calculate fps
frametimes.append(time.time() - last)
if len(frametimes) > 60:
frametimes.pop(0)
fps = int(1 / (sum(frametimes) / len(frametimes)))
if show_image:
cv2.imshow('frame', frame)
key = cv2.waitKey(wait_time)
if wait_for_continue:
while key != exit_key and key != continue_key:
key = cv2.waitKey(1)
if key == exit_key: # exit on ESC
break
# record time for fps calculation
last = time.time()
except Exception as e:
# in real life situations on the field, we want to continue even if something goes really wrong.
# just keep looping :)
print(e)
comms.set_high_goal_state(State.POWERED_OFF)
comms.set_gear_state(State.POWERED_OFF)
log.info("Main loop exited successfully")
log.info("FPS at time of exit: %s", fps)
def high_goal_targeting(hsv, turret_angle):
# print average brightness
if True:#turret_cam_server.frame_count < 20:
log.info('BRIGHTNESS:' + str(cv2.mean(hsv[2])[0]))
# threshold
mask = cv2.inRange(hsv, shooter_thresh_low, shooter_thresh_high)
# remove noise
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
res = mask.copy()
# get a list of continuous lines in the image
contours, _ = cv2.findContours(mask, 1, 2)
# identify the target if there is one
target = None
contours_with_areas = []
if len(contours) >= 2:
for c in contours:
area = cv2.contourArea(c)
x, y, w, h = cv2.boundingRect(c)
# minimum area check and solidity check
if area > config.MIN_SHOOTER_AREA:# and area / (w * h) > 0.62:
contours_with_areas.append((c, area))
contours_with_areas.sort(key=lambda x: -x[1])
contours_with_areas = contours_with_areas[:6]
def high_goal_cost(c1, c2):
M1 = cv2.moments(c1)
cx1 = int(M1['m10'] / M1['m00'])
cy1 = int(M1['m01'] / M1['m00'])
M2 = cv2.moments(c2)
cx2 = int(M2['m10'] / M2['m00'])
cy2 = int(M2['m01'] / M2['m00'])
offset_x = abs(cx1 - cx2)
offset_y = abs(cy1 - cy2)
return offset_x * 6 + offset_y
lowest_cost = 300
best_contours = None
for p1, p2 in combinations(contours_with_areas, 2):
c1, a1 = p1[0], p1[1]
c2, a2 = p2[0], p2[1]
cost = high_goal_cost(c1, c2)
if cost < lowest_cost:
lowest_cost = cost
best_contours = (c1, c2)
if best_contours:
target = cv2.convexHull(np.concatenate((best_contours[0], best_contours[1])))
# if we found a target
if target is not None:
# set state
comms.set_high_goal_state(State.TARGET_FOUND)
# find the centroid of the target
M = cv2.moments(target)
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
# calculate the angle needed in order to align the target
angle_x = ((res_x / 2) - cx) * ptd # pixel distance * conversion factor
angle_y = ((res_y / 2) - cy) * ptd + config.CAMERA_ANGLE
distance = abs((config.STEAMWORKS_HIGH_GOAL_CENTER_HEIGHT - config.CAMERA_HEIGHT) / math.tan(math.radians(angle_y)))
# send the (absolute) angle and distance to the RIO
comms.set_high_goal(turret_angle + angle_x, distance)
# draw debug information about the target on the frame
if draw:
cv2.drawContours(frame, [target + 12], 0, (0, 255, 0), 3)
cv2.drawContours(frame, [np.array([[cx + 12, cy + 12]])], 0, (0, 0, 255), 10)
cv2.putText(frame, str(angle_x), (10, 450), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2, 9)
cv2.putText(frame, str(distance), (10, 410), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255),2,9)
else:
comms.set_high_goal_state(State.TARGET_NOT_FOUND)
return frame, res