Python中使用geometry_msgs.msgPoseStamped()实现机器人的路径显示

geometry_msgs.msgPoseStamped()是ROS中的一种消息类型，用于定义一个带有时间戳的姿态(stamped pose)消息。它通常用于描述机器人在空间中的位置和姿态信息，并在ROS中的各个节点之间进行通信。

下面是一个示例，演示了如何使用geometry_msgs.msgPoseStamped()实现机器人的路径显示：

import rospy
from geometry_msgs.msg import PoseStamped
import matplotlib.pyplot as plt

# 创建一个用于存储机器人路径的列表
robot_path = []

def pose_callback(msg):
    # 每当接收到一个pose消息时，将其添加到机器人路径列表中
    robot_path.append((msg.pose.position.x, msg.pose.position.y))

def plot_robot_path():
    # 提取机器人路径x和y坐标
    robot_path_x = [point[0] for point in robot_path]
    robot_path_y = [point[1] for point in robot_path]

    # 绘制机器人路径
    plt.figure()
    plt.plot(robot_path_x, robot_path_y, 'b')
    plt.xlabel('X')
    plt.ylabel('Y')
    plt.title('Robot Path')

    # 显示图形
    plt.show()

if __name__ == '__main__':
    rospy.init_node('robot_path_display')

    # 创建一个订阅器，订阅机器人的pose消息
    rospy.Subscriber('robot_pose', PoseStamped, pose_callback)

    # 循环等待ROS消息
    rospy.spin()

    # 绘制机器人路径
    plot_robot_path()

在这个例子中，我们首先导入所需的模块和消息类型。然后，我们创建一个全局变量robot_path，用于存储机器人的路径。

接下来，我们定义了一个回调函数pose_callback，用于处理接收到的机器人pose消息。在回调函数中，我们提取了机器人的位置信息，并将其添加到机器人路径列表中。

然后，我们定义了一个plot_robot_path函数，用于绘制机器人的路径。在函数中，我们提取了机器人路径列表中的x和y坐标，并使用matplotlib库绘制了路径。

在main函数中，我们初始化了ROS节点，并创建了一个订阅器rospy.Subscriber，用于订阅机器人的pose消息。然后，我们通过rospy.spin()等待ROS消息，并在接收到消息后调用回调函数pose_callback。

最后，我们调用plot_robot_path函数，绘制机器人的路径。

要使用这个例子，可以将其保存为一个Python脚本，并在终端中运行ROS节点。确保已经正确设置ROS环境，并且已经发布了机器人的pose消息到/robot_pose话题。启动ROS节点后，它将开始接收机器人的pose消息并绘制机器人路径。

这只是一个简单的例子，演示了如何使用geometry_msgs.msgPoseStamped()实现机器人的路径显示。你可以根据自己的需求进行修改和扩展。