From 0f85d37d26961bce271219a617d2712cedd19f0d Mon Sep 17 00:00:00 2001 From: pixhawk Date: Thu, 20 Jan 2011 17:33:13 +0100 Subject: [PATCH] Brought 2d map widget back to life. Not perfect yet, but with support for most important features --- src/uas/UASWaypointManager.cc | 30 ++++++++------ src/uas/UASWaypointManager.h | 12 +++--- src/ui/MapWidget.cc | 43 ++++++++++++-------- src/ui/map/MAV2DIcon.cc | 74 +++++++++++++++++------------------ 4 files changed, 88 insertions(+), 71 deletions(-) diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index c444c5991..e56823651 100644 --- a/src/uas/UASWaypointManager.cc +++ b/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() { diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 1522adffd..74593d99e 100644 --- a/src/uas/UASWaypointManager.h +++ b/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 &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 &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 */ diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index 683c5ec2e..be1fc01f7 100644 --- a/src/ui/MapWidget.cc +++ b/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 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(uasIcons.value(uas->getUASID())); - if (p) - { +// p = dynamic_cast(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()); diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc index dc0e187cf..8920f76d9 100644 --- a/src/ui/map/MAV2DIcon.cc +++ b/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); } -- 2.22.0