From 4e210c38fabb349d75a2c66c5df9a75876ac8a89 Mon Sep 17 00:00:00 2001
From: liangweihao <1986627896@qq.com>
Date: Wed, 28 May 2025 09:51:48 +0800
Subject: [PATCH] =?UTF-8?q?=E8=8E=B7=E5=8F=96=E6=A0=87=E5=AE=9A=E4=BD=8D?=
 =?UTF-8?q?=E5=A7=BF?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../MassageControl/hardware/dobot_nova5.py    |  2 +-
 Massage/MassageControl/tools/capture.py       | 60 +++++++++++++++++++
 .../tools/get_visual_valibration_pose.py      | 36 -----------
 3 files changed, 61 insertions(+), 37 deletions(-)
 create mode 100644 Massage/MassageControl/tools/capture.py
 delete mode 100644 Massage/MassageControl/tools/get_visual_valibration_pose.py

diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py
index c9cd746..f815592 100644
--- a/Massage/MassageControl/hardware/dobot_nova5.py
+++ b/Massage/MassageControl/hardware/dobot_nova5.py
@@ -718,7 +718,7 @@ class dobot_nova5:
         self.last_pose_command = np.zeros(6)
         self.last_input_command = None
         self.tcp_offset = None
-        self.init_J = [0,30,-120,0,90,0]
+        self.init_J = [-45,60,-120,0,90,0]
         self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
         self.filter_matrix = np.zeros((1,6)) # 角度伺服用
         sleep(1)
diff --git a/Massage/MassageControl/tools/capture.py b/Massage/MassageControl/tools/capture.py
new file mode 100644
index 0000000..964dff3
--- /dev/null
+++ b/Massage/MassageControl/tools/capture.py
@@ -0,0 +1,60 @@
+from hardware.dobot_nova5 import dobot_nova5
+import cv2
+from hardware.remote_cam import ToolCamera
+import threading
+
+import numpy as np
+import os
+class Getpose:
+    def __init__(self):
+        self.arm = dobot_nova5("192.168.5.1")
+        self.arm.start()
+        self.arm.init()
+        self.cam = ToolCamera(host='127.0.0.1')
+        self.cam.start()
+        
+        self.thread1=threading.Thread(target=self.show)
+        self.thread1.start()
+        self.pose_data=[]
+        self.image_data=[]
+        self.arm.start_drag()
+    
+    def show(self):
+        while True:
+            self.rgb, self.depth, intrinsics = self.cam.get_latest_frame()
+            cv2.imshow(self.rgb)
+
+    def add_data(self):
+        angle=self.arm.getAngel()
+        self.pose_data=self.pose_data.append(angle)
+        print(angle)
+
+    def save_data(self):
+        data=np.array(self.pose_data)
+        np.savetxt('pose.txt', data ,fmt='%.5f')
+
+if __name__ == "__main__":
+    save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images'
+    if not os.path.exists(save_directory):
+        os.makedirs(save_directory)
+    sele=Getpose()
+    
+    key = cv2.waitKey(1) & 0xFF
+    while True:
+        i=1
+        if key == ord('s'):
+            img_path = f'photo{i}.png'
+            filename = os.path.join(save_directory, img_path)
+            cv2.imwrite(filename, sele.rgb) # 保存照片
+            sele.add_data()
+            
+            filename2 = os.path.join(save_directory, f'pose.txt')
+            np.savetxt(filename2, sele.pose_data)
+            # pose = increase_dof(poselist[index])
+            
+            i += 1
+            # 按下'q'键退出循环
+        elif key == ord('q'):
+            sele.arm.disableRobot()
+            break
+    
\ No newline at end of file
diff --git a/Massage/MassageControl/tools/get_visual_valibration_pose.py b/Massage/MassageControl/tools/get_visual_valibration_pose.py
deleted file mode 100644
index 30f8d4a..0000000
--- a/Massage/MassageControl/tools/get_visual_valibration_pose.py
+++ /dev/null
@@ -1,36 +0,0 @@
-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退出
\ No newline at end of file