28 lines
944 B
Python
28 lines
944 B
Python
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
|