Commit a3ff7d45 authored by hengli's avatar hengli

Added visualization of obstacles. Fixed relative font path issue in 3D widgets...

Added visualization of obstacles. Fixed relative font path issue in 3D widgets by using QFont within osgText::Font instead of using static font filename.
parent e28f03cc
......@@ -234,10 +234,9 @@ message("Compiling for linux 32")
-losgGA \
-losgDB \
-losgText \
-losgQt \
-lOpenThreads
#-losgQt \
DEFINES += QGC_OSG_ENABLED
}
......@@ -318,10 +317,9 @@ linux-g++-64 {
-losgGA \
-losgDB \
-losgText \
-losgQt \
-lOpenThreads
# -losgQt \
DEFINES += QGC_OSG_ENABLED
}
......
......@@ -387,7 +387,8 @@ contains(DEPENDENCIES_PRESENT, protobuf):contains(MAVLINK_CONF, pixhawk) {
message("Including headers for Protocol Buffers")
# Enable only if protobuf is available
HEADERS += ../mavlink/include/pixhawk/pixhawk.pb.h
HEADERS += ../mavlink/include/pixhawk/pixhawk.pb.h \
src/ui/map3D/ObstacleGroupNode.h
}
contains(DEPENDENCIES_PRESENT, libfreenect) {
message("Including headers for libfreenect")
......@@ -527,7 +528,8 @@ contains(DEPENDENCIES_PRESENT, protobuf):contains(MAVLINK_CONF, pixhawk) {
message("Including sources for Protocol Buffers")
# Enable only if protobuf is available
SOURCES += ../mavlink/src/pixhawk/pixhawk.pb.cc
SOURCES += ../mavlink/src/pixhawk/pixhawk.pb.cc \
src/ui/map3D/ObstacleGroupNode.cc
}
contains(DEPENDENCIES_PRESENT, libfreenect) {
message("Including sources for libfreenect")
......
......@@ -961,6 +961,11 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
rgbdImage.CopyFrom(*message);
emit rgbdImageChanged(this);
}
else if (message->GetTypeName() == obstacleList.GetTypeName())
{
obstacleList.CopyFrom(*message);
emit obstacleListChanged(this);
}
}
#endif
......
......@@ -136,6 +136,10 @@ public:
px::RGBDImage getRGBDImage() const {
return rgbdImage;
}
px::ObstacleList getObstacleList() const {
return obstacleList;
}
#endif
friend class UASWaypointManager;
......@@ -223,6 +227,7 @@ protected: //COMMENTS FOR TEST UNIT
#ifdef QGC_PROTOBUF_ENABLED
px::PointCloudXYZRGB pointCloud;
px::RGBDImage rgbdImage;
px::ObstacleList obstacleList;
#endif
QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters
......@@ -556,10 +561,14 @@ signals:
void imageStarted(quint64 timestamp);
/** @brief A new camera image has arrived */
void imageReady(UASInterface* uas);
#ifdef QGC_PROTOBUF_ENABLED
/** @brief Point cloud data has been changed */
void pointCloudChanged(UASInterface* uas);
/** @brief RGBD image data has been changed */
void rgbdImageChanged(UASInterface* uas);
/** @brief Obstacle list data has been changed */
void obstacleListChanged(UASInterface* uas);
#endif
/** @brief HIL controls have changed */
void hilControlsChanged(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode);
......
......@@ -97,6 +97,7 @@ public:
#ifdef QGC_PROTOBUF_ENABLED
virtual px::PointCloudXYZRGB getPointCloud() const = 0;
virtual px::RGBDImage getRGBDImage() const = 0;
virtual px::ObstacleList getObstacleList() const = 0;
#endif
virtual bool isArmed() const = 0;
......
/*=====================================================================
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 ObstacleGroupNode.
*
* @author Lionel Heng <hengli@inf.ethz.ch>
*
*/
#include "ObstacleGroupNode.h"
#include <osg/PositionAttitudeTransform>
#include <osg/ShapeDrawable>
#include "Imagery.h"
ObstacleGroupNode::ObstacleGroupNode()
{
}
void
ObstacleGroupNode::init(void)
{
}
void
ObstacleGroupNode::update(MAV_FRAME frame, UASInterface *uas)
{
if (!uas || frame == MAV_FRAME_GLOBAL)
{
return;
}
double robotX = uas->getLocalX();
double robotY = uas->getLocalY();
double robotZ = uas->getLocalZ();
if (getNumChildren() > 0)
{
removeChild(0, getNumChildren());
}
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
px::ObstacleList obstacleList = uas->getObstacleList();
for (int i = 0; i < obstacleList.obstacles_size(); ++i)
{
const px::Obstacle& obs = obstacleList.obstacles(i);
osg::Vec3d obsPos(obs.y() - robotY, obs.x() - robotX, robotZ - obs.z());
osg::ref_ptr<osg::Box> box =
new osg::Box(obsPos, obs.width(), obs.width(), obs.height());
osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable(box);
sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
sd->setColor(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f));
geode->addDrawable(sd);
}
addChild(geode);
}
/*=====================================================================
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 ObstacleGroupNode.
*
* @author Lionel Heng <hengli@inf.ethz.ch>
*
*/
#ifndef OBSTACLEGROUPNODE_H
#define OBSTACLEGROUPNODE_H
#include <osg/Group>
#include "UASInterface.h"
class ObstacleGroupNode : public osg::Group
{
public:
ObstacleGroupNode();
void init(void);
void update(MAV_FRAME frame, UASInterface* uas);
};
#endif // OBSTACLEGROUPNODE_H
This diff is collapsed.
......@@ -25,7 +25,7 @@
* @file
* @brief Definition of the class Pixhawk3DWidget.
*
* @author Lionel Heng <hengli@student.ethz.ch>
* @author Lionel Heng <hengli@inf.ethz.ch>
*
*/
......@@ -38,6 +38,9 @@
#include "Imagery.h"
#include "ImageWindowGeode.h"
#include "WaypointGroupNode.h"
#ifdef QGC_PROTOBUF_ENABLED
#include "ObstacleGroupNode.h"
#endif
#include "Q3DWidget.h"
......@@ -113,6 +116,7 @@ private:
void updateTarget(double robotX, double robotY);
#ifdef QGC_PROTOBUF_ENABLED
void updateRGBD(double robotX, double robotY, double robotZ);
void updateObstacles(void);
#endif
int findWaypoint(int mouseX, int mouseY);
......@@ -133,6 +137,7 @@ private:
bool displayWaypoints;
bool displayRGBD2D;
bool displayRGBD3D;
bool displayObstacleList;
bool enableRGBDColor;
bool enableTarget;
......@@ -157,6 +162,9 @@ private:
osg::ref_ptr<WaypointGroupNode> waypointGroupNode;
osg::ref_ptr<osg::Node> targetNode;
osg::ref_ptr<osg::Geode> rgbd3DNode;
#ifdef QGC_PROTOBUF_ENABLED
osg::ref_ptr<ObstacleGroupNode> obstacleGroupNode;
#endif
QVector< osg::ref_ptr<osg::Node> > vehicleModels;
......
......@@ -35,6 +35,7 @@ This file is part of the QGROUNDCONTROL project
#include <osg/Geometry>
#include <osg/LineWidth>
#include <osg/MatrixTransform>
#include <osgQt/QFontImplementation>
#ifdef Q_OS_MACX
#include <Carbon/Carbon.h>
#endif
......@@ -57,6 +58,10 @@ Q3DWidget::Q3DWidget(QWidget* parent)
cameraParams.minClipRange = 1.0f;
cameraParams.maxClipRange = 10000.0f;
osg::ref_ptr<osgText::Font::FontImplementation> fontImpl;
fontImpl = new osgQt::QFontImplementation(QFont(":/general/vera.ttf"));
font = new osgText::Font(fontImpl);
osgGW = new osgViewer::GraphicsWindowEmbedded(0, 0, width(), height());
setThreadingModel(osgViewer::Viewer::SingleThreaded);
......
......@@ -38,6 +38,7 @@ This file is part of the QGROUNDCONTROL project
#include <osg/LineSegment>
#include <osg/PositionAttitudeTransform>
#include <osgGA/TrackballManipulator>
#include <osgText/Font>
#include <osgViewer/Viewer>
#include "GCManipulator.h"
......@@ -259,6 +260,8 @@ protected:
CameraParams cameraParams; /**< Struct representing camera parameters. */
float fps;
osg::ref_ptr<osgText::Font> font;
};
#endif // Q3DWIDGET_H
......@@ -25,7 +25,7 @@ This file is part of the QGROUNDCONTROL project
* @file
* @brief Definition of the class WaypointGroupNode.
*
* @author Lionel Heng <hengli@student.ethz.ch>
* @author Lionel Heng <hengli@inf.ethz.ch>
*
*/
......@@ -51,102 +51,115 @@ WaypointGroupNode::init(void)
void
WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
{
if (uas) {
double robotX = 0.0;
double robotY = 0.0;
double robotZ = 0.0;
if (frame == MAV_FRAME_GLOBAL) {
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
robotZ = -altitude;
} else if (frame == MAV_FRAME_LOCAL_NED) {
robotX = uas->getLocalX();
robotY = uas->getLocalY();
robotZ = uas->getLocalZ();
}
if (getNumChildren() > 0) {
removeChild(0, getNumChildren());
}
if (!uas)
{
return;
}
const QVector<Waypoint *>& list = uas->getWaypointManager()->getWaypointEditableList();
double robotX = 0.0;
double robotY = 0.0;
double robotZ = 0.0;
if (frame == MAV_FRAME_GLOBAL)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
for (int i = 0; i < list.size(); i++) {
Waypoint* wp = list.at(i);
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
robotZ = -altitude;
}
else if (frame == MAV_FRAME_LOCAL_NED)
{
robotX = uas->getLocalX();
robotY = uas->getLocalY();
robotZ = uas->getLocalZ();
}
double wpX, wpY, wpZ;
getPosition(wp, wpX, wpY, wpZ);
if (getNumChildren() > 0)
{
removeChild(0, getNumChildren());
}
osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
osg::ref_ptr<osg::Cylinder> cylinder =
new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
wp->getAcceptanceRadius(),
fabs(wpZ));
const QVector<Waypoint *>& list = uas->getWaypointManager()->getWaypointEditableList();
sd->setShape(cylinder);
sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
for (int i = 0; i < list.size(); i++)
{
Waypoint* wp = list.at(i);
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));
}
double wpX, wpY, wpZ;
getPosition(wp, wpX, wpY, wpZ);
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
geode->addDrawable(sd);
osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
osg::ref_ptr<osg::Cylinder> cylinder =
new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
wp->getAcceptanceRadius(),
fabs(wpZ));
char wpLabel[10];
sprintf(wpLabel, "wp%d", i);
geode->setName(wpLabel);
sd->setShape(cylinder);
sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
if (i < list.size() - 1) {
double nextWpX, nextWpY, nextWpZ;
getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ);
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::Geometry> geometry = new osg::Geometry;
osg::ref_ptr<osg::Vec3dArray> vertices = new osg::Vec3dArray;
vertices->push_back(osg::Vec3d(0.0, 0.0, -wpZ));
vertices->push_back(osg::Vec3d(nextWpY - wpY,
nextWpX - wpX,
-nextWpZ));
geometry->setVertexArray(vertices);
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
geode->addDrawable(sd);
osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
colors->push_back(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
geometry->setColorArray(colors);
geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
char wpLabel[10];
sprintf(wpLabel, "wp%d", i);
geode->setName(wpLabel);
geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2));
if (i < list.size() - 1)
{
double nextWpX, nextWpY, nextWpZ;
getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ);
osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
linewidth->setWidth(2.0f);
geometry->getOrCreateStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
geometry->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
osg::ref_ptr<osg::Vec3dArray> vertices = new osg::Vec3dArray;
vertices->push_back(osg::Vec3d(0.0, 0.0, -wpZ));
vertices->push_back(osg::Vec3d(nextWpY - wpY,
nextWpX - wpX,
-nextWpZ));
geometry->setVertexArray(vertices);
geode->addDrawable(geometry);
}
osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
colors->push_back(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
geometry->setColorArray(colors);
geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
osg::ref_ptr<osg::PositionAttitudeTransform> pat =
new osg::PositionAttitudeTransform;
geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2));
pat->setPosition(osg::Vec3d(wpY - robotY,
wpX - robotX,
robotZ));
osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
linewidth->setWidth(2.0f);
geometry->getOrCreateStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
geometry->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
addChild(pat);
pat->addChild(geode);
geode->addDrawable(geometry);
}
osg::ref_ptr<osg::PositionAttitudeTransform> pat =
new osg::PositionAttitudeTransform;
pat->setPosition(osg::Vec3d(wpY - robotY,
wpX - robotX,
robotZ));
addChild(pat);
pat->addChild(geode);
}
}
void
WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z)
{
if (wp->getFrame() == MAV_FRAME_GLOBAL) {
if (wp->getFrame() == MAV_FRAME_GLOBAL)
{
double latitude = wp->getY();
double longitude = wp->getX();
double altitude = wp->getZ();
......@@ -154,7 +167,9 @@ WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z)
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
z = -altitude;
} else if (wp->getFrame() == MAV_FRAME_LOCAL_NED) {
}
else if (wp->getFrame() == MAV_FRAME_LOCAL_NED)
{
x = wp->getX();
y = wp->getY();
z = wp->getZ();
......
......@@ -25,7 +25,7 @@ This file is part of the QGROUNDCONTROL project
* @file
* @brief Definition of the class WaypointGroupNode.
*
* @author Lionel Heng <hengli@student.ethz.ch>
* @author Lionel Heng <hengli@inf.ethz.ch>
*
*/
......
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