Development

.flake8

@@ -0,0 +1,2 @@
+[flake8]
+max-line-length = 999

.gitignore

@@ -1,3 +1,5 @@
 __pycache__/
 *.pyc
 .venv/
+*.jpg
+.DS_Store

.vscode/launch.json

@@ -8,7 +8,7 @@
 			"name": "Download and Run",
 			"type": "ev3devBrowser",
 			"request": "launch",
-			"program": "/home/robot/${workspaceRootFolderName}/main.py",
+			"program": "/home/robot/${workspaceRootFolderName}/src/on_ev3/EV3Main.py",
 			"interactiveTerminal": false
 		}
 	]

calibration_matrix.yaml

@@ -0,0 +1,16 @@
+camera_matrix:
+- - 37164.97931469605
+  - 0.0
+  - 638.2287694582017
+- - 0.0
+  - 25034.419645088034
+  - 364.4191971665351
+- - 0.0
+  - 0.0
+  - 1.0
+dist_coeff:
+- - 5.219524221436717
+  - 3.8691893079446813
+  - -0.24236571736516715
+  - 0.08563888557233278
+  - 0.0030729958759180345

src/on_computer/CameraCalibration.py

@@ -0,0 +1,134 @@
+from cv2.gapi import video
+import numpy as np
+import cv2
+import glob
+import pathlib
+import yaml
+
+x = 1280
+y = 720
+resolution = (x, y)
+
+def get_calibration_data(filename="calibration_matrix.yaml"):
+    with open(filename, "r") as f:
+        data = yaml.safe_load(f)
+    return np.array(data['camera_matrix']), np.array(data['dist_coeff'])
+
+def capture_calibration_images():
+    no_of_calibration_images = 20
+
+    video_capture = cv2.VideoCapture(1, cv2.CAP_DSHOW) #Open camera WINDOWS OS
+    #video_capture = cv.VideoCapture(0) #Open camera MAC OS
+
+    path = 'images'
+    pathlib.Path(path).mkdir(parents=True, exist_ok=True)
+
+    i = 0
+    while i < no_of_calibration_images:
+        ret, frame = video_capture.read()
+        frame = cv2.resize(frame, resolution, interpolation=cv2.INTER_NEAREST)
+        cv2.imshow('capture calibration image - "q" to capture', frame)
+        if cv2.waitKey(1) & 0xFF == ord('q'):
+            cv2.imwrite(path + '/calib' + str(i) + '.jpg', frame)
+            print("Image " + str(i) + " saved")
+            i += 1
+
+    video_capture.release()
+    cv2.destroyAllWindows()
+
+def calibrate_camera():
+    chessboardCorners = (6, 9)
+
+    # Define the size of the squares in the chessboard, in meters
+    squaresize = 0.05  # 2.3 cm = 0.023 m
+
+    # termination criteria
+    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+    # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
+    objp = np.zeros((chessboardCorners[0]*chessboardCorners[1],3), np.float32)
+    objp[:,:2] = np.mgrid[0:chessboardCorners[0],0:chessboardCorners[1]].T.reshape(-1,2)
+    objp = objp * squaresize  # Multiply by the size of the squares to get the actual dimensions
+    # Arrays to store object points and image points from all the images.
+    objpoints = [] # 3d point in real world space
+    imgpoints = [] # 2d points in image plane.
+
+    images = glob.glob(r'images/*.jpg')
+    print(f'Found {len(images)} images.')
+
+    # path = 'results'
+    # pathlib.Path(path).mkdir(parents=True, exist_ok=True) 
+
+    found = 0
+    for fname in images:
+        img = cv2.imread(fname) # Capture frame-by-frame
+        #print(images[im_i])
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+        img = cv2.resize(img, resolution, interpolation=cv2.INTER_NEAREST)
+        # Find the chess board corners
+        ret, corners = cv2.findChessboardCorners(gray, chessboardCorners, None)
+        # If found, add object points, image points (after refining them)
+        if ret == True:
+            print('Found corners in image', fname)
+            objpoints.append(objp)   # Certainly, every loop objp is the same, in 3D.
+            corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
+            imgpoints.append(corners2)
+            # Draw and display the corners
+            img = cv2.drawChessboardCorners(img, chessboardCorners, corners2, ret)
+            found += 1
+            cv2.imshow('img', img)
+            cv2.waitKey(500)
+            # if you want to save images with detected corners 
+            # uncomment following 2 lines and lines 5, 18 and 19
+            # image_name = path + '/calibresult' + str(found) + '.png'
+            # cv2.imwrite(image_name, img)
+        else:
+            print('Corners not found in image', fname)
+
+    print("Number of images used for calibration: ", found)
+
+    # When everything done, release the capture
+    # cap.release()
+    cv2.destroyAllWindows()
+
+    # calibration
+    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
+
+    cap = cv2.VideoCapture(1, cv2.CAP_DSHOW)
+
+    while True:
+        # Capture frame-by-frame
+        ret, frame = cap.read()
+
+        frame = cv2.resize(frame, resolution, interpolation=cv2.INTER_NEAREST)
+
+        # If frame is read correctly, ret is True
+        if not ret:
+            print("Can't receive frame (stream end?). Exiting ...")
+            break
+
+        # Undistort the frame
+        undistorted_frame = cv2.undistort(frame, mtx, dist, None, mtx)
+
+        # Display the resulting frame
+        cv2.imshow('Undistorted Video Capture - "q" to close', undistorted_frame)
+
+        # Break the loop on 'q' key press
+        if cv2.waitKey(1) == ord('q'):
+            break
+
+    # After the loop release the cap object
+    cap.release()
+    # Destroy all the windows
+    cv2.destroyAllWindows()
+
+
+    # transform the matrix and distortion coefficients to writable lists
+    data = {'camera_matrix': np.asarray(mtx).tolist(),
+            'dist_coeff': np.asarray(dist).tolist()}
+
+    # and save it to a file
+    with open("calibration_matrix.yaml", "w") as f:
+        yaml.dump(data, f)
+
+#capture_calibration_images()
+calibrate_camera()

src/on_computer/ComputerMain.py

@@ -1,24 +1,134 @@
-import socket
-import PathfindingAlgorithm
-from PathfindingAlgorithm import grid, start, end, Node
+import copy
 import json
+import socket
+import threading
+import time
+
+import Globals as G
+
+from pathfinding.feedback import is_robot_position_correct
+from pathfinding.PathfindingAlgorithm import a_star
+from positions.Positions import find_start_node, find_first_ball, sort_balls_by_distance
+from computer_vision.Camera import capture_frames
+from computer_vision.ComputerVision import update_positions
+from helpers.end_of_path_pickup import distance_between
+from helpers.get_path_to_goal import get_path_to_goal
+from pathfinding.PathfindingAlgorithm import a_star
+from pathfinding.feedback import is_robot_position_correct
+from positions.Positions import find_first_ball, find_start_node
+from positions.Robot_direction import calculate_heading
+
+
+
+# Assign thread to capture continous frames
+threading.Thread(target=capture_frames).start()
+
+
+while G.BIG_FRAME is None or G.SMALL_FRAME is None:
+    time.sleep(0.2)
+
+# Assign thread to update positions using CV
+threading.Thread(target=update_positions).start()
+
+# We are in danger of going on old data cause we dont check if a new position,
+# is found in pictures. Dont really know if it is a problem,
+# shouldnt be if recognition is good enough
+while G.ROBOT_POSITION is None or G.ROBOT_HEADING is None or G.GRID is None or G.BALLS is None:
+    print('Heading: ', G.ROBOT_HEADING)
+    print('The robot position: ', G.ROBOT_POSITION)
+    print('The ball position: ', G.BALLS)
+    time.sleep(0.2)
 
 # Creates a socket object, and established a connection to the robot
-socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+HOST = "192.168.8.111"
+PORT = 9999
+client_socket.connect((HOST, PORT))
+
+print(G.ROBOT_POSITION, G.ROBOT_HEADING, G.BALLS[0])
+end_node = G.GRID[G.BALLS[0][1]][G.BALLS[0][0]] #Is it possible to do this?
+
+
+balls_picked_up = 0
+
+while True:
+    # If statement for depositing the balls in goal
+    if balls_picked_up == 3:
+        client_socket.send('GOAL'.encode('utf-8'))
+        goal_path = get_path_to_goal()
+        to_goal_start_node = goal_path[0]
+        if goal_path is None:
+            print('The algorithm could not find a path to the goal')
+            time.sleep(5)
+        else:
+            path_as_tuples = [(node.x, node.y) for node in goal_path]
+            print(path_as_tuples)
+            path_as_json = json.dumps(path_as_tuples)
+            json_length = len(path_as_json)
+            client_socket.send(json_length.to_bytes(4, 'big'))
+            client_socket.send(path_as_json.encode('utf-8'))
+            balls_picked_up = 0
+            while is_robot_position_correct(G.ROBOT_HEADING, goal_path, to_goal_start_node):
+                client_socket.send('KEEP'.encode('utf-8'))
+                time.sleep(1)
+                to_goal_start_node = find_start_node()
+            print('attempting to stop robot')
+            for i in range(10):
+                client_socket.send('STOP'.encode('utf-8'))
+            response = client_socket.recv(7).decode('utf-8').strip()
+
+    # If statement for picking up balls
+    elif distance_between(G.ROBOT_POSITION, (end_node.x, end_node.y)) < 50:
+        if G.BALLS is not None:
+            heading_to_ball = calculate_heading(G.ROBOT_POSITION, G.BALLS[0])
+            distance = distance_between(G.ROBOT_POSITION, G.BALLS[0])
+            client_socket.send('PICK'.encode('utf-8'))
+            client_socket.send(str(distance).encode('utf-8'))
+            client_socket.send(str(int(heading_to_ball)).encode('utf-8'))
+            client_socket.send(str(int(G.ROBOT_HEADING)).encode('utf-8'))
+            response = client_socket.recv(7).decode('utf-8').strip()
+            balls_picked_up += 1
+            end_node = G.GRID[G.BALLS[0][1]][G.BALLS[0][0]]  # We make sure the robot is going to the next ball
+
+    # The robot will follow a path to the first ball in G.BALLS
+    if balls_picked_up != 3:
+        client_socket.send('PATH'.encode('utf-8'))
+
+        start_node = find_start_node()  # Function for diffing the calculated robot position with the camera robot position
+
+        if G.ROBOT_HEADING is None:
+            print('ERROR: No heading calcultated')
+        else:
+            heading_as_string = str(int(G.ROBOT_HEADING))
+            client_socket.send(heading_as_string.encode('utf-8'))
+            print('Heading send')
 
-host = "192.168.8.111"
-port = 9999
+        grid_copy = copy.deepcopy(G.GRID)
+        end_node_copy = grid_copy[end_node.y][end_node.x]
+        start_node_copy = grid_copy[start_node.y][start_node.x]
+        path = a_star(grid_copy, start_node_copy, end_node_copy)
 
-socket.connect((host, port))
+        if path is None:
+            print('The algorithm could not find a path')
+        else:
+            print('Path: OK')
+            path_as_touples = [(node.x, node.y) for node in path[:-5]]
+            path_as_json = json.dumps(path_as_touples)
+            json_length = len(path_as_json)
+            client_socket.send(json_length.to_bytes(4, 'big'))
+            client_socket.send(path_as_json.encode('utf-8'))
 
-# Send the path to the robot
-path = PathfindingAlgorithm.a_star(grid, start, end)
-path_as_dictionaries = [{'x': node.x, 'y': node.y} for node in path]
-path_as_json = json.dumps(path_as_dictionaries)
+        while is_robot_position_correct(G.ROBOT_HEADING, path, start_node):
+            client_socket.send('KEEP'.encode('utf-8'))
+            time.sleep(1)
+            start_node = find_start_node()
+            if (distance_between(G.ROBOT_POSITION, (end_node.x, end_node.y)) < 50):
+                break
 
-command = 'PATH'
-socket.send(command.encode('utf-8'))
-socket.send(path_as_json.encode('utf-8'))
+        # Send the stop command to the robot
+        print('attempting to stop robot')
+        for i in range(10):
+            client_socket.send('STOP'.encode('utf-8'))
 
-# Close the connection
-socket.close()
+        response = client_socket.recv(7).decode('utf-8').strip()
+        print(response)

src/on_computer/Globals.py

@@ -0,0 +1,8 @@
+BIG_FRAME = None
+SMALL_FRAME = None
+
+ROBOT_POSITION = None
+ROBOT_HEADING = None
+GRID_DATA = None
+GRID = None
+BALLS = None