Added command to select target position for robot - command can be accessed...

Added command to select target position for robot - command can be accessed via a right-click in the 3D View perspective --Lionel

qgroundcontrol.pro

@@ -45,7 +45,7 @@ INCLUDEPATH += . \
     $$BASEDIR/../mavlink/contrib/slugs/include \
     $$BASEDIR/../mavlink/include \
     /usr/include/freetype2
-LIBS += /usr/lib/libftgl.so
+LIBS += /usr/lib/libftgl.so /usr/lib/libglut.so
 
 # ../mavlink/include \
 # MAVLink/include \

src/uas/UAS.cc

@@ -1336,6 +1336,16 @@ void UAS::shutdown()
     }
 }
 
+void UAS::setTargetPosition(float x, float y, float z, float yaw)
+{
+    mavlink_message_t msg;
+    mavlink_msg_position_target_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, x, y, z, yaw);
+
+    // Send message twice to increase chance of reception
+    sendMessage(msg);
+    sendMessage(msg);
+}
+
 /**
  * @return The name of this system as string in human-readable form
  */

src/uas/UAS.h

@@ -179,6 +179,9 @@ public slots:
     /** @brief Shut the system cleanly down. Will shut down any onboard computers **/
     void shutdown();
 
+    /** @brief Set the target position for the robot to navigate to. */
+    void setTargetPosition(float x, float y, float z, float yaw);
+
     void startLowBattAlarm();
     void stopLowBattAlarm();
 

src/uas/UASInterface.h

@@ -177,6 +177,13 @@ public slots:
      * Works only if already landed and will cleanly shut down all onboard computers.
      */
     virtual void shutdown() = 0;
+    /** @brief Set the target position for the robot to navigate to.
+     *  @param x x-coordinate of the target position
+     *  @param y y-coordinate of the target position
+     *  @param z z-coordinate of the target position
+     *  @param yaw heading of the target position
+     */
+    virtual void setTargetPosition(float x, float y, float z, float yaw) = 0;
     /** @brief Request the list of stored waypoints from the robot */
     //virtual void requestWaypoints() = 0;
     /** @brief Clear all existing waypoints on the robot */

src/ui/map3D/CheetahGL.cc

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Generates a display list which renders a Pixhawk Cheetah MAV.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
 /*
 This file was produced by Deep Exploration Plugin: CPP Export filter. 
 

src/ui/map3D/CheetahGL.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Generates a display list which renders a Pixhawk Cheetah MAV.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
 #ifndef CHEETAHGL_H_
 #define CHEETAHGL_H_
 
 #include <GL/gl.h>
 
+/**
+ * @brief Creates a display list which renders a Pixhawk Cheetah MAV.
+ * @param red Red intensity of the MAV model
+ * @param green Green intensity of the MAV model
+ * @param blue Blue intensity of the MAV model
+ *
+ * @return The index of the display list.
+ **/
 GLint generatePixhawkCheetah(float red, float green, float blue);
 
 #endif

src/ui/map3D/CheetahModel.cc

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class CheetahModel.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
 #include "CheetahModel.h"
 
 #include "CheetahGL.h"
@@ -11,14 +42,14 @@ CheetahModel::CheetahModel()
 void
 CheetahModel::init(float red, float green, float blue)
 {
-	cheetah_dl = generatePixhawkCheetah(red, green, blue);
+    cheetah_dl = generatePixhawkCheetah(red, green, blue);
 }
 
 void
 CheetahModel::draw(void)
 {
-	glPushMatrix();
-        glScalef(0.5f, 0.5f, -0.5f);
-	glCallList(cheetah_dl);
-	glPopMatrix();
+    glPushMatrix();
+    glScalef(0.5f, 0.5f, -0.5f);
+    glCallList(cheetah_dl);
+    glPopMatrix();
 }

src/ui/map3D/CheetahModel.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class CheetahModel.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
 #ifndef CHEETAHMODEL_H_
 #define CHEETAHMODEL_H_
 
 #include <GL/gl.h>
 
+/**
+ * @brief Container for display list which renders the Pixhawk Cheetah MAV.
+ **/
 class CheetahModel
 {
 public:
-        CheetahModel();
+    CheetahModel();
+
+    /**
+     * @brief Initializes the display list.
+     * @param red Red intensity of the MAV model
+     * @param green Green intensity of the MAV model
+     * @param blue Blue intensity of the MAV model
+     **/
+    void init(float red, float green, float blue);
 
-        void init(float red, float green, float blue);
-	void draw(void);
+    /**
+     * @brief Executes the display list.
+     **/
+    void draw(void);
 
 private:
-	GLint cheetah_dl;
+    GLint cheetah_dl;
 };
 
 #endif /* CHEETAHMODEL_H_ */

src/ui/map3D/Q3DWidget.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class Q3DWidget.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
 #ifndef Q3DWIDGET_H_
 #define Q3DWIDGET_H_
 
@@ -50,6 +81,9 @@ typedef void (*KeyboardFunc)(char, void *);
 typedef void (*MouseFunc)(Qt::MouseButton, MouseState, int32_t, int32_t, void *);
 typedef void (*MotionFunc)(int32_t, int32_t, void *);
 
+/**
+ * @brief A base 3D widget which executes OpenGL commands.
+ **/
 class Q3DWidget: public QGLWidget
 {
     Q_OBJECT
@@ -67,7 +101,6 @@ public:
                          float maxClipRange);
 
     void setCameraLimit(bool onoff);
-    void setCameraLock(bool onoff);
 
     void set2DCameraParams(float zoomSensitivity,
                            float rotateSensitivity,
@@ -166,7 +199,6 @@ private:
     bool _forceRedraw;
     bool allow2DRotation;
     bool limitCamera;
-    bool lockCamera;
 
     CameraParams cameraParams;
 

src/ui/map3D/QMap3DWidget.cc

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class QMap3DWidget.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
 #include "QMap3DWidget.h"
 
 #include <FTGL/ftgl.h>
+#include <GL/glut.h>
 #include <QCheckBox>
 #include <sys/time.h>
 
@@ -14,14 +46,20 @@ QMap3DWidget::QMap3DWidget(QWidget* parent)
      , lastRedrawTime(0.0)
      , displayGrid(false)
      , displayTrail(false)
-     , lockCamera(false)
+     , lockCamera(true)
+     , updateLastUnlockedPose(true)
+     , displayTarget(false)
 {
     setFocusPolicy(Qt::StrongFocus);
 
     initialize(10, 10, 1000, 900, 10.0f);
     setCameraParams(0.05f, 0.5f, 0.01f, 0.5f, 30.0f, 0.01f, 400.0f);
 
+    int32_t argc = 0;
+    glutInit(&argc, NULL);
+
     setDisplayFunc(display, this);
+    setMouseFunc(mouse, this);
     addTimerFunc(100, timer, this);
 
     buildLayout();
@@ -58,7 +96,6 @@ QMap3DWidget::buildLayout(void)
     QGridLayout* layout = new QGridLayout(this);
     layout->setMargin(0);
     layout->setSpacing(2);
-    //layout->addWidget(mc, 0, 0, 1, 2);
     layout->addWidget(gridCheckBox, 1, 0);
     layout->addWidget(trailCheckBox, 1, 1);
     layout->addWidget(recenterButton, 1, 2);
@@ -106,7 +143,25 @@ QMap3DWidget::displayHandler(void)
         robotYaw = uas->getYaw();
     }
 
-    setCameraLock(lockCamera);
+    if (updateLastUnlockedPose)
+    {
+        lastUnlockedPose.x = robotX;
+        lastUnlockedPose.y = robotY;
+        lastUnlockedPose.z = robotZ;
+
+        camOffset.x = 0.0f;
+        camOffset.y = 0.0f;
+        camOffset.z = 0.0f;
+
+        updateLastUnlockedPose = false;
+    }
+
+    if (!lockCamera)
+    {
+        camOffset.x = robotX - lastUnlockedPose.x;
+        camOffset.y = robotY - lastUnlockedPose.y;
+        camOffset.z = robotZ - lastUnlockedPose.z;
+    }
 
     // turn on smooth lines
     glEnable(GL_LINE_SMOOTH);
@@ -118,6 +173,9 @@ QMap3DWidget::displayHandler(void)
     glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
     glClear(GL_COLOR_BUFFER_BIT);
 
+    glPushMatrix();
+    glTranslatef(camOffset.x, camOffset.y, camOffset.z);
+
     // draw Cheetah model
     drawPlatform(robotRoll, robotPitch, robotYaw);
 
@@ -131,6 +189,13 @@ QMap3DWidget::displayHandler(void)
         drawTrail(robotX, robotY, robotZ);
     }
 
+    if (displayTarget)
+    {
+        drawTarget(robotX, robotY, robotZ);
+    }
+
+    glPopMatrix();
+
     // switch to 2D
     setDisplayMode2D();
 
@@ -143,7 +208,7 @@ QMap3DWidget::displayHandler(void)
     glVertex2f(getWindowWidth(), 0.0f);
     glEnd();
 
-    char buffer[6][255];
+    char buffer[7][255];
 
     sprintf(buffer[0], "x = %.2f", robotX);
     sprintf(buffer[1], "y = %.2f", robotY);
@@ -152,12 +217,17 @@ QMap3DWidget::displayHandler(void)
     sprintf(buffer[4], "p = %.2f", robotPitch);
     sprintf(buffer[5], "y = %.2f", robotYaw);
 
+    std::pair<float,float> mouseWorldCoords =
+            getPositionIn3DMode(getMouseX(), getMouseY());
+    sprintf(buffer[6], "Cursor [%.2f %.2f]",
+            mouseWorldCoords.first + robotX, mouseWorldCoords.second + robotY);
+
     font->FaceSize(10);
     glColor3f(1.0f, 1.0f, 1.0f);
     glPushMatrix();
 
     glTranslatef(0.0f, 30.0f, 0.0f);
-    for (int32_t i = 0; i < 6; ++i)
+    for (int32_t i = 0; i < 7; ++i)
     {
         glTranslatef(60.0f, 0.0f, 0.0f);
         font->Render(buffer[i]);
@@ -165,19 +235,25 @@ QMap3DWidget::displayHandler(void)
     glPopMatrix();
 }
 
-/**
- *
- * @param uas the UAS/MAV to monitor/display with the HUD
- */
-void QMap3DWidget::setActiveUAS(UASInterface* uas)
+void
+QMap3DWidget::mouse(Qt::MouseButton button, MouseState state,
+                    int32_t x, int32_t y, void* clientData)
 {
-    if (this->uas != NULL && this->uas != uas)
+    QMap3DWidget* map3d = reinterpret_cast<QMap3DWidget *>(clientData);
+    map3d->mouseHandler(button, state, x, y);
+}
+
+void
+QMap3DWidget::mouseHandler(Qt::MouseButton button, MouseState state,
+                           int32_t x, int32_t y)
+{
+    if (button == Qt::RightButton && state == MOUSE_STATE_DOWN)
     {
-        // Disconnect any previously connected active MAV
-        //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
+        QMenu menu(this);
+        QAction* targetAction = menu.addAction("Mark as Target");
+        connect(targetAction, SIGNAL(triggered()), this, SLOT(markTarget()));
+        menu.exec(mapToGlobal(QPoint(x, y)));
     }
-
-    this->uas = uas;
 }
 
 void
@@ -210,6 +286,45 @@ QMap3DWidget::getTime(void) const
              static_cast<double>(tv.tv_usec) / 1000000.0;
 }
 
+/**
+ *
+ * @param uas the UAS/MAV to monitor/display with the HUD
+ */
+void QMap3DWidget::setActiveUAS(UASInterface* uas)
+{
+    if (this->uas != NULL && this->uas != uas)
+    {
+        // Disconnect any previously connected active MAV
+        //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
+    }
+
+    this->uas = uas;
+}
+
+void
+QMap3DWidget::markTarget(void)
+{
+    std::pair<float,float> mouseWorldCoords =
+            getPositionIn3DMode(getLastMouseX(), getLastMouseY());
+
+    float robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f;
+    if (uas != NULL)
+    {
+        robotX = uas->getLocalX();
+        robotY = uas->getLocalY();
+        robotZ = uas->getLocalZ();
+    }
+
+    targetPosition.x = mouseWorldCoords.first + robotX;
+    targetPosition.y = mouseWorldCoords.second + robotY;
+    targetPosition.z = robotZ;
+
+    displayTarget = true;
+
+    uas->setTargetPosition(targetPosition.x, targetPosition.y,
+                           targetPosition.z, 0.0f);
+}
+
 void
 QMap3DWidget::showGrid(int32_t state)
 {
@@ -244,6 +359,8 @@ QMap3DWidget::showTrail(int32_t state)
 void
 QMap3DWidget::recenterCamera(void)
 {
+    updateLastUnlockedPose = true;
+
     recenter();
 }
 
@@ -355,3 +472,36 @@ QMap3DWidget::drawTrail(float x, float y, float z)
     }
     glEnd();
 }
+
+void
+QMap3DWidget::drawTarget(float x, float y, float z)
+{
+    static double radius = 0.2;
+    static bool expand = true;
+
+    if (radius < 0.1)
+    {
+        expand = true;
+    }
+    else if (radius > 0.25)
+    {
+        expand = false;
+    }
+
+    glPushMatrix();
+    glTranslatef(targetPosition.x - x, targetPosition.y - y, 0.0f);
+    glColor3f(0.0f, 0.7f, 1.0f);
+    glLineWidth(1.0f);
+    glutWireSphere(radius, 10, 10);
+
+    if (expand)
+    {
+        radius += 0.02;
+    }
+    else
+    {
+        radius -= 0.02;
+    }
+
+    glPopMatrix();
+}

src/ui/map3D/QMap3DWidget.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class QMap3DWidget.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
 #ifndef QMAP3DWIDGET_H
 #define QMAP3DWIDGET_H
 
@@ -7,6 +38,9 @@ class CheetahModel;
 class FTTextureFont;
 class UASInterface;
 
+/**
+ * @brief A 3D View widget which displays vehicle-centric information.
+ **/
 class QMap3DWidget : public Q3DWidget
 {
     Q_OBJECT
@@ -20,6 +54,11 @@ public:
     static void display(void* clientData);
     void displayHandler(void);
 
+    static void mouse(Qt::MouseButton button, MouseState state,
+                      int32_t x, int32_t y, void* clientData);
+    void mouseHandler(Qt::MouseButton button, MouseState state,
+                      int32_t x, int32_t y);
+
     static void timer(void* clientData);
     void timerHandler(void);
 
@@ -27,6 +66,7 @@ public:
 
 public slots:
     void setActiveUAS(UASInterface* uas);
+    void markTarget(void);
 
 private slots:
     void showGrid(int state);
@@ -41,12 +81,12 @@ private:
     void drawPlatform(float roll, float pitch, float yaw);
     void drawGrid(void);
     void drawTrail(float x, float y, float z);
+    void drawTarget(float x, float y, float z);
 
     double lastRedrawTime;
 
     bool displayGrid;
     bool displayTrail;
-    bool lockCamera;
 
     typedef struct
     {
@@ -54,8 +94,17 @@ private:
         float y;
         float z;
     } Pose3D;
+
+    bool lockCamera;
+    Pose3D lastUnlockedPose;
+    bool updateLastUnlockedPose;
+    Pose3D camOffset;
+
     QVarLengthArray<Pose3D, 10000> trail;
 
+    bool displayTarget;
+    Pose3D targetPosition;
+
     QScopedPointer<CheetahModel> cheetahModel;
     QScopedPointer<FTTextureFont> font;
 };