current_weights.py

'''
@author: Sammy Pfeiffer

Plot current weights

'''
import rospy
from std_msgs.msg import Float64MultiArray
import matplotlib.pyplot as plt

WEIGHTS_TOPIC = '/particle_laser_match_weight'


class PlotWeights:
    def __init__(self):
        #plt.axis([0, MAX_PARTICLES, 0, MAX_WEIGHT_VAL])
        plt.ion()
        plt.show()
        rospy.sleep(0.3)
        self.last_weights = []
        self.new_weights = False
        self.weights_sub = rospy.Subscriber(WEIGHTS_TOPIC, Float64MultiArray, self.weights_array_cb)
        
    def weights_array_cb(self, data):
        self.last_weights = data.data
        self.new_weights = True

        
    def run(self):
        while not rospy.is_shutdown():
            if self.new_weights:
                self.new_weights = False
                plt.clf()
                plt.title("Plot of ordered weights")
                plt.xlabel("# Particles")
                plt.ylabel("Laser match confidence")
                # update plot with the new data
                x_data = range(len(self.last_weights) + 1) 
                y_data = list(self.last_weights)
                y_data.sort()

                y_data.extend([0.0] * (len(x_data) - len(y_data)))
                plt.plot(x_data, y_data)
                
            plt.draw()
            #print "drawing!"
            rospy.sleep(0.05)

if __name__ == '__main__':
    rospy.init_node("weightsplot_node")
    rospy.sleep(0.3)
    pw = PlotWeights()
    pw.run()