folder structure

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__':

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()

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"))

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)

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"}