rospy通过matplotlib画出/odom轨迹图

本文介绍了一种使用Python和ROS实现同时监听两个Odometry话题,并将数据绘制成图表进行对比的方法。通过多线程处理和rospy库,文章详细展示了如何订阅数据、处理回调函数以及利用matplotlib进行实时绘图。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

注意事项

  1. Python没有回调函数这个东西,所以ROS中回调函数的实现其实是通过多线程来解决的
  2. 又因为是多线程,所以其实rospy.spin()并不是必须的,只要确保主线程(main)存活那么rospy中的回调函数就可以一直接收数据

代码

实现了同时监听两个 /odom 并画图进行对比

import rospy
from matplotlib import pyplot as plt
from nav_msgs.msg import Odometry
import numpy as np
import time

class DrawError(object):
    odom_time, cb_time = 0, 0
    x_odom = []
    y_odom = []
    x_rf2o = []
    y_rf2o = []
    def __init__(self):
        pass
    def isCBAlive(self):
        return False if self.cb_time - self.odom_time > 2 else True
    
    def odomCallBack(self, data):
        self.x_odom.append(data.pose.pose.position.x)
        self.y_odom.append(data.pose.pose.position.y)
        self.odom_time = time.time()

    def rf2oCallBack(self, data):
        self.x_rf2o.append(data.pose.pose.position.x)
        self.y_rf2o.append(data.pose.pose.position.y)

    # check if data is not received. If so, exit and plot the figure
    def cbMonitor(self, plt):
        self.cb_time = time.time()
        if not self.isCBAlive():
            rospy.logwarn("No data received!Exit!")
            return True
        else:
            return False


if __name__ == "__main__":
    rospy.init_node("drawingError")
    de = DrawError()

    # gazebo's odom, without nosie
    rospy.Subscriber("odom", Odometry, de.odomCallBack)
    # rf2o_odometry's odom
    rospy.Subscriber("odom_rf2o", Odometry, de.rf2oCallBack)
    rospy.Timer(rospy.Duration(2), de.cbMonitor)
    plt.title("odom display")

    while not rospy.is_shutdown():
        if de.cbMonitor(plt) == True:
            break
    plt.plot(de.x_odom, de.y_odom, color="b", label="/odom")
    plt.plot(de.x_rf2o, de.y_rf2o, color="r", label="/odom_rf2o")
    plt.legend()
    plt.show()

最终效果

在这里插入图片描述

#!/usr/bin/env python # -*- coding: utf-8 -*- # 添加这一行 import rospy import matplotlib.pyplot as plt from nav_msgs.msg import OccupancyGrid, Odometry from matplotlib.animation import FuncAnimation class TrajectoryPlotter: def __init__(self): self.fig, self.ax = plt.subplots() self.map_data = None self.traj_x, self.traj_y = [], [] self.current_pose = None # ROS初始化 rospy.init_node('trajectory_visualizer') self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_cb) self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb) # 设置动更新 self.ani = FuncAnimation(self.fig, self.update_plot, interval=200) def map_cb(self, msg): """ 地数据处理 """ self.map_data = { 'resolution': msg.info.resolution, 'width': msg.info.width, 'height': msg.info.height, 'origin': msg.info.origin, 'data': msg.data } self.plot_map() def odom_cb(self, msg): """ 里程计数据处理 """ self.current_pose = msg.pose.pose # 坐标转换(假设map和odom坐标系对齐) self.traj_x.append(msg.pose.pose.position.x) self.traj_y.append(msg.pose.pose.position.y) def plot_map(self): """ 绘制静态地 """ if not self.map_data: return # 创建网格坐标系 grid = [[0]*self.map_data['width'] for _ in range(self.map_data['height'])] for i in range(self.map_data['height']): for j in range(self.map_data['width']): grid[i][j] = self.map_data['data'][i*self.map_data['width']+j] # 转换为物理坐标系 origin_x = self.map_data['origin'].position.x origin_y = self.map_data['origin'].position.y self.ax.imshow(grid, cmap='gray', origin='lower', extent=[origin_x, origin_x + self.map_data['width']*self.map_data['resolution'], origin_y, origin_y + self.map_data['height']*self.map_data['resolution']]) def update_plot(self, frame): """ 动态更新轨迹 """ if self.current_pose: self.ax.plot(self.traj_x, self.traj_y, 'r-', linewidth=2) self.ax.plot(self.traj_x[-1], self.traj_y[-1], 'bo', markersize=8) self.ax.set_xlabel('X (m)') self.ax.set_ylabel('Y (m)') self.ax.set_title('Real-time Trajectory') plt.draw() if __name__ == '__main__': tp = TrajectoryPlotter() plt.show()这个代码可以加一个轨迹长度的显示吗
03-08
#!/usr/bin/env python # -*- coding: utf-8 -*- import rospy import matplotlib.pyplot as plt from nav_msgs.msg import OccupancyGrid, Odometry from matplotlib.animation import FuncAnimation import math # 添加数学库 class TrajectoryPlotter: def __init__(self): self.fig, self.ax = plt.subplots() self.map_data = None self.traj_x, self.traj_y = [], [] self.current_pose = None self.traj_length = 0.0 # 新增轨迹长度变量 # ROS初始化 rospy.init_node('trajectory_visualizer') self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_cb) self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb) # 初始化文本对象 self.length_text = self.ax.text(0.05, 0.95, '', transform=self.ax.transAxes, bbox=dict(facecolor='white', alpha=0.8)) # 新增文本框 self.ani = FuncAnimation(self.fig, self.update_plot, interval=200) def map_cb(self, msg): """ 地数据处理 """ self.map_data = { 'resolution': msg.info.resolution, 'width': msg.info.width, 'height': msg.info.height, 'origin': msg.info.origin, 'data': msg.data } self.plot_map() def odom_cb(self, msg): """ 里程计数据处理 """ self.current_pose = msg.pose.pose x = msg.pose.pose.position.x y = msg.pose.pose.position.y # 计算轨迹长度 if len(self.traj_x) > 0: # 当有历史点时计算距离 dx = x - self.traj_x[-1] dy = y - self.traj_y[-1] self.traj_length += math.hypot(dx, dy) # 累加欧式距离 self.traj_x.append(x) self.traj_y.append(y) def plot_map(self): """ 绘制静态地 """ if not self.map_data: return # 创建网格坐标系 grid = [[0]*self.map_data['width'] for _ in range(self.map_data['height'])] for i in range(self.map_data['height']): for j in range(self.map_data['width']): grid[i][j] = self.map_data['data'][i*self.map_data['width']+j] # 转换为物理坐标系 origin_x = self.map_data['origin'].position.x origin_y = self.map_data['origin'].position.y self.ax.imshow(grid, cmap='gray', origin='lower', extent=[origin_x, origin_x + self.map_data['width']*self.map_data['resolution'], origin_y, origin_y + self.map_data['height']*self.map_data['resolution']]) def update_plot(self, frame): """ 动态更新轨迹 """ if self.current_pose: # 更新轨迹 self.ax.plot(self.traj_x, self.traj_y, 'r-', linewidth=2) self.ax.plot(self.traj_x[-1], self.traj_y[-1], 'bo', markersize=8) # 更新轨迹长度显示(保留两位小数) self.length_text.set_text(f'Trajectory Length: {self.traj_length:.2f} m') # 更新文本内容 # 设置坐标轴和标题 self.ax.set_xlabel('X (m)') self.ax.set_ylabel('Y (m)') self.ax.set_title('Real-time Trajectory') plt.draw() if __name__ == '__main__': tp = TrajectoryPlotter() plt.show()在这段代码的基础上我还想订阅/move_base/local_costmap/costmap来得到局部障碍物的地
03-08
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值