#!/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来得到局部障碍物的地图

<think>好的，我需要帮助用户在现有的ROS Python代码基础上，添加对局部代价地图/move_base/local_costmap/costmap的订阅功能。让我仔细分析一下用户的需求和现有代码的结构。 首先，用户现有的代码已经订阅了全局地图/map和里程计/odom，并实时绘制轨迹和地图。现在用户希望同时显示局部代价地图。局部代价地图和全局地图的消息类型都是nav_msgs/OccupancyGrid，所以订阅的方式应该类似。 我需要考虑几个方面：如何在类中新增局部地图的数据存储，如何处理两个地图的订阅回调，以及如何在Matplotlib中同时显示两个地图而不冲突。现有的代码中有一个map_data变量用于存储全局地图，可能需要新增一个类似的变量，比如local_map_data，并在回调函数中更新它。 然后，在绘制地图的部分，原来的plot_map函数只处理了全局地图。现在需要修改这个函数，或者在另一个函数中绘制局部地图。需要注意的是，局部地图的坐标系可能和全局地图不同，需要正确转换坐标。此外，局部地图可能会频繁更新，所以需要高效地重绘，避免性能问题。 另外，颜色设置方面，为了避免混淆，全局地图和局部地图应该使用不同的颜色方案。例如，全局地图保持灰色，局部地图可以使用半透明的其他颜色，如蓝色，以便区分。 还需要考虑地图的叠加顺序，局部地图应该在全局地图之上，所以要先绘制全局地图，再绘制局部地图。在现有的plot_map函数中，可能需要添加绘制局部地图的代码，或者创建一个新的函数，并在update_plot中调用。 还有，局部地图的话题是/move_base/local_costmap/costmap，需要创建新的订阅者，比如local_map_sub，并在回调函数local_map_cb中处理数据，类似于map_cb。 最后，测试时需要注意两个地图的数据是否能够正确接收和显示，尤其是坐标转换是否正确，避免地图位置错乱。 可能的步骤总结： 1. 在__init__方法中新增局部地图的订阅者。 2. 添加local_map_data变量来存储局部地图数据。 3. 编写local_map_cb回调函数，类似map_cb，但存储到local_map_data。 4. 修改plot_map函数，或者在update_plot中添加局部地图的绘制逻辑，确保正确显示两个地图。 5. 调整颜色和透明度，使两者区分明显。 6. 处理可能的坐标转换问题，尤其是局部地图的原点可能与全局不同。</think>要在现有代码基础上增加局部代价地图的显示功能，我们可以按照以下步骤进行修改： ```python #!/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.global_map_data = None # 原map_data重命名为global_map_data self.local_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.global_map_sub = rospy.Subscriber('/map', OccupancyGrid, self.global_map_cb) # 新增局部代价地图订阅 self.local_map_sub = rospy.Subscriber('/move_base/local_costmap/costmap', OccupancyGrid, self.local_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 global_map_cb(self, msg): """ 全局地图回调 """ self.global_map_data = self.process_map(msg) self.plot_map() def local_map_cb(self, msg): """ 局部地图回调 """ self.local_map_data = self.process_map(msg) self.plot_map() def process_map(self, msg): """ 通用地图处理 """ return { 'resolution': msg.info.resolution, 'width': msg.info.width, 'height': msg.info.height, 'origin': msg.info.origin, 'data': msg.data } def plot_map(self): """ 绘制地图（全局和局部） """ self.ax.clear() # 清除之前的地图 # 先绘制全局地图 if self.global_map_data: self.draw_single_map(self.global_map_data, 'gray', 'Global Map') # 再叠加局部地图（使用半透明蓝色） if self.local_map_data: self.draw_single_map(self.local_map_data, 'Blues', 'Local Map', alpha=0.5) def draw_single_map(self, map_data, cmap, label, alpha=1.0): """ 绘制单个地图的通用方法 """ # 创建网格数据 grid = [[0]*map_data['width'] for _ in range(map_data['height'])] for i in range(map_data['height']): for j in range(map_data['width']): grid[i][j] = map_data['data'][i*map_data['width']+j] # 转换为物理坐标系 origin = map_data['origin'] extent = [ origin.position.x, origin.position.x + map_data['width']*map_data['resolution'], origin.position.y, origin.position.y + map_data['height']*map_data['resolution'] ] # 绘制地图并添加图例 im = self.ax.imshow(grid, cmap=cmap, origin='lower', extent=extent, alpha=alpha) im.set_label(label) # 为图例设置标签 # 其他原有方法保持不变... # （保持原有的odom_cb, update_plot等方法不变） if __name__ == '__main__': tp = TrajectoryPlotter() plt.show() ``` 主要改进点说明： 1. **双地图订阅**：新增了局部代价地图订阅者`local_map_sub`，订阅话题`/move_base/local_costmap/costmap` 2. **数据分离存储**： - 将原来的`map_data`重命名为`global_map_data` - 新增`local_map_data`存储局部地图数据 3. **通用处理方法**： - `process_map()`方法统一处理地图数据 - `draw_single_map()`方法实现通用地图绘制逻辑 4. **可视化增强**： - 全局地图使用灰色显示（'gray'） - 局部地图使用半透明蓝色（'Blues'，alpha=0.5） - 添加图例标签方便区分 5. **显示优化**： - 每次更新时先清空画布（`self.ax.clear()`） - 绘制顺序：先全局地图，后局部地图（保证局部地图显示在上层） 使用效果： - 全局地图显示为灰色静态背景 - 局部代价地图显示为半透明的蓝色覆盖层 - 两者坐标会自动对齐（依赖ROS发布的坐标系信息） - 轨迹和位置标记会显示在最上层 **注意**：需要确保以下ROS参数配置正确： 1. 全局/局部代价地图的坐标系需要与里程计坐标系一致 2. 地图更新频率建议设置在1-2Hz（高频更新可能影响性能）