from hardware.dobot_nova5 import dobot_nova5
import cv2
from hardware.remote_cam import ToolCamera
import threading
import keyboard
import numpy as np
class Getpose:
    def __init__(self):
        self.arm = dobot_nova5("192.168.5.1")
        self.arm.start()
        self.cam = ToolCamera(host='127.0.0.1')
        self.cam.start()
        self.thread1=threading.Thread(target=self.show)
        self.thread1.start()
        self.data=[]
        self.arm.start_drag()
    def show(self):
        while True:
            rgb, depth, intrinsics = self.cam.get_latest_frame()
            cv2.imshow(rgb)
    def add_data(self):
        angle=self.arm.getAngel()
        self.data=self.data.append(angle)
        print(angle)
    def save_data(self):
        data=np.array(self.data)
        np.savetxt('pose.txt', data ,fmt='%.5f')
if __name__ == "__main__":
    
    sele=Getpose()

    keyboard.add_hotkey('ctrl+shift+a', lambda: sele.add_data())
    keyboard.add_hotkey('ctrl+shift+s', lambda: sele.save_data())
    keyboard.add_hotkey('ctrl+shift+c', lambda: sele.arm.disableRobot())
    # 阻塞监听（保持程序运行）
    keyboard.wait('esc')  # 按ESC退出