用python 写一段ros2 订阅geometry_msgs::msg::PoseStampe这个类型数据类型的程序,并用plt将收到的x y坐标画出来
时间: 2024-02-05 12:04:22 浏览: 134
下面是一个简单的 ROS2 Python 程序,用于订阅 `geometry_msgs::msg::PoseStamped` 消息,并将收到的 x 和 y 坐标绘制在 Matplotlib 中:
```python
import rclpy
from geometry_msgs.msg import PoseStamped
import matplotlib.pyplot as plt
x_list = []
y_list = []
def pose_callback(msg):
x_list.append(msg.pose.position.x)
y_list.append(msg.pose.position.y)
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('pose_subscriber')
subscription = node.create_subscription(
PoseStamped,
'pose_topic',
pose_callback,
10)
fig, ax = plt.subplots()
while rclpy.ok():
plt.cla()
ax.plot(x_list, y_list, 'ro')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_xlim([-10, 10])
ax.set_ylim([-10, 10])
plt.draw()
plt.pause(0.001)
rclpy.spin_once(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
```
在这个程序中,我们首先导入了 ROS2 和 Matplotlib 相关的库,然后定义了一个 `x_list` 和 `y_list` 列表,用于存储收到的 x 和 y 坐标数据。接着定义了一个 `pose_callback` 回调函数,用于处理收到的 `PoseStamped` 消息,将其 x 和 y 坐标添加到对应的列表中。
在 `main` 函数中,我们创建了一个 ROS2 节点,并订阅了一个 `PoseStamped` 类型的主题。然后创建了一个 Matplotlib 的 Figure 和 Axes 对象,并进入一个无限循环中,在每次循环中绘制当前收到的 x 和 y 坐标,并刷新画布。
最后,我们在 `__main__` 中调用 `main` 函数,运行整个程序。请确保将 `pose_topic` 替换为你要订阅的实际主题名称。
阅读全文