import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from std_msgs.msg import String

class JoyControlNode(Node):
    def __init__(self):
        super().__init__('game_pad_node')
        # 订阅joy话题
        self.joy_subscriber = self.create_subscription(
            Joy,
            'joy',
            self.joy_callback,
            10
        )
        # 发布指令到joy_command话题
        self.command_publisher = self.create_publisher(
            String,
            'gamepad_msg',
            10
        )
        # 罗技F710按键映射
        self.button_map = {
            0: 'A',
            1: 'B',
            2: 'X',
            3: 'Y',
            4: 'LB',
            5: 'RB',
            6: 'BACK',
            7: 'START',
            8: 'HOME',
            9: '左遥杆',
            10: '右摇杆'
        }
        # 罗技F710摇杆映射
        self.axis_map = {
            0: '左杆水平',
            1: '左杆垂直',
            2: '左触发器',
            3: '右杆水平',
            4: '右杆垂直',
            5: '右触发器',
            6: '方向水平',
            7: '方向垂直'
        }

        self.default_buttons = [0,0,0,0,0,0,0,0,0,0,0]
        self.default_axes = [0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0]

        self.previous_buttons = self.default_buttons
        self.previous_axes = self.default_axes


        self.get_logger().info('game pad node initialized')

    def joy_callback(self, msg):
        # 处理按键
        for i in range(len(msg.buttons)):
            if msg.buttons[i] != self.previous_buttons[i]:
                self.previous_buttons[i] = msg.buttons[i]
                command = self.button_map[i]+","+str(msg.buttons[i])
                self.get_logger().info("key:"+command)
                self.command_publisher.publish(String(data=command))
                
        #处理axis
        for i in range(len(msg.axes)):
            if msg.axes[i] != self.previous_axes[i]:
                self.previous_axes[i] = msg.axes[i]
                command = self.axis_map[i]+","  +str(msg.axes[i])
                self.get_logger().info("axis:"+command+","+str(msg.axes[i]))
                self.command_publisher.publish(String(data=command))
def main(args=None):
    rclpy.init(args=args)
    node = JoyControlNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
    