MassageRobot_Dobot/Massage/MassageControl/hardware/remote_cam.py

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)