WaypointGroupNode.cc 6.77 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
/*=====================================================================

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.
 *
28
 *   @author Lionel Heng <hengli@inf.ethz.ch>
29 30 31 32 33 34 35 36 37 38
 *
 */

#include "WaypointGroupNode.h"

#include <osg/LineWidth>
#include <osg/PositionAttitudeTransform>
#include <osg/ShapeDrawable>

#include "Imagery.h"
39
#include "UASManager.h"
40

41 42
WaypointGroupNode::WaypointGroupNode(const QColor& color)
 : mColor(color)
43 44 45 46 47 48 49 50 51 52 53
{

}

void
WaypointGroupNode::init(void)
{

}

void
54
WaypointGroupNode::update(UASInterface* uas, MAV_FRAME frame)
55
{
56 57 58 59
    if (!uas)
    {
        return;
    }
60

61 62 63 64 65 66 67 68
    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();
69

70 71 72 73
        QString utmZone;
        Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
        robotZ = -altitude;
    }
74 75 76 77 78 79 80 81 82 83
    else if (frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)
    {
        double latitude = uas->getLatitude();
        double longitude = uas->getLongitude();
        double altitude = uas->getAltitude() + UASManager::instance()->getHomeAltitude();

        QString utmZone;
        Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
        robotZ = -altitude;
    }
84 85 86 87 88 89
    else if (frame == MAV_FRAME_LOCAL_NED)
    {
        robotX = uas->getLocalX();
        robotY = uas->getLocalY();
        robotZ = uas->getLocalZ();
    }
90

91 92 93 94
    if (getNumChildren() > 0)
    {
        removeChild(0, getNumChildren());
    }
95

96
    const QList<Waypoint *>& list = uas->getWaypointManager()->getWaypointEditableList();
97

98 99 100
    for (int i = 0; i < list.size(); i++)
    {
        Waypoint* wp = list.at(i);
101

102 103
        double wpX, wpY, wpZ;
        getPosition(wp, wpX, wpY, wpZ);
104
        double wpYaw = osg::DegreesToRadians(wp->getYaw());
105

106 107 108
        osg::ref_ptr<osg::Group> group = new osg::Group;

        // cone indicates waypoint orientation
109
        osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
110 111 112 113 114 115 116 117 118 119
        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())
        {
120
            sd->setColor(osg::Vec4(1.0f, 0.8f, 0.0f, 1.0f));
121 122 123
        }
        else
        {
124 125
            sd->setColor(osg::Vec4(mColor.redF(), mColor.greenF(),
                                   mColor.blueF(), 0.5f));
126 127 128 129 130 131 132 133 134 135 136 137 138 139 140
        }

        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;
141 142 143 144
        osg::ref_ptr<osg::Cylinder> cylinder =
            new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
                              wp->getAcceptanceRadius(),
                              fabs(wpZ));
145

146 147
        sd->setShape(cylinder);
        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
148

149 150
        sd->setColor(osg::Vec4(mColor.redF(), mColor.greenF(),
                               mColor.blueF(), 0.5f));
151

152
        geode = new osg::Geode;
153
        geode->addDrawable(sd);
154
        group->addChild(geode);
155

156 157
        char wpLabel[10];
        sprintf(wpLabel, "wp%d", i);
158
        group->setName(wpLabel);
159

160 161 162 163
        if (i < list.size() - 1)
        {
            double nextWpX, nextWpY, nextWpZ;
            getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ);
164

165 166 167 168 169 170 171
            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);
172

173
            osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
174 175
            colors->push_back(osg::Vec4(mColor.redF(), mColor.greenF(),
                                        mColor.blueF(), 0.5f));
176 177
            geometry->setColorArray(colors);
            geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
178

179
            geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2));
180

181 182 183 184
            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);
185

186
            geode->addDrawable(geometry);
187
        }
188

189
        pat = new osg::PositionAttitudeTransform;
190 191 192 193 194
        pat->setPosition(osg::Vec3d(wpY - robotY,
                                    wpX - robotX,
                                    robotZ));

        addChild(pat);
195
        pat->addChild(group);
196 197 198 199 200 201
    }
}

void
WaypointGroupNode::getPosition(Waypoint* wp, double &x, double &y, double &z)
{
202 203
    if (wp->getFrame() == MAV_FRAME_GLOBAL)
    {
204 205 206 207 208 209 210
        double latitude = wp->getY();
        double longitude = wp->getX();
        double altitude = wp->getZ();

        QString utmZone;
        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
        z = -altitude;
211 212 213
    }
    else if (wp->getFrame() == MAV_FRAME_LOCAL_NED)
    {
214 215 216 217 218
        x = wp->getX();
        y = wp->getY();
        z = wp->getZ();
    }
}