Brought 2d map widget back to life. Not perfect yet, but with support for most important features

src/uas/UASWaypointManager.cc

@@ -441,18 +441,24 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
 }
 
 
-void UASWaypointManager::globalAddWaypoint(Waypoint *wp)
-{
-    // FIXME Will be removed
-    Q_UNUSED(wp);
-}
-
-int UASWaypointManager::globalRemoveWaypoint(quint16 seq)
-{
-    // FIXME Will be removed
-    Q_UNUSED(seq);
-    return 0;
-}
+//void UASWaypointManager::globalAddWaypoint(Waypoint *wp)
+//{
+//    // FIXME Will be removed
+//    Q_UNUSED(wp);
+//}
+
+//int UASWaypointManager::globalRemoveWaypoint(quint16 seq)
+//{
+//    // FIXME Will be removed
+//    Q_UNUSED(seq);
+//    return 0;
+//}
+
+//void UASWaypointManager::waypointUpdated(Waypoint*)
+//{
+//    // FIXME Add rate limiter
+//    emit waypointListChanged(uas.getUASID());
+//}
 
 void UASWaypointManager::clearWaypointList()
 {

src/uas/UASWaypointManager.h

@@ -101,12 +101,12 @@ public:
 
     UAS& getUAS() { return this->uas; }                         ///< Returns the owning UAS
 
-    /** @name Global waypoint list operations */
-    /*@{*/
-    const QVector<Waypoint *> &getGlobalWaypointList(void) { return waypoints; }  ///< Returns a const reference to the global waypoint list.
-    void globalAddWaypoint(Waypoint *wp);                        ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
-    int globalRemoveWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
-    /*@}*/
+//    /** @name Global waypoint list operations */
+//    /*@{*/
+//    const QVector<Waypoint *> &getGlobalWaypointList(void) { return waypoints; }  ///< Returns a const reference to the global waypoint list.
+//    void globalAddWaypoint(Waypoint *wp);                        ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
+//    int globalRemoveWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
+//    /*@}*/
 
 private:
     /** @name Message send functions */

src/ui/MapWidget.cc

@@ -486,7 +486,13 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp)
                     wpIcons.at(wp->getId())->setCoordinate(coordinate);
                     //mc->layer("Waypoints")->addGeometry(wpIcons.at(wp->getId()));
                     // Then waypoint line coordinate
-                    Point* linesegment = waypointPath->points().at(mav->getWaypointManager()->getWaypointList().indexOf(wp));
+                    int linesegmentId = mav->getWaypointManager()->getWaypointList().indexOf(wp);
+                    qDebug() << "SEGMENT" << linesegmentId;
+                    Point* linesegment = NULL;
+                    if (waypointPath->points().size() > linesegmentId)
+                    {
+                        linesegment = waypointPath->points().at(linesegmentId);
+                    }
 
                     if (linesegment)
                     {
@@ -761,7 +767,10 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
     //pointpen->setWidth(3);
     //points.append(new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen));
 
-    MAV2DIcon* p;
+    qmapcontrol::Point* p;
+    QPointF coordinate;
+    coordinate.setX(lat);
+    coordinate.setY(lon);
 
     if (!uasIcons.contains(uas->getUASID()))
     {
@@ -769,40 +778,42 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
         QColor uasColor = uas->getColor();
 
         // Icon
-        QPen* pointpen = new QPen(uasColor);
-        qDebug() << uas->getUASName();
-        p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, pointpen);
+        //QPen* pointpen = new QPen(uasColor);
+        qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__;
+        //p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, mavPens.value(uas->getUASID()));
+        p = new Waypoint2DIcon(lat, lon, 20, QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
         uasIcons.insert(uas->getUASID(), p);
-        tracks->addGeometry(p);
+        mc->layer("Waypoints")->addGeometry(p);
 
         // Line
         // A QPen also can use transparency
 
         QList<qmapcontrol::Point*> points;
-        points.append(new qmapcontrol::Point(lat, lon, ""));
+        points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
         QPen* linepen = new QPen(uasColor.darker());
         linepen->setWidth(2);
 
         // Create tracking line string
-        qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, uas->getUASName(), linepen);
+        qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen);
         uasTrails.insert(uas->getUASID(), ls);
 
         // Add the LineString to the layer
-        mc->layer("Tracking")->addGeometry(ls);
+        mc->layer("Waypoints")->addGeometry(ls);
     }
     else
     {
-        p = dynamic_cast<MAV2DIcon*>(uasIcons.value(uas->getUASID()));
-        if (p)
-        {
+//        p = dynamic_cast<MAV2DIcon*>(uasIcons.value(uas->getUASID()));
+//        if (p)
+//        {
+         p = uasIcons.value(uas->getUASID());
             p->setCoordinate(QPointF(lat, lon));
-            p->setYaw(uas->getYaw());
-        }
+            //p->setYaw(uas->getYaw());
+//        }
         // Extend trail
-        uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(lat, lon, ""));
+        uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
     }
 
-    mc->updateRequest(p->boundingBox().toRect());
+mc->updateRequest(p->boundingBox().toRect());
 
     //mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect());
 

src/ui/map/MAV2DIcon.cc

@@ -46,50 +46,50 @@ void MAV2DIcon::setYaw(float yaw)
 void MAV2DIcon::drawIcon(QPen* pen)
 {
 
-//    mypixmap = new QPixmap(radius+1, radius+1);
-//    mypixmap->fill(Qt::transparent);
-//    QPainter painter(mypixmap);
+    mypixmap = new QPixmap(radius+1, radius+1);
+    mypixmap->fill(Qt::transparent);
+    QPainter painter(mypixmap);
 
-//    // DRAW WAYPOINT
-//    QPointF p(radius/2, radius/2);
+    // DRAW WAYPOINT
+    QPointF p(radius/2, radius/2);
 
-//    float waypointSize = radius;
-//    QPolygonF poly(4);
-//    // Top point
-//    poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
-//    // Right point
-//    poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
-//    // Bottom point
-//    poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
-//    poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
+    float waypointSize = radius;
+    QPolygonF poly(4);
+    // Top point
+    poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
+    // Right point
+    poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
+    // Bottom point
+    poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
+    poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
 
-////    // Select color based on if this is the current waypoint
-////    if (list.at(i)->getCurrent())
-////    {
-////        color = QGC::colorCyan;//uas->getColor();
-////        pen.setWidthF(refLineWidthToPen(0.8f));
-////    }
-////    else
-////    {
-////        color = uas->getColor();
-////        pen.setWidthF(refLineWidthToPen(0.4f));
-////    }
-
-//    //pen.setColor(color);
-//    if (pen)
+//    // Select color based on if this is the current waypoint
+//    if (list.at(i)->getCurrent())
 //    {
-//        pen->setWidthF(2);
-//        painter.setPen(*pen);
+//        color = QGC::colorCyan;//uas->getColor();
+//        pen.setWidthF(refLineWidthToPen(0.8f));
 //    }
 //    else
 //    {
-//        QPen pen2(Qt::red);
-//        pen2.setWidth(2);
-//        painter.setPen(pen2);
+//        color = uas->getColor();
+//        pen.setWidthF(refLineWidthToPen(0.4f));
 //    }
-//    painter.setBrush(Qt::NoBrush);
 
-//    float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
-//    painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad);
-//    painter.drawPolygon(poly);
+    //pen.setColor(color);
+    if (pen)
+    {
+        pen->setWidthF(2);
+        painter.setPen(*pen);
+    }
+    else
+    {
+        QPen pen2(Qt::red);
+        pen2.setWidth(2);
+        painter.setPen(pen2);
+    }
+    painter.setBrush(Qt::NoBrush);
+
+    float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
+    painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad);
+    painter.drawPolygon(poly);
 }