plot_tcat_state.py

Go to the documentation of this file.

#!/usr/bin/env python

# ####################################################################
# Copyright (c) 2013, Shadow Robot Company, All rights reserved.
#
# This library is free software; you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation; either
# version 3.0 of the License, or (at your option) any later version.
#
# This library is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public
# License along with this library.
# ####################################################################

import rospy
from sr_ronex_msgs.msg import TCATState
# from PyQt4.QtCore import *
from PyQt4.QtGui import QMainWindow, QApplication, QWidget, QVBoxLayout

import numpy
from std_msgs.msg import Int8
import sys

import matplotlib
from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.backends.backend_qt4agg import NavigationToolbar2QTAgg as NavigationToolbar
from matplotlib.figure import Figure

import ctypes
from math import sqrt


class PlotWindow(QMainWindow):
    def __init__(self, parent=None):
        QMainWindow.__init__(self, parent)
        self.setWindowTitle('Impulse Responses')
        self.create_main_frame()

        self.setMinimumSize(1600, 900)

        self.on_draw()

    def save_plot(self):
        pass

    def on_about(self):
        pass

    def on_pick(self, event):
        pass

    def on_draw(self):
        for ax in self.axes:
            ax.clear()
            ax.grid(True)
        self.canvas.draw()

    def create_main_frame(self):
        self.main_frame = QWidget()
        self.dpi = 100
        self.fig = Figure((5.0, 4.0), dpi=self.dpi)
        self.canvas = FigureCanvas(self.fig)
        self.canvas.setParent(self.main_frame)
        self.axes = []
        for i in range(1, 5):
            self.axes.append(self.fig.add_subplot(4, 1, i))
        self.canvas.mpl_connect('pick_event', self.on_pick)
        self.mpl_toolbar = NavigationToolbar(self.canvas, self.main_frame)
        vbox = QVBoxLayout()
        vbox.addWidget(self.canvas)
        vbox.addWidget(self.mpl_toolbar)
        self.main_frame.setLayout(vbox)
        self.setCentralWidget(self.main_frame)


class TCATPlot(PlotWindow):
    def __init__(self):
        PlotWindow.__init__(self)

        self.window_size = 20

        rospy.init_node('visualizer', anonymous=True)
        self.subscriber = rospy.Subscriber("/ronex/tcat/1394812611/state", TCATState, self.plotResults, queue_size=1)

    def plotResults(self, data):
        receiver_index = 0

        print
        base = [r.timestamp_ns - (r.FPI - r.first_sample_number) for r in data.received_data]
        base_min = min(base)
        print base, base_min

        base = [b - base_min for b in base]
        print base

        for receiver, ax in zip(data.received_data, self.axes):
            ax.clear()
            ax.set_xlim([0, 64])
            # ax.set_ylim([-32768,32768])
            y1 = []
            y2 = []
            y3 = []
            x = []
            x_value = base[receiver_index]

            for impulse in receiver.impulse_response:
                y1.append(impulse.real)
                y2.append(impulse.imaginary)
                y3.append(sqrt(impulse.real**2 + impulse.imaginary**2))
                x.append(x_value)
                x_value = x_value + 1

            # x = range(receiver*10, len(y1)+(receiver*10))
            # x = range(0, len(y1))
            ax.set_ylim([min(min(y2), min(y1)), max(y3)])
            self.line1 = ax.plot(x, y1, c='#A0A0FF', label="real")
            self.line1 = ax.plot(x, y2, c='#A0FFA0', label="imag")
            self.line1 = ax.plot(x, y3, c='#A00000', label="magn")
            ax.legend(loc=0, scatterpoints=1)
            # ax.set_title("Impulse Response["+str(receiver_index)+"]")
            receiver_index += 1

        self.canvas.draw()

if __name__ == "__main__":
    app = QApplication(sys.argv)
    window = TCATPlot()
    window.show()
    app.exec_()