RobotTest.py

import cv2 as cv
import requests
import numpy as np
import serial
import time
URL = r'http://choilab409.iptime.org:12345/'
THRESHOLD = 180
ANIMAL_SIZE = 1000
sr = serial.Serial("COM3", 57600, timeout=0, rtscts=False)

if not sr.is_open:
    print('Serial Port is NOT Open')


# denoise
def getKernel(size):
    size = int(size)
    return cv.getStructuringElement(cv.MORPH_ELLIPSE, (size, size),
                                    ((int((size - 1) / 2), int((size - 1) / 2))))
def denoiseBinaryImage(binaryImage):
    """
    __denoiseBinaryImage : remove small artifacts from the binary Image using the cv.morphologyEx function
    :param binaryImage: boolean image
    :return: denoised binary image
    """
    # opening -> delete noise : erode and dilate
    # closing -> make into big object : dilate and erode
    denoisedBinaryImage = binaryImage
    denoisedBinaryImage = cv.morphologyEx(denoisedBinaryImage, cv.MORPH_OPEN, getKernel(3))
    denoisedBinaryImage = cv.morphologyEx(denoisedBinaryImage, cv.MORPH_OPEN, getKernel(5))
    denoisedBinaryImage = cv.morphologyEx(denoisedBinaryImage, cv.MORPH_CLOSE, getKernel(10))
    return denoisedBinaryImage



# Read Image
def checkAnimal(THRESHOLD):
    vc = cv.VideoCapture(URL)
    _ , img = vc.read()

    binaryImg = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
    binaryImg = cv.threshold(binaryImg, THRESHOLD, 255, cv.THRESH_TOZERO)[1]
    binaryImg = denoiseBinaryImage(binaryImg)
    cv.imshow('Robot', binaryImg)
    cv.waitKey(10)
    cnts = cv.findContours(binaryImg, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_NONE)[0]
    
    # Find the largest contour
    cnts = cv.findContours(binaryImg, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_NONE)[0]
    if len(cnts) == 0:
        return 0

    maxCntSize = 0
    maxCntIndex = None
    for j, cnt in enumerate(cnts):
        area = cv.contourArea(cnt)
        if area > maxCntSize:
            maxCntSize = area
            maxCntIndex = j
    img = cv.drawContours(img, cnts, maxCntIndex, [0,0,255], thickness=2)
    cv.imshow('Robot', img)
    cv.waitKey(1)
    return maxCntSize

    # img = cv.drawContours(img, cnts, maxCntIndex, [0,0,255], thickness=2)
    # cv.imshow('Robot', img)
    # cv.waitKey(0)
    # cv.destroyWindow('Robot')
startTime = time.time()
lastSendTime = startTime
# while time.time() - startTime < 120:
#     if time.time() - lastSendTime > 0.6:
#         lastSendTime = time.time()
#         animalLevel = checkAnimal(THRESHOLD)
#         if animalLevel < ANIMAL_SIZE:
#             print(f'animalNotFound {animalLevel}')
#             sr.write('xa'.encode())
#             print('Send')
#         else:
#             print(f'animalFound {animalLevel}')
#             sr.write('xw'.encode())
#             print('Send')

while time.time() - startTime < 120:
    if time.time() - lastSendTime > 0.5:
        lastSendTime = time.time()
        #animalLevel = checkAnimal(THRESHOLD)
        if (time.time() % 10) > 5:
            print('Rotate')
            sr.write(b'x')
        else:
            print('Forward')
            sr.write(b'w')


sr.write('x'.encode())