Augmented Maze Game
https://thanatchai12.wixsite.com/website
user_1231569063
python
4 years ago
11 kB
6
Indexable
import cv2
import imutils
import numpy as np
import cvzone
import paho.mqtt.client as mqtt
# -------------------
host = "service.anto.io"
port = 1883
user = "Overdamped"
password = "7dED62Ze0gazqVxtjsGc9TtYC2PPW5o9IWWQT8es"
milli = 200
sx = 200
sy = 520
wx = 610
wy = 410
offset = 20
def on_connect(self, client, userdata, rc):
print("MQTT Connected.")
self.subscribe("channel/Overdamped/Lidarbot/State_car")
self.subscribe("channel/Overdamped/Lidarbot/Start_position")
self.subscribe("channel/Overdamped/Lidarbot/Current_position")
self.subscribe("channel/Overdamped/Lidarbot/Set_angle")
def on_massage(client, userdata, msg):
print(msg.payload.decode("utf-8", "strict"))
client = mqtt.Client()
client.username_pw_set(user, password=password)
client.on_connect = on_connect
client.on_message = on_massage
client.connect(host, port=port)
# -------------------
lower_blue = np.array([40, 86, 0])
upper_blue = np.array([125, 255, 255])
lower_red = np.array([150, 80, 0])
upper_red = np.array([180, 255, 255])
cap = cv2.VideoCapture(0)
cap.set(3, 800)
cap.set(4, 600)
png_blue_1 = cv2.imread("PNG/maze_easy_blue.png", cv2.IMREAD_UNCHANGED)
# png_blue_2 = cv2.imread("PNG/complete_maze_blue_2.png", cv2.IMREAD_UNCHANGED)
png_show = cv2.imread("PNG/maze_easy_black.png", cv2.IMREAD_UNCHANGED)
png2 = cv2.imread("PNG/maze_easy_red.png", cv2.IMREAD_UNCHANGED)
png_win = cv2.imread("PNG/maze_easy_win.png", cv2.IMREAD_UNCHANGED)
rot_g = cv2.imread("PNG/rotation_guide.png", cv2.IMREAD_UNCHANGED)
K = 1
h, w, _ = rot_g.shape
robot_p = 0
mask_rp = 0
while True:
client.loop_start()
if K == 1:
client.publish("channel/Overdamped/Lidarbot/State_car", "0")
client.publish("channel/Overdamped/Lidarbot/Start_position", f'{sx},{sy},{offset}')
K = 0
A = 0
# B = 0
# C = 0
RP = 0
r_key = 1
success, frame = cap.read()
blurred_frame = cv2.GaussianBlur(frame, (5, 5), 0)
combine = cvzone.overlayPNG(blurred_frame, png_show, [0, 0])
cx = 0
cy = 0
# --------------------------------------------------
hsv = cv2.cvtColor(blurred_frame, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, lower_blue, upper_blue)
cnts = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
for c in cnts:
area2 = cv2.contourArea(c)
if area2 > 10:
# cv2.drawContours(robot_p, [c], -1, (255, 0, 0), 1)
M = cv2.moments(c)
cx = int(M["m10"] / M["m00"])
cy = int(M["m01"] / M["m00"])
cv2.circle(combine, (cx, cy), 3, (255, 255, 255), -1)
try:
ox = cx - w // 2
oy = cy - w // 2
robot_p = cvzone.overlayPNG(blurred_frame, rot_g, [ox, oy])
hsv_rp = cv2.cvtColor(robot_p, cv2.COLOR_BGR2HSV)
mask_rp = cv2.inRange(hsv_rp, lower_blue, upper_blue)
cnts_rp = cv2.findContours(mask_rp, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
cnts_rp = imutils.grab_contours(cnts_rp)
for c_rp in cnts_rp:
area = cv2.contourArea(c_rp)
if area > 100:
cv2.drawContours(robot_p, [c_rp], -1, (0, 255, 0), 1)
RP = area
except:
pass
# print(str(int(RP)))
# print("centriod is at ...", cx, cy)
# if 629 <= RP <= 632:
# client.publish("room31231432/Set_angle", "0")
# else:
# client.publish("room31231432/Set_angle", "1")
# --------------------------------------------------
combine_b1 = cvzone.overlayPNG(blurred_frame, png_blue_1, [0, 0])
hsv_b1 = cv2.cvtColor(combine_b1, cv2.COLOR_BGR2HSV)
mask_b1 = cv2.inRange(hsv_b1, lower_blue, upper_blue)
cnts_b1 = cv2.findContours(mask_b1, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
cnts_b1 = imutils.grab_contours(cnts_b1)
for c_b1 in cnts_b1:
area = cv2.contourArea(c_b1)
if area > 1000:
cv2.drawContours(combine, [c_b1], -1, (0, 0, 255), 1)
A = area
#
# combine_b2 = cvzone.overlayPNG(blurred_frame, png_blue_2, [0, 0])
# hsv_b2 = cv2.cvtColor(combine_b2, cv2.COLOR_BGR2HSV)
#
# mask_b2 = cv2.inRange(hsv_b2, lower_blue, upper_blue)
#
# cnts_b2 = cv2.findContours(mask_b2, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
# cnts_b2 = imutils.grab_contours(cnts_b2)
#
# for c_b2 in cnts_b2:
# area = cv2.contourArea(c_b2)
#
# if area > 1000:
# cv2.drawContours(combine, [c_b2], -1, (0, 0, 255), 1)
# B = area
# C = A + B
# print(str(int(A)))
cv2.imshow("Frame", combine)
cv2.imshow("Mask1", mask_b1)
cv2.imshow("Robot Position", robot_p)
cv2.imshow("Mask_rp", mask_rp)
# cv2.imshow("Mask2", mask_b2)
key = cv2.waitKey(milli)
if A > 45540:
client.publish("channel/Overdamped/Lidarbot/State_car", "1")
while True:
success, frame = cap.read()
blurred_frame = cv2.GaussianBlur(frame, (5, 5), 0)
combine2 = cvzone.overlayPNG(blurred_frame, png2, [0, 0])
hsv2 = cv2.cvtColor(blurred_frame, cv2.COLOR_BGR2HSV)
mask2 = cv2.inRange(hsv2, lower_blue, upper_blue)
cnts2 = cv2.findContours(mask2, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
cnts2 = imutils.grab_contours(cnts2)
for c_2 in cnts2:
area2 = cv2.contourArea(c_2)
if area2 > 10:
cv2.drawContours(combine2, [c_2], -1, (0, 0, 255), 1)
M = cv2.moments(c_2)
cx = int(M["m10"] / M["m00"])
cy = int(M["m01"] / M["m00"])
cv2.circle(combine2, (cx, cy), 3, (255, 255, 255), -1)
# print("centriod is at ...", cx, cy)
client.publish("channel/Overdamped/Lidarbot/Current_position", f'{cx},{cy}')
try:
ox = cx - w // 2
oy = cy - w // 2
robot_p = cvzone.overlayPNG(blurred_frame, rot_g, [ox, oy])
hsv_rp = cv2.cvtColor(robot_p, cv2.COLOR_BGR2HSV)
mask_rp = cv2.inRange(hsv_rp, lower_blue, upper_blue)
cnts_rp = cv2.findContours(mask_rp, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
cnts_rp = imutils.grab_contours(cnts_rp)
for c_rp in cnts_rp:
area = cv2.contourArea(c_rp)
if area > 100:
cv2.drawContours(robot_p, [c_rp], -1, (0, 255, 0), 1)
RP = area
except:
pass
# print(str(int(RP)))
# print("centriod is at ...", cx, cy)
if r_key == 1:
if 629 <= RP <= 636:
client.publish("channel/Overdamped/Lidarbot/Set_angle", "0")
r_key = 0
else:
client.publish("channel/Overdamped/Lidarbot/Set_angle", "1")
# print("RP", RP)
cv2.imshow("Frame", combine2)
cv2.imshow("Mask1", mask2)
cv2.imshow("Robot Position", robot_p)
cv2.imshow("Mask_rp", mask_rp)
key = cv2.waitKey(milli)
if sx - offset < cx < sx + offset and sy - offset < cy < sy + offset:
break
K = 1
client.loop_stop()
if wx - offset < cx < wx + offset and wy - offset < cy < wy + offset:
client.publish("channel/Overdamped/Lidarbot/State_car", "2")
while True:
success, frame = cap.read()
blurred_frame = cv2.GaussianBlur(frame, (5, 5), 0)
combine_win = cvzone.overlayPNG(blurred_frame, png_win, [0, 0])
hsv_win = cv2.cvtColor(blurred_frame, cv2.COLOR_BGR2HSV)
mask_win = cv2.inRange(hsv_win, lower_blue, upper_blue)
cnts_win = cv2.findContours(mask_win, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
cnts_win = imutils.grab_contours(cnts_win)
for c_win in cnts_win:
area2 = cv2.contourArea(c_win)
if area2 > 10:
cv2.drawContours(combine_win, [c_win], -1, (255, 40, 0), 3)
M = cv2.moments(c_win)
cx = int(M["m10"] / M["m00"])
cy = int(M["m01"] / M["m00"])
cv2.circle(combine_win, (cx, cy), 3, (255, 255, 255), -1)
# print("centriod is at ...", cx, cy)
client.publish("channel/Overdamped/Lidarbot/Current_position", f'{cx},{cy}')
try:
ox = cx - w // 2
oy = cy - w // 2
robot_p = cvzone.overlayPNG(blurred_frame, rot_g, [ox, oy])
hsv_rp = cv2.cvtColor(robot_p, cv2.COLOR_BGR2HSV)
mask_rp = cv2.inRange(hsv_rp, lower_blue, upper_blue)
cnts_rp = cv2.findContours(mask_rp, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
cnts_rp = imutils.grab_contours(cnts_rp)
for c_rp in cnts_rp:
area = cv2.contourArea(c_rp)
if area > 100:
cv2.drawContours(robot_p, [c_rp], -1, (0, 255, 0), 1)
RP = area
except:
pass
# print(str(int(RP)))
# print("centriod is at ...", cx, cy)
if r_key == 1:
if 629 <= RP <= 636:
client.publish("channel/Overdamped/Lidarbot/Set_angle", "0")
r_key = 0
else:
client.publish("channel/Overdamped/Lidarbot/Set_angle", "1")
cv2.imshow("Frame", combine_win)
cv2.imshow("Mask1", mask_win)
cv2.imshow("Robot Position", robot_p)
cv2.imshow("Mask_rp", mask_rp)
key = cv2.waitKey(milli)
if sx - offset < cx < sx + offset and sy - offset < cy < sy + offset:
break
K = 1
client.loop_stop()
if key == 27:
break
cap.release()
cv2.destroyAllWindows()Editor is loading...