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

config.py

@@ -4,7 +4,15 @@ 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"))

main.py

@@ -1,49 +1,16 @@
-import requests
 import logging
-from fastapi import FastAPI
 import uvicorn
 
-
+# OUR MODULES
 import config
+import server
+import robot
 
 
-logging.basicConfig(level=logging.DEBUG if config.debug ==
-                    "true" else logging.INFO)
-
-print(config.robot_control_server_url)
-
-
-def InitRobot():
-    logging.info("InitRobot")
-
-    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)
-
-
-app = FastAPI()
-
-
-@app.get("/")
-async def root():
-    return {"message": "Hello World"}
-
-
-@app.get("/api/v1/ping")
-async def ping():
-    return {"status": "ok"}
-
 if __name__ == '__main__':
-    logging.info("main")
+    logging.basicConfig(level=logging.DEBUG if config.debug ==
+                        "true" else logging.INFO)
 
-    InitRobot()
+    robot.InitRobot()
 
-    uvicorn.run(app, port=5000)
+    uvicorn.run(server.app, host=config.host, port=config.port)

readme.md

@@ -0,0 +1,9 @@
+ImportError: libGL.so.1: cannot open shared object file: No such file or directory
+
+```
+apt install python3-opencv
+```
+
+```
+apt install ffmpeg libsm6 libxext6
+```

robot.py

@@ -0,0 +1,28 @@
+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)

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