Enabled real-time mouse-based selection of target and waypoint headings.

src/ui/map3D/Pixhawk3DWidget.cc

@@ -114,7 +114,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
 
     buildLayout();
 
-    updateHUD(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, "132N");
+    updateHUD(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, "32N");
 
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
             this, SLOT(setActiveUAS(UASInterface*)));
@@ -292,7 +292,8 @@ Pixhawk3DWidget::selectTarget(void)
         double altitude = uas->getAltitude();
 
         std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(),
+                                    altitude);
 
         target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0);
     }
@@ -301,14 +302,14 @@ Pixhawk3DWidget::selectTarget(void)
         double z = uas->getLocalZ();
 
         std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z);
 
         target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0);
     }
 
     enableTarget = true;
 
-    mode = SELECT_TARGET_YAW_MODE;
+    mode = SELECT_TARGET_HEADING_MODE;
 }
 
 void
@@ -316,7 +317,8 @@ Pixhawk3DWidget::setTarget(void)
 {
     selectTargetHeading();
 
-    uas->setTargetPosition(target.x(), target.y(), 0.0, target.z());
+    uas->setTargetPosition(target.x(), target.y(), 0.0,
+                           osg::RadiansToDegrees(target.z()));
 }
 
 void
@@ -338,7 +340,8 @@ Pixhawk3DWidget::insertWaypoint(void)
         Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
 
         std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(),
+                                    altitude);
 
         Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
                          latitude, longitude);
@@ -350,7 +353,7 @@ Pixhawk3DWidget::insertWaypoint(void)
         double z = uas->getLocalZ();
 
         std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z);
 
         wp = new Waypoint(0, cursorWorldCoords.first,
                           cursorWorldCoords.second, z, 0.0, 0.25);
@@ -361,17 +364,20 @@ Pixhawk3DWidget::insertWaypoint(void)
         wp->setFrame(frame);
         uas->getWaypointManager()->addWaypointEditable(wp);
     }
-}
 
-void
-Pixhawk3DWidget::moveWaypoint(void)
-{
-    mode = MOVE_WAYPOINT_MODE;
+    selectedWpIndex = wp->getId();
+    mode = MOVE_WAYPOINT_HEADING_MODE;
 }
 
 void
-Pixhawk3DWidget::setWaypoint(void)
+Pixhawk3DWidget::moveWaypointPosition(void)
 {
+    if (mode != MOVE_WAYPOINT_POSITION_MODE)
+    {
+        mode = MOVE_WAYPOINT_POSITION_MODE;
+        return;
+    }
+
     if (!uas)
     {
         return;
@@ -393,12 +399,11 @@ Pixhawk3DWidget::setWaypoint(void)
         std::pair<double,double> cursorWorldCoords =
             getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
 
-        Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
-                         latitude, longitude);
+        Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second,
+                         utmZone, latitude, longitude);
 
         waypoint->setX(longitude);
         waypoint->setY(latitude);
-        waypoint->setZ(altitude);
     }
     else if (frame == MAV_FRAME_LOCAL_NED)
     {
@@ -409,10 +414,52 @@ Pixhawk3DWidget::setWaypoint(void)
 
         waypoint->setX(cursorWorldCoords.first);
         waypoint->setY(cursorWorldCoords.second);
-        waypoint->setZ(z);
     }
 }
 
+void
+Pixhawk3DWidget::moveWaypointHeading(void)
+{
+    if (mode != MOVE_WAYPOINT_HEADING_MODE)
+    {
+        mode = MOVE_WAYPOINT_HEADING_MODE;
+        return;
+    }
+
+    if (!uas)
+    {
+        return;
+    }
+
+    const QVector<Waypoint *> waypoints =
+        uas->getWaypointManager()->getWaypointEditableList();
+    Waypoint* waypoint = waypoints.at(selectedWpIndex);
+
+    double x = 0.0, y = 0.0, z = 0.0;
+
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double latitude = waypoint->getY();
+        double longitude = waypoint->getX();
+        z = -waypoint->getZ();
+        QString utmZone;
+        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        z = uas->getLocalZ();
+    }
+
+    std::pair<double,double> cursorWorldCoords =
+        getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+
+    double yaw = atan2(cursorWorldCoords.second - waypoint->getY(),
+                       cursorWorldCoords.first - waypoint->getX());
+    yaw = osg::RadiansToDegrees(yaw);
+
+    waypoint->setYaw(yaw);
+}
+
 void
 Pixhawk3DWidget::deleteWaypoint(void)
 {
@@ -738,27 +785,23 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
 {
     if (event->button() == Qt::LeftButton)
     {
-        if (mode == MOVE_WAYPOINT_MODE)
+        if (mode == SELECT_TARGET_HEADING_MODE)
         {
-            setWaypoint();
-            mode = DEFAULT_MODE;
-
-            return;
+            setTarget();
         }
 
-        if (mode == SELECT_TARGET_YAW_MODE)
+        if (mode != DEFAULT_MODE)
         {
-            setTarget();
             mode = DEFAULT_MODE;
-
-            return;
         }
 
         if (event->modifiers() == Qt::ShiftModifier)
         {
-            selectedWpIndex = findWaypoint(event->x(), event->y());
+            selectedWpIndex = findWaypoint(event->pos());
             if (selectedWpIndex == -1)
             {
+                cachedMousePos = event->pos();
+
                 showInsertWaypointMenu(event->globalPos());
             }
             else
@@ -776,10 +819,18 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
 void
 Pixhawk3DWidget::mouseMoveEvent(QMouseEvent* event)
 {
-    if (mode == SELECT_TARGET_YAW_MODE)
+    if (mode == SELECT_TARGET_HEADING_MODE)
     {
         selectTargetHeading();
     }
+    if (mode == MOVE_WAYPOINT_POSITION_MODE)
+    {
+        moveWaypointPosition();
+    }
+    if (mode == MOVE_WAYPOINT_HEADING_MODE)
+    {
+        moveWaypointHeading();
+    }
 
     Q3DWidget::mouseMoveEvent(event);
 }
@@ -986,7 +1037,7 @@ Pixhawk3DWidget::createTarget(void)
 
     pat->setPosition(osg::Vec3d(0.0, 0.0, 0.0));
 
-    osg::ref_ptr<osg::Cone> cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.1f, 0.4f);
+    osg::ref_ptr<osg::Cone> cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.2f, 0.6f);
     osg::ref_ptr<osg::ShapeDrawable> coneDrawable = new osg::ShapeDrawable(cone);
     coneDrawable->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f));
     coneDrawable->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
@@ -1285,14 +1336,9 @@ Pixhawk3DWidget::updateTarget(double robotX, double robotY)
 
     osg::Geode* geode = dynamic_cast<osg::Geode*>(pat->getChild(0));
     osg::ShapeDrawable* sd = dynamic_cast<osg::ShapeDrawable*>(geode->getDrawable(0));
-    if (mode == SELECT_TARGET_YAW_MODE)
-    {
-        sd->setColor(osg::Vec4f(1.0f, 0.8f, 0.0f, 1.0f));
-    }
-    else
-    {
-        sd->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f));
-    }
+
+
+    sd->setColor(osg::Vec4f(1.0f, 0.8f, 0.0f, 1.0f));
 }
 
 float colormap_jet[128][3] = {
@@ -1526,13 +1572,14 @@ Pixhawk3DWidget::updateObstacles(void)
 #endif
 
 int
-Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
+Pixhawk3DWidget::findWaypoint(const QPoint& mousePos)
 {
     if (getSceneData())
     {
         osgUtil::LineSegmentIntersector::Intersections intersections;
 
-        if (computeIntersections(mouseX, height() - mouseY, intersections))
+        if (computeIntersections(mousePos.x(), height() - mousePos.y(),
+                                 intersections))
         {
             for (osgUtil::LineSegmentIntersector::Intersections::iterator
                     it = intersections.begin(); it != intersections.end(); it++)
@@ -1596,7 +1643,10 @@ Pixhawk3DWidget::showEditWaypointMenu(const QPoint &cursorPos)
 
     QString text;
     text = QString("Move waypoint %1").arg(QString::number(selectedWpIndex));
-    menu.addAction(text, this, SLOT(moveWaypoint()));
+    menu.addAction(text, this, SLOT(moveWaypointPosition()));
+
+    text = QString("Change heading of waypoint %1").arg(QString::number(selectedWpIndex));
+    menu.addAction(text, this, SLOT(moveWaypointHeading()));
 
     text = QString("Change altitude of waypoint %1").arg(QString::number(selectedWpIndex));
     menu.addAction(text, this, SLOT(setWaypointAltitude()));

src/ui/map3D/Pixhawk3DWidget.h

@@ -74,8 +74,8 @@ private slots:
     void selectTarget(void);
     void setTarget(void);
     void insertWaypoint(void);
-    void moveWaypoint(void);
-    void setWaypoint(void);
+    void moveWaypointPosition(void);
+    void moveWaypointHeading(void);
     void deleteWaypoint(void);
     void setWaypointAltitude(void);
     void clearAllWaypoints(void);
@@ -123,15 +123,16 @@ private:
     void updateObstacles(void);
 #endif
 
-    int findWaypoint(int mouseX, int mouseY);
+    int findWaypoint(const QPoint& mousePos);
     bool findTarget(int mouseX, int mouseY);
     void showInsertWaypointMenu(const QPoint& cursorPos);
     void showEditWaypointMenu(const QPoint& cursorPos);
 
     enum Mode {
         DEFAULT_MODE,
-        MOVE_WAYPOINT_MODE,
-        SELECT_TARGET_YAW_MODE
+        MOVE_WAYPOINT_POSITION_MODE,
+        MOVE_WAYPOINT_HEADING_MODE,
+        SELECT_TARGET_HEADING_MODE
     };
     Mode mode;
     int selectedWpIndex;
@@ -175,6 +176,7 @@ private:
 
     MAV_FRAME frame;
     osg::Vec3d target;
+    QPoint cachedMousePos;
     double lastRobotX, lastRobotY, lastRobotZ;
 };
 

src/ui/map3D/WaypointGroupNode.cc

@@ -89,8 +89,42 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
 
         double wpX, wpY, wpZ;
         getPosition(wp, wpX, wpY, wpZ);
+        double wpYaw = osg::DegreesToRadians(wp->getYaw());
 
+        osg::ref_ptr<osg::Group> group = new osg::Group;
+
+        // cone indicates waypoint orientation
         osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
+        double coneRadius = wp->getAcceptanceRadius() / 2.0;
+        osg::ref_ptr<osg::Cone> cone =
+            new osg::Cone(osg::Vec3d(wpZ, 0.0, 0.0),
+                          coneRadius, wp->getAcceptanceRadius() * 2.0);
+
+        sd->setShape(cone);
+        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+
+        if (wp->getCurrent())
+        {
+            sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
+        }
+        else
+        {
+            sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
+        }
+
+        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+        geode->addDrawable(sd);
+
+        osg::ref_ptr<osg::PositionAttitudeTransform> pat =
+            new osg::PositionAttitudeTransform;
+        pat->addChild(geode);
+        pat->setAttitude(osg::Quat(wpYaw - M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
+                                   M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f),
+                                   0.0, osg::Vec3d(0.0f, 0.0f, 1.0f)));
+        group->addChild(pat);
+
+        // cylinder indicates waypoint position
+        sd = new osg::ShapeDrawable;
         osg::ref_ptr<osg::Cylinder> cylinder =
             new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
                               wp->getAcceptanceRadius(),
@@ -108,12 +142,13 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
             sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
         }
 
-        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+        geode = new osg::Geode;
         geode->addDrawable(sd);
+        group->addChild(geode);
 
         char wpLabel[10];
         sprintf(wpLabel, "wp%d", i);
-        geode->setName(wpLabel);
+        group->setName(wpLabel);
 
         if (i < list.size() - 1)
         {
@@ -143,15 +178,13 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
             geode->addDrawable(geometry);
         }
 
-        osg::ref_ptr<osg::PositionAttitudeTransform> pat =
-            new osg::PositionAttitudeTransform;
-
+        pat = new osg::PositionAttitudeTransform;
         pat->setPosition(osg::Vec3d(wpY - robotY,
                                     wpX - robotX,
                                     robotZ));
 
         addChild(pat);
-        pat->addChild(geode);
+        pat->addChild(group);
     }
 }