import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import PointStamped
from cv_bridge import CvBridge

import cv2
import mediapipe as mp

class HandTrackingNode(Node):
    def __init__(self):
        super().__init__('hand_tracking_node')
        self.bridge = CvBridge()

        self.sub = self.create_subscription(
            Image,
            '/camera/left/image_raw',
            self.image_cb,
            10
        )

        self.pub = self.create_publisher(
            PointStamped,
            '/hand/wrist_pixel',
            10
        )

        self.mp_hands = mp.solutions.hands.Hands(
            static_image_mode=False,
            max_num_hands=1,
            min_detection_confidence=0.7,
            min_tracking_confidence=0.7
        )

    def image_cb(self, msg):
        img = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
        rgb = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)

        results = self.mp_hands.process(rgb)
        if not results.multi_hand_landmarks:
            return

        h, w, _ = img.shape
        wrist = results.multi_hand_landmarks[0].landmark[0]

        u = int(wrist.x * w)
        v = int(wrist.y * h)

        p = PointStamped()
        p.header = msg.header
        p.point.x = float(u)
        p.point.y = float(v)
        p.point.z = 0.0  # 暂时不用

        self.pub.publish(p)

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

if __name__ == '__main__':
    main()
