<jupyter_start><jupyter_text>GRID SOFTWARE v6.0- Some Useful Functions- Communication Module - Tracking Module- Navigation Algorithm- Main Function*STATUS* : Runnning*PROBLEMS TO SOLVE* :1. Time delay between command and execution ; preventing overshoot2. Extending the code for entire arena3. Getting an estimate of angular velocity and turn proportional to the angle4. Reducing the frequent turning of the bot5. Using Cam Caliberation to determine Marker Pose (Optional)6. Implement Reverse ( difficult)7. Check for input video feed speed  Some useful Functions<jupyter_code>import cv2
from cv2 import aruco
import numpy as np
import math 
import time
import socket
import os
import glob
import shutil
from os.path import isfile, join
import datetime
size = 0.127
bot_ids = [22, 1, 9, 22]  # Give the aruco code of the bot

def distCalc(r1, r2):
    x1, y1 = r1
    x2, y2 = r2 
    
    return math.dist((x1,y1), (x2,y2))

def centreCalc(r1, r2):
    return (int((r1[0]+r2[0])/2), int((r1[1]+r2[1])/2))

def angleCalc(r1, r2):
    x1, y1 = r1
    x2, y2 = r2
    
    inner_prod = x1*x2 + y1*y2
    len1 = math.hypot(x1, y1)
    len2 = math.hypot(x2, y2)
    
    angle = math.acos(inner_prod/(len1*len2))
    
    if (x1*y2 - x2*y1) < 0:
        angle = -angle
    
    return angle<jupyter_output><empty_output><jupyter_text>Establishing Server Connection<jupyter_code>hostname = socket.gethostname()
my_ip = socket.gethostbyname(hostname)
print(my_ip) #Give this ID in the ESP32 sketch

def startServer(port):
    global serversocket
    serversocket = socket.socket()
    host = '0.0.0.0'
   
    try:
        serversocket.bind((host, port))
    except socket.error as e:
        print(str(e))
    
    print("Waiting for connection")
    serversocket.listen(5)

    global client, addr
    client, addr = serversocket.accept()
    print("Connection made with " + str(client))

def closeServer():
    client.close()
    serversocket.close()
    
def sendCommand(command, t=0):
    client.sendto(command.encode('UTF-8'), addr)
    time.sleep(t)<jupyter_output>192.168.239.1<jupyter_text>Bot Tracker<jupyter_code>def arucoDetector(img) :
    imgGray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    key = getattr(aruco, f'<KEY>')
    arucoDict = aruco.Dictionary_get(key)
    arucoParam = aruco.DetectorParameters_create()

    bbox, ids, rejected = aruco.detectMarkers(imgGray, arucoDict, parameters=arucoParam)
    
    img = aruco.drawDetectedMarkers(img, bbox)
    
    return img, bbox, ids<jupyter_output><empty_output><jupyter_text>Navigation Algorithm<jupyter_code>def arucoOrient(img, bbox, ids, bot, dest = 0) :
    bot_flag = 0
    dest_flag = 0
    for i in range(len(ids)):
        if ids[i]==dest:
            dest_coords = centreCalc(bbox[i][0][0],bbox[i][0][2])
            dest_flag = 1
        if ids[i]==bot_ids[bot]:
            bot_coords = centreCalc(bbox[i][0][0], bbox[i][0][2])
            bot_front = centreCalc(bbox[i][0][0], bbox[i][0][1])
            bot_rear = centreCalc(bbox[i][0][2], bbox[i][0][3])
            bot_flag = 1


    img = cv2.arrowedLine(img, bot_rear, bot_front, (0,0,255), 2)
    img = cv2.arrowedLine(img, bot_coords, dest_coords, (0,255,0), 2)
    angle = angleCalc(np.subtract(np.array(bot_front), np.array(bot_rear)),np.subtract(np.array(dest_coords), np.array(bot_coords)))
    distance = distCalc(bot_coords, dest_coords)
    return img, bot_coords, dest_coords, distance, angle
def commandfunc(distance, angle, r_thr = 50, theta_thr = 0.3):
   
    command = '0'
    flag = 0
    
    if abs(angle) > theta_thr :
        if angle>theta_thr :
            command = ('5')
        else:
            command = ('7')
            
    elif distance < r_thr :
        command = ('0')
        flag=1
    else:
        command = ('1')
         
  
    return command, flag<jupyter_output><empty_output><jupyter_text>Save the video file<jupyter_code>def convert_frames_to_video(pathIn,pathOut,fps):
    frame_array = []
    files = [f for f in os.listdir(pathIn) if isfile(join(pathIn, f))]

    #for sorting the file names properly
    files = sorted(files, key=lambda x: int(x.split('.')[1]))

    for i in range(len(files)):
        filename=pathIn + files[i]
        #reading each files
        img = cv2.imread(filename)
        height, width, layers = img.shape
        size = (width,height)
        #print(filename)
        #inserting the frames into an image array
        frame_array.append(img)

    out = cv2.VideoWriter(pathOut,cv2.VideoWriter_fourcc(*'DIVX'), fps, size)

    for i in range(len(frame_array)):
        # writing to a image array
        out.write(frame_array[i])
    out.release()
    shutil.rmtree('./data')<jupyter_output><empty_output><jupyter_text>Sample Command<jupyter_code>def sampleCommand():
    sendCommand('0', 0)
    sendCommand('1', 0)
    sendCommand('0', 0)
'''    
startServer()
sampleCommand()
closeServer()
'''
#startServer(8093)
#for i in range(0,6):
   # sendCommand('10')<jupyter_output><empty_output><jupyter_text>Main Function<jupyter_code>if __name__ == '__main__':

    p = 60
    cap = cv2.VideoCapture(1)
    if cap is not None :
        print("GoingGood")
    dest = 0
    command = '0'
    angle = 0
    flag = 0
    k = 0
    i = 0
    bot = 0
    no_bots = 2
    path = [[23,24,23,25],[24,23,25,23],[7,8,7,6],[11,12,11,10]]
    flip = [25, 4, 8, 12]
    
    startServer(8093)
    
    try :
        os.mkdir('./data')
    except :
        shutil.rmtree('./data')
        os.mkdir('./data')

    try :
        os.mkdir('./TestingVideos')
    except :
        pass
        
    start = time.time()
    
    while True :
        success, frame = cap.read()

        if frame is not None :
            
            frame, bbox, ids = arucoDetector(frame)
            #print(ids)
            if ids is not None:
                if bot_ids[bot] in ids and path[bot][i] in ids :
                    if flag==0:
                        
                        frame, bot_coords, dest_coords, distance, angle = arucoOrient(frame, bbox, ids,bot, path[bot][i])
                        command, flag = commandfunc(distance, angle)
                        sendCommand(command, bot)
                        frame = cv2.putText(frame, "Command : " + str(command),(50,50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2 )
                        frame = cv2.putText(frame, "Distance : " + str(distance),(50,75), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2 )
                        frame = cv2.putText(frame, "Angle : " + str(angle),(50,100), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2 ) 
                        
                    if flag==1:
                        flag = 0
                        frame = cv2.putText(frame, "Next to Go " + str(path[bot][i]),(50,100), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2 )  

                        if path[bot][i] in flip:
                            print("Flip Now")
                            frame = cv2.putText(frame, "Flipping Happening " + str(path[bot][i]),(50,100), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2 )  
                            
                        if i<3:
                            i+=1
                        elif bot<no_bots:
                            bot+=1
                            i=0
                            port+=1
                            
                            closeServer()
                            startServer(port)
                            print("Closing and Startin the port number")
                            print(port)
                        else:
                            break
                                           
            else:
                frame = cv2.putText(frame, "No Aruco Detected", (50,50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2)
          
            cv2.namedWindow("Live Tracking", cv2.WINDOW_NORMAL)
            cv2.resizeWindow("Live Tracking",int(frame.shape[1]*p/100),int(frame.shape[0]*p/100))
            cv2.imshow("Live Tracking", frame)
            
            cv2.imwrite('./data/frames.'+str(k)+'.png',frame)
            k+=1
            
        else :
            print("Error in reading frame")

        q = cv2.waitKey(1)
        if q == ord("q"):
            break

end = time.time()
laptime = end - start
print(laptime)
frame = cv2.putText(frame, "LAPTIME :" + str(laptime), (150,150), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2)
cv2.imwrite('./data/frames.'+str(k)+'.png',frame)
k+=1
cv2.destroyAllWindows()
closeServer()

ct = datetime.datetime.today()
ct1 = str(ct).split()
ct = ct1[1].split('.')[0].split(':')
convert_frames_to_video('./data/', './TestingVideos/' +str(ct1[0])+'-'+str(ct[0])+'-'+str(ct[1])+'-'+str(ct[2])+'.mp4', 30.0)
startServer(8093)
sendCommand('9')
closeServer()<jupyter_output>Waiting for connection
{'fraction_non_alphanumeric': 0.13381163876551647, 'fraction_numerical': 0.039517139277986564, 'mean_word_length': 2.2203887055372205, 'pattern_counts': {'":': 0, '<': 32, '<?xml version=': 0, '>': 30, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 2, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '7', 'dir': 'jupyter-structured-clean-dedup', 'id': '64966', 'license': 'MIT', 'n_tokens_mistral': 3214, 'n_tokens_neox': 3023, 'n_words': 644, 'path': 'centralnav.ipynb', 'repo_name': 'Srrrrini/Central-Navigation-System-GRID-3.0-', 'size': '8788'}