From 54577871c76a2797fe8f1f3006cc723bbbdca3d3 Mon Sep 17 00:00:00 2001
From: alex <alex@roese.dev>
Date: Sun, 5 Nov 2023 15:42:23 +0100
Subject: [PATCH] folder structure

---
 main.py                    |  6 ++--
 robot/apriltag_detector.py | 67 ++++++++++++++++++++++++++++++++++++++
 robot/config.py            | 18 ++++++++++
 robot/robot.py             | 29 +++++++++++++++++
 robot/server.py            | 40 +++++++++++++++++++++++
 5 files changed, 157 insertions(+), 3 deletions(-)
 create mode 100644 robot/apriltag_detector.py
 create mode 100644 robot/config.py
 create mode 100644 robot/robot.py
 create mode 100644 robot/server.py

diff --git a/main.py b/main.py
index c6b8400..6953292 100644
--- a/main.py
+++ b/main.py
@@ -2,9 +2,9 @@ import logging
 import uvicorn
 
 # OUR MODULES
-import config
-import server
-import robot
+import robot.config as config
+import robot.server as server
+import robot.robot as robot
 
 
 if __name__ == '__main__':
diff --git a/robot/apriltag_detector.py b/robot/apriltag_detector.py
new file mode 100644
index 0000000..19483b0
--- /dev/null
+++ b/robot/apriltag_detector.py
@@ -0,0 +1,67 @@
+import cv2
+import pyapriltags
+
+at_detector = pyapriltags.Detector(families="tag36h11")
+
+
+def open_camera():
+    # cap = cv2.VideoCapture(config.usb_camera_device)
+    cap = cv2.VideoCapture("http://localhost:50085/video")
+
+    if not cap.isOpened():
+        print("Cannot open camera")
+        exit()
+
+    # cv2.namedWindow("Object and AprilTag Detection", cv2.WINDOW_NORMAL)
+    # cv2.resizeWindow("Object and AprilTag Detection", 800, 600)
+
+    while True:
+        print("Frame")
+
+        ret, frame = cap.read()
+
+        if not ret:
+            break
+
+        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+        at_results = at_detector.detect(gray)
+
+        for i, at_result in enumerate(at_results):
+            # print(
+            #    f"Erkannter AprilTag {i + 1}: ID={at_result.tag_id}, Koordinaten={at_result}")
+
+            apriltag_x = 890
+            apriltag_y = 500
+
+            print(
+                f"ID: {at_result.tag_id} Corners: {at_result.center}")
+
+            corner_x = at_result.center[0]
+            corner_y = at_result.center[1]
+
+            offset = 5
+
+            if corner_x > apriltag_x + offset:
+                print("move right")
+
+            if corner_x < apriltag_x - offset:
+                print("move left")
+
+            if corner_y > apriltag_y + offset:
+                print("move back")
+
+            if corner_y < apriltag_y - offset:
+                print("move forward")
+
+        # for at_result in at_results:
+        #    corners = at_result.corners.astype(int)
+        #    cv2.polylines(frame, [corners], isClosed=True,
+        #                  color=(0, 255, 255), thickness=2)
+
+        # cv2.imshow("Object and AprilTag Detection", frame)
+
+        # if cv2.waitKey(1) & 0xFF == ord('q'):
+        #   break
+
+        # cap.release()
+        # cv2.destroyAllWindows()
diff --git a/robot/config.py b/robot/config.py
new file mode 100644
index 0000000..b56bcff
--- /dev/null
+++ b/robot/config.py
@@ -0,0 +1,18 @@
+import os
+from dotenv import load_dotenv
+
+load_dotenv()
+
+debug = os.getenv("DEBUG")
+host = os.getenv("HOST")
+port = int(os.getenv("PORT"))
+
+robot_control_server_url = os.getenv("ROBOT_CONTROL_SERVER_URL")
+robot_id = os.getenv("ROBOT_ID")
+robot_type = int(os.getenv("ROBOT_TYPE"))
+robot_firmware_version = os.getenv("ROBOT_FIRMWARE_VERSION")
+
+joint1_length = int(os.getenv("JOINT1_LENGTH"))
+joint2_length = int(os.getenv("JOINT2_LENGTH"))
+
+usb_camera_device = int(os.getenv("USB_CAMERA_DEVICE"))
diff --git a/robot/robot.py b/robot/robot.py
new file mode 100644
index 0000000..eb69565
--- /dev/null
+++ b/robot/robot.py
@@ -0,0 +1,29 @@
+import requests
+import logging
+
+# OUR MODULES
+import config
+
+# DEFINE GLOBAL VARIABLES
+
+X = None
+Y = None
+Z = None
+ConnectedModule = None
+
+
+def InitRobot():
+    # TODO: get amount of joints and connected module from the esp (greifer)
+
+    res = requests.post(config.robot_control_server_url + "/robot",
+                        json={
+                            "id": config.robot_id,
+                            "type": config.robot_type,
+                            "firmwareVersion": config.robot_firmware_version
+                        })
+
+    logging.info(res.status_code)
+
+    if res.status_code == 403:
+        logging.error("Permit join is disabled")
+        exit(1)
diff --git a/robot/server.py b/robot/server.py
new file mode 100644
index 0000000..a775ecb
--- /dev/null
+++ b/robot/server.py
@@ -0,0 +1,40 @@
+from typing import Union
+from fastapi import FastAPI
+from pydantic import BaseModel
+import time
+
+# OUR MODULES
+import apriltag_detector
+
+app = FastAPI()
+
+
+@app.get("/")
+async def root():
+    return {"message": "Hello World"}
+
+
+@app.get("/api/v1/ping")
+async def ping():
+    return {"status": "ok"}
+
+
+class ControlBody(BaseModel):
+    X: Union[int, None] = None
+    Y: Union[int, None] = None
+    Z: Union[int, None] = None
+    ToolHead: Union[int, None] = None
+    AprilTagId: Union[int, None] = None
+
+
+@app.post("/api/v1/control")
+async def control(item: ControlBody):
+    time.sleep(1)
+    print("control", item)
+
+    if item.AprilTagId != None:
+        print("AprilTag detected:", item.AprilTagId)
+
+        #  apriltag_detector.open_camera()
+
+    return {"status": "ok"}