import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32MultiArray
import numpy as np

if __name__ == "__main__":
    rclpy.init()  # 初始化 ROS 2
    node = Node("diana_sim_publisher")  # 创建节点
    publisher = node.create_publisher(Float32MultiArray, 'mj_joint_states', 10)

    try:
        while rclpy.ok():  # 使用 rclpy.ok() 检查 ROS 2 是否正在运行
            angles = np.zeros(7)  # 生成一个 7 个元素为 0 的数组
            angles = [float(x) for x in angles]  # 转换为 float 列表
            msg = Float32MultiArray()
            msg.data = angles  # 设置消息数据
            publisher.publish(msg)  # 发布消息

            node.get_logger().info(f'Publishing joint angles: {msg.data}')  # 日志输出


    except KeyboardInterrupt:
        pass  # 捕获 Ctrl+C

    finally:
        node.destroy_node()  # 确保节点被销毁
        rclpy.shutdown()  # 关闭 ROS 2
