import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped

import threading
import sys
import termios
import tty

def get_key():
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)

    try:
        tty.setraw(fd)
        ch = sys.stdin.read(1)

        if ch == '\x1b':
            ch += sys.stdin.read(2)
            if ch == '\x1b[':
                ch += sys.stdin.read(1)

        return ch

    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)

class KeyboardCtrlNode(Node):
    def __init__(self):
        super().__init__('keyboard_ctrl_node')
        self.get_logger().info('正在初始化Keyboard控制节点...')
        # 在这里添加键盘控制的初始化代码，例如订阅键盘输入的话题或设置定时器等

        self.pub = self.create_publisher(
            PoseStamped,
            "/hand_3d",
            10
        )

        self.running = True

        # 启动键盘线程
        self.thread = threading.Thread(target=self.keyboard_loop)
        self.thread.start()

    def keyboard_loop(self):

        while self.running:

            key = get_key()

            msg = PoseStamped()
            msg.header.frame_id = "base_link"

            if key == '\x1b[A':  # up
                msg.pose.position.x = 0.01
            elif key == '\x1b[B':  # down
                msg.pose.position.x = -0.01

            elif key == '\x1b[C':  # right
                msg.pose.position.y = -0.01

            elif key == '\x1b[D':  # left
                msg.pose.position.y = 0.01

            elif key == '.':  # period
                msg.pose.position.z = -0.01

            elif key == ';':  # semicolon
                msg.pose.position.z = 0.01

            elif key == 'q':
                self.running = False
                self.destroy_node()
                rclpy.shutdown()
                break
            else:
                continue

            self.pub.publish(msg)

            self.get_logger().info("Command sent")

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

if __name__ == "__main__":
    main()