用rclpy监控moveit发布关节的节点，代码如何编写

在Robot Operating System (ROS)的Python库rclpy中，要监控MoveIt!发布的关节状态，你需要订阅`/joint_states`话题，该话题通常由MoveIt!节点用于发布关节信息。以下是一个简单的示例，展示如何使用rclpy编写代码来实现这个功能：

# 导入必要的rclpy模块
from rclpy.node import Node
from rclpy.qos import QoSProfile
from geometry_msgs.msg import JointState

class JointStateSubscriber(Node):
    def __init__(self):
        super().__init__('joint_state_subscriber')
        
        # 创建一个QoSProfile实例，设置历史深度和可靠性
        qos_profile = QoSProfile(depth=10, reliability=rclpy.qos.ReliabilityPolicy.RELIABLE)

        # 订阅/joint_states主题
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_state_callback,
            qos_profile
        )

        self.subscription  # 确保回调已注册

    def joint_state_callback(self, msg):
        # 当接收到新消息时，打印关节名称和角度
        for i, name in enumerate(msg.name):
            print(f"Joint {name}: {msg.position[i]}")

def main(args=None):
    rclpy.init(args=args)
    node = JointStateSubscriber()
    rclpy.spin(node)
    
    # 关闭并清理资源
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

在这个例子中，我们创建了一个名为`JointStateSubscriber`的Node，它订阅`/joint_states`主题并在`joint_state_callback`方法中处理接收到的消息。当有新的`JointState`消息到达时，它会遍历每个关节的名字和位置。