CoronalZMPPlot::CoronalZMPPlot(QWidget *parent)
: DataPlot(parent) {
for (int i = 0; i < PLOT_SIZE; ++i) {
data_l[i] = 0.0;
data_r[i] = 0.0;
data_t[i] = 0.0;
}
setTitle("Coronal Plane");
QwtPlotCurve *curveL = new QwtPlotCurve("Left");
curveL->attach(this);
curveL->setRawData(t, data_l, PLOT_SIZE);
curveL->setPen(QPen(Qt::red));
QwtPlotCurve *curveR = new QwtPlotCurve("Right");
curveR->attach(this);
curveR->setRawData(t, data_r, PLOT_SIZE);
curveR->setPen(QPen(Qt::blue));
QwtPlotCurve *curveT = new QwtPlotCurve("Total");
curveT->attach(this);
curveT->setRawData(t, data_t, PLOT_SIZE);
setAxisScale(QwtPlot::xBottom, 0, PLOT_SIZE - 1);
setAxisScale(QwtPlot::yLeft, -100, 100);
}
OdomPlot::OdomPlot(QWidget *parent)
: DataPlot(parent) {
for (int i = 0; i < PLOT_SIZE; ++i){
motionData[i] = 0.0; visionData[i] = 0.0; combinedData[i] = 0.0;
}
for (int i = 0; i < PLOT_SIZE-1; ++i){
motionDiff[i] = 0.0; visionDiff[i] = 0.0; combinedDiff[i] = 0.0;
}
setTitle("Odometry turn");
QwtPlotCurve *fcurve = new QwtPlotCurve("Vision");
fcurve->attach(this);
fcurve->setRawData(t, visionDiff, PLOT_SIZE-1);
fcurve->setPen(QPen(Qt::red));
QwtPlotCurve *rcurve = new QwtPlotCurve("Motion");
rcurve->attach(this);
rcurve->setRawData(t, motionDiff, PLOT_SIZE-1);
rcurve->setPen(QPen(Qt::blue));
QwtPlotCurve *vcurve = new QwtPlotCurve("Combined");
vcurve->attach(this);
vcurve->setRawData(t, combinedDiff, PLOT_SIZE-1);
vcurve->setPen(QPen(Qt::green));
setAxisScale(QwtPlot::xBottom, 0, PLOT_SIZE - 2);
setAxisScale(QwtPlot::yLeft, -10, 10);
}
//changing to angle x comparison plot
FsrPlot::FsrPlot(QWidget *parent)
: DataPlot(parent) {
for (int i = 0; i < PLOT_SIZE; ++i)
data_r2[i] = data_l[i] = data_r[i] = data_t[i] = 0.0;
setTitle("angleX");
QwtPlotCurve *curveL = new QwtPlotCurve("filtered angleX");
curveL->attach(this);
curveL->setRawData(t, data_l, PLOT_SIZE);
curveL->setPen(QPen(Qt::red));
QwtPlotCurve *curveR = new QwtPlotCurve("raw angleX");
curveR->attach(this);
curveR->setRawData(t, data_r, PLOT_SIZE);
curveR->setPen(QPen(Qt::blue));
// QwtPlotCurve *curveR2 = new QwtPlotCurve("Right2");
// curveR2->attach(this);
// curveR2->setRawData(t, data_r2, PLOT_SIZE);
// curveR2->setPen(QPen(Qt::black));
// QwtPlotCurve *curveT = new QwtPlotCurve("Total");
// curveT->attach(this);
// curveT->setRawData(t, data_t, PLOT_SIZE);
setAxisScale(QwtPlot::xBottom, 0, PLOT_SIZE - 1);
setAxisScale(QwtPlot::yLeft, -12, 12);
}
//using it for gyrY
CurrentPlot::CurrentPlot(QWidget *parent)
: DataPlot(parent) {
for (int i = 0; i < PLOT_SIZE; ++i) {
data[i] = 0.0; data2[i] = 0.0;
}
setTitle("gyroY");
QwtPlotCurve *fcurve = new QwtPlotCurve("filtered gyroY");
fcurve->attach(this);
fcurve->setRawData(t, data, PLOT_SIZE);
fcurve->setPen(QPen(Qt::red));
QwtPlotCurve *rcurve = new QwtPlotCurve("raw gyroY");
rcurve->attach(this);
rcurve->setRawData(t, data2, PLOT_SIZE);
rcurve->setPen(QPen(Qt::blue));
setAxisScale(QwtPlot::xBottom, 0, PLOT_SIZE - 1);
setAxisScale(QwtPlot::yLeft, -70, 70);
}
SurfPlot::SurfPlot(QWidget *parent)
: DataPlot(parent) {
for (int i = 0; i < SURF_PLOT_SIZE; ++i)
data[i] = 0.0;
setTitle("Surf landmark matching score");
QwtPlotCurve *curve = new QwtPlotCurve("Surf");
curve->attach(this);
curve->setRawData(t, data, SURF_PLOT_SIZE);
setAxisScale(QwtPlot::xBottom, 0, SURF_PLOT_SIZE - 1);
setAxisScale(QwtPlot::yLeft, 0, 2000);
}
COMyPlot::COMyPlot(QWidget *parent)
: XYZPlot(parent) {
for (int i = 0; i < PLOT_SIZE; ++i)
data[i] = 0.0;
setTitle("COM relative to foot touching ground y");
QwtPlotCurve *curve = new QwtPlotCurve("COMy");
curve->attach(this);
curve->setRawData(t, data, PLOT_SIZE);
setAxisScale(QwtPlot::xBottom, 0, PLOT_SIZE - 1);
setAxisScale(QwtPlot::yLeft, -100, 100);
}
SagittalZMPPlot::SagittalZMPPlot(QWidget *parent)
: DataPlot(parent) {
for (int i = 0; i < PLOT_SIZE; ++i)
data[i] = 0.0;
setTitle("Sagittal Plane");
QwtPlotCurve *curve = new QwtPlotCurve("SagittalZMP");
curve->attach(this);
curve->setRawData(t, data, PLOT_SIZE);
setAxisScale(QwtPlot::xBottom, 0, PLOT_SIZE - 1);
setAxisScale(QwtPlot::yLeft, -100, 100);
}
SonarPlot::SonarPlot(QWidget *parent)
: DataPlot(parent) {
for (int i = 0; i < PLOT_SIZE; ++i)
for (int j = 0; j < 10; ++j)
data[j][i] = 0.0;
setTitle("Sonar");
QwtPlotCurve *curve = new QwtPlotCurve("Sonar");
curve->attach(this);
curve->setRawData(t, data[0], PLOT_SIZE);
setAxisScale(QwtPlot::xBottom, 0, PLOT_SIZE - 1);
setAxisScale(QwtPlot::yLeft, 0, 2.55);
}
MultiPlot::MultiPlot(QWidget *parent, const std::string &title, u_int size, int min, int max)
: DataPlot(parent) {
datum = new double*[size];
for (u_int i = 0; i < size; ++i) {
datum[i] = new double[PLOT_SIZE];
for (u_int j = 0; j < PLOT_SIZE; j++) {
datum[i][j] = 0.0;
}
QwtPlotCurve *compareCurve = new QwtPlotCurve();
compareCurve->attach(this);
compareCurve->setRawData(t, datum[i], PLOT_SIZE);
Qt::GlobalColor colour = static_cast<Qt::GlobalColor>(Qt::red + i);
compareCurve->setPen(QPen(colour));
}
setTitle(QString::fromUtf8(title.c_str()));
setAxisScale(QwtPlot::xBottom, 0, PLOT_SIZE - 1);
setAxisScale(QwtPlot::yLeft, min, max);
}
//
// Initialize main window
//
DataPlot::DataPlot(QWidget *parent):
QwtPlot(parent),
d_interval(0),
d_timerId(-1)
{
// Disable polygon clipping
QwtPainter::setDeviceClipping(false);
// We don't need the cache here
canvas()->setPaintAttribute(QwtPlotCanvas::PaintCached, false);
canvas()->setPaintAttribute(QwtPlotCanvas::PaintPacked, false);
#if QT_VERSION >= 0x040000
#ifdef Q_WS_X11
/*
Qt::WA_PaintOnScreen is only supported for X11, but leads
to substantial bugs with Qt 4.2.x/Windows
*/
canvas()->setAttribute(Qt::WA_PaintOnScreen, true);
#endif
#endif
alignScales();
// Initialize data
for (int i = 0; i< PLOT_SIZE; i++)
{
d_x[i] = 0.5 * i; // time axis
d_y[i] = 0;
d_z[i] = 0;
}
// Assign a title
setTitle("A Test for High Refresh Rates");
insertLegend(new QwtLegend(), QwtPlot::BottomLegend);
// Insert new curves
QwtPlotCurve *cRight = new QwtPlotCurve("Data Moving Right");
cRight->attach(this);
QwtPlotCurve *cLeft = new QwtPlotCurve("Data Moving Left");
cLeft->attach(this);
// Set curve styles
cRight->setPen(QPen(Qt::red));
cLeft->setPen(QPen(Qt::blue));
// Attach (don't copy) data. Both curves use the same x array.
cRight->setRawData(d_x, d_y, PLOT_SIZE);
cLeft->setRawData(d_x, d_z, PLOT_SIZE);
#if 0
// Insert zero line at y = 0
QwtPlotMarker *mY = new QwtPlotMarker();
mY->setLabelAlignment(Qt::AlignRight|Qt::AlignTop);
mY->setLineStyle(QwtPlotMarker::HLine);
mY->setYValue(0.0);
mY->attach(this);
#endif
// Axis
setAxisTitle(QwtPlot::xBottom, "Time/seconds");
setAxisScale(QwtPlot::xBottom, 0, 100);
setAxisTitle(QwtPlot::yLeft, "Values");
setAxisScale(QwtPlot::yLeft, -1.5, 1.5);
setTimerInterval(0.0);
}
// Generate new values
void DataPlot::timerEvent(QTimerEvent *)
{
for (int k = 0; k < numberOfInputPorts; k++)
{
int timeout = 0;
int maxTimeout = 2;
Bottle *b = NULL;
while(b==NULL && timeout < maxTimeout)
{
b = inputPorts[k].read(false);
Time::delay(0.005);
timeout++;
}
if (timeout==maxTimeout)
{
if(VERBOSE) fprintf(stderr, "MESSAGE: Couldn't receive data. Going to put a zero! \n");
for(int i = 0; i < numberOfPlots[k]; i++)
{
d_y[k][i][PLOT_SIZE - 1] = 0;
for ( int j = 0; j < PLOT_SIZE - 1; j++ )
{
d_y[k][i][j] = d_y[k][i][j+1];
}
}
}
else
{
for(int i = 0; i < numberOfPlots[k]; i++)
{
Stamp stmp;
inputPorts[k].getEnvelope(stmp);
#ifdef ENABLE_REALTIME
if (stmp.isValid())
d_x_real_time[PLOT_SIZE - 1] = (stmp.getTime() - initialTime);
for ( int j = 0; j < PLOT_SIZE - 1; j++ )
{
if(realTime)
d_x_real_time[j] = d_x_real_time[j+1];
}
#endif
if (b==NULL)
d_y[k][i][PLOT_SIZE - 1] = 0;
else
if (b->size()-1 >= index[k][i])
{
d_y[k][i][PLOT_SIZE - 1] = b->get(index[k][i]).asDouble();
//if(VERBOSE) fprintf(stderr, "MESSAGE: Getting from port %d the index %d\n", k, index[k][i]);
}
// y moves from left to right:
// Shift y array right and assign new value to y[0].
for ( int j = 0; j < PLOT_SIZE - 1; j++ )
{
d_y[k][i][j] = d_y[k][i][j+1];
}
// update the display
//setAxisScale(QwtPlot::yLeft, min, max);
#ifdef ENABLE_REALTIME
if(numberAcquiredData==PLOT_SIZE && realTime)
{
if(VERBOSE) fprintf(stderr, "MESSAGE: switching to real time\n");
QwtPlotCurve *timeBasedCurve = new QwtPlotCurve("Data");
timeBasedCurve->attach(this);
timeBasedCurve->setRawData(d_x_real_time, d_y[k][i], PLOT_SIZE);
timeBasedCurve->setPen(coloredPens[k%NUMBER_OF_LIN][i%NUMBER_OF_COL]);
nonTimeBasedCurve[k][i].attach(NULL);
//Set title
char cTitle[256];
sprintf(cTitle, "Data(%d)", index[k][i]);
QwtText curveTitle(cTitle, QwtText::PlainText);
curveTitle.setFont(plotFont);
timeBasedCurve->setTitle(curveTitle);
}
#endif
}
}
}
if (acquire)
replot();
numberAcquiredData++;
//if(VERBOSE) fprintf(stderr, "Number of acquired data is %d\n", numberAcquiredData);
//QSize plotSize= this->sizeHint();
//if(VERBOSE) fprintf(stderr, "Hint is: hInt=%d, vInt=%d\n", plotSize.height(), plotSize.width());
static double before = Time::now();
double now = Time::now();
static double meanEffectiveTime = 0;
double currentEffectiveTime = (now - before)*1000.0;
if (numberAcquiredData >= 2)
meanEffectiveTime = (meanEffectiveTime*(numberAcquiredData-2) + currentEffectiveTime)/(numberAcquiredData-1);
//if(VERBOSE) fprintf(stderr, "Iteration %d: Current time is %f and mean is %f\n", numberAcquiredData, currentEffectiveTime, meanEffectiveTime);
if (currentEffectiveTime*0.5 > d_interval)
{
if(VERBOSE) fprintf(stderr, "Real Timer is %f but I was supposed to run at %d ms. Mean is: %f \n", currentEffectiveTime, d_interval, meanEffectiveTime);
if(VERBOSE) fprintf(stderr, "You should slow down to %d ms \n", (int) (meanEffectiveTime * 2));
//setTimerInterval((int) (meanEffectiveTime * 2));
}
before = now;
}
