From 0b8a0f37dfb771d5864547421e4435d04de46ee0 Mon Sep 17 00:00:00 2001 From: alex Date: Wed, 25 Oct 2023 22:03:00 +0200 Subject: [PATCH] robot --- apriltag_detector.py | 67 ++++++++++++++++++++++++++++++++++++++++++++ config.py | 8 ++++++ main.py | 47 +++++-------------------------- readme.md | 9 ++++++ robot.py | 28 ++++++++++++++++++ server.py | 40 ++++++++++++++++++++++++++ 6 files changed, 159 insertions(+), 40 deletions(-) create mode 100644 apriltag_detector.py create mode 100644 readme.md create mode 100644 robot.py create mode 100644 server.py diff --git a/apriltag_detector.py b/apriltag_detector.py new file mode 100644 index 0000000..19483b0 --- /dev/null +++ b/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/config.py b/config.py index 280d2c5..b56bcff 100644 --- a/config.py +++ b/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")) diff --git a/main.py b/main.py index 1c40784..c6b8400 100644 --- a/main.py +++ b/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) diff --git a/readme.md b/readme.md new file mode 100644 index 0000000..568d3d7 --- /dev/null +++ b/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 +``` diff --git a/robot.py b/robot.py new file mode 100644 index 0000000..c36b0a5 --- /dev/null +++ b/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) diff --git a/server.py b/server.py new file mode 100644 index 0000000..a775ecb --- /dev/null +++ b/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"}