Waypoint2DIcon.cc 12 KB
Newer Older
1 2
#include "Waypoint2DIcon.h"
#include <QPainter>
3
#include "opmapcontrol.h"
lm's avatar
lm committed
4
#include "QGC.h"
5

6 7 8
Waypoint2DIcon::Waypoint2DIcon(mapcontrol::MapGraphicItem* map, mapcontrol::OPMapWidget* parent, qreal latitude, qreal longitude, qreal altitude, int listindex, QString name, QString description, int radius)
    : mapcontrol::WayPointItem(internals::PointLatLng(latitude, longitude), altitude, description, map),
    parent(parent),
9
    waypoint(),
10 11 12 13
    radius(radius),
    showAcceptanceRadius(true),
    showOrbit(false),
    color(Qt::red)
14
{
LM's avatar
LM committed
15 16
    Q_UNUSED(name);

17 18 19 20
    SetHeading(0);
    SetNumber(listindex);
    this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true);
    picture = QPixmap(radius+1, radius+1);
lm's avatar
lm committed
21 22 23
    autoreachedEnabled = false; // In contrast to the use in OpenPilot, we don't
                                // want to let the map interfere with the actual mission logic
                                // wether a WP is reached depends solely on the UAV's state machine
24
    drawIcon();
25 26
}

27 28 29 30 31 32 33 34
Waypoint2DIcon::Waypoint2DIcon(mapcontrol::MapGraphicItem* map, mapcontrol::OPMapWidget* parent, Waypoint* wp, const QColor& color, int listindex, int radius)
    : mapcontrol::WayPointItem(internals::PointLatLng(wp->getLatitude(), wp->getLongitude()), wp->getAltitude(), wp->getDescription(), map),
    parent(parent),
    waypoint(wp),
    radius(radius),
    showAcceptanceRadius(true),
    showOrbit(false),
    color(color)
35
{
36 37 38 39
    SetHeading(wp->getYaw());
    SetNumber(listindex);
    this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true);
    picture = QPixmap(radius+1, radius+1);
lm's avatar
lm committed
40 41 42
    autoreachedEnabled = false; // In contrast to the use in OpenPilot, we don't
                                // want to let the map interfere with the actual mission logic
                                // wether a WP is reached depends solely on the UAV's state machine
43
    updateWaypoint();
44 45
}

46
Waypoint2DIcon::~Waypoint2DIcon()
47 48 49
{
}

50
void Waypoint2DIcon::SetHeading(float heading)
51
{
52 53
    mapcontrol::WayPointItem::SetHeading(heading);
    drawIcon();
54 55
}

56 57
void Waypoint2DIcon::updateWaypoint()
{
58
    if (!waypoint.isNull()) {
59 60 61
        // Store old size
        static QRectF oldSize;

62 63
        SetHeading(waypoint->getYaw());
        SetCoord(internals::PointLatLng(waypoint->getLatitude(), waypoint->getLongitude()));
64

65
        // qDebug() << "UPDATING WP:" << waypoint->getId() << "LAT:" << waypoint->getLatitude() << "LON:" << waypoint->getLongitude();
66

67 68 69 70
        SetDescription(waypoint->getDescription());
        SetAltitude(waypoint->getAltitude());
        // FIXME Add SetNumber (currently needs a separate call)
        drawIcon();
71 72
        QRectF newSize = boundingRect();

73
        // qDebug() << "WIDTH" << newSize.width() << "<" << oldSize.width();
74 75 76 77 78 79 80 81 82

        // If new size is smaller than old size, update surrounding
        if ((newSize.width() <= oldSize.width()) || (newSize.height() <= oldSize.height()))
        {
            // If the symbol size was reduced, enforce an update of the environment
//            update(oldSize);
            int oldWidth = oldSize.width() + 20;
            int oldHeight = oldSize.height() + 20;
            map->update(this->x()-10, this->y()-10, oldWidth, oldHeight);
83 84
            //// qDebug() << "UPDATING DUE TO SMALLER SIZE";
            //// qDebug() << "X:" << this->x()-1 << "Y:" << this->y()-1 << "WIDTH:" << oldWidth << "HEIGHT:" << oldHeight;
85 86 87 88 89 90 91
        }
        else
        {
            // Symbol size stayed constant or increased, use new size for update
            this->update();
        }
        oldSize = boundingRect();
92 93 94 95 96 97 98 99
    }
}

QRectF Waypoint2DIcon::boundingRect() const
{
    int loiter = 0;
    int acceptance = 0;
    internals::PointLatLng coord = (internals::PointLatLng)Coord();
100 101 102 103 104 105 106 107 108 109

    if (!waypoint.isNull()) {
        if (showAcceptanceRadius && (waypoint->getAction() == (int)MAV_CMD_NAV_WAYPOINT))
        {
            acceptance = map->metersToPixels(waypoint->getAcceptanceRadius(), coord);
        }
        if (((waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_UNLIM) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TIME) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TURNS)))
        {
            loiter = map->metersToPixels(waypoint->getLoiterOrbit(), coord);
        }
110 111 112 113 114 115
    }

    int width = qMax(picture.width()/2, qMax(loiter, acceptance));
    int height = qMax(picture.height()/2, qMax(loiter, acceptance));

    return QRectF(-width,-height,2*width,2*height);
116 117
}

118
void Waypoint2DIcon::drawIcon()
119
{
120 121
    picture.fill(Qt::transparent);
    QPainter painter(&picture);
122 123 124
    painter.setRenderHint(QPainter::Antialiasing, true);
    painter.setRenderHint(QPainter::HighQualityAntialiasing, true);

lm's avatar
lm committed
125 126 127 128 129 130 131 132 133 134 135
    QFont font("Bitstream Vera Sans");
    int fontSize = picture.height()*0.8f;
    font.setPixelSize(fontSize);

    QFontMetrics metrics = QFontMetrics(font);
    int border = qMax(4, metrics.leading());
    painter.setFont(font);
    painter.setRenderHint(QPainter::TextAntialiasing);



136 137 138 139 140
    QPen pen1(Qt::black);
    pen1.setWidth(4);
    QPen pen2(color);
    pen2.setWidth(2);
    painter.setBrush(Qt::NoBrush);
141

lm's avatar
lm committed
142 143
    int penWidth = pen1.width();

144
    // DRAW WAYPOINT
145
    QPointF p(picture.width()/2, picture.height()/2);
146 147 148

    QPolygonF poly(4);
    // Top point
lm's avatar
lm committed
149
    poly.replace(0, QPointF(p.x(), p.y()-picture.height()/2.0f+penWidth/2));
150
    // Right point
lm's avatar
lm committed
151
    poly.replace(1, QPointF(p.x()+picture.width()/2.0f-penWidth/2, p.y()));
152
    // Bottom point
lm's avatar
lm committed
153 154
    poly.replace(2, QPointF(p.x(), p.y() + picture.height()/2.0f-penWidth/2));
    poly.replace(3, QPointF(p.x() - picture.width()/2.0f+penWidth/2, p.y()));
155

156
    int waypointSize = qMin(picture.width(), picture.height());
157
    float rad = (waypointSize/2.0f) * 0.7f * (1/sqrt(2.0f));
158 159 160 161

    // If this is not a waypoint (only the default representation)
    // or it is a waypoint, but not one where direction has no meaning
    // then draw the heading indicator
162
    if (waypoint.isNull() || (waypoint && (
163 164 165
            (waypoint->getAction() != (int)MAV_CMD_NAV_TAKEOFF) &&
            (waypoint->getAction() != (int)MAV_CMD_NAV_LAND) &&
            (waypoint->getAction() != (int)MAV_CMD_NAV_LOITER_UNLIM) &&
166
            (waypoint->getAction() != (int)MAV_CMD_NAV_LOITER_TIME) &&
167 168
            (waypoint->getAction() != (int)MAV_CMD_NAV_LOITER_TURNS) &&
            (waypoint->getAction() != (int)MAV_CMD_NAV_RETURN_TO_LAUNCH)
169 170
            )))
    {
171 172 173 174
        painter.setPen(pen1);
        painter.drawLine(p.x(), p.y(), p.x()+sin(Heading()/180.0f*M_PI) * rad, p.y()-cos(Heading()/180.0f*M_PI) * rad);
        painter.setPen(pen2);
        painter.drawLine(p.x(), p.y(), p.x()+sin(Heading()/180.0f*M_PI) * rad, p.y()-cos(Heading()/180.0f*M_PI) * rad);
175 176
    }

177
    if (((!waypoint.isNull())) && (waypoint->getAction() == (int)MAV_CMD_NAV_TAKEOFF))
178 179
    {
        // Takeoff waypoint
lm's avatar
lm committed
180 181 182 183 184 185 186 187 188 189 190 191
        int width = picture.width()-penWidth;
        int height = picture.height()-penWidth;

        painter.setPen(pen1);
        painter.drawRect(penWidth/2, penWidth/2, width, height);
        painter.setPen(pen2);
        painter.drawRect(penWidth/2, penWidth/2, width, height);

        painter.setPen(pen1);
        painter.drawRect(width*0.3, height*0.3f, width*0.6f, height*0.6f);
        painter.setPen(pen2);
        painter.drawRect(width*0.3, height*0.3f, width*0.6f, height*0.6f);
192
    }
193
    else if (((!waypoint.isNull())) && (waypoint->getAction() == (int)MAV_CMD_NAV_LAND))
194 195
    {
        // Landing waypoint
lm's avatar
lm committed
196 197
        int width = (picture.width())/2-penWidth;
        int height = (picture.height())/2-penWidth;
198 199
        painter.setPen(pen1);
        painter.drawEllipse(p, width, height);
lm's avatar
lm committed
200
        painter.drawLine(p.x()-width/2, p.y()-height/2, 2*width, 2*height);
201
        painter.setPen(pen2);
202
        painter.drawEllipse(p, width, height);
lm's avatar
lm committed
203
        painter.drawLine(p.x()-width/2, p.y()-height/2, 2*width, 2*height);
204
    }
205
    else if (((!waypoint.isNull())) && ((waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_UNLIM) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TIME) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TURNS)))
206 207
    {
        // Loiter waypoint
lm's avatar
lm committed
208 209
        int width = (picture.width()-penWidth)/2;
        int height = (picture.height()-penWidth)/2;
210 211 212 213
        painter.setPen(pen1);
        painter.drawEllipse(p, width, height);
        painter.drawPoint(p);
        painter.setPen(pen2);
214
        painter.drawEllipse(p, width, height);
215 216
        painter.drawPoint(p);
    }
217
    else if (((!waypoint.isNull())) && (waypoint->getAction() == (int)MAV_CMD_NAV_RETURN_TO_LAUNCH))
218 219
    {
        // Return to launch waypoint
lm's avatar
lm committed
220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237
        int width = picture.width()-penWidth;
        int height = picture.height()-penWidth;
        painter.setPen(pen1);
        painter.drawRect(penWidth/2, penWidth/2, width, height);
        painter.setPen(pen2);
        painter.drawRect(penWidth/2, penWidth/2, width, height);

        QString text("R");

        painter.setPen(pen1);
        QRect rect = metrics.boundingRect(0, 0, width - 2*border, height, Qt::AlignLeft | Qt::TextWordWrap, text);
        painter.drawText(width/4, height/6, rect.width(), rect.height(),
                          Qt::AlignCenter | Qt::TextWordWrap, text);
        painter.setPen(pen2);

        font.setPixelSize(fontSize*0.85f);
        painter.setFont(font);
        painter.drawText(width/4, height/6, rect.width(), rect.height(), Qt::AlignCenter | Qt::TextWordWrap, text);
238 239 240 241
    }
    else
    {
        // Navigation waypoint
242 243 244
        painter.setPen(pen1);
        painter.drawPolygon(poly);
        painter.setPen(pen2);
245 246 247 248
        painter.drawPolygon(poly);
    }
}

lm's avatar
lm committed
249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272
void Waypoint2DIcon::SetShowNumber(const bool &value)
{
    shownumber=value;
    if((numberI==0) && value)
    {
        numberI=new QGraphicsSimpleTextItem(this);
        numberIBG=new QGraphicsRectItem(this);
        numberIBG->setBrush(Qt::black);
        numberIBG->setOpacity(0.5);
        numberI->setZValue(3);
        numberI->setPen(QPen(QGC::colorCyan));
        numberI->setPos(5,-picture.height());
        numberIBG->setPos(5,-picture.height());
        numberI->setText(QString::number(number));
        numberIBG->setRect(numberI->boundingRect().adjusted(-2,0,1,0));
    }
    else if (!value && numberI)
    {
        delete numberI;
        delete numberIBG;
    }
    this->update();
}

273 274 275 276
void Waypoint2DIcon::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget)
{
    Q_UNUSED(option);
    Q_UNUSED(widget);
277 278
    QPen pen = painter->pen();
    pen.setWidth(2);
279 280 281
    painter->drawPixmap(-picture.width()/2,-picture.height()/2,picture);
    if (this->isSelected())
    {
282
        pen.setColor(Qt::yellow);
283 284 285
        painter->drawRect(QRectF(-picture.width()/2,-picture.height()/2,picture.width()-1,picture.height()-1));
    }

286 287
    QPen penBlack(Qt::black);
    penBlack.setWidth(4);
288 289
    pen.setColor(color);

290
    if ((!waypoint.isNull()) && (waypoint->getAction() == (int)MAV_CMD_NAV_WAYPOINT) && showAcceptanceRadius)
291 292
    {
        QPen redPen = QPen(pen);
293
        redPen.setColor(Qt::yellow);
lm's avatar
lm committed
294
        redPen.setWidth(1);
295 296
        painter->setPen(redPen);
        const int acceptance = map->metersToPixels(waypoint->getAcceptanceRadius(), Coord());
297 298 299 300 301 302
        if (acceptance > 0) {
            painter->setPen(penBlack);
            painter->drawEllipse(QPointF(0, 0), acceptance, acceptance);
            painter->setPen(redPen);
            painter->drawEllipse(QPointF(0, 0), acceptance, acceptance);
        }
303
    }
304
    if ((!waypoint.isNull()) && ((waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_UNLIM) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TIME) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TURNS)))
305
    {
306 307 308
        QPen penDash(color);
        penDash.setWidth(1);
        //penDash.setStyle(Qt::DotLine);
Lorenz Meier's avatar
Lorenz Meier committed
309 310
        // A negative radius indicates counter-clockwise rotation, but we still want to draw it positive
        const int loiter = map->metersToPixels(fabsf(waypoint->getLoiterOrbit()), Coord());
311 312 313 314 315 316 317
        if (loiter > picture.width()/2)
        {
            painter->setPen(penBlack);
            painter->drawEllipse(QPointF(0, 0), loiter, loiter);
            painter->setPen(penDash);
            painter->drawEllipse(QPointF(0, 0), loiter, loiter);
        }
318
    }
319
}