From e9b8d945238dfab4a75a07487e23158e351e001e Mon Sep 17 00:00:00 2001 From: Your Name Date: Sat, 1 Feb 2020 11:48:20 -0500 Subject: [PATCH] 1.fixed animations 2.added target sorting 3.made object tracking and calibration consistent --- main.py | 62 +++++++++++++++++++---------------- processing/ball_tracker2.py | 53 +++++++++++++++++------------- processing/bay_tracker.py | 34 +++++++++---------- processing/color_calibrate.py | 2 +- processing/port_tracker.py | 12 +++---- processing/shape_util.py | 6 ++-- web/image_stream_handlers.py | 1 + web/tornado_server.py | 6 ++-- web/www/camera.html | 1 - web/www/index.html | 16 ++++----- web/www/tracker.js | 57 ++++++++++++++++---------------- web/www/ws_streamer.js | 5 ++- 12 files changed, 135 insertions(+), 120 deletions(-) diff --git a/main.py b/main.py index 866ae78..b09cc34 100644 --- a/main.py +++ b/main.py @@ -11,7 +11,6 @@ from cameras import Camera from cameras import image_converter -from profiles import color_profiles from processing import bay_tracker from processing import port_tracker from processing import ball_tracker2 @@ -105,10 +104,10 @@ def main(): time.sleep(5) - tracking_ws = create_connection("ws://localhost:8080/tracking/ws") camera_ws = create_connection("ws://localhost:8080/camera/ws") processed_ws = create_connection("ws://localhost:8080/processed/ws") calibration_ws = create_connection("ws://localhost:8080/calibration/ws") + tracking_ws = create_connection("ws://localhost:8080/tracking/ws") controller_listener.start("ws://localhost:8080/dashboard/ws") @@ -124,13 +123,33 @@ def main(): print('opening camera') cap.open(config.video_source_number) - _, raw_frame = cap.read() + _, bgr_frame = cap.read() + + resized_frame = cv2.resize(bgr_frame, ((int)(640), (int)(480)), 0, 0, cv2.INTER_CUBIC) + rgb_frame = cv2.cvtColor(resized_frame, cv2.COLOR_BGR2RGB) + + + if main_controller.enable_camera_feed: + + jpg=image_converter.convert_to_jpg(rgb_frame) + camera_ws.send_binary(jpg) + + if main_controller.enable_calibration_feed: + + calibration_frame = rgb_frame.copy() + + calibration_frame = color_calibrate.process(calibration_frame, + camera_mode = main_controller.calibration.get('camera_mode', 'RAW'), + color_mode = main_controller.calibration.get('color_mode'), + apply_mask = main_controller.calibration.get('apply_mask', False)) + + jpg=image_converter.convert_to_jpg(calibration_frame) + calibration_ws.send_binary(jpg) - rgb_frame = cv2.cvtColor(raw_frame, cv2.COLOR_BGR2RGB) if main_controller.camera_mode == CAMERA_MODE_RAW: - frame = frame + processed_frame = rgb_frame elif main_controller.camera_mode == CAMERA_MODE_LOADING_BAY: @@ -140,19 +159,19 @@ def main(): camera, frame_cnt, color_profile) - + # print(tracking_data) dashboard.putStringArray(networktables.keys.vision_target_data, tracking_data) tracking_ws.send(json.dumps(dict(targets=tracking_data))) elif main_controller.camera_mode == CAMERA_MODE_BALL: color_profile=main_controller.color_profiles[CAMERA_MODE_BALL] + # print("ball") processed_frame, tracking_data = ball_tracker2.process(rgb_frame, camera, frame_cnt, color_profile) - tracking_ws.send(json.dumps(dict(targets=tracking_data))) elif main_controller.camera_mode == CAMERA_MODE_HEXAGON: @@ -165,22 +184,7 @@ def main(): color_profile) - if main_controller.enable_camera_feed: - - jpg=image_converter.convert_to_jpg(rgb_frame) - camera_ws.send_binary(jpg) - - if main_controller.enable_calibration_feed: - - calibration_frame = raw_frame.copy() - calibration_frame = color_calibrate.process(calibration_frame, - camera_mode = main_controller.calibration.get('camera_mode', 'RAW'), - color_mode = main_controller.calibration.get('color_mode'), - apply_mask = main_controller.calibration.get('apply_mask', False)) - - jpg=image_converter.convert_to_jpg(calibration_frame) - calibration_ws.send_binary(jpg) if main_controller.enable_processing_feed: @@ -199,8 +203,8 @@ def main(): # if out is not None: # out.write(frame) - cv2.imshow('frame', processed_frame ) - #cv2.waitKey(1) + # cv2.imshow('frame', processed_frame ) + # cv2.waitKey(0) else: logger.info('waiting for control socket') @@ -210,13 +214,13 @@ def main(): cap.release() time.sleep(.3) - if cv2.waitKey(1) & 0xFF == ord('q'): - break + # if cv2.waitKey(1) & 0xFF == ord('q'): + # break if __name__ == '__main__': - p = Process(target=start_web.main) - p.start() + #p = Process(target=start_web.main) + #p.start() main() - p.join() \ No newline at end of file + #p.join() diff --git a/processing/ball_tracker2.py b/processing/ball_tracker2.py index 03b6e78..ebd4ebf 100644 --- a/processing/ball_tracker2.py +++ b/processing/ball_tracker2.py @@ -14,35 +14,41 @@ from processing import shape_util import time -from profiles import color_profiles from controls import CAMERA_MODE_RAW, CAMERA_MODE_LOADING_BAY, CAMERA_MODE_BALL, CAMERA_MODE_HEXAGON import network -MIN_AREA = 30 +MIN_AREA = 15 BALL_RADIUS = 3.5 - -debug = True +debug = False def process(img, camera, frame_cnt, color_profile): global rgb_window_active, hsv_window_active FRAME_WIDTH = camera.FRAME_WIDTH FRAME_HEIGHT = camera.FRAME_HEIGHT + red = color_profile.red + green = color_profile.green + blue = color_profile.blue + hue = color_profile.hsv_hue + sat = color_profile.hsv_sat + val = color_profile.hsv_val tracking_data = [] original_img = img img = cv2.GaussianBlur(img, (13, 13), 0) - hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) - hsv_mask = cvfilters.hsv_threshold(img, color_profile) - img = cv2.bitwise_and(img, img, hsv_mask) + #cv2.imshow('img', img) + hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV) + mask_hsv = cv2.inRange(hsv, (hue.min, sat.min, val.min), (hue.max, sat.max, val.max)) + mask_rgb = cv2.inRange(img, (red.min, green.min, blue.min), (red.max, green.max, blue.max)) + img = cvfilters.apply_mask(img, mask_hsv) + img = cvfilters.apply_mask(img, mask_rgb) img = cv2.erode(img, None, iterations=2) img = cv2.dilate(img, None, iterations=2) img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) if debug: - cv2.imshow('ball tracker hsv', hsv_mask) cv2.imshow('ball tracker img', img) _, contours, hierarchy = cv2.findContours(img, @@ -57,16 +63,17 @@ def process(img, camera, frame_cnt, color_profile): peri = cv2.arcLength(contour, True) approx = cv2.approxPolyDP(contour, 0.04 * peri, True) area = cv2.contourArea(approx) - x, y, w, h = cv2.boundingRect(approx) - ((x, y), radius) = cv2.minEnclosingCircle(contour) # limit the number of contours to process # #print('%s area:%s' %(index, area) ) if area > MIN_AREA: - contour_list.append(contour) + x, y, w, h = cv2.boundingRect(approx) center_mass_x = x + w / 2 center_mass_y = y + h / 2 + ((x,y), radius) = cv2.minEnclosingCircle(contour) + contour_list.append(contour) + # # tests for if its width is around its height which should be true @@ -74,7 +81,7 @@ def process(img, camera, frame_cnt, color_profile): if True : #convert distance to inches - distance = shape_util.get_distance(w, 2 * radius, camera.FOCAL_LENGTH) + distance = shape_util.distance_in_inches(w) angle = shape_util.get_angle(camera, center_mass_x, center_mass_y) font = cv2.FONT_HERSHEY_DUPLEX @@ -85,21 +92,20 @@ def process(img, camera, frame_cnt, color_profile): data = dict(shape='BALL', radius=radius, - index=index, dist=distance, angle=angle, xpos=center_mass_x, ypos=center_mass_y) - if(not tracking_data): - tracking_data.append(data) - else: - for target in tracking_data: - if(data["dist"] < target["dist"]): - tracking_data.insert(tracking_data.index(target), data) - + tracking_data.append(data) # sorter goes here - + # if len(tracking_data) == 0: + # tracking_data.append(data) + # else: + # for target in tracking_data: + # if distance < target["dist"]: + # tracking_data.insert(tracking_data.index(target), data) + # break #labels image radius_text = 'radius:%s' % (radius) coordinate_text = 'x:%s y:%s ' % (center_mass_x, center_mass_y) @@ -111,9 +117,8 @@ def process(img, camera, frame_cnt, color_profile): cv2.putText(original_img, angle_text, (int(x), int(y) - 5), font, .4, colors.WHITE, 1, cv2.LINE_AA) cv2.putText(original_img, radius_text, (int(x), int(y) - 50), font, .4, colors.WHITE, 1, cv2.LINE_AA) - cv2.circle(original_img, (int(center_mass_x), int(center_mass_y)), 5, colors.GREEN, -1) - cv2.drawContours(original_img, contours, index, colors.GREEN, 2) cv2.line(original_img, (FRAME_WIDTH // 2, FRAME_HEIGHT), (int(center_mass_x), int(center_mass_y)), colors.GREEN, 2) + cv2.drawContours(original_img, contours, index, colors.GREEN, 2) elif debug: @@ -128,4 +133,6 @@ def process(img, camera, frame_cnt, color_profile): top_center = (FRAME_WIDTH // 2, FRAME_HEIGHT) bottom_center = (FRAME_WIDTH // 2, 0) cv2.line(original_img, top_center, bottom_center, colors.WHITE, 4) + + tracking_data = sorted(tracking_data, key = lambda i: i['dist']) return original_img, tracking_data diff --git a/processing/bay_tracker.py b/processing/bay_tracker.py index 22d0c5b..ecec5b7 100644 --- a/processing/bay_tracker.py +++ b/processing/bay_tracker.py @@ -19,7 +19,7 @@ from profiles import color_profiles -MIN_AREA = 10 +MIN_AREA = 50 BAY_LENGTH = 7 # @@ -29,17 +29,18 @@ def process(img, camera, frame_cnt, color_profile): global rgb_window_active, hsv_window_active - FRAME_WIDTH = camera.FRAME_WIDTH FRAME_HEIGHT = camera.FRAME_HEIGHT + hue = color_profile.hsv_hue + sat = color_profile.hsv_sat + val = color_profile.hsv_val tracking_data = [] original_img = img - img = cv2.GaussianBlur(img, (13, 13), 0) - hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) - hsv_mask = cvfilters.hsv_threshold(img, color_profile) - img = cv2.bitwise_and(img, img, hsv_mask) + hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV) + mask = cv2.inRange(hsv, (hue.min, sat.min, val.min), (hue.max, sat.max, val.max)) + img = cvfilters.apply_mask(img, mask) img = cv2.erode(img, None, iterations=2) img = cv2.dilate(img, None, iterations=2) img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) @@ -47,11 +48,9 @@ def process(img, camera, frame_cnt, color_profile): # if debug: # cv2.imshow('hsv', img) - _, contours, hierarchy = cv2.findContours(img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - contour_list = [] # algorithm for detecting rectangular object (loading bay) for (index, contour) in enumerate(contours): @@ -63,13 +62,12 @@ def process(img, camera, frame_cnt, color_profile): # limit the number of contours to process # num_vertices = shape_util.find_vertices(contour) - if area > MIN_AREA: contour_list.append(contour) center_mass_x = x + w / 2 center_mass_y = y + h / 2 # - if shape_util.dimensions_match(contour, 4, WIDTH_TO_HEIGHT_RATIO): + if shape_util.dimensions_match(contour, 4, 2, WIDTH_TO_HEIGHT_RATIO): # print 'x:%s, y:%s angle:%s ' % ( center_mass_x, center_mass_y, angle ) distance = shape_util.distance_in_inches(w) angle = shape_util.get_angle(camera, center_mass_x, center_mass_y) @@ -77,21 +75,23 @@ def process(img, camera, frame_cnt, color_profile): # set tracking_data data = dict(shape='BAY', - w=w, - h=h, - index=index, + width=w, + height=h, dist=distance, angle=angle, xpos=center_mass_x, ypos=center_mass_y) - if(not tracking_data): + if len(tracking_data) == 0: tracking_data.append(data) else: for target in tracking_data: - if(data["dist"] < target["dist"]): + if distance < target["dist"]: tracking_data.insert(tracking_data.index(target), data) + break + + vertices_text = 'vertices:%s' % (num_vertices) coordinate_text = 'x:%s y:%s ' % (center_mass_x, center_mass_y) area_text = 'area:%s width:%s height:%s' % (area, w, h) @@ -106,9 +106,9 @@ def process(img, camera, frame_cnt, color_profile): cv2.drawContours(original_img, contours, index, colors.random(), 2) #cv2.circle(original_img, (int(center_mass_x), int(center_mass_y)), 5, colors.GREEN, -1) cv2.line(original_img, (FRAME_WIDTH // 2, FRAME_HEIGHT), (int(center_mass_x), int(center_mass_y)), colors.GREEN, 2) - elif debug: + # elif debug: - cv2.drawContours(original_img, contours, index, colors.random(), 2) + # cv2.drawContours(original_img, contours, index, colors.random(), 2) #cv2.rectangle(original_img, (x, y), (x + w, y + h), colors.WHITE, 2) # print the rectangle that did not match diff --git a/processing/color_calibrate.py b/processing/color_calibrate.py index 7bb5557..6b11bd6 100644 --- a/processing/color_calibrate.py +++ b/processing/color_calibrate.py @@ -16,7 +16,7 @@ def process(image, color_mode='rgb', apply_mask=False): - image = cv2.resize(image, ((int)(640), (int)(400)), 0, 0, cv2.INTER_CUBIC) + image = cv2.resize(image, ((int)(640), (int)(480)), 0, 0, cv2.INTER_CUBIC) if camera_mode != 'RAW': diff --git a/processing/port_tracker.py b/processing/port_tracker.py index b3518a8..94c720d 100644 --- a/processing/port_tracker.py +++ b/processing/port_tracker.py @@ -68,17 +68,16 @@ def process(img, camera, frame_cnt, color_profile): center_mass_x = x + w / 2 center_mass_y = y + h / 2 # - if shape_util.dimensions_match(contour, 6, WIDTH_TO_HEIGHT_RATIO): + if shape_util.dimensions_match(contour, 6,2, WIDTH_TO_HEIGHT_RATIO): # print 'x:%s, y:%s angle:%s ' % ( center_mass_x, center_mass_y, angle ) distance = shape_util.distance_in_inches(w) angle = shape_util.get_angle(camera, center_mass_x, center_mass_y) font = cv2.FONT_HERSHEY_DUPLEX # set tracking_data - data = dict(shape='PORT', - w=w, - h=h, - index=index, + data = dict(shape='BAY', + width=w, + height=h, dist=distance, angle=angle, xpos=center_mass_x, @@ -88,8 +87,9 @@ def process(img, camera, frame_cnt, color_profile): tracking_data.append(data) else: for target in tracking_data: - if(data["dist"] < target["dist"]): + if(distance < target["dist"]): tracking_data.insert(tracking_data.index(target), data) + break num_vertices = shape_util.find_vertices(contour) vertices_text = 'vertices:%s' % (num_vertices) diff --git a/processing/shape_util.py b/processing/shape_util.py index 351c330..2f5439d 100644 --- a/processing/shape_util.py +++ b/processing/shape_util.py @@ -3,12 +3,12 @@ ### ### -def dimensions_match(contour, vertices, desired_ratio): +def dimensions_match(contour, vertices, range, desired_ratio): peri = cv2.arcLength(contour, True) approx = cv2.approxPolyDP(contour, 0.04 * peri, True) # print(len(approx)) # if - if len(approx) == vertices or len(approx) == vertices - 1 or len(approx) == vertices + 1: + if vertices - range <= len(approx) <= vertices + range: # compute the bounding box of the contour and use the # bounding box to compute the aspect ratio (x, y, w, h) = cv2.boundingRect(approx) @@ -53,4 +53,4 @@ def get_distance(width_pixel, width_actual, focal_length): return focal_length * width_actual / width_pixel def distance_in_inches(width_pixel): - return 762 * width_pixel ** -0.8 + return 762 * (width_pixel ** -0.8) diff --git a/web/image_stream_handlers.py b/web/image_stream_handlers.py index 5f8c163..bb5e4dd 100644 --- a/web/image_stream_handlers.py +++ b/web/image_stream_handlers.py @@ -27,6 +27,7 @@ from processing import colors from processing import cvfilters +# not part of calibration anymore def convert_to_jpg(image): """TBW.""" im = Image.fromarray(image) diff --git a/web/tornado_server.py b/web/tornado_server.py index 3016f37..8a63fef 100644 --- a/web/tornado_server.py +++ b/web/tornado_server.py @@ -4,6 +4,7 @@ import config import logging import os.path +import controls from cameras.camera import USBCam from tornado.web import StaticFileHandler @@ -16,7 +17,7 @@ from web.handlers import CalibrationFeedWS from profiles.color_profile import ColorProfile -import controls +from controls import main_controller logger = logging.getLogger(__name__) @@ -34,7 +35,6 @@ def start(): color_profile_map[profile] = ColorProfile(profile) - app = tornado.web.Application( handlers=[ ("/dashboard/ws", ControllerWS), @@ -58,5 +58,7 @@ def start(): # Start the app logger.info("Listening on http://localhost:%s/", config.tornado_server_port) + main_controller.color_profiles = color_profile_map + app.listen(config.tornado_server_port) IOLoop.current().start() diff --git a/web/www/camera.html b/web/www/camera.html index d87e867..c93511b 100644 --- a/web/www/camera.html +++ b/web/www/camera.html @@ -85,7 +85,6 @@
- @@ -51,7 +52,7 @@
@@ -85,7 +86,7 @@ - + @@ -139,23 +140,22 @@ - +
- +
- diff --git a/web/www/tracker.js b/web/www/tracker.js index c737e74..149f4b2 100644 --- a/web/www/tracker.js +++ b/web/www/tracker.js @@ -27,35 +27,34 @@ function showFPS(){ function draw(targets) { - var offscreenCanvas = document.createElement('canvas'); offscreenCanvas.width = canvas.width; offscreenCanvas.height = canvas.height; - var octx = offscreenCanvas.getContext("2d"); + octx.fillStyle = "#0095DD"; targets.forEach(function(target){ - octx.fillStyle = "#0095DD"; - if(target.shape == 'BALL'){ - octx.beginPath(); - octx.arc(target.xpos, target.ypos, target.radius, 0, Math.PI*2); - octx.fill(); - octx.closePath(); - } - if(target.shape == 'BAY'){ - octx.strokeRect(target.xpos,target.ypos, target.width, target.width * 11/7); - octx.fillRect(target.xpos,target.ypos, target.width, target.width * 11/7); - } - if(target.shape =='PORT'){ - x = target.xpos; - y = target.ypos; - size = target.width; - octx.beginPath(); - octx.moveTo(x + size * Math.cos(0), y + size * Math.sin(0)); - for (side; side < 7; side++) { - octx.lineTo(x + size * Math.cos(side * 2 * Math.PI / 6), y + size * Math.sin(side * 2 * Math.PI / 6)); - } - } - }) + if(target.shape == 'BALL'){ + octx.beginPath(); + octx.arc(target.xpos, target.ypos, target.radius, 0, Math.PI*2); + octx.fill(); + octx.closePath(); + } + if(target.shape == 'BAY'){ + octx.strokeRect(target.xpos,target.ypos, target.width, target.width * 11/7); + octx.fillRect(target.xpos,target.ypos, target.width, target.width * 11/7); + } + if(target.shape =='PORT'){ + x = target.xpos; + y = target.ypos; + size = target.width; + octx.beginPath(); + octx.moveTo(x + size * Math.cos(0), y + size * Math.sin(0)); + for (var side; side < 7; side++) { + octx.lineTo(x + size * Math.cos(side * 2 * Math.PI / 6), y + size * Math.sin(side * 2 * Math.PI / 6)); + } + octx.fill(); + } + }) ctx.clearRect(0, 0, canvas.width, canvas.height); ctx.drawImage(offscreenCanvas, 0, 0); showFPS() @@ -70,7 +69,7 @@ var connect = function() { const loc = window.location; const protocol = loc.protocol === "https:" ? "wss:" : "ws:"; const address = `${protocol}//${loc.host}/tracking/ws`; - + var socket = new WebSocket(address); if (socket) { socket.onopen = function() { @@ -79,10 +78,10 @@ var connect = function() { }; socket.onmessage = function(msg) { - const data = JSON.parse(msg.data); - if(data.hasOwnProperty('targets')){ - draw(data['targets']) - } + const data = JSON.parse(msg.data); + if(data.hasOwnProperty('targets')){ + draw(data['targets']) + } }; socket.onclose = function() { diff --git a/web/www/ws_streamer.js b/web/www/ws_streamer.js index c04620f..d5e8705 100644 --- a/web/www/ws_streamer.js +++ b/web/www/ws_streamer.js @@ -25,7 +25,10 @@ var start_camera_stream = function( websocket_source, target) { tdif = (new Date()).getTime() - time_0; time_0 = (new Date()).getTime(); fps = Math.round(5 * 1.0 / (tdif / 1000.0)); - $('#actual').text(fps); + $(document).ready(function(){ + $('#actual').text(fps); + }); + } }