Lionel Heng
15 years ago
7 changed files with 486 additions and 295 deletions
@ -0,0 +1,173 @@
@@ -0,0 +1,173 @@
|
||||
/*=====================================================================
|
||||
|
||||
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 WaypointGroupNode. |
||||
* |
||||
* @author Lionel Heng <hengli@student.ethz.ch> |
||||
* |
||||
*/ |
||||
|
||||
#include "WaypointGroupNode.h" |
||||
|
||||
#include <osg/LineWidth> |
||||
#include <osg/PositionAttitudeTransform> |
||||
#include <osg/ShapeDrawable> |
||||
|
||||
#include "Imagery.h" |
||||
|
||||
WaypointGroupNode::WaypointGroupNode() |
||||
{ |
||||
|
||||
} |
||||
|
||||
void |
||||
WaypointGroupNode::init(void) |
||||
{ |
||||
|
||||
} |
||||
|
||||
void |
||||
WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas) |
||||
{ |
||||
if (uas) |
||||
{ |
||||
double robotX, robotY, robotZ; |
||||
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) |
||||
{ |
||||
robotX = uas->getLocalX(); |
||||
robotY = uas->getLocalY(); |
||||
robotZ = uas->getLocalZ(); |
||||
} |
||||
|
||||
if (getNumChildren() > 0) |
||||
{ |
||||
removeChild(0, getNumChildren()); |
||||
} |
||||
|
||||
const QVector<Waypoint *>& list = uas->getWaypointManager().getWaypointList(); |
||||
|
||||
for (int i = 0; i < list.size(); i++) |
||||
{ |
||||
Waypoint* wp = list.at(i); |
||||
|
||||
double wpX, wpY, wpZ; |
||||
getPosition(wp, wpX, wpY, wpZ); |
||||
|
||||
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->getOrbit(), |
||||
fabs(wpZ)); |
||||
|
||||
sd->setShape(cylinder); |
||||
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); |
||||
|
||||
char wpLabel[10]; |
||||
sprintf(wpLabel, "wp%d", i); |
||||
geode->setName(wpLabel); |
||||
|
||||
if (i < list.size() - 1) |
||||
{ |
||||
double nextWpX, nextWpY, nextWpZ; |
||||
getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ); |
||||
|
||||
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::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); |
||||
|
||||
geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2)); |
||||
|
||||
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); |
||||
|
||||
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) |
||||
{ |
||||
double latitude = wp->getY(); |
||||
double longitude = wp->getX(); |
||||
double altitude = wp->getZ(); |
||||
|
||||
QString utmZone; |
||||
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); |
||||
z = -altitude; |
||||
} |
||||
else if (wp->getFrame() == MAV_FRAME_LOCAL) |
||||
{ |
||||
x = wp->getX(); |
||||
y = wp->getY(); |
||||
z = wp->getZ(); |
||||
} |
||||
} |
||||
@ -0,0 +1,51 @@
@@ -0,0 +1,51 @@
|
||||
/*=====================================================================
|
||||
|
||||
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 WaypointGroupNode. |
||||
* |
||||
* @author Lionel Heng <hengli@student.ethz.ch> |
||||
* |
||||
*/ |
||||
|
||||
#ifndef WAYPOINTGROUPNODE_H |
||||
#define WAYPOINTGROUPNODE_H |
||||
|
||||
#include <osg/Group> |
||||
|
||||
#include "UASInterface.h" |
||||
|
||||
class WaypointGroupNode : public osg::Group |
||||
{ |
||||
public: |
||||
WaypointGroupNode(); |
||||
|
||||
void init(void); |
||||
void update(MAV_FRAME frame, UASInterface* uas); |
||||
|
||||
private: |
||||
void getPosition(Waypoint* wp, double& x, double& y, double& z); |
||||
}; |
||||
|
||||
#endif // WAYPOINTGROUPNODE_H
|
||||
Loading…
Reference in new issue