Блог

У меня есть график карты сетки занятости, который я пытаюсь обновить всякий раз, когда запускается обратный вызов. Следующий код показывает обратный вызов, который принимает данные лазерного сканирования в качестве входных данных (с помощью rospy ), обновляет карту сетки занятости, используя эти данные, а затем отображает карту сетки для пользователя:

 def lidar_callback(self, scan: LaserScan):
    scan_parameters = [scan.angle_min, scan.angle_max, scan.angle_increment]
    scan_ranges = np.array(scan.ranges)

    self.mapper.update_map(self.pose, scan_ranges, scan.angle_min, scan.angle_increment)

    fig = plt.figure()
    ax = self.fig.add_subplot(111)
    ax.matshow(self.mapper.probability_map(), cmap="Greys")
    plt.draw()
  

Для одной итерации вывод карты сетки занятости выглядит следующим образом:

Однако приведенный выше код создает новый график всякий раз, когда запускается обратный вызов, вместо того, чтобы сохранять один график и обновлять его при обработке новых лидарных данных. Поскольку lidar_callback() функция является частью более крупного MapperNode класса, я попытался решить эту проблему, переместив fig переменную в __init__() конструктор MapperNode класса, указав следующий код,

 class MapperNode:
    def __init__(self, pose: Pose, mapper: Mapper):
        self.pose = pose
        self.mapper = mapper

        rospy.Subscriber("scan", LaserScan, self.lidar_callback)
        rospy.Subscriber("odom", Odometry, self.odometry_callback)

        self.fig = plt.figure()

    def lidar_callback(self, scan: LaserScan):
        scan_parameters = [scan.angle_min, scan.angle_max, scan.angle_increment]
        scan_ranges = np.array(scan.ranges)

        self.mapper.update_map(self.pose, scan_ranges, scan.angle_min, scan.angle_increment)
       
        ax = self.fig.add_subplot(111)
        ax.matshow(self.mapper.probability_map(), cmap="Greys")
        plt.draw()
  

Но теперь, при использовании self.fig при попытке определить ax = self.fig.add_subplot(111) , я получаю следующую ошибку:

 ax = self.fig.add_subplot(111)
AttributeError: 'MapperNode' object has no attribute 'fig'
  

Таким образом, кажется, что я не могу определить plt.figure() объект в конструкторе и использовать его позже в другой функции класса. Кто-нибудь знает, какую ошибку я совершаю, пытаясь обновить существующую цифру внутри обратного вызова? Если есть лучший способ показать / обновить карту сетки занятости, я был бы более чем рад узнать об этом. Спасибо!

вы можете попробовать это,

 import matplotlib.pyplot as plt
import rospy
import tf
from sensor_msgs.msg import LaserScan
from tf.transformations import quaternion_matrix
import numpy as np
from matplotlib.animation import FuncAnimation

class Visualiser:
    def __init__(self):
        self.fig, self.ax = plt.subplots()
        self.ln, = plt.plot([], [], 'ro')
        self.max_x = 300
        self.max_y = 300
    
    def plot_init(self):
        self.ax.set_xlim(0, self.max_x)
        self.ax.set_ylim(0, self.max_y)
        return self.ln  

    def update_plot(self, frame):
        # data_map = np.random.randint(60, size=(60, 60))
        # self.ax.matshow(data_map, cmap="Greys")
        self.ax.matshow(self.mapper.probability_map(), cmap="Greys")
        return self.ln
    
    def lidar_callback(self, scan):
        scan_parameters = [scan.angle_min, scan.angle_max, scan.angle_increment]
        scan_ranges = np.array(scan.ranges)
        self.mapper.update_map(self.pose, scan_ranges, scan.angle_min, scan.angle_increment)


rospy.init_node('lidar_visual_node')
vis = Visualiser()
sub = rospy.Subscriber('/scan', LaserScan, vis.lidar_callback)
ani = FuncAnimation(vis.fig, vis.update_plot, init_func=vis.plot_init)
plt.show(block=True) 
  

Примечание: установите правильные значения для self.max_x и self.max_y , я тестировал со случайной картой. Дайте мне знать, сработает ли это для вас

Например,