import rclpy
from rclpy.node import Node
from control_msgs.msg import JointTrajectoryControllerState

# 2. 定义节点类（继承Node）
class MySubscriber(Node):
    def __init__(self):
        # 调用父类构造函数，初始化节点名称（如"py_subscriber_node"）
        super().__init__("py_subscriber_node")
        
        # 3. 创建订阅者
        # 参数1：消息类型（如String，与发布者一致）
        # 参数2：话题名称（如"chatter"，与发布者一致）
        # 参数3：回调函数（消息到达时触发，用于处理消息）
        # 参数4：队列长度（10，缓存消息的最大数量）
        self.subscriber_ = self.create_subscription(
            JointTrajectoryControllerState, 
            "/right_joint_trajectory_controller/controller_state", 
            self.listener_callback, 
            100
        )
        self.subscriber_  # 防止未使用变量警告
 
    # 4. 消息处理回调函数（核心）
    # 当订阅到消息时，自动调用此函数处理消息
    def listener_callback(self, msg):
        # 打印接收的消息（可根据需求修改处理逻辑，如存储、解析、控制硬件等）
        self.get_logger().info(f"接收到消息: {msg}")


def main():
    rclpy.init()
    node = MySubscriber()
    node.get_logger().info('111111111111')
    node.get_logger().warn('222222222222222')

    rclpy.spin(node)  # 启动事件循环（持续监听话题，等待消息）
    node.destroy_node()  # 退出时销毁节点
    rclpy.shutdown()  # 关闭ROS 2运行时
