### 如何使用Python编写ROS节点以发布 `geometry_msgs/PoseStamped` 消息 以下是通过Python编写的ROS节点示例代码，展示如何发布 `geometry_msgs/PoseStamped` 类型的消息： ```python #!/usr/bin/env python3 import rospy from geometry_msgs.msg import PoseStamped def publish_pose_stamped(): # 初始化ROS节点 rospy.init_node('pose_publisher', anonymous=True) # 创建一个Publisher对象，指定话题名称为 'pose_topic' 和消息类型为 PoseStamped pub = rospy.Publisher('pose_topic', PoseStamped, queue_size=10) # 设置发布频率 rate = rospy.Rate(1) # 1 Hz while not rospy.is_shutdown(): # 创建PoseStamped消息实例 pose_msg = PoseStamped() # 填充Header部分 pose_msg.header.stamp = rospy.Time.now() pose_msg.header.frame_id = "map" # 设定参考坐标系 # 填充Pose部分 pose_msg.pose.position.x = 1.0 pose_msg.pose.position.y = 2.0 pose_msg.pose.position.z = 3.0 pose_msg.pose.orientation.x = 0.0 pose_msg.pose.orientation.y = 0.0 pose_msg.pose.orientation.z = 0.0 pose_msg.pose.orientation.w = 1.0 # 发布消息 pub.publish(pose_msg) rospy.loginfo(f"Published Pose: {pose_msg}") # 控制循环速率 rate.sleep() if __name__ == '__main__': try: publish_pose_stamped() except rospy.ROSInterruptException: pass ``` #### 解析 - 上述代码初始化了一个名为 `pose_publisher` 的ROS节点，并创建了一个发布者（Publisher），该发布者向 `pose_topic` 主题发送 `geometry_msgs/PoseStamped` 类型的消息[^1]。 - 在每次循环中，程序会创建一个新的 `PoseStamped` 对象并填充其字段。其中，`header.stamp` 表示时间戳，`header.frame_id` 定义了参考坐标系，而 `pose` 部分则包含了位置和方向的信息[^2]。 - 使用 `rospy.loginfo()` 输出发布的消息以便调试。 此实现方式遵循标准的ROS编程实践，适用于大多数基于ROS的应用场景[^4]。