import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped, PointStamped
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge
import numpy as np

class Hand3DNode(Node):
    def __init__(self):
        super().__init__('hand_3d_node')
        self.bridge = CvBridge()

        self.fx = self.fy = self.cx = self.cy = None

        self.sub_pixel = self.create_subscription(
            PointStamped,
            '/hand/wrist_pixel',
            self.pixel_cb,
            10
        )

        self.sub_depth = self.create_subscription(
            Image,
            '/camera/depth/image',
            self.depth_cb,
            10
        )

        self.sub_info = self.create_subscription(
            CameraInfo,
            '/camera/left/camera_info',
            self.info_cb,
            10
        )

        self.pub = self.create_publisher(
            PoseStamped,
            '/hand/pose_camera',
            10
        )

        self.last_pixel = None
        self.depth_img = None

    def info_cb(self, msg):
        self.fx = msg.k[0]
        self.fy = msg.k[4]
        self.cx = msg.k[2]
        self.cy = msg.k[5]

    def pixel_cb(self, msg):
        self.last_pixel = (int(msg.point.x), int(msg.point.y), msg.header)

    def depth_cb(self, msg):
        self.depth_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
        self.try_publish()

    def try_publish(self):
        if self.last_pixel is None or self.depth_img is None:
            return
        if self.fx is None:
            return

        u, v, header = self.last_pixel
        depth = self.depth_img[v, u]

        if depth <= 0.0:
            return

        Z = depth
        X = (u - self.cx) * Z / self.fx
        Y = (v - self.cy) * Z / self.fy

        pose = PoseStamped()
        pose.header = header
        pose.header.frame_id = 'camera_frame'
        pose.pose.position.x = X
        pose.pose.position.y = Y
        pose.pose.position.z = Z

        pose.pose.orientation.w = 1.0  # 先固定姿态

        self.pub.publish(pose)

def main():
    rclpy.init()
    node = Hand3DNode()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()
