Merge branch 'main' of https://git.robotstorm.tech/DevelopmentTeam/MassageRobot_Dobot

Massage/MassageControl/hardware/capture.py

@@ -0,0 +1,124 @@
+import os
+import sys
+import cv2
+import threading
+import numpy as np
+import time
+
+current_file_path = os.path.abspath(__file__)
+dobot_nova5_path = os.path.dirname(os.path.dirname(current_file_path))
+sys.path.append(dobot_nova5_path)
+
+from hardware.dobot_nova5 import dobot_nova5
+from hardware.remote_cam import ToolCamera
+
+class Getpose:
+    def __init__(self):
+        self.running = True
+        self.arm = dobot_nova5()
+        self.arm.start()
+        self.arm.init()
+        self.cam = ToolCamera(host='127.0.0.1')
+        self.cam.start()
+        time.sleep(2)
+        
+        self.pose_data = []
+        self.image_data = []
+        # self.arm.start_drag()
+        self.rgb = None
+            
+    def test_show(self):
+        self.rgb, self.depth, intrinsics = self.cam.get_latest_frame()
+        if self.rgb is not None:
+            cv2.imshow("Video Feed", self.rgb)
+            cv2.waitKey(1000)
+    
+    def add_data(self):
+        angle = self.arm.getAngel()
+        self.pose_data.append(angle)
+        print(f"Current angle: {angle}")
+    
+    def save_data(self):
+        if self.pose_data:
+            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)
+    
+    myTest = Getpose()
+    
+    myTest.arm.start_drag()
+    
+    time.sleep(0.5)
+    try:
+        count = 1
+        i = 1
+        while True:
+            # 读取摄像头的帧
+            r, d, intrinsics = myTest.cam.get_latest_frame()
+            # ret, frame = cam.read()
+            frame = r
+
+            # if not ret:
+            #     print("Error: Could not read frame from video device.")
+            #     break
+
+                        # 显示摄像头的帧
+            cv2.imshow('found', frame)
+
+            # 等待按键事件
+            key = cv2.waitKey(1) & 0xFF
+
+
+            # if key == ord('r'):
+            #     pose = increase_dof(poselist[index])
+            #     index+=1
+            #     code = cp.arm.move_joint(pose, 4,wait=True)
+            #     if code == -1:
+            #         print("运动到拍照位置失败")
+            #     else:
+
+            # 按下's'键保存照片
+            if key == ord('s'):
+                img_path = f'photo{i}'
+                if not os.path.exists(os.path.join(save_directory, img_path)):
+                    os.makedirs(os.path.join(save_directory, img_path))
+                filename = os.path.join(save_directory, f'{img_path}/rgb_image.png')
+                cv2.imwrite(filename, frame) # 保存照片
+                filename1 = os.path.join(save_directory, f'{img_path}/depth_image.png')
+                cv2.imwrite(filename1, d) # 保存照片
+                angle = myTest.arm.Get_feedInfo_angle()
+                print("angle = ",angle)
+                filename2 = os.path.join(save_directory, f'{img_path}/angle.txt')
+                np.savetxt(filename2, angle.shape(-1,6),delimiter=',')
+                print(f'Saved {filename}')
+                
+                # pose = increase_dof(poselist[index])
+                # index+=1
+                # code = cp.arm.move_joint(poselist[index], 4,wait=True)
+                # if code == -1:
+                #     print("运动到拍照位置失败")
+                count += 1 # 增加照片计数
+                i += 1
+            # 按下'q'键退出循环
+            elif key == ord('q'):
+                break
+            
+
+    except KeyboardInterrupt:
+        # 处理Ctrl+C中断
+        print("\nCamera session ended by user.")
+    finally:
+        # 释放资源
+        myTest.arm.stop_drag()
+        time.sleep(0.5)
+        myTest.arm.disableRobot()
+        myTest.cam.stop()
+        cv2.destroyAllWindows()
+
+        # 总结拍摄的照片
+        print(f'Total number of images captured: {count}')

Massage/MassageControl/hardware/dobot_nova5.py

@@ -871,24 +871,24 @@ if __name__ == "__main__":
     posJ_ready = [-45,55.5785,-120.6882,5.111,90,-90] 
     dobot.RunPoint_P_inJoint(posJ_ready)
     
-    # sleep(1)
+    # # sleep(1)
     dobot.chooseRightFrame()
     # print("Arm pose1:",dobot.getPose())
-    # print("Arm pose:",dobot.getPose(user=1,tool=0))
+    # # print("Arm pose:",dobot.getPose(user=1,tool=0))
     dobot.setEndEffector(i=8,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}")
     dobot.chooseEndEffector(i=8)
     print("Arm pose2:",dobot.getPose(user=1,tool=8))
     
-    print(dobot.get_arm_position())
+    # print(dobot.get_arm_position())
 
-    # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90])
-    cur_pose = dobot.getPose(user=1,tool=8)
-    cur_pose[0] += 30
-    target_pose = cur_pose
-    print("target",target_pose)
-    dobot.RunPoint_P_inPose(target_pose)
-    dobot.ServoPose(dobot.getPose(user=1,tool=8))
-    print("new ready joint",dobot.feedbackData.QActual)
-    # dobot.start_drag()
-    # dobot.stop_drag()
+    # # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90])
+    # cur_pose = dobot.getPose(user=1,tool=8)
+    # cur_pose[0] += 30
+    # target_pose = cur_pose
+    # print("target",target_pose)
+    # dobot.RunPoint_P_inPose(target_pose)
+    # dobot.ServoPose(dobot.getPose(user=1,tool=8))
+    # print("new ready joint",dobot.feedbackData.QActual)
+    # # dobot.start_drag()
+    dobot.stop_drag()
     dobot.disableRobot()

Massage/MassageControl/hardware/remote_cam.py

@@ -0,0 +1,235 @@
+import requests
+import base64
+import cv2
+import open3d as o3d
+import numpy as np
+import paramiko
+import time
+class ToolCamera:
+    def __init__(self, host):
+        """
+        初始化 CameraClient 类。
+        """
+        self.host = host
+        self.port = 22               # SSH端口号，默认是22
+        self.username = "jsfb"      # SSH用户名
+        self.password = "jsfb"      # SSH密码
+        self.root_password = "jsfb"
+        
+        # 要执行的命令
+        self.start_command = "systemctl restart cam_server"
+        self.stop_command = "systemctl stop cam_server"
+        
+    def start(self):
+        self.execute_command_on_remote(
+            host=self.host, 
+            username=self.username, 
+            password=self.password, 
+            root_password=self.root_password, 
+            command=self.start_command
+        )
+
+    def stop(self):
+        self.execute_command_on_remote(
+            host=self.host, 
+            username=self.username, 
+            password=self.password, 
+            root_password=self.root_password, 
+            command=self.stop_command
+        )
+
+    def execute_command_on_remote(self, host, username, password, root_password, command, port=22):
+        # 创建SSH客户端
+        ssh = paramiko.SSHClient()
+        ssh.set_missing_host_key_policy(paramiko.AutoAddPolicy())
+
+        try:
+            # 连接到远程服务器，指定端口
+            ssh.connect(hostname=host, port=port, username=username, password=password)
+
+            # 构建完整的命令字符串，将root密码传递给sudo
+            sudo_command = f'echo {root_password} | sudo -S {command}'
+
+            # 获取通道对象
+            transport = ssh.get_transport()
+            channel = transport.open_session()
+            channel.get_pty()  # 获取伪终端
+            channel.exec_command(sudo_command)  # 执行命令
+
+            # 检查命令是否在后台执行
+            while True:
+                # 如果命令在后台运行, channel.exit_status_ready() 将会一直是 False
+                if channel.exit_status_ready():
+                    break
+
+                # 非阻塞方式读取输出
+                if channel.recv_ready():
+                    output = channel.recv(1024).decode('utf-8')
+                    print(output)
+
+                # 非阻塞方式读取错误输出
+                if channel.recv_stderr_ready():
+                    error = channel.recv_stderr(1024).decode('utf-8')
+                    print(error)
+
+                # 延时，防止CPU占用过高
+                time.sleep(0.1)
+
+            return channel.recv_exit_status()
+        finally:
+            # 关闭SSH连接
+            ssh.close()
+
+    def get_latest_frame(self):
+        """
+        发送请求到服务器以获取最新的RGB和深度图像数据。
+
+        返回:
+        tuple: 包含RGB图像和深度图像的元组 (rgb_image, depth_image)，如果请求失败则返回 (None, None)。
+        """
+        try:
+            # 发送GET请求到服务器
+            response = requests.get("http://" + self.host + ":8000/get_latest_frame", timeout=(3, 5))
+
+            # 检查请求是否成功
+            if response.status_code == 200:
+                data = response.json()
+
+                # 获取Base64编码的RGB和深度图像数据
+                rgb_image_base64 = data['rgb_image']
+                depth_image_base64 = data['depth_image']
+                camera_intrinsics = data['camera_intrinsics']
+
+                # 解码Base64为二进制数据
+                rgb_image_data = base64.b64decode(rgb_image_base64)
+                depth_image_data = base64.b64decode(depth_image_base64)
+
+                # 转换为NumPy数组
+                rgb_image_np = np.frombuffer(rgb_image_data, np.uint8)
+                depth_image_np = np.frombuffer(depth_image_data, np.uint8)
+
+                # 解码为OpenCV图像
+                rgb_image = cv2.imdecode(rgb_image_np, cv2.IMREAD_COLOR)
+                depth_image = cv2.imdecode(depth_image_np, cv2.IMREAD_UNCHANGED)
+
+
+                return rgb_image, depth_image, camera_intrinsics
+            else:
+                print(f"Failed to retrieve data from server, status code: {response.status_code}")
+                return None, None, None
+        except Exception as e:
+            print(f"Exception occurred: {e}")
+            return None, None, None
+
+    def display_images(self, rgb_image, depth_image):
+        """
+        显示RGB和深度图像。
+
+        参数:
+        rgb_image (numpy.ndarray): RGB图像。
+        depth_image (numpy.ndarray): 深度图像。
+        """
+        if rgb_image is not None and depth_image is not None:
+            cv2.imshow('RGB Image', rgb_image)
+            cv2.imshow('Depth Image', depth_image)
+            cv2.waitKey(0)
+            cv2.destroyAllWindows()
+        else:
+            print("No image data to display.")
+
+    def display_rgb_point_cloud(self, rgb_image, depth_image, camera_intrinsics):
+        """
+        显示RGB点云，将RGB图像和深度图像转换为点云并显示。
+
+        参数:
+        rgb_image (np.ndarray): RGB图像。
+        depth_image (np.ndarray): 深度图像。
+        camera_intrinsics (dict): 相机的内参字典，包含 'fx', 'fy', 'cx', 'cy'。
+
+        返回:
+        None
+        """
+        # 获取RGB图像和深度图像的尺寸
+        rgb_h, rgb_w = rgb_image.shape[:2]
+        depth_h, depth_w = depth_image.shape[:2]
+
+        # 计算裁剪区域
+        start_x = (rgb_w - depth_w) // 2
+        start_y = (rgb_h - depth_h) // 2
+
+        # 裁剪RGB图像以匹配深度图像的尺寸
+        rgb_image_cropped = rgb_image[start_y:start_y + depth_h, start_x:start_x + depth_w]
+        rgb_image_cropped = cv2.cvtColor(rgb_image_cropped, cv2.COLOR_RGB2BGR)
+
+        # 将深度图像转换为浮点型并将单位从毫米转换为米（假设深度图像以毫米为单位）
+        depth_image = depth_image.astype(np.float32) / 1000.0
+
+        # 创建点云的空数组
+        points = []
+        colors = []
+
+        # 相机内参
+        fx = camera_intrinsics['fx']
+        fy = camera_intrinsics['fy']
+        cx = camera_intrinsics['cx']
+        cy = camera_intrinsics['cy']
+
+        # 遍历每个像素，将其转换为3D点
+        for v in range(depth_h):
+            for u in range(depth_w):
+                z = depth_image[v, u]
+                if z > 0:  # 排除无效深度
+                    x = (u - cx) * z / fx
+                    y = (v - cy) * z / fy
+                    points.append([x, y, z])
+                    colors.append(rgb_image_cropped[v, u] / 255.0)  # 颜色归一化到[0,1]
+
+        # 将点云和颜色转换为NumPy数组
+        points = np.array(points)
+        colors = np.array(colors)
+
+        # 创建Open3D点云对象
+        point_cloud = o3d.geometry.PointCloud()
+        point_cloud.points = o3d.utility.Vector3dVector(points)
+        point_cloud.colors = o3d.utility.Vector3dVector(colors)
+
+        # 显示点云
+        o3d.visualization.draw_geometries([point_cloud])
+
+if __name__ == "__main__":
+    import time
+    import os
+    # 初始化客户端并指定服务器地址
+    cam = ToolCamera(host='127.0.0.1')
+    # cam.stop()
+    cam.start()
+    time.sleep(2)
+    # 获取最新的RGB和深度图像
+    rgb_image, depth_image, camera_intrinsics = cam.get_latest_frame()
+    print(camera_intrinsics)
+
+    max_depth = np.max(depth_image)
+    print(np.min(depth_image),np.max(depth_image))
+    # print(depth_image[200, 320])
+    # depth_image = (depth_image / max_depth * 65535).astype(np.uint16)
+    # print(np.min(depth_image),np.max(depth_image))
+    # 对图像进行水平翻转
+    # rgb_image = cv2.flip(rgb_image, 1)
+    # depth_image = cv2.flip(depth_image, 1)
+
+    # 显示图像
+    cam.display_images(rgb_image, depth_image)
+    cv2.imwrite("aucpuncture2point/configs/using_img/color.png",rgb_image)
+    cv2.imwrite("aucpuncture2point/configs/using_img/depth.png",depth_image)
+    # 获取当前时间并格式化为文件夹名 (格式: 年-月-日_时-分-秒)
+    current_time = time.strftime("%Y-%m-%d_%H-%M-%S")
+    base_dir = "/home/jsfb/jsfb_ws/collected_data/test_images"
+    # 创建一个根据时间命名的新文件夹
+    folder_path = os.path.join(base_dir, current_time)
+    os.makedirs(folder_path, exist_ok=True)  # 如果文件夹不存在则创建
+
+    cv2.imwrite(os.path.join(folder_path, "color.png"), rgb_image)
+    cv2.imwrite(os.path.join(folder_path, "depth.png"), depth_image)
+
+    # 显示RGB点云
+    # cam.display_rgb_point_cloud(rgb_image, depth_image, camera_intrinsics)

Massage/MassageControl/tools/auto_visual_valibration.py

@@ -10,7 +10,7 @@ from scipy.spatial.transform import Rotation as R
 sys.path.append(str(Path(__file__).resolve().parent.parent))
 sys.path.append('/home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage/MassageControl')
 from hardware.remote_cam import ToolCamera
-from hardware.aubo_C5 import AuboC5
+from hardware.dobot_nova5 import dobot_nova5
 
 np.set_printoptions(precision=8, suppress=True)
 # 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001
@@ -41,16 +41,20 @@ class Calibration:
         self.size = None
         self.intrinsics_mtx = None
         self.disorder = None
+        time.sleep(2)
         rgb_image, depth_image, camera_intrinsics = self.cam.get_latest_frame()
+        self.save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images'
+       
         if rgb_image is None :
             print(f'===============相机连接失败请检查并重试==============')
         else:
-            print(f'===============相机连接成功==============')
             self.size = rgb_image.shape[:2]
+            print(f'===============相机连接成功==============  ',self.size)
+            
         self.intrinsics = camera_intrinsics
         
-        self.arm = AuboC5()
-        self.arm.init()
+        self.arm = dobot_nova5("192.168.5.1")
+        self.arm.start()
         self.obj_points = []
         print(f'===============机械臂连接成功==============')      
         
@@ -62,24 +66,29 @@ class Calibration:
   
     def collect_data(self):
         #移动到初始位置 
-        pose = [(90) * (np.pi / 180), 4.23 * (np.pi / 180), 124.33 * (np.pi / 180), 61.56 * (np.pi / 180), -89.61 * (np.pi / 180), 0 * (np.pi / 180)]
-        code = self.arm.move_joint(pose,4,wait=True)
+        pose = [-45.0009079,55.5785141,-120.68821716,5.11103201,-90.00195312,-90.00085449]
+        code = self.arm.RunPoint_P_inJoint(pose)
         if code == -1:
             print("运动到拍照位置失败")
         #拍照
         self.check_corners()
         #运动到下一个位置
         # 目前只设计了44条轨迹    
-        for i in range (0,45):
+        for i in range (0,18):
             next_pose = self.get_next_pose_ugly(i)
-            self.arm.move_joint(next_pose, 4,wait=True)
+            code=self.arm.RunPoint_P_inJoint(next_pose)
+            time.sleep(0.5)
             if code == -1:
                 print("运动到拍照位置失败")
             else:
                 self.check_corners()
-    
+                time.sleep(0.5)
+                
     def check_corners(self):
         rgb, depth, intrinsics = self.cam.get_latest_frame()
+        # img_path = f'photo{i}'
+        # filename = os.path.join(self.save_directory, f'{img_path}/rgb_image.png')
+        # rgb=cv2.imread(filename)
         gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY)
         ret, corners = cv2.findChessboardCorners(gray, self.board_size, None)
         if ret:
@@ -115,14 +124,12 @@ class Calibration:
         self.robot_poses.append(robot_pose)
           
     def get_pose_homomat(self):
-        pose = self.arm.robot_rpc_client.getRobotInterface(self.arm.robot_name).getRobotState().getTcpPose()
-        x, y, z, rx, ry, rz = pose
-        r = R.from_euler('xyz', [rx, ry, rz])
-        rotation_matrix = r.as_matrix()
-        translation_vector = np.array([x, y, z]).reshape(3, 1)
+        arm_position,arm_orientation = self.arm.get_arm_position()
+        rotation_matrix = R.from_quat(arm_orientation).as_matrix()
+        translation_vector = arm_position
         homo_mat = np.eye(4) # 创建 4x4 单位矩阵
         homo_mat[:3, :3] = rotation_matrix
-        homo_mat[:3, 3] = translation_vector.flatten() 
+        homo_mat[:3, 3] = translation_vector
         return homo_mat
     
     def camera_calib(self):
@@ -132,6 +139,7 @@ class Calibration:
         if ret:
             print("内参矩阵:\n", mtx)  # 内参数矩阵
             print("畸变系数:\n", dist)  # 畸变系数   distortion cofficients = (k_1,k_2,p_1,p_2,k_3)
+            print("重投影误差:\n",ret)
             print("++++++++++相机标定完成++++++++++++++")
             self.intrinsics_mtx = mtx
             self.disorder = dist
@@ -185,52 +193,24 @@ class Calibration:
         return None
     def get_next_pose_ugly(self,index):
         poselist =[
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.5116296651522887, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.4592697875924587, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0220648099678793, -1.5116296651522887, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080494, -1.4592697875924587, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9173450548482196, -1.4069099100326288, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.6163494202719486, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.7210691753916085, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.4941763726323454, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.424363202552572, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.3545500324727988, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.2847368623930255, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.633802712791892, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.7036158828716652, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [1.5707963267948966, 0.06510078109938851, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
-            [1.5707963267948966, 0.05637413483941686, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
-            [1.5707963267948966, 0.047647488579445216, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [1.562069680534925, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
-            [1.5533430342749535, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
-            [1.544616388014982, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
-            [1.5358897417550104, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0],
-            [1.5271630954950388, 0.07382742735936015, 2.16996785900455, 0.8998917623282761, -1.2149236923132523, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.5988961277520053, 0.0],
-            [1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.633802712791892, 0.0],
-            [1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0],
-            [1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.7036158828716652, 0.0],
-            [1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.2489576127271425, -1.7385224679115518, 0.0],
-            [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
-            [1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.581442835232062, 0.0],
-            [1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.5988961277520053, 0.0],
-            [1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.6163494202719486, 0.0],
-            [1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.283864197767029, -1.633802712791892, 0.0],
-            [1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.336224075326859, -1.6512560053118353, 0.0]
+            [-4.513801574707031250e+01,5.518265151977539062e+01,-1.237858963012695312e+02,9.085088729858398438e+00,-9.337700653076171875e+01,-9.000016784667968750e+01],
+            [-4.594435882568359375e+01,3.636156082153320312e+01,-1.064173660278320312e+02,-2.125140428543090820e+00,-9.337675476074218750e+01,-9.000016784667968750e+01],
+            [-4.595432662963867188e+01,1.059147930145263672e+01,-8.284246063232421875e+01,-1.484527587890625000e+01,-9.337491607666015625e+01,-9.000013732910156250e+01],
+            [-4.638331604003906250e+01,-6.824899196624755859e+00,-6.758448791503906250e+01,-2.286166954040527344e+01,-9.337348937988281250e+01,-9.000016784667968750e+01],
+            [-4.636801910400390625e+01,-1.807893753051757812e+01,-6.532709503173828125e+01,-2.261142921447753906e+01,-9.336659240722656250e+01,-9.000010681152343750e+01],
+            [-4.637482833862304688e+01,-4.540402412414550781e+00,-1.098018112182617188e+02,3.086268234252929688e+01,-9.305430603027343750e+01,-9.000022125244140625e+01],
+            [-4.640084075927734375e+01,8.068140029907226562e+00,-1.362033843994140625e+02,6.259086990356445312e+01,-9.334789276123046875e+01,-9.000019073486328125e+01],
+            [-4.640056610107421875e+01,8.061657905578613281e+00,-1.511797027587890625e+02,8.927394104003906250e+01,-9.331819915771484375e+01,-9.000019073486328125e+01],
+            [-4.638466262817382812e+01,3.287191867828369141e+00,-1.566032409667968750e+02,1.035896453857421875e+02,-9.330465698242187500e+01,-9.000022125244140625e+01],
+            [-3.178509473800659180e+00,6.977911472320556641e+00,-1.417592010498046875e+02,6.005132293701171875e+01,-1.280102996826171875e+02,-9.000010681152343750e+01],
+            [-1.424913024902343750e+01,-2.980865538120269775e-01,-1.193396759033203125e+02,3.752756500244140625e+01,-1.278784713745117188e+02,-8.999975585937500000e+01],
+            [-2.393187522888183594e+01,-2.973175048828125000e-01,-1.121088027954101562e+02,3.928178024291992188e+01,-1.183067703247070312e+02,-8.999983215332031250e+01],
+            [-3.699544525146484375e+01,-2.981689572334289551e-01,-1.078408889770507812e+02,3.178039932250976562e+01,-1.024006271362304688e+02,-8.999983215332031250e+01],
+            [-6.734904479980468750e+01,-4.368630886077880859e+00,-1.035898971557617188e+02,2.342595481872558594e+01,-7.085527801513671875e+01,-8.999980926513671875e+01],
+            [-8.632910919189453125e+01,-1.039798259735107422e+01,-1.038105545043945312e+02,2.330961036682128906e+01,-5.476583099365234375e+01,-8.999980926513671875e+01],
+            [-8.156823730468750000e+01,5.878097534179687500e+00,-1.439221954345703125e+02,6.846669006347656250e+01,-4.482905197143554688e+01,-8.999922943115234375e+01],
+            [-8.601764678955078125e+01,3.570293045043945312e+01,-1.308228912353515625e+02,2.318510818481445312e+01,-5.378013610839843750e+01,-8.999922943115234375e+01],
+            [-7.132132053375244141e+00,3.722338104248046875e+01,-1.065378265380859375e+02,-3.337371826171875000e+00,-1.185582199096679688e+02,-8.999931335449218750e+01]
             ]
         return poselist[index]
         
@@ -241,7 +221,7 @@ if __name__ == '__main__':
     # 标定板的width 对应的角点的个数 6 
     # 标定板的height 对应的角点的个数 3 
     calibration_board_size = [6,3]
-    calibration_board_square_size = 0.027 #unit - meter 
+    calibration_board_square_size = 0.03 #unit - meter 
     systemPath = "/home/jsfb/Documents/"
     
     now = datetime.now()
@@ -252,7 +232,9 @@ if __name__ == '__main__':
 
     calib = Calibration(calibration_board_size,calibration_board_square_size,systemPath)
     calib.collect_data()
+    print("=================数据采集完成===================")
     R, t,intrin = calib.calibrate()
+    
     print("旋转矩阵：")
     print(R)
     print("平移向量：")