使用geometry_msgs.msgPoseStamped()在Python中实现机器人的位姿发布与订阅

在ROS中，我们可以使用消息类型geometry_msgs.msg.PoseStamped来发布和订阅机器人的位姿信息。geometry_msgs.msg.PoseStamped消息类型包含一个PoseStamped数据段，其中包含了机器人在三维空间中的位置和姿态信息。

下面是一个使用geometry_msgs.msg.PoseStamped消息类型发布和订阅机器人位姿信息的示例代码：

发布机器人位姿的代码：

import rospy
from geometry_msgs.msg import PoseStamped

def publish_pose():
    # 初始化ROS节点
    rospy.init_node('pose_publisher', anonymous=True)
    
    # 创建一个Publisher，用于发布机器人的位姿信息
    pub = rospy.Publisher('robot_pose', PoseStamped, queue_size=10)
    
    # 定义机器人的位姿信息
    pose = PoseStamped()
    pose.header.stamp = rospy.Time.now()  # 设置时间戳
    pose.header.frame_id = "map"  # 设置坐标系
    
    pose.pose.position.x = 1.0  # 设置机器人的x坐标
    pose.pose.position.y = 2.0  # 设置机器人的y坐标
    pose.pose.position.z = 0.0  # 设置机器人的z坐标
    
    pose.pose.orientation.x = 0.0  # 设置机器人的姿态x分量
    pose.pose.orientation.y = 0.0  # 设置机器人的姿态y分量
    pose.pose.orientation.z = 0.0  # 设置机器人的姿态z分量
    pose.pose.orientation.w = 1.0  # 设置机器人的姿态w分量
    
    # 循环发布机器人位姿信息
    rate = rospy.Rate(10)  # 设置发布速率为10Hz
    while not rospy.is_shutdown():
        pose.header.stamp = rospy.Time.now()  # 更新位姿信息的时间戳
        pub.publish(pose)  # 发布机器人的位姿信息
        rate.sleep()

if __name__ == '__main__':
    try:
        publish_pose()
    except rospy.ROSInterruptException:
        pass

上述代码实现了一个ROS节点，其中包含一个Publisher，用于发布机器人的位姿信息。在主循环中，通过设置geometry_msgs.msg.PoseStamped消息的各个字段来定义机器人的位姿信息，并在每次发布前更新时间戳。

接下来是订阅机器人位姿的代码：

import rospy
from geometry_msgs.msg import PoseStamped

def callback(data):
    # 接收到机器人位姿信息的回调函数
    position = data.pose.position
    orientation = data.pose.orientation

    rospy.loginfo("Received pose:
\tPosition: x={}, y={}, z={}
\tOrientation: x={}, y={}, z={}, w={}".format(
        position.x, position.y, position.z, orientation.x, orientation.y, orientation.z, orientation.w))

def subscribe_pose():
    # 初始化ROS节点
    rospy.init_node('pose_subscriber', anonymous=True)
    
    # 创建一个Subscriber，订阅机器人的位姿信息
    rospy.Subscriber('robot_pose', PoseStamped, callback)
    
    rospy.spin()

if __name__ == '__main__':
    try:
        subscribe_pose()
    except rospy.ROSInterruptException:
        pass

上述代码实现了一个ROS节点，其中包含一个Subscriber，用于订阅机器人的位姿信息。在回调函数中，获取并打印接收到的机器人位姿信息。

要运行上述代码，需要将其保存为两个不同的Python文件，并分别使用rosrun命令来运行这两个节点：

$ rosrun package_name publish_pose.py
$ rosrun package_name subscribe_pose.py

其中，package_name是包含这两个Python文件的ROS软件包的名称。

在运行这两个节点后，你应该能够看到机器人的位姿信息被发布和订阅，并打印到终端上。