Augmented Maze Game

https://thanatchai12.wixsite.com/website
 avatar
user_1231569063
python
2 years ago
11 kB
2
Indexable
Never
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()