Commit 57207de3 authored by hengli's avatar hengli

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
parent 8b414e7d
......@@ -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 \
......
......@@ -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
*/
......
......@@ -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();
......
......@@ -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 */
......
/*=====================================================================
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.
/*=====================================================================
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
/*=====================================================================
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();
}
/*=====================================================================
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_ */
This diff is collapsed.
/*=====================================================================
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;
......
/*=====================================================================
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();
}
/*=====================================================================
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;
};
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment