SonarPlot Class Reference

#include <SonarPlot.h>

Inherits QFrame.

Public Member Functions
	SonarPlot (QWidget *parent=0)
virtual	~SonarPlot ()
void	setData (const base::samples::Sonar &sonar)
void	setSectorScan (bool continuous, base::Angle leftLimit, base::Angle rightLimit)

Protected Slots
void	rangeChanged (int)
void	sonarPaletteChanged (int)
void	gridChanged (bool)

Protected Member Functions
void	paintEvent (QPaintEvent *event)
void	resizeEvent (QResizeEvent *event)
void	drawOverlay ()
void	generateMultibeamTransferTable (const base::samples::Sonar &sonar)
void	generateScanningTransferTable (const base::samples::Sonar &sonar)
void	applyColormap (ColorGradientType type)
bool	isMotorStepChanged (const base::Angle &bearing)
void	addScanningData (const base::samples::Sonar &sonar)

Protected Attributes
base::samples::Sonar	lastSonar
double	scaleX
double	scaleY
int	range
int	numSteps
bool	changedSize
bool	changedSectorScan
bool	isMultibeamSonar
bool	continuous
bool	enabledGrid
QPoint	origin
std::vector< int >	transfer
std::vector< QColor >	colorMap
ColorGradient	heatMapGradient
base::Angle	motorStep
base::Angle	lastDiffStep
base::Angle	leftLimit
base::Angle	rightLimit
std::vector< float >	sonarData

Detailed Description

Definition at line 16 of file SonarPlot.h.

Constructor & Destructor Documentation

SonarPlot::SonarPlot

(

QWidget *

parent = 0

)

Definition at line 7 of file SonarPlot.cc.

    : QFrame(parent), scaleX(1), scaleY(1), range(5), changedSize(true), changedSectorScan(false), isMultibeamSonar(true), continuous(true), enabledGrid(true)
{
    motorStep.rad = 0;
    lastDiffStep.rad = 0;
    leftLimit.rad = 0;
    rightLimit.rad = 0;

    // apply default colormap
    applyColormap(COLORGRADIENT_JET);

      QPalette Pal(palette());
      Pal.setColor(QPalette::Background, QColor(0,0,255));
      setAutoFillBackground(true);
      setPalette(Pal);
}

SonarPlot::~SonarPlot ( )

virtual

Definition at line 24 of file SonarPlot.cc.

{
}

Member Function Documentation

void SonarPlot::addScanningData ( const base::samples::Sonar &  sonar )

protected

Definition at line 105 of file SonarPlot.cc.

                                                             {
    int id_beam = round((numSteps - 1) * (sonar.bearings[0].rad + M_PI) / (2 * M_PI));
    memcpy(&sonarData[id_beam * sonar.bin_count], &sonar.bins[0], sonar.bin_count * sizeof(float));
}

void SonarPlot::applyColormap ( ColorGradientType  type )

protected

Definition at line 252 of file SonarPlot.cc.

                                                   {

    heatMapGradient.colormapSelector(type);

    colorMap.clear();
    try {
        float red, green, blue;
        for (int i = 0; i < 256; ++i) {
            heatMapGradient.getColorAtValue((1.0 / 255) * i, red, green, blue);
            colorMap.push_back(QColor(red * 255, green * 255, blue * 255));
        }
    } catch (const std::out_of_range& e) {
        std::cout << e.what() << std::endl;
    }
}

void SonarPlot::drawOverlay ( )

protected

Definition at line 155 of file SonarPlot.cc.

{
    QPainter painter(this);
    painter.setRenderHint(QPainter::Antialiasing, true);

    // draw color pallete
    for(size_t i = 0; i < 256; i++) {
        painter.setPen(QPen(colorMap[i]));
        painter.drawRect(width() - 30,height() - 10 -i * 2, 20, 2);
    }

    if(!enabledGrid)
        return;

    // draw sonar grid
    painter.setPen(QPen(Qt::white));

    // multibeam sonar
    if (isMultibeamSonar) {

        base::Angle sectorSize = base::Angle::fromRad((lastSonar.beam_width.rad / lastSonar.beam_count) * (lastSonar.beam_count - 1));

        for(int i=1;i<=5;i++){
            painter.drawArc(origin.x() - i * scaleX * 100, origin.y() - i * scaleY * 100, i * 200 * scaleX, i * 200 * scaleY, (90 - sectorSize.getDeg() / 2) * 16, sectorSize.getDeg() * 16);
            QString str = QString::number(i * range * 1.0 / 5);
            int x = origin.x() + i * 100 * scaleX * sin(sectorSize.rad / 2);
            int y = height() - i * 100 * scaleY * cos(sectorSize.rad / 2);
            painter.drawText(x,y-5,str);

            base::Angle ang = lastSonar.bearings[((lastSonar.beam_count - 1) * 1.0 / 4) * (i-1)];
            QPoint point(origin.rx() + BINS_REF_SIZE * sin(ang.rad) * scaleX, origin.ry() - BINS_REF_SIZE * cos(ang.rad) * scaleY);
            painter.drawLine(origin, point);
            str = QString::number(ang.getDeg(), 'f', 1);
            painter.drawText(point.x() - 10, point.y() - 10, str);
        }
    }

    // scanning sonar
    else {
        double offsetX = BINS_REF_SIZE * 0.75 * scaleX;
        double offsetY = BINS_REF_SIZE * 0.55 * scaleY;
        painter.drawLine(QPoint(origin.rx(), origin.ry() - offsetY), QPoint(origin.rx(), origin.ry() + offsetY));
        painter.drawLine(QPoint(origin.rx() - offsetX, origin.ry()), QPoint(origin.rx() + offsetX, origin.ry()));

        for (int i = 1; i <= 5; ++i) {
            int x = i * offsetX / 5;
            int y = i * offsetY / 5;
            painter.drawEllipse(origin, x, y);
            QString str_radius = QString::number(i * range * 1.0 / 5);
            painter.drawText(origin.rx() + x + 2, origin.ry() - 5, str_radius);
        }

        QString str = QString::number(lastSonar.bearings[0].getDeg(), 'f', 1);
        QPoint point(origin.rx() - offsetX * sin(lastSonar.bearings[0].rad), origin.ry() - offsetY * cos(lastSonar.bearings[0].rad));
        painter.setPen(QPen(Qt::green));
        painter.drawLine(origin, point);
        painter.drawText(point.x() - 10, point.y() - 10, str);

        if (!continuous) {
            QPoint point1(origin.rx() - offsetX * sin(leftLimit.rad), origin.ry() - offsetY * cos(leftLimit.rad));
            QPoint point2(origin.rx() - offsetX * sin(rightLimit.rad), origin.ry() - offsetY * cos(rightLimit.rad));
            painter.drawLine(origin, point1);
            painter.drawLine(origin, point2);
        }
    }
}

void SonarPlot::generateMultibeamTransferTable ( const base::samples::Sonar &  sonar )

protected

Definition at line 269 of file SonarPlot.cc.

                                                                            {

    transfer.clear();

    // set the origin
    origin.setY(height() - 30);

    // check pixels
    for (int j = 0; j < height(); j++) {
        int beam_idx = 0;
        for (int i = 0; i < width(); i++) {

            QPoint point(i - origin.x(), j - origin.y());
            point.rx() /= scaleX * BINS_REF_SIZE / sonar.bin_count;
            point.ry() /= scaleY * BINS_REF_SIZE / sonar.bin_count;

            double radius = sqrt(point.x() * point.x() + point.y() * point.y());
            double angle = atan2(point.x(), -point.y());

            // pixels out of sonar image
            if (angle < sonar.bearings[0].rad || angle > sonar.bearings[sonar.beam_count - 1].rad || radius > sonar.bin_count || !radius || j > origin.y())
                transfer.push_back(-1);

            // pixels in the sonar image
            else {
                while (angle < sonar.bearings[beam_idx].rad || angle >= sonar.bearings[beam_idx + 1].rad)
                    beam_idx++;
                transfer.push_back(beam_idx * sonar.bin_count + radius);
            }
        }
    }
}

void SonarPlot::generateScanningTransferTable ( const base::samples::Sonar &  sonar )

protected

Definition at line 303 of file SonarPlot.cc.

                                                                           {

    transfer.clear();

    // check motor step value
    if (!motorStep.rad)
        return;

    // set the origin
    origin.setY(height() / 2);

    // check pixels
    for (int j = 0; j < height(); j++) {
        for (int i = 0; i < width(); i++) {

            QPoint point(i - origin.x(), j - origin.y());
            point.rx() /= scaleX * BINS_REF_SIZE * 0.75 / sonar.bin_count;
            point.ry() /= scaleY * BINS_REF_SIZE * 0.55 / sonar.bin_count;

            double radius = sqrt(point.x() * point.x() + point.y() * point.y());

            // pixels out of sonar image
            if (radius > sonar.bin_count || !radius)
                transfer.push_back(-1);

            // pixels in the sonar image
            else {
                double angle = atan2(-point.x(), -point.y());
                int idBeam = round((numSteps - 1) * (angle + M_PI) / (2 * M_PI));
                transfer.push_back(idBeam * sonar.bin_count + radius);
            }
        }
    }
}

void SonarPlot::gridChanged ( bool  value )

protectedslot

Definition at line 234 of file SonarPlot.cc.

                                     {
    enabledGrid = value;
}

bool SonarPlot::isMotorStepChanged ( const base::Angle &  bearing )

protected

Definition at line 83 of file SonarPlot.cc.

                                                           {
    base::Angle diffStep = bearing - lastSonar.bearings[0];
    diffStep.rad = fabs(diffStep.rad);

    // if the sector scanning is enabled, the diffStep could be lower than motorStep when the bearing is closer to one of the corners
    if (!continuous && (fabs((leftLimit - bearing).rad) < motorStep.rad || fabs((rightLimit - bearing).rad) < motorStep.rad)) {
        lastDiffStep = diffStep;
        return false;
    }

    if (!motorStep.isApprox(diffStep) && lastDiffStep.isApprox(diffStep)) {
        motorStep = diffStep;
        numSteps = M_PI * 2 / motorStep.rad;
        lastDiffStep = diffStep;
        return true;
    }

    lastDiffStep = diffStep;
    return false;
}

void SonarPlot::paintEvent ( QPaintEvent *  event )

protected

Definition at line 110 of file SonarPlot.cc.

{
    if (!transfer.size())
        return;

    QPainter painter(this);
    painter.setRenderHint(QPainter::Antialiasing, true);

    // draw sonar image
    QImage img(width(), height(), QImage::Format_RGB888);
    img.fill(QColor(0, 0, 255));

    if (isMultibeamSonar)
        sonarData = lastSonar.bins;

    for (unsigned int i = 0; i < transfer.size() && !changedSize; ++i) {
        if (transfer[i] != -1) {
            QColor c = colorMap[round(sonarData[transfer[i]] * 255)];
            img.setPixel(i % width(), i / width(), qRgb(c.red(), c.green(), c.blue()));
        }
    }

    painter.drawImage(0, 0, img);

    // draw overlay
    drawOverlay();

}

void SonarPlot::rangeChanged ( int  value )

protectedslot

Definition at line 223 of file SonarPlot.cc.

{
    range = value;
}

void SonarPlot::resizeEvent ( QResizeEvent *  event )

protected

Definition at line 139 of file SonarPlot.cc.

{
    scaleX = 0.2;
    if(width()>400){
        scaleX = double(width())/(BASE_WIDTH-134);
    }
    scaleY = 0.2;
    if(height()>200){
        scaleY = double(height()-100)/(BASE_HEIGHT-100);
    }
    origin.setX(width()/2);
    isMultibeamSonar ? origin.setY(height() - 30) : origin.setY(height() / 2);
    changedSize=true;
    QWidget::resizeEvent (event);
}

void SonarPlot::setData

(

const base::samples::Sonar &

sonar

)

Definition at line 29 of file SonarPlot.cc.

{
    if (!sonar.beam_count || !sonar.bin_count)
        return;

    isMultibeamSonar = (sonar.beam_count > 1);

    // process multibeam sonar data
    if (isMultibeamSonar) {
        if(changedSize
               || sonar.bin_count != lastSonar.bin_count
               || sonar.beam_count  != lastSonar.beam_count
               || !(sonar.bearings[0]  == lastSonar.bearings[0])
               || !((sonar.bearings[1] - sonar.bearings[0]) == (lastSonar.bearings[1] - lastSonar.bearings[0]))) {

            // set the transfer vector between image pixels and sonar data
            generateMultibeamTransferTable(sonar);

            changedSize = false;
        }
    }

    // process scanning sonar data
    else {

        // check if the motor step size is changed
        bool changedMotorStep = lastSonar.beam_count && isMotorStepChanged(sonar.bearings[0]);

        if ((changedSize
                || changedMotorStep
                || changedSectorScan
                || sonar.bin_count != lastSonar.bin_count) && lastSonar.beam_count && motorStep.rad) {

            // set the transfer vector between image pixels and sonar data
            generateScanningTransferTable(sonar);

            // if the number of bins, the motor step or the sector scan changes, the accumulated sonar data will be reseted
            if (sonar.bin_count != lastSonar.bin_count || changedMotorStep || changedSectorScan)
                sonarData.assign(numSteps * sonar.bin_count, 0.0);

            changedSize = false;
            changedSectorScan = false;
        }

        // add current beam to accumulated scanning sonar data
        if (sonarData.size())
            addScanningData(sonar);
    }

    lastSonar = sonar;
    update();
}

void SonarPlot::setSectorScan	(	bool	continuous,
		base::Angle	leftLimit,
		base::Angle	rightLimit
	)

Definition at line 239 of file SonarPlot.cc.

                                                                                       {

    // if the parameters changes, the screen will be clean
    if (this->continuous != continuous
            || ((this->leftLimit.rad != leftLimit.rad || this->rightLimit.rad != rightLimit.rad) && !continuous))
        changedSectorScan = true;

    this->continuous = continuous;
    this->leftLimit = leftLimit;
    this->rightLimit = rightLimit;
}

void SonarPlot::sonarPaletteChanged ( int  index )

protectedslot

Definition at line 229 of file SonarPlot.cc.

                                            {
    applyColormap((ColorGradientType) index);
}

Member Data Documentation

bool SonarPlot::changedSectorScan

protected

Definition at line 42 of file SonarPlot.h.

bool SonarPlot::changedSize

protected

Definition at line 41 of file SonarPlot.h.

std::vector<QColor> SonarPlot::colorMap

protected

Definition at line 48 of file SonarPlot.h.

bool SonarPlot::continuous

protected

Definition at line 44 of file SonarPlot.h.

bool SonarPlot::enabledGrid

protected

Definition at line 45 of file SonarPlot.h.

ColorGradient SonarPlot::heatMapGradient

protected

Definition at line 49 of file SonarPlot.h.

bool SonarPlot::isMultibeamSonar

protected

Definition at line 43 of file SonarPlot.h.

base::Angle SonarPlot::lastDiffStep

protected

Definition at line 50 of file SonarPlot.h.

base::samples::Sonar SonarPlot::lastSonar

protected

Definition at line 36 of file SonarPlot.h.

base::Angle SonarPlot::leftLimit

protected

Definition at line 51 of file SonarPlot.h.

base::Angle SonarPlot::motorStep

protected

Definition at line 50 of file SonarPlot.h.

int SonarPlot::numSteps

protected

Definition at line 40 of file SonarPlot.h.

QPoint SonarPlot::origin

protected

Definition at line 46 of file SonarPlot.h.

int SonarPlot::range

protected

Definition at line 39 of file SonarPlot.h.

base::Angle SonarPlot::rightLimit

protected

Definition at line 51 of file SonarPlot.h.

double SonarPlot::scaleX

protected

Definition at line 37 of file SonarPlot.h.

double SonarPlot::scaleY

protected

Definition at line 38 of file SonarPlot.h.

std::vector<float> SonarPlot::sonarData

protected

Definition at line 52 of file SonarPlot.h.

std::vector<int> SonarPlot::transfer

protected

Definition at line 47 of file SonarPlot.h.

---

The documentation for this class was generated from the following files:

• /build/rock-master-18.09-gui-rock-widget-collection-0.20180124/src/sonar_widget/SonarPlot.h
• /build/rock-master-18.09-gui-rock-widget-collection-0.20180124/src/sonar_widget/SonarPlot.cc