Waypoint2DIcon.cc 11.7 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 9 10 11 12 13
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),
    waypoint(NULL),
    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) {
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 100 101 102
    }
}

QRectF Waypoint2DIcon::boundingRect() const
{
    int loiter = 0;
    int acceptance = 0;
    internals::PointLatLng coord = (internals::PointLatLng)Coord();
    if (waypoint && showAcceptanceRadius && (waypoint->getAction() == (int)MAV_CMD_NAV_WAYPOINT))
    {
        acceptance = map->metersToPixels(waypoint->getAcceptanceRadius(), coord);
103
    }
104 105 106 107 108 109 110 111 112
    if (waypoint && ((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);
    }

    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);
113 114
}

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

lm's avatar
lm committed
122 123 124 125 126 127 128 129 130 131 132
    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);



133 134 135 136 137
    QPen pen1(Qt::black);
    pen1.setWidth(4);
    QPen pen2(color);
    pen2.setWidth(2);
    painter.setBrush(Qt::NoBrush);
138

lm's avatar
lm committed
139 140
    int penWidth = pen1.width();

141
    // DRAW WAYPOINT
142
    QPointF p(picture.width()/2, picture.height()/2);
143 144 145

    QPolygonF poly(4);
    // Top point
lm's avatar
lm committed
146
    poly.replace(0, QPointF(p.x(), p.y()-picture.height()/2.0f+penWidth/2));
147
    // Right point
lm's avatar
lm committed
148
    poly.replace(1, QPointF(p.x()+picture.width()/2.0f-penWidth/2, p.y()));
149
    // Bottom point
lm's avatar
lm committed
150 151
    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()));
152

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

    // 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
    if (!waypoint || (waypoint && (
            (waypoint->getAction() != (int)MAV_CMD_NAV_TAKEOFF) &&
            (waypoint->getAction() != (int)MAV_CMD_NAV_LAND) &&
            (waypoint->getAction() != (int)MAV_CMD_NAV_LOITER_UNLIM) &&
163
            (waypoint->getAction() != (int)MAV_CMD_NAV_LOITER_TIME) &&
164 165
            (waypoint->getAction() != (int)MAV_CMD_NAV_LOITER_TURNS) &&
            (waypoint->getAction() != (int)MAV_CMD_NAV_RETURN_TO_LAUNCH)
166 167
            )))
    {
168 169 170 171
        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);
172 173 174 175 176
    }

    if ((waypoint != NULL) && (waypoint->getAction() == (int)MAV_CMD_NAV_TAKEOFF))
    {
        // Takeoff waypoint
lm's avatar
lm committed
177 178 179 180 181 182 183 184 185 186 187 188
        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);
189 190 191 192
    }
    else if ((waypoint != NULL) && (waypoint->getAction() == (int)MAV_CMD_NAV_LAND))
    {
        // Landing waypoint
lm's avatar
lm committed
193 194
        int width = (picture.width())/2-penWidth;
        int height = (picture.height())/2-penWidth;
195 196
        painter.setPen(pen1);
        painter.drawEllipse(p, width, height);
lm's avatar
lm committed
197
        painter.drawLine(p.x()-width/2, p.y()-height/2, 2*width, 2*height);
198
        painter.setPen(pen2);
199
        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 202 203 204
    }
    else if ((waypoint != NULL) && ((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 waypoint
lm's avatar
lm committed
205 206
        int width = (picture.width()-penWidth)/2;
        int height = (picture.height()-penWidth)/2;
207 208 209 210
        painter.setPen(pen1);
        painter.drawEllipse(p, width, height);
        painter.drawPoint(p);
        painter.setPen(pen2);
211
        painter.drawEllipse(p, width, height);
212 213 214 215 216
        painter.drawPoint(p);
    }
    else if ((waypoint != NULL) && (waypoint->getAction() == (int)MAV_CMD_NAV_RETURN_TO_LAUNCH))
    {
        // Return to launch waypoint
lm's avatar
lm committed
217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234
        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);
235 236 237 238
    }
    else
    {
        // Navigation waypoint
239 240 241
        painter.setPen(pen1);
        painter.drawPolygon(poly);
        painter.setPen(pen2);
242 243 244 245
        painter.drawPolygon(poly);
    }
}

lm's avatar
lm committed
246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269
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();
}

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

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

    if ((waypoint) && (waypoint->getAction() == (int)MAV_CMD_NAV_WAYPOINT) && showAcceptanceRadius)
    {
        QPen redPen = QPen(pen);
290
        redPen.setColor(Qt::yellow);
lm's avatar
lm committed
291
        redPen.setWidth(1);
292 293
        painter->setPen(redPen);
        const int acceptance = map->metersToPixels(waypoint->getAcceptanceRadius(), Coord());
294 295 296 297
        painter->setPen(penBlack);
        painter->drawEllipse(QPointF(0, 0), acceptance, acceptance);
        painter->setPen(redPen);
        painter->drawEllipse(QPointF(0, 0), acceptance, acceptance);
298 299 300
    }
    if ((waypoint) && ((waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_UNLIM) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TIME) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TURNS)))
    {
301 302 303
        QPen penDash(color);
        penDash.setWidth(1);
        //penDash.setStyle(Qt::DotLine);
304
        const int loiter = map->metersToPixels(waypoint->getLoiterOrbit(), Coord());
305 306 307 308 309 310 311
        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);
        }
312
    }
313
}