import invert
import overlay
import preprocessing_image
import process_image
from datetime import datetime
# -------------------- Start Time --------------------
start_time = datetime.now()
# -------------------- Process Image --------------------
# Proses gambar dan cari bagian yang tidak bisa dilewati dan rencanakan jalur untuk dilewati
occupied_grids, planned_path = process_image.main('output/final.jpg')
# -------------------- Maze --------------------
print '\n========== Maze ==========\n'
# Tentukan ukuran maze yang diperlukan.
column_count = 0
row_count = 0
for occupied_grid in occupied_grids:
if (column_count < occupied_grid[0]):
column_count = occupied_grid[0]
if (row_count < occupied_grid[1]):
# print('yahan tak to ho gya')
frame = cv2.resize(frame, (640, 640))
# blur = cv2.GaussianBlur(frame, (5,5),0)
source = []
if len(position) == 2 and flag == 1:
newx = position[0][0] / 64
newy = position[0][1] / 64
dest = (position[1][0] / 64, position[1][1] / 64)
flag = 0
source = (newx, newy)
#print( source , dest)
occupied_grids, planned_path = process_image.main(source, dest, cap)
# print "Occupied Grids : "
# print occupied_grids
# print "Planned Path :"
# print planned_path
# print("actual coordinates")
path = []
for x, y in planned_path:
path.append((x * 64, y * 64))
pts = np.array(path, np.int32)
new_frame = cv2.polylines(frame, [pts], False, (255, 120, 255), 3)
def main():
SEND_COMMAND = STOP
LAST_COMMAND = SEND_COMMAND
# s, conn = make_connections()
cap = cv2.VideoCapture(numCam)
draw_source_dest(cap)
source, dest = get_source_dest()
occupied_grids, planned_path = process_image.main(source, dest, cap,
grid_size, frame_width,
frame_height, decision)
qt, path, pts = get_qt_path_pts(planned_path)
index = 0 # index of the list qt
bbox = intialize_tracker(cap)
cX, cY = 0, 0
min_v = 500
m_x, m_y = 0, 0
cmd = 0
tempx, tempy = 0, 0
# destination of the path to reach
final_x, final_y = qt[-1]
(winW, winH) = (grid_size, grid_size)
while True:
# accel, gyro, mag, temp = get_IMU_data(conn)
# print("accel: ", accel)
# print("gyro: ", gyro)
# print("mag: ", mag)
# print("temp: ", temp)
timer = cv2.getTickCount()
success, img = cap.read()
img = cv2.resize(img, (frame_width, frame_height))
success, bbox = tracker.update(img)
if success:
drawBox(img, bbox)
cX = int(bbox[0] + (bbox[2] / 2))
cY = int(bbox[1] + (bbox[3] / 2))
cv2.putText(img,
str(cX) + " " + str(cY), (cX, cY),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
else:
cv2.putText(img, "Lost", (100, 75), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
(0, 0, 255), 2)
cv2.rectangle(img, (15, 15), (200, 90), (255, 0, 255), 2)
# this code is used for making grids
for (x, y, window) in process_image.sliding_window(img,
stepSize=grid_size,
windowSize=(winW,
winH)):
# if the window does not meet our desired window size, ignore it
if window.shape[0] != winH or window.shape[1] != winW:
continue
cv2.rectangle(img, (x, y), (x + winW, y + winH), (0, 255, 0), 2)
fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer)
cv2.putText(img, str(int(fps)), (75, 40), cv2.FONT_HERSHEY_SIMPLEX,
0.7, (20, 20, 230), 2)
img = cv2.polylines(img, [pts], False, (255, 120, 255), 3)
#This function is not working correctly
if destination_reached(cX, cY, final_x, final_y):
print("destination Reached")
break
if index < len(qt) - 1:
# assume vehicle did not stop at any of the subgoal
if distance(tempx, tempy, cX, cY) < grid_size:
print('next point')
index += 1
min_v = 500
tempx, tempy = qt[index]
tempx, tempy = tempx * grid_size, tempy * grid_size
draw_pos_info(img, cX, cY, tempx, tempy)
SEND_COMMAND = get_cmd(cX, cY, min_v, tempx, tempy)
# to print the direcction of the car
print('Action ' + direction[SEND_COMMAND])
if LAST_COMMAND != SEND_COMMAND: # or timeElapsed % 3 == 0 :
# s.send(SEND_COMMAND.encode())
LAST_COMMAND = SEND_COMMAND
if len(path):
for i in range(len(path)):
source = (path[i][0], path[i][1])
if i == 0 or i == len(path) - 1:
cv2.circle(img, source, 2, (255, 0, 0), 10)
else:
cv2.circle(img, source, 2, (255, 0, 0), 2)
cv2.imshow('window', img)
if cv2.waitKey(2) & 0xFF == 27:
break
SEND_COMMAND = STOP
def get_video_data():
"""Returns palette_groups, textures, palette_changes, frames, frame_order, static_palette"""
return process_image.main()
""" main """
import process_image
OCCUPIED_GRIDS, PLANNED_PATH = process_image.main(
"Test_Images/test_image1.jpg")
print("Occupied Grids : ")
print(OCCUPIED_GRIDS)
print("Planned Path :")
for obj in PLANNED_PATH:
print('path from:', end=' ')
print(obj, end=' ')
print('path:', end=' ')
print(PLANNED_PATH[obj])
import process_image
occupied_grids, planned_path = process_image.main("test_images/test_image2.jpg")
print ("Occupied Grids : ")
print (occupied_grids)
print ("Planned Path :")
print (planned_path)
import process_image
import random
occupied_grids, planned_path, obstacles = process_image.main(
"test_images/test_" + str(random.randrange(1, 20, 1)) + ".png")
print("Obstacle Grids : ")
print(obstacles)
print("Planned Path :")
print(planned_path)
import process_image
import time
import os
import psutil
t1 = time.time()
occupied_grids = process_image.main("20x20_600p/t4.jpg")
planned_path = process_image.main("20x20_600p/t4.jpg")
print("Occupied Grids : ")
print(planned_path)
t2 = time.time()
print()
pid = os.getpid()
py = psutil.Process(pid)
memoryUse = py.memory_info()[0] / 2.**30
print('Memory Use:', memoryUse)
print("Time Elapsed: ", t2 - t1)
def main():
SEND_COMMAND = STOP
LAST_COMMAND = SEND_COMMAND
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((TCP_IP, TCP_PORT))
# deque for movement detection
pt = deque(maxlen=10)
cap = cv2.VideoCapture(numCam)
cv2.namedWindow('window')
cv2.setMouseCallback('window' , draw_circle)
# function to draw the source and destination
while True:
_, frame1 = cap.read()
frame1 = cv2.resize(frame1,(frame_width, frame_height))
if len(position):
for i in range(len(position)):
source = (position[i][0], position[i][1])
cv2.circle(frame1,source, 10, (255, 0, 0), -1)
cv2.imshow('window', frame1)
key = cv2.waitKey(2)
if key == 27:
break
source = []
dest = []
if len(position)==2:
source = (position[0][0]//grid_size, position[0][1]//grid_size)
dest = (position[1][0]//grid_size , position[1][1]//grid_size)
occupied_grids, planned_path = process_image.main(source , dest,cap,grid_size, frame_width, frame_height,decision)
print("Planned Path :", planned_path)
print("actual coordinates")
path = []
for x, y in planned_path:
path.append((x*grid_size, y*grid_size))
print(path)
frame = np.zeros((frame_width, frame_height,3),np.uint8)
qt = list()
for x in range(len(planned_path)):
i , j = planned_path[x]
qt.append((i , j))
pts = np.array(path , np.int32)
index = 0 # index of the list qt
success, frame = cap.read()
frame = cv2.resize(frame,(frame_width, frame_height))
bbox = cv2.selectROI("window",frame, False)
tracker.init(frame, bbox)
cX = 0
cY = 0
min_v = 500
m_x, m_y = 0,0
val = 0
tempx = 0
tempy = 0
# destination of the path to reach
final_x, final_y = qt[-1]
(winW, winH) = (grid_size, grid_size)
while True:
timer = cv2.getTickCount()
success, img = cap.read()
img = cv2.resize(img,(frame_width, frame_height))
success, bbox = tracker.update(img)
if success:
drawBox(img,bbox)
x = int(bbox[0] + (bbox[2]/2))
y = int(bbox[1] + (bbox[3]/2))
cX = x
cY = y
cv2.putText(img,str(x) + " " +str(y), (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
else:
cv2.putText(img, "Lost", (100, 75), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
cv2.rectangle(img,(15,15),(200,90),(255,0,255),2)
# this code is used for making grids
for (x, y, window) in process_image.sliding_window(img, stepSize=grid_size, windowSize=(winW, winH)):
# if the window does not meet our desired window size, ignore it
if window.shape[0] != winH or window.shape[1] != winW:
continue
cv2.rectangle(img, (x, y), (x + winW, y + winH), (0, 255, 0), 2)
fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer);
if fps>60: myColor = (20,230,20)
elif fps>20: myColor = (230,20,20)
else: myColor = (20,20,230)
cv2.putText(img,str(int(fps)), (75, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.7, myColor, 2);
img= cv2.polylines(img, [pts] , False, (255,120,255), 3)
xt = cX
yt = cY
if distance(xt, yt ,final_x, final_y)<grid_size:
print("destination Reached")
break
if index < len(qt) - 1:
# assume evchicle did not stop at any of the subgoal
if distance(tempx, tempy ,xt, yt)<grid_size:
print('next point')
index+=1
min_v = 500
tempx, tempy = qt[index]
tempx = tempx*grid_size
tempy = tempy*grid_size
x_est, y_est = xt, yt
print(tempx, tempy)
cv2.circle(img,(int(x_est), int(y_est)), 2, (0, 255, 0), 5)
cv2.putText(img, str(int(x_est)) + " "+ str(int(y_est)), (int(x_est), int(y_est)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255,255 ), 2)
cv2.circle(img,(int(tempx), int(tempy)), 10, (0, 255, 0), 5)
cv2.circle(img,(int(tempx), int(tempy)), 15, (0, 255, 0), 5)
cv2.putText(img, str(int(tempx)) + " "+ str(int(tempy)), (int(tempx), int(tempy)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255,255 ), 2)
p1_x, p1_y = xt + 1, yt
p2_x, p2_y = xt - 1, yt
p3_x, p3_y = xt , yt +1
p4_x, p4_y = xt , yt -1
dis1 = distance(p1_x, p1_y, tempx, tempy)
dis2 = distance(p2_x, p2_y, tempx, tempy)
dis3 = distance(p3_x, p3_y, tempx, tempy)
dis4 = distance(p4_x, p4_y, tempx, tempy)
if dis1 < min_v:
min_v = dis1
m_x , m_y= p1_x, p1_y
val = RIGHT
if dis2 < min_v:
min_v = dis2
m_x, m_y= p2_x, p2_y
val = LEFT
if dis3 < min_v:
min_v = dis3
m_x,m_y = p3_x, p3_y
val = DOWN
if dis4 < min_v:
min_v = dis4
m_x, m_y = p4_x, p4_y
val = UP
x_est, y_est = m_x, m_y
SEND_COMMAND = str(val)
# to print the direcction of the car
print('Action ' + direction[SEND_COMMAND])
if LAST_COMMAND != SEND_COMMAND:# or timeElapsed % 3 == 0 :
s.send(str(SEND_COMMAND).encode())
LAST_COMMAND = SEND_COMMAND
if len(path):
for i in range(len(path)):
source = (path[i][0], path[i][1])
if i==0 or i==len(path)-1:
cv2.circle(img,source, 2, (255, 0, 0), 10)
else:
cv2.circle(img,source, 2, (255, 0, 0), 2)
cv2.imshow('window', img)
time.sleep(0.05)
k = cv2.waitKey(2) & 0xFF
if k == 27:
break
SEND_COMMAND = STOP
s.send(str(SEND_COMMAND).encode())
s.close()
cap.release()
cv2.destroyAllWindows()
if key == 27:
break
cap1.release()
cv2.destroyAllWindows()
source = []
dest = []
if len(position) == 2:
source = (position[0][0] // grid_size, position[0][1] // grid_size)
dest = (position[1][0] // grid_size, position[1][1] // grid_size)
cap = cv2.VideoCapture(1)
occupied_grids, planned_path = process_image.main(source, dest, cap, grid_size,
frame_width, frame_height,
decision)
print("Planned Path :", planned_path)
print("actual coordinates")
path = []
for x, y in planned_path:
path.append((x * grid_size, y * grid_size))
print(path)
frame = np.zeros((frame_width, frame_height, 3), np.uint8)
qt = list()
def main():
SEND_COMMAND = STOP
LAST_COMMAND = SEND_COMMAND
s = make_connections()
cap = cv2.VideoCapture(numCam)
draw_source_dest(cap)
source, dest = get_source_dest()
occupied_grids, planned_path = process_image.main(source, dest, cap, grid_size, frame_width, frame_height, decision)
qt, path, pts = get_qt_path_pts(planned_path)
min_v = 500
x_act = list()
y_act = list()
# destination of the path to reach
final_x, final_y = qt[-1]
(winW, winH) = (grid_size, grid_size)
if runAlgorithm == 'tracker':
bbox = intialize_tracker(cap)
size = (frame_width, frame_height)
# folderNum = get_folder_num()
filePath = str(folderNum) + '/' + 'video.mp4'
result = cv2.VideoWriter(filePath,
cv2.VideoWriter_fourcc(*'MP4V'),
20, size)
while True:
# accel, gyro, mag, temp = get_IMU_data(conn)
timer = cv2.getTickCount()
_, img = cap.read()
img = cv2.resize(img,(frame_width, frame_height))
draw_circle_on_source(img, path)
result.write(img)
make_grids(img, grid_size, winW, winH)
img = cv2.polylines(img, [pts] , False, (255,120,255), 3)
if runAlgorithm == 'tracker':
SEND_COMMAND = run_tracker_algo(tracker, img, final_x, final_y, qt, grid_size, min_v)
else:
SEND_COMMAND, xt, yt = run_heading_algo(img, stkr1minHSV,stkr1maxHSV, stkr2minHSV, stkr2maxHSV, qt)
x_act.append(xt)
y_act.append(yt)
if SEND_COMMAND == 'done':
break
LAST_COMMAND = send_command(s, LAST_COMMAND, SEND_COMMAND)
# to print the direcction of the car
print('Action ' + direction[SEND_COMMAND])
cv2.imshow('window', img)
if cv2.waitKey(2) & 0xFF == 27:
break
# can be used to make the graph of the actual and experimental path
make_graph(x_act, y_act, pts)
SEND_COMMAND = STOP
finish(s, cap, SEND_COMMAND)