Skip to content

Q
qgroundcontrol
Commit 5a0a9297 authored by Bryant Mairs's avatar Bryant Mairs
Browse files
Options

Merge remote-tracking branch 'remotes/upstream/master'

parents 6dbf6dab 1592061b
Show whitespace changes
Inline Side-by-side
Showing with 1803 additions and 350 deletions
qgroundcontrol.pri
View file @ 5a0a9297
......@@ -246,11 +246,11 @@ message("Compiling for linux 32")
-losgGA \
-losgDB \
-losgText \
-losgQt \
-lOpenThreads
#-losgQt \
DEFINES += QGC_OSG_ENABLED
DEFINES += QGC_OSG_QT_ENABLED
}
exists(/usr/local/include/google/protobuf) {
......@@ -330,11 +330,11 @@ linux-g++-64 {
-losgGA \
-losgDB \
-losgText \
-losgQt \
-lOpenThreads
# -losgQt \
DEFINES += QGC_OSG_ENABLED
DEFINES += QGC_OSG_QT_ENABLED
}
exists(/usr/local/include/google/protobuf) {
......
qgroundcontrol.pro
View file @ 5a0a9297
......@@ -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")
......
src/uas/UAS.cc
View file @ 5a0a9297
......@@ -993,6 +993,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
......@@ -1906,25 +1911,6 @@ void UAS::executeCommand(MAV_CMD command)
sendMessage(msg);
}
void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component)
{
mavlink_message_t msg;
mavlink_command_long_t cmd;
cmd.command = (uint8_t)command;
cmd.confirmation = confirmation;
cmd.param1 = param1;
cmd.param2 = param2;
cmd.param3 = param3;
cmd.param4 = param4;
cmd.param5 = 0.0f;
cmd.param6 = 0.0f;
cmd.param7 = 0.0f;
cmd.target_system = uasId;
cmd.target_component = component;
mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
sendMessage(msg);
}
void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component)
{
mavlink_message_t msg;
......@@ -2191,7 +2177,7 @@ void UAS::shutdown()
void UAS::setTargetPosition(float x, float y, float z, float yaw)
{
mavlink_message_t msg;
mavlink_msg_set_local_position_setpoint_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_FRAME_LOCAL_NED, x, y, z, yaw);
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 2, 3, 0, yaw, x, y, z);
sendMessage(msg);
}
......
src/uas/UAS.h
View file @ 5a0a9297
......@@ -142,6 +142,10 @@ public:
px::RGBDImage getRGBDImage() const {
return rgbdImage;
}
px::ObstacleList getObstacleList() const {
return obstacleList;
}
#endif
friend class UASWaypointManager;
......@@ -230,6 +234,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
......@@ -406,8 +411,6 @@ public slots:
void setUASName(const QString& name);
/** @brief Executes a command **/
void executeCommand(MAV_CMD command);
/** @brief Executes a command **/
void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component);
/** @brief Executes a command with 7 params */
void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component);
/** @brief Set the current battery type and voltages */
......@@ -563,10 +566,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);
......
src/uas/UASInterface.h
View file @ 5a0a9297
......@@ -46,7 +46,7 @@ This file is part of the QGROUNDCONTROL project
#ifdef QGC_PROTOBUF_ENABLED
#include <tr1/memory>
#include <pixhawk.pb.h>
#include <pixhawk/pixhawk.pb.h>
#endif
/**
......@@ -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;
......@@ -214,7 +215,7 @@ public slots:
/** @brief Execute command immediately **/
virtual void executeCommand(MAV_CMD command) = 0;
/** @brief Executes a command **/
virtual void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component) = 0;
virtual void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) = 0;
/** @brief Selects the airframe */
virtual void setAirframe(int airframe) = 0;
......
src/ui/HUD.cc
View file @ 5a0a9297
......@@ -1431,10 +1431,13 @@ void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int s
void HUD::copyImage()
{
if (isVisible())
{
qDebug() << "HUD::copyImage()";
UAS* u = dynamic_cast<UAS*>(this->uas);
if (u)
{
this->glImage = QGLWidget::convertToGLFormat(u->getImage());
}
}
}
src/ui/MAVLinkDecoder.cc
View file @ 5a0a9297
......@@ -27,8 +27,9 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
messageFilter.insert(MAVLINK_MSG_ID_MISSION_ITEM, false);
messageFilter.insert(MAVLINK_MSG_ID_MISSION_COUNT, false);
messageFilter.insert(MAVLINK_MSG_ID_MISSION_ACK, false);
#ifdef MAVLINK_ENABLED_PIXHAWK
messageFilter.insert(MAVLINK_MSG_ID_DATA_STREAM, false);
#ifdef MAVLINK_ENABLED_PIXHAWK
messageFilter.insert(MAVLINK_MSG_ID_ENCAPSULATED_DATA, false);
messageFilter.insert(MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, false);
#endif
messageFilter.insert(MAVLINK_MSG_ID_EXTENDED_MESSAGE, false);
......
src/ui/QGCFirmwareUpdate.h
View file @ 5a0a9297
......@@ -17,9 +17,23 @@ public:
protected:
void changeEvent(QEvent *e);
void showEvent(QShowEvent* event)
{
QWidget::showEvent(event);
emit visibilityChanged(true);
}
void hideEvent(QHideEvent* event)
{
QWidget::hideEvent(event);
emit visibilityChanged(false);
}
private:
Ui::QGCFirmwareUpdate *ui;
signals:
void visibilityChanged(bool visible);
};
#endif // QGCFIRMWAREUPDATE_H
src/ui/QGCToolBar.cc
View file @ 5a0a9297
......@@ -392,6 +392,6 @@ void QGCToolBar::receiveTextMessage(int uasid, int componentid, int severity, QS
QGCToolBar::~QGCToolBar()
{
delete toggleLoggingAction;
delete logReplayAction;
if (toggleLoggingAction) toggleLoggingAction->deleteLater();
if (logReplayAction) logReplayAction->deleteLater();
}
src/ui/designer/QGCCommandButton.cc
View file @ 5a0a9297
......@@ -79,7 +79,6 @@ QGCCommandButton::~QGCCommandButton()
void QGCCommandButton::sendCommand()
{
if (QGCToolWidgetItem::uas) {
// FIXME
int index = ui->editCommandComboBox->itemData(ui->editCommandComboBox->currentIndex()).toInt();
MAV_CMD command = static_cast<MAV_CMD>(index);
int confirm = (ui->editConfirmationCheckBox->isChecked()) ? 1 : 0;
......@@ -87,9 +86,12 @@ void QGCCommandButton::sendCommand()
float param2 = ui->editParam2SpinBox->value();
float param3 = ui->editParam3SpinBox->value();
float param4 = ui->editParam4SpinBox->value();
float param5 = ui->editParam5SpinBox->value();
float param6 = ui->editParam6SpinBox->value();
float param7 = ui->editParam7SpinBox->value();
int component = ui->editComponentSpinBox->value();
QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, component);
QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, param5, param6, param7, component);
qDebug() << __FILE__ << __LINE__ << "SENDING COMMAND" << index;
} else {
qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING";
......
src/ui/map/QGCMapWidget.cc
View file @ 5a0a9297
......@@ -284,6 +284,68 @@ void QGCMapWidget::updateGlobalPosition()
}
}
void QGCMapWidget::updateLocalPosition()
{
QList<UASInterface*> systems = UASManager::instance()->getUASList();
foreach (UASInterface* system, systems)
{
// Get reference to graphic UAV item
mapcontrol::UAVItem* uav = GetUAV(system->getUASID());
// Check if reference is valid, else create a new one
if (uav == NULL)
{
MAV2DIcon* newUAV = new MAV2DIcon(map, this, system);
AddUAV(system->getUASID(), newUAV);
uav = newUAV;
uav->SetTrailTime(1);
uav->SetTrailDistance(5);
uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed);
}
// Set new lat/lon position of UAV icon
double latitude = UASManager::instance()->getHomeLatitude();
double longitude = UASManager::instance()->getHomeLongitude();
double altitude = UASManager::instance()->getHomeAltitude();
internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
uav->SetUAVPos(pos_lat_lon, system->getAltitude());
// Follow status
if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
// Convert from radians to degrees and apply
uav->SetUAVHeading((system->getYaw()/M_PI)*180.0f);
}
}
void QGCMapWidget::updateLocalPositionEstimates()
{
QList<UASInterface*> systems = UASManager::instance()->getUASList();
foreach (UASInterface* system, systems)
{
// Get reference to graphic UAV item
mapcontrol::UAVItem* uav = GetUAV(system->getUASID());
// Check if reference is valid, else create a new one
if (uav == NULL)
{
MAV2DIcon* newUAV = new MAV2DIcon(map, this, system);
AddUAV(system->getUASID(), newUAV);
uav = newUAV;
uav->SetTrailTime(1);
uav->SetTrailDistance(5);
uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed);
}
// Set new lat/lon position of UAV icon
double latitude = UASManager::instance()->getHomeLatitude();
double longitude = UASManager::instance()->getHomeLongitude();
double altitude = UASManager::instance()->getHomeAltitude();
internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
uav->SetUAVPos(pos_lat_lon, system->getAltitude());
// Follow status
if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
// Convert from radians to degrees and apply
uav->SetUAVHeading((system->getYaw()/M_PI)*180.0f);
}
}
void QGCMapWidget::updateSystemSpecs(int uas)
{
......
src/ui/map/QGCMapWidget.h
View file @ 5a0a9297
......@@ -46,6 +46,10 @@ public slots:
void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec);
/** @brief Update the global position of all systems */
void updateGlobalPosition();
/** @brief Update the local position and draw it converted to GPS reference */
void updateLocalPosition();
/** @brief Update the local position estimates (individual sensors) and draw it converted to GPS reference */
void updateLocalPositionEstimates();
/** @brief Update the type, size, etc. of this system */
void updateSystemSpecs(int uas);
/** @brief Change current system in focus / editing */
......
src/ui/map3D/HUDScaleGeode.cc
View file @ 5a0a9297
......@@ -40,7 +40,7 @@ HUDScaleGeode::HUDScaleGeode()
}
void
HUDScaleGeode::init(void)
HUDScaleGeode::init(osg::ref_ptr<osgText::Font>& font)
{
osg::ref_ptr<osg::Vec2Array> outlineVertices(new osg::Vec2Array);
outlineVertices->push_back(osg::Vec2(20.0f, 50.0f));
......@@ -96,7 +96,7 @@ HUDScaleGeode::init(void)
text = new osgText::Text;
text->setCharacterSize(11);
text->setFont("images/Vera.ttf");
text->setFont(font);
text->setAxisAlignment(osgText::Text::SCREEN);
text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
text->setPosition(osg::Vec3(40.0f, 45.0f, -1.5f));
......
src/ui/map3D/HUDScaleGeode.h
View file @ 5a0a9297
......@@ -41,7 +41,7 @@ class HUDScaleGeode : public osg::Geode
public:
HUDScaleGeode();
void init(void);
void init(osg::ref_ptr<osgText::Font>& font);
void update(int windowHeight, float cameraFov, float cameraDistance,
bool darkBackground);
......
src/ui/map3D/ImageWindowGeode.cc
View file @ 5a0a9297
......@@ -31,11 +31,17 @@
#include "ImageWindowGeode.h"
ImageWindowGeode::ImageWindowGeode(const QString& caption,
const osg::Vec4& backgroundColor,
osg::ref_ptr<osg::Image>& image)
ImageWindowGeode::ImageWindowGeode()
: border(5)
{
}
void
ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor,
osg::ref_ptr<osg::Image>& image,
osg::ref_ptr<osgText::Font>& font)
{
// image
osg::ref_ptr<osg::Geometry> imageGeometry = new osg::Geometry;
imageVertices = new osg::Vec3Array(4);
......@@ -82,7 +88,7 @@ ImageWindowGeode::ImageWindowGeode(const QString& caption,
text = new osgText::Text;
text->setText(caption.toStdString().c_str());
text->setCharacterSize(11);
text->setFont("images/Vera.ttf");
text->setFont(font);
text->setAxisAlignment(osgText::Text::SCREEN);
text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
......
src/ui/map3D/ImageWindowGeode.h
View file @ 5a0a9297
......@@ -40,8 +40,10 @@
class ImageWindowGeode : public osg::Geode
{
public:
ImageWindowGeode(const QString& caption, const osg::Vec4& backgroundColor,
osg::ref_ptr<osg::Image>& image);
ImageWindowGeode();
void init(const QString& caption, const osg::Vec4& backgroundColor,
osg::ref_ptr<osg::Image>& image,
osg::ref_ptr<osgText::Font>& font);
void setAttributes(int x, int y, int width, int height);
......
src/ui/map3D/ObstacleGroupNode.cc 0 → 100644
View file @ 5a0a9297
/*=====================================================================
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);
}
src/ui/map3D/ObstacleGroupNode.h 0 → 100644
View file @ 5a0a9297
/*=====================================================================
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
src/ui/map3D/Pixhawk3DWidget.cc
View file @ 5a0a9297
......@@ -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>
*
*/
......@@ -46,7 +46,7 @@
#ifdef QGC_PROTOBUF_ENABLED
#include <tr1/memory>
#include <pixhawk.pb.h>
#include <pixhawk/pixhawk.pb.h>
#endif
Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
......@@ -60,6 +60,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
, displayWaypoints(true)
, displayRGBD2D(false)
, displayRGBD3D(true)
, displayObstacleList(true)
, enableRGBDColor(false)
, enableTarget(false)
, followCamera(true)
......@@ -98,7 +99,13 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
// generate RGBD model
rgbd3DNode = createRGBD3D();
egocentricMap->addChild(rgbd3DNode);
rollingMap->addChild(rgbd3DNode);
#ifdef QGC_PROTOBUF_ENABLED
obstacleGroupNode = new ObstacleGroupNode;
obstacleGroupNode->init();
rollingMap->addChild(obstacleGroupNode);
#endif
setupHUD();
......@@ -107,6 +114,8 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
buildLayout();
updateHUD(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, "32N");
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
this, SLOT(setActiveUAS(UASInterface*)));
}
......@@ -123,7 +132,8 @@ Pixhawk3DWidget::~Pixhawk3DWidget()
void
Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
{
if (this->uas != NULL && this->uas != 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)));
}
......@@ -134,9 +144,12 @@ Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
void
Pixhawk3DWidget::selectFrame(QString text)
{
if (text.compare("Global") == 0) {
if (text.compare("Global") == 0)
{
frame = MAV_FRAME_GLOBAL;
} else if (text.compare("Local") == 0) {
}
else if (text.compare("Local") == 0)
{
frame = MAV_FRAME_LOCAL_NED;
}
......@@ -148,9 +161,12 @@ Pixhawk3DWidget::selectFrame(QString text)
void
Pixhawk3DWidget::showGrid(int32_t state)
{
if (state == Qt::Checked) {
if (state == Qt::Checked)
{
displayGrid = true;
} else {
}
else
{
displayGrid = false;
}
}
......@@ -158,13 +174,17 @@ Pixhawk3DWidget::showGrid(int32_t state)
void
Pixhawk3DWidget::showTrail(int32_t state)
{
if (state == Qt::Checked) {
if (!displayTrail) {
if (state == Qt::Checked)
{
if (!displayTrail)
{
trail.clear();
}
displayTrail = true;
} else {
}
else
{
displayTrail = false;
}
}
......@@ -172,9 +192,12 @@ Pixhawk3DWidget::showTrail(int32_t state)
void
Pixhawk3DWidget::showWaypoints(int state)
{
if (state == Qt::Checked) {
if (state == Qt::Checked)
{
displayWaypoints = true;
} else {
}
else
{
displayWaypoints = false;
}
}
......@@ -184,9 +207,12 @@ Pixhawk3DWidget::selectMapSource(int index)
{
mapNode->setImageryType(static_cast<Imagery::ImageryType>(index));
if (mapNode->getImageryType() == Imagery::BLANK_MAP) {
if (mapNode->getImageryType() == Imagery::BLANK_MAP)
{
displayImagery = false;
} else {
}
else
{
displayImagery = true;
}
}
......@@ -211,45 +237,101 @@ Pixhawk3DWidget::recenter(void)
void
Pixhawk3DWidget::toggleFollowCamera(int32_t state)
{
if (state == Qt::Checked) {
if (state == Qt::Checked)
{
followCamera = true;
} else {
}
else
{
followCamera = false;
}
}
void
Pixhawk3DWidget::selectTarget(void)
Pixhawk3DWidget::selectTargetHeading(void)
{
if (uas) {
if (frame == MAV_FRAME_GLOBAL) {
if (!uas)
{
return;
}
osg::Vec2d p;
if (frame == MAV_FRAME_GLOBAL)
{
double altitude = uas->getAltitude();
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
target.set(cursorWorldCoords.first, cursorWorldCoords.second);
} else if (frame == MAV_FRAME_LOCAL_NED) {
p.set(cursorWorldCoords.first, cursorWorldCoords.second);
}
else if (frame == MAV_FRAME_LOCAL_NED)
{
double z = uas->getLocalZ();
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
target.set(cursorWorldCoords.first, cursorWorldCoords.second);
p.set(cursorWorldCoords.first, cursorWorldCoords.second);
}
uas->setTargetPosition(target.x(), target.y(), 0.0, 0.0);
target.z() = atan2(p.y() - target.y(), p.x() - target.x());
}
enableTarget = true;
void
Pixhawk3DWidget::selectTarget(void)
{
if (!uas)
{
return;
}
if (frame == MAV_FRAME_GLOBAL)
{
double altitude = uas->getAltitude();
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(),
altitude);
target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0);
}
else if (frame == MAV_FRAME_LOCAL_NED)
{
double z = uas->getLocalZ();
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z);
target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0);
}
enableTarget = true;
mode = SELECT_TARGET_HEADING_MODE;
}
void
Pixhawk3DWidget::setTarget(void)
{
selectTargetHeading();
uas->setTargetPosition(target.x(), target.y(), 0.0,
osg::RadiansToDegrees(target.z()));
}
void
Pixhawk3DWidget::insertWaypoint(void)
{
if (uas) {
if (!uas)
{
return;
}
Waypoint* wp = NULL;
if (frame == MAV_FRAME_GLOBAL) {
if (frame == MAV_FRAME_GLOBAL)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
......@@ -258,44 +340,55 @@ Pixhawk3DWidget::insertWaypoint(void)
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(),
altitude);
Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
latitude, longitude);
wp = new Waypoint(0, longitude, latitude, altitude);
} else if (frame == MAV_FRAME_LOCAL_NED) {
wp = new Waypoint(0, longitude, latitude, altitude, 0.0, 0.25);
}
else if (frame == MAV_FRAME_LOCAL_NED)
{
double z = uas->getLocalZ();
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z);
wp = new Waypoint(0, cursorWorldCoords.first,
cursorWorldCoords.second, z);
cursorWorldCoords.second, z, 0.0, 0.25);
}
if (wp) {
if (wp)
{
wp->setFrame(frame);
uas->getWaypointManager()->addWaypointEditable(wp);
}
}
}
void
Pixhawk3DWidget::moveWaypoint(void)
{
mode = MOVE_WAYPOINT_MODE;
selectedWpIndex = wp->getId();
mode = MOVE_WAYPOINT_HEADING_MODE;
}
void
Pixhawk3DWidget::setWaypoint(void)
Pixhawk3DWidget::moveWaypointPosition(void)
{
if (uas) {
if (mode != MOVE_WAYPOINT_POSITION_MODE)
{
mode = MOVE_WAYPOINT_POSITION_MODE;
return;
}
if (!uas)
{
return;
}
const QVector<Waypoint *> waypoints =
uas->getWaypointManager()->getWaypointEditableList();
Waypoint* waypoint = waypoints.at(selectedWpIndex);
if (frame == MAV_FRAME_GLOBAL) {
if (frame == MAV_FRAME_GLOBAL)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
......@@ -306,13 +399,14 @@ Pixhawk3DWidget::setWaypoint(void)
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
latitude, longitude);
Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second,
utmZone, latitude, longitude);
waypoint->setX(longitude);
waypoint->setY(latitude);
waypoint->setZ(altitude);
} else if (frame == MAV_FRAME_LOCAL_NED) {
}
else if (frame == MAV_FRAME_LOCAL_NED)
{
double z = uas->getLocalZ();
std::pair<double,double> cursorWorldCoords =
......@@ -320,15 +414,57 @@ Pixhawk3DWidget::setWaypoint(void)
waypoint->setX(cursorWorldCoords.first);
waypoint->setY(cursorWorldCoords.second);
waypoint->setZ(z);
}
}
void
Pixhawk3DWidget::moveWaypointHeading(void)
{
if (mode != MOVE_WAYPOINT_HEADING_MODE)
{
mode = MOVE_WAYPOINT_HEADING_MODE;
return;
}
if (!uas)
{
return;
}
const QVector<Waypoint *> waypoints =
uas->getWaypointManager()->getWaypointEditableList();
Waypoint* waypoint = waypoints.at(selectedWpIndex);
double x = 0.0, y = 0.0, z = 0.0;
if (frame == MAV_FRAME_GLOBAL)
{
double latitude = waypoint->getY();
double longitude = waypoint->getX();
z = -waypoint->getZ();
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
}
else if (frame == MAV_FRAME_LOCAL_NED)
{
z = uas->getLocalZ();
}
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
double yaw = atan2(cursorWorldCoords.second - waypoint->getY(),
cursorWorldCoords.first - waypoint->getX());
yaw = osg::RadiansToDegrees(yaw);
waypoint->setYaw(yaw);
}
void
Pixhawk3DWidget::deleteWaypoint(void)
{
if (uas) {
if (uas)
{
uas->getWaypointManager()->removeWaypoint(selectedWpIndex);
}
}
......@@ -336,26 +472,34 @@ Pixhawk3DWidget::deleteWaypoint(void)
void
Pixhawk3DWidget::setWaypointAltitude(void)
{
if (uas) {
if (!uas)
{
return;
}
bool ok;
const QVector<Waypoint *> waypoints =
uas->getWaypointManager()->getWaypointEditableList();
Waypoint* waypoint = waypoints.at(selectedWpIndex);
double altitude = waypoint->getZ();
if (frame == MAV_FRAME_LOCAL_NED) {
if (frame == MAV_FRAME_LOCAL_NED)
{
altitude = -altitude;
}
double newAltitude =
QInputDialog::getDouble(this, tr("Set altitude of waypoint %1").arg(selectedWpIndex),
tr("Altitude (m):"), waypoint->getZ(), -1000.0, 1000.0, 1, &ok);
if (ok) {
if (frame == MAV_FRAME_GLOBAL) {
if (ok)
{
if (frame == MAV_FRAME_GLOBAL)
{
waypoint->setZ(newAltitude);
} else if (frame == MAV_FRAME_LOCAL_NED) {
waypoint->setZ(-newAltitude);
}
else if (frame == MAV_FRAME_LOCAL_NED)
{
waypoint->setZ(-newAltitude);
}
}
}
......@@ -363,10 +507,12 @@ Pixhawk3DWidget::setWaypointAltitude(void)
void
Pixhawk3DWidget::clearAllWaypoints(void)
{
if (uas) {
if (uas)
{
const QVector<Waypoint *> waypoints =
uas->getWaypointManager()->getWaypointEditableList();
for (int i = waypoints.size() - 1; i >= 0; --i) {
for (int i = waypoints.size() - 1; i >= 0; --i)
{
uas->getWaypointManager()->removeWaypoint(i);
}
}
......@@ -393,13 +539,17 @@ Pixhawk3DWidget::findVehicleModels(void)
nodes.push_back(sphereGeode);
// add all other models in folder
for (int i = 0; i < files.size(); ++i) {
for (int i = 0; i < files.size(); ++i)
{
osg::ref_ptr<osg::Node> node =
osgDB::readNodeFile(directory.absoluteFilePath(files[i]).toStdString().c_str());
if (node) {
if (node)
{
nodes.push_back(node);
} else {
}
else
{
printf("%s\n", QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str());
}
}
......@@ -457,7 +607,8 @@ Pixhawk3DWidget::buildLayout(void)
QLabel* modelLabel = new QLabel("Vehicle", this);
QComboBox* modelComboBox = new QComboBox(this);
for (int i = 0; i < vehicleModels.size(); ++i) {
for (int i = 0; i < vehicleModels.size(); ++i)
{
modelComboBox->addItem(vehicleModels[i]->getName().c_str());
}
......@@ -505,10 +656,32 @@ Pixhawk3DWidget::buildLayout(void)
this, SLOT(toggleFollowCamera(int)));
}
void
Pixhawk3DWidget::resizeGL(int width, int height)
{
Q3DWidget::resizeGL(width, height);
resizeHUD();
}
void
Pixhawk3DWidget::display(void)
{
if (uas == NULL) {
// set node visibility
rollingMap->setChildValue(gridNode, displayGrid);
rollingMap->setChildValue(trailNode, displayTrail);
rollingMap->setChildValue(mapNode, displayImagery);
rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
rollingMap->setChildValue(targetNode, enableTarget);
#ifdef QGC_PROTOBUF_ENABLED
rollingMap->setChildValue(obstacleGroupNode, displayObstacleList);
#endif
rollingMap->setChildValue(rgbd3DNode, displayRGBD3D);
hudGroup->setChildValue(rgb2DGeode, displayRGBD2D);
hudGroup->setChildValue(depth2DGeode, displayRGBD2D);
if (!uas)
{
return;
}
......@@ -516,7 +689,8 @@ Pixhawk3DWidget::display(void)
QString utmZone;
getPose(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
if (lastRobotX == 0.0f && lastRobotY == 0.0f && lastRobotZ == 0.0f) {
if (lastRobotX == 0.0f && lastRobotY == 0.0f && lastRobotZ == 0.0f)
{
lastRobotX = robotX;
lastRobotY = robotY;
lastRobotZ = robotZ;
......@@ -524,7 +698,8 @@ Pixhawk3DWidget::display(void)
recenterCamera(robotY, robotX, -robotZ);
}
if (followCamera) {
if (followCamera)
{
double dx = robotY - lastRobotY;
double dy = robotX - lastRobotX;
double dz = lastRobotZ - robotZ;
......@@ -537,41 +712,40 @@ Pixhawk3DWidget::display(void)
robotPitch, osg::Vec3d(1.0f, 0.0f, 0.0f),
robotRoll, osg::Vec3d(0.0f, 1.0f, 0.0f)));
if (displayTrail) {
if (displayTrail)
{
updateTrail(robotX, robotY, robotZ);
}
if (frame == MAV_FRAME_GLOBAL && displayImagery) {
if (frame == MAV_FRAME_GLOBAL && displayImagery)
{
updateImagery(robotX, robotY, robotZ, utmZone);
}
if (displayWaypoints) {
if (displayWaypoints)
{
updateWaypoints();
}
if (enableTarget) {
if (enableTarget)
{
updateTarget(robotX, robotY);
}
#ifdef QGC_PROTOBUF_ENABLED
if (displayRGBD2D || displayRGBD3D) {
if (displayRGBD2D || displayRGBD3D)
{
updateRGBD(robotX, robotY, robotZ);
}
if (displayObstacleList)
{
updateObstacles();
}
#endif
updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone);
// set node visibility
rollingMap->setChildValue(gridNode, displayGrid);
rollingMap->setChildValue(trailNode, displayTrail);
rollingMap->setChildValue(mapNode, displayImagery);
rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
rollingMap->setChildValue(targetNode, enableTarget);
egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D);
hudGroup->setChildValue(rgb2DGeode, displayRGBD2D);
hudGroup->setChildValue(depth2DGeode, displayRGBD2D);
lastRobotX = robotX;
lastRobotY = robotY;
lastRobotZ = robotZ;
......@@ -582,8 +756,10 @@ Pixhawk3DWidget::display(void)
void
Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
{
if (!event->text().isEmpty()) {
switch (*(event->text().toAscii().data())) {
if (!event->text().isEmpty())
{
switch (*(event->text().toAscii().data()))
{
case '1':
displayRGBD2D = !displayRGBD2D;
break;
......@@ -594,6 +770,10 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
case 'C':
enableRGBDColor = !enableRGBDColor;
break;
case 'o':
case 'O':
displayObstacleList = !displayObstacleList;
break;
}
}
......@@ -603,19 +783,29 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
void
Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
{
if (event->button() == Qt::LeftButton) {
if (mode == MOVE_WAYPOINT_MODE) {
setWaypoint();
mode = DEFAULT_MODE;
if (event->button() == Qt::LeftButton)
{
if (mode == SELECT_TARGET_HEADING_MODE)
{
setTarget();
}
return;
if (mode != DEFAULT_MODE)
{
mode = DEFAULT_MODE;
}
if (event->modifiers() == Qt::ShiftModifier) {
selectedWpIndex = findWaypoint(event->x(), event->y());
if (selectedWpIndex == -1) {
if (event->modifiers() == Qt::ShiftModifier)
{
selectedWpIndex = findWaypoint(event->pos());
if (selectedWpIndex == -1)
{
cachedMousePos = event->pos();
showInsertWaypointMenu(event->globalPos());
} else {
}
else
{
showEditWaypointMenu(event->globalPos());
}
......@@ -626,20 +816,46 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
Q3DWidget::mousePressEvent(event);
}
void
Pixhawk3DWidget::mouseMoveEvent(QMouseEvent* event)
{
if (mode == SELECT_TARGET_HEADING_MODE)
{
selectTargetHeading();
}
if (mode == MOVE_WAYPOINT_POSITION_MODE)
{
moveWaypointPosition();
}
if (mode == MOVE_WAYPOINT_HEADING_MODE)
{
moveWaypointHeading();
}
Q3DWidget::mouseMoveEvent(event);
}
void
Pixhawk3DWidget::getPose(double& x, double& y, double& z,
double& roll, double& pitch, double& yaw,
QString& utmZone)
{
if (uas) {
if (frame == MAV_FRAME_GLOBAL) {
if (!uas)
{
return;
}
if (frame == MAV_FRAME_GLOBAL)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
z = -altitude;
} else if (frame == MAV_FRAME_LOCAL_NED) {
}
else if (frame == MAV_FRAME_LOCAL_NED)
{
x = uas->getLocalX();
y = uas->getLocalY();
z = uas->getLocalZ();
......@@ -648,7 +864,6 @@ Pixhawk3DWidget::getPose(double& x, double& y, double& z,
roll = uas->getRoll();
pitch = uas->getPitch();
yaw = uas->getYaw();
}
}
void
......@@ -663,8 +878,11 @@ void
Pixhawk3DWidget::getPosition(double& x, double& y, double& z,
QString& utmZone)
{
if (uas)
if (!uas)
{
return;
}
if (frame == MAV_FRAME_GLOBAL)
{
double latitude = uas->getLatitude();
......@@ -680,7 +898,6 @@ Pixhawk3DWidget::getPosition(double& x, double& y, double& z,
y = uas->getLocalY();
z = uas->getLocalZ();
}
}
}
void
......@@ -820,14 +1037,15 @@ Pixhawk3DWidget::createTarget(void)
pat->setPosition(osg::Vec3d(0.0, 0.0, 0.0));
osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.1f);
osg::ref_ptr<osg::ShapeDrawable> sphereDrawable = new osg::ShapeDrawable(sphere);
sphereDrawable->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f));
osg::ref_ptr<osg::Geode> sphereGeode = new osg::Geode;
sphereGeode->addDrawable(sphereDrawable);
sphereGeode->setName("Target");
osg::ref_ptr<osg::Cone> cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.2f, 0.6f);
osg::ref_ptr<osg::ShapeDrawable> coneDrawable = new osg::ShapeDrawable(cone);
coneDrawable->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f));
coneDrawable->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
osg::ref_ptr<osg::Geode> coneGeode = new osg::Geode;
coneGeode->addDrawable(coneDrawable);
coneGeode->setName("Target");
pat->addChild(sphereGeode);
pat->addChild(coneGeode);
return pat;
}
......@@ -850,31 +1068,29 @@ Pixhawk3DWidget::setupHUD(void)
statusText = new osgText::Text;
statusText->setCharacterSize(11);
statusText->setFont("images/Vera.ttf");
statusText->setFont(font);
statusText->setAxisAlignment(osgText::Text::SCREEN);
statusText->setColor(osg::Vec4(255, 255, 255, 1));
resizeHUD();
osg::ref_ptr<osg::Geode> statusGeode = new osg::Geode;
statusGeode->addDrawable(hudBackgroundGeometry);
statusGeode->addDrawable(statusText);
hudGroup->addChild(statusGeode);
rgbImage = new osg::Image;
rgb2DGeode = new ImageWindowGeode("RGB Image",
osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
rgbImage);
rgb2DGeode = new ImageWindowGeode;
rgb2DGeode->init("RGB Image", osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
rgbImage, font);
hudGroup->addChild(rgb2DGeode);
depthImage = new osg::Image;
depth2DGeode = new ImageWindowGeode("Depth Image",
osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
depthImage);
depth2DGeode = new ImageWindowGeode;
depth2DGeode->init("Depth Image", osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
depthImage, font);
hudGroup->addChild(depth2DGeode);
scaleGeode = new HUDScaleGeode;
scaleGeode->init();
scaleGeode->init(font);
hudGroup->addChild(scaleGeode);
}
......@@ -920,8 +1136,6 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
double robotRoll, double robotPitch, double robotYaw,
const QString& utmZone)
{
resizeHUD();
std::pair<double,double> cursorPosition =
getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ);
......@@ -1113,8 +1327,18 @@ void
Pixhawk3DWidget::updateTarget(double robotX, double robotY)
{
osg::PositionAttitudeTransform* pat =
static_cast<osg::PositionAttitudeTransform*>(targetNode.get());
dynamic_cast<osg::PositionAttitudeTransform*>(targetNode.get());
pat->setPosition(osg::Vec3d(target.y() - robotY, target.x() - robotX, 0.0));
pat->setAttitude(osg::Quat(target.z() - 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)));
osg::Geode* geode = dynamic_cast<osg::Geode*>(pat->getChild(0));
osg::ShapeDrawable* sd = dynamic_cast<osg::ShapeDrawable*>(geode->getDrawable(0));
sd->setColor(osg::Vec4f(1.0f, 0.8f, 0.0f, 1.0f));
}
float colormap_jet[128][3] = {
......@@ -1295,7 +1519,8 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(geometry->getVertexArray());
osg::Vec4Array* colors = static_cast<osg::Vec4Array*>(geometry->getColorArray());
for (int i = 0; i < pointCloud.points_size(); ++i) {
for (int i = 0; i < pointCloud.points_size(); ++i)
{
const px::PointCloudXYZRGB_PointXYZRGB& p = pointCloud.points(i);
double x = p.x() - robotX;
......@@ -1305,7 +1530,8 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
(*vertices)[i].set(y, x, -z);
if (enableRGBDColor) {
if (enableRGBDColor)
{
float rgb = p.rgb();
float b = *(reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
......@@ -1313,7 +1539,9 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
float r = *(2 + reinterpret_cast<unsigned char*>(&rgb)) / 255.0f;
(*colors)[i].set(r, g, b, 1.0f);
} else {
}
else
{
double dist = sqrt(x * x + y * y + z * z);
int colorIndex = static_cast<int>(fmin(dist / 7.0 * 127.0, 127.0));
(*colors)[i].set(colormap_jet[colorIndex][0],
......@@ -1323,28 +1551,44 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
}
}
if (geometry->getNumPrimitiveSets() == 0) {
if (geometry->getNumPrimitiveSets() == 0)
{
geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,
0, pointCloud.points_size()));
} else {
}
else
{
osg::DrawArrays* drawarrays = static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
drawarrays->setCount(pointCloud.points_size());
}
}
void
Pixhawk3DWidget::updateObstacles(void)
{
obstacleGroupNode->update(frame, uas);
}
#endif
int
Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
Pixhawk3DWidget::findWaypoint(const QPoint& mousePos)
{
if (getSceneData() != NULL) {
if (getSceneData())
{
osgUtil::LineSegmentIntersector::Intersections intersections;
if (computeIntersections(mouseX, height() - mouseY, intersections)) {
if (computeIntersections(mousePos.x(), height() - mousePos.y(),
intersections))
{
for (osgUtil::LineSegmentIntersector::Intersections::iterator
it = intersections.begin(); it != intersections.end(); it++) {
for (uint i = 0 ; i < it->nodePath.size(); ++i) {
it = intersections.begin(); it != intersections.end(); it++)
{
for (uint i = 0 ; i < it->nodePath.size(); ++i)
{
std::string nodeName = it->nodePath[i]->getName();
if (nodeName.substr(0, 2).compare("wp") == 0) {
if (nodeName.substr(0, 2).compare("wp") == 0)
{
return atoi(nodeName.substr(2).c_str());
}
}
......@@ -1358,15 +1602,20 @@ Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
bool
Pixhawk3DWidget::findTarget(int mouseX, int mouseY)
{
if (getSceneData() != NULL) {
if (getSceneData())
{
osgUtil::LineSegmentIntersector::Intersections intersections;
if (computeIntersections(mouseX, height() - mouseY, intersections)) {
if (computeIntersections(mouseX, height() - mouseY, intersections))
{
for (osgUtil::LineSegmentIntersector::Intersections::iterator
it = intersections.begin(); it != intersections.end(); it++) {
for (uint i = 0 ; i < it->nodePath.size(); ++i) {
it = intersections.begin(); it != intersections.end(); it++)
{
for (uint i = 0 ; i < it->nodePath.size(); ++i)
{
std::string nodeName = it->nodePath[i]->getName();
if (nodeName.compare("Target") == 0) {
if (nodeName.compare("Target") == 0)
{
return true;
}
}
......@@ -1394,7 +1643,10 @@ Pixhawk3DWidget::showEditWaypointMenu(const QPoint &cursorPos)
QString text;
text = QString("Move waypoint %1").arg(QString::number(selectedWpIndex));
menu.addAction(text, this, SLOT(moveWaypoint()));
menu.addAction(text, this, SLOT(moveWaypointPosition()));
text = QString("Change heading of waypoint %1").arg(QString::number(selectedWpIndex));
menu.addAction(text, this, SLOT(moveWaypointHeading()));
text = QString("Change altitude of waypoint %1").arg(QString::number(selectedWpIndex));
menu.addAction(text, this, SLOT(setWaypointAltitude()));
......
src/ui/map3D/Pixhawk3DWidget.h
View file @ 5a0a9297
......@@ -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"
......@@ -67,10 +70,12 @@ private slots:
void recenter(void);
void toggleFollowCamera(int state);
void selectTargetHeading(void);
void selectTarget(void);
void setTarget(void);
void insertWaypoint(void);
void moveWaypoint(void);
void setWaypoint(void);
void moveWaypointPosition(void);
void moveWaypointHeading(void);
void deleteWaypoint(void);
void setWaypointAltitude(void);
void clearAllWaypoints(void);
......@@ -78,12 +83,28 @@ private slots:
protected:
QVector< osg::ref_ptr<osg::Node> > findVehicleModels(void);
void buildLayout(void);
virtual void resizeGL(int width, int height);
virtual void display(void);
virtual void keyPressEvent(QKeyEvent* event);
virtual void mousePressEvent(QMouseEvent* event);
void showEvent(QShowEvent* event)
{
QWidget::showEvent(event);
emit visibilityChanged(true);
}
void hideEvent(QHideEvent* event)
{
QWidget::hideEvent(event);
emit visibilityChanged(false);
}
virtual void mouseMoveEvent(QMouseEvent* event);
UASInterface* uas;
signals:
void visibilityChanged(bool visible);
private:
void getPose(double& x, double& y, double& z,
double& roll, double& pitch, double& yaw,
......@@ -113,16 +134,19 @@ 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);
int findWaypoint(const QPoint& mousePos);
bool findTarget(int mouseX, int mouseY);
void showInsertWaypointMenu(const QPoint& cursorPos);
void showEditWaypointMenu(const QPoint& cursorPos);
enum Mode {
DEFAULT_MODE,
MOVE_WAYPOINT_MODE
MOVE_WAYPOINT_POSITION_MODE,
MOVE_WAYPOINT_HEADING_MODE,
SELECT_TARGET_HEADING_MODE
};
Mode mode;
int selectedWpIndex;
......@@ -133,6 +157,7 @@ private:
bool displayWaypoints;
bool displayRGBD2D;
bool displayRGBD3D;
bool displayObstacleList;
bool enableRGBDColor;
bool enableTarget;
......@@ -157,11 +182,15 @@ 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;
MAV_FRAME frame;
osg::Vec2d target;
osg::Vec3d target;
QPoint cachedMousePos;
double lastRobotX, lastRobotY, lastRobotZ;
};
......
src/ui/map3D/Q3DWidget.cc
View file @ 5a0a9297
......@@ -35,6 +35,9 @@ This file is part of the QGROUNDCONTROL project
#include <osg/Geometry>
#include <osg/LineWidth>
#include <osg/MatrixTransform>
#ifdef QGC_OSG_QT_ENABLED
#include <osgQt/QFontImplementation>
#endif
#ifdef Q_OS_MACX
#include <Carbon/Carbon.h>
#endif
......@@ -56,12 +59,21 @@ Q3DWidget::Q3DWidget(QWidget* parent)
cameraParams.cameraFov = 30.0f;
cameraParams.minClipRange = 1.0f;
cameraParams.maxClipRange = 10000.0f;
#ifdef QGC_OSG_QT_ENABLED
osg::ref_ptr<osgText::Font::FontImplementation> fontImpl;
fontImpl = new osgQt::QFontImplementation(QFont(":/general/vera.ttf"));
#else
osg::ref_ptr<osgText::Font::FontImplementation> fontImpl;
fontImpl = 0;//new osgText::Font::Font("images/Vera.ttf");
#endif
font = new osgText::Font(fontImpl);
osgGW = new osgViewer::GraphicsWindowEmbedded(0, 0, width(), height());
setThreadingModel(osgViewer::Viewer::SingleThreaded);
setFocusPolicy(Qt::StrongFocus);
setMouseTracking(true);
}
Q3DWidget::~Q3DWidget()
......
src/ui/map3D/Q3DWidget.h
View file @ 5a0a9297
......@@ -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
src/ui/map3D/WaypointGroupNode.cc
View file @ 5a0a9297
......@@ -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,11 +51,16 @@ WaypointGroupNode::init(void)
void
WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
{
if (uas) {
if (!uas)
{
return;
}
double robotX = 0.0;
double robotY = 0.0;
double robotZ = 0.0;
if (frame == MAV_FRAME_GLOBAL) {
if (frame == MAV_FRAME_GLOBAL)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
......@@ -63,25 +68,63 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
robotZ = -altitude;
} else if (frame == MAV_FRAME_LOCAL_NED) {
}
else if (frame == MAV_FRAME_LOCAL_NED)
{
robotX = uas->getLocalX();
robotY = uas->getLocalY();
robotZ = uas->getLocalZ();
}
if (getNumChildren() > 0) {
if (getNumChildren() > 0)
{
removeChild(0, getNumChildren());
}
const QVector<Waypoint *>& list = uas->getWaypointManager()->getWaypointEditableList();
for (int i = 0; i < list.size(); i++) {
for (int i = 0; i < list.size(); i++)
{
Waypoint* wp = list.at(i);
double wpX, wpY, wpZ;
getPosition(wp, wpX, wpY, wpZ);
double wpYaw = osg::DegreesToRadians(wp->getYaw());
osg::ref_ptr<osg::Group> group = new osg::Group;
// cone indicates waypoint orientation
osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
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())
{
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);
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;
osg::ref_ptr<osg::Cylinder> cylinder =
new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
wp->getAcceptanceRadius(),
......@@ -90,20 +133,25 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
sd->setShape(cylinder);
sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
if (wp->getCurrent()) {
if (wp->getCurrent())
{
sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
} else {
}
else
{
sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
}
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
geode = new osg::Geode;
geode->addDrawable(sd);
group->addChild(geode);
char wpLabel[10];
sprintf(wpLabel, "wp%d", i);
geode->setName(wpLabel);
group->setName(wpLabel);
if (i < list.size() - 1) {
if (i < list.size() - 1)
{
double nextWpX, nextWpY, nextWpZ;
getPosition(list.at(i + 1), nextWpX, nextWpY, nextWpZ);
......@@ -130,23 +178,21 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
geode->addDrawable(geometry);
}
osg::ref_ptr<osg::PositionAttitudeTransform> pat =
new osg::PositionAttitudeTransform;
pat = new osg::PositionAttitudeTransform;
pat->setPosition(osg::Vec3d(wpY - robotY,
wpX - robotX,
robotZ));
addChild(pat);
pat->addChild(geode);
}
pat->addChild(group);
}
}
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 +200,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();
......
src/ui/map3D/WaypointGroupNode.h
View file @ 5a0a9297
......@@ -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>
*
*/
......
thirdParty/mavlink/include/mavlink_protobuf_manager.hpp
View file @ 5a0a9297
......@@ -42,6 +42,18 @@ public:
registerType(msg);
}
// register ObstacleList
{
std::tr1::shared_ptr<px::ObstacleList> msg(new px::ObstacleList);
registerType(msg);
}
// register ObstacleMap
{
std::tr1::shared_ptr<px::ObstacleMap> msg(new px::ObstacleMap);
registerType(msg);
}
srand(time(NULL));
mStreamID = rand() + 1;
}
......@@ -186,6 +198,11 @@ public:
if (offset == 0)
{
queue.push_back(msg);
if ((flags & 0x1) != 0x1)
{
reassemble = true;
}
}
else
{
......
thirdParty/mavlink/include/mavlink_types.h
View file @ 5a0a9297
......@@ -15,10 +15,21 @@
#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length
#if (defined __GNUC__) || (defined _MSC_VER) || (defined __MINGW32__) || (defined __WATCOMC__) || (defined __CMB__) || (defined __BORLANDC__) || (defined __clang__)
#define MAVLINK_EXTENDED_MESSAGES_ENABLED
#endif
// EXTENDED message definition - the extended message set is compatible to standard MAVLink message passing
// but does NOT have to be supported by the platform. The extended message set will NOT consume
// any memory if the messages are not explicitely used
#ifdef MAVLINK_EXTENDED_MESSAGES_ENABLED
#define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255
#define MAVLINK_EXTENDED_HEADER_LEN 14
#define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507
#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
#endif
typedef struct param_union {
union {
......@@ -51,12 +62,13 @@ typedef struct __mavlink_message {
uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
} mavlink_message_t;
#ifdef MAVLINK_EXTENDED_MESSAGES_ENABLED
typedef struct __mavlink_extended_message {
mavlink_message_t base_msg;
int32_t extended_payload_len; ///< Length of extended payload if any
uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
} mavlink_extended_message_t;
#endif
typedef enum {
......
thirdParty/mavlink/include/pixhawk/pixhawk.pb.h
View file @ 5a0a9297
......@@ -37,6 +37,9 @@ class PointCloudXYZI_PointXYZI;
class PointCloudXYZRGB;
class PointCloudXYZRGB_PointXYZRGB;
class RGBDImage;
class Obstacle;
class ObstacleList;
class ObstacleMap;
// ===================================================================
......@@ -729,6 +732,409 @@ class RGBDImage : public ::google::protobuf::Message {
void InitAsDefaultInstance();
static RGBDImage* default_instance_;
};
// -------------------------------------------------------------------
class Obstacle : public ::google::protobuf::Message {
public:
Obstacle();
virtual ~Obstacle();
Obstacle(const Obstacle& from);
inline Obstacle& operator=(const Obstacle& from) {
CopyFrom(from);
return *this;
}
inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
return _unknown_fields_;
}
inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
return &_unknown_fields_;
}
static const ::google::protobuf::Descriptor* descriptor();
static const Obstacle& default_instance();
void Swap(Obstacle* other);
// implements Message ----------------------------------------------
Obstacle* New() const;
void CopyFrom(const ::google::protobuf::Message& from);
void MergeFrom(const ::google::protobuf::Message& from);
void CopyFrom(const Obstacle& from);
void MergeFrom(const Obstacle& from);
void Clear();
bool IsInitialized() const;
int ByteSize() const;
bool MergePartialFromCodedStream(
::google::protobuf::io::CodedInputStream* input);
void SerializeWithCachedSizes(
::google::protobuf::io::CodedOutputStream* output) const;
::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
int GetCachedSize() const { return _cached_size_; }
private:
void SharedCtor();
void SharedDtor();
void SetCachedSize(int size) const;
public:
::google::protobuf::Metadata GetMetadata() const;
// nested types ----------------------------------------------------
// accessors -------------------------------------------------------
// optional float x = 1;
inline bool has_x() const;
inline void clear_x();
static const int kXFieldNumber = 1;
inline float x() const;
inline void set_x(float value);
// optional float y = 2;
inline bool has_y() const;
inline void clear_y();
static const int kYFieldNumber = 2;
inline float y() const;
inline void set_y(float value);
// optional float z = 3;
inline bool has_z() const;
inline void clear_z();
static const int kZFieldNumber = 3;
inline float z() const;
inline void set_z(float value);
// optional float length = 4;
inline bool has_length() const;
inline void clear_length();
static const int kLengthFieldNumber = 4;
inline float length() const;
inline void set_length(float value);
// optional float width = 5;
inline bool has_width() const;
inline void clear_width();
static const int kWidthFieldNumber = 5;
inline float width() const;
inline void set_width(float value);
// optional float height = 6;
inline bool has_height() const;
inline void clear_height();
static const int kHeightFieldNumber = 6;
inline float height() const;
inline void set_height(float value);
// @@protoc_insertion_point(class_scope:px.Obstacle)
private:
inline void set_has_x();
inline void clear_has_x();
inline void set_has_y();
inline void clear_has_y();
inline void set_has_z();
inline void clear_has_z();
inline void set_has_length();
inline void clear_has_length();
inline void set_has_width();
inline void clear_has_width();
inline void set_has_height();
inline void clear_has_height();
::google::protobuf::UnknownFieldSet _unknown_fields_;
float x_;
float y_;
float z_;
float length_;
float width_;
float height_;
mutable int _cached_size_;
::google::protobuf::uint32 _has_bits_[(6 + 31) / 32];
friend void protobuf_AddDesc_pixhawk_2eproto();
friend void protobuf_AssignDesc_pixhawk_2eproto();
friend void protobuf_ShutdownFile_pixhawk_2eproto();
void InitAsDefaultInstance();
static Obstacle* default_instance_;
};
// -------------------------------------------------------------------
class ObstacleList : public ::google::protobuf::Message {
public:
ObstacleList();
virtual ~ObstacleList();
ObstacleList(const ObstacleList& from);
inline ObstacleList& operator=(const ObstacleList& from) {
CopyFrom(from);
return *this;
}
inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
return _unknown_fields_;
}
inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
return &_unknown_fields_;
}
static const ::google::protobuf::Descriptor* descriptor();
static const ObstacleList& default_instance();
void Swap(ObstacleList* other);
// implements Message ----------------------------------------------
ObstacleList* New() const;
void CopyFrom(const ::google::protobuf::Message& from);
void MergeFrom(const ::google::protobuf::Message& from);
void CopyFrom(const ObstacleList& from);
void MergeFrom(const ObstacleList& from);
void Clear();
bool IsInitialized() const;
int ByteSize() const;
bool MergePartialFromCodedStream(
::google::protobuf::io::CodedInputStream* input);
void SerializeWithCachedSizes(
::google::protobuf::io::CodedOutputStream* output) const;
::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
int GetCachedSize() const { return _cached_size_; }
private:
void SharedCtor();
void SharedDtor();
void SetCachedSize(int size) const;
public:
::google::protobuf::Metadata GetMetadata() const;
// nested types ----------------------------------------------------
// accessors -------------------------------------------------------
// optional uint64 utime = 1;
inline bool has_utime() const;
inline void clear_utime();
static const int kUtimeFieldNumber = 1;
inline ::google::protobuf::uint64 utime() const;
inline void set_utime(::google::protobuf::uint64 value);
// repeated .px.Obstacle obstacles = 2;
inline int obstacles_size() const;
inline void clear_obstacles();
static const int kObstaclesFieldNumber = 2;
inline const ::px::Obstacle& obstacles(int index) const;
inline ::px::Obstacle* mutable_obstacles(int index);
inline ::px::Obstacle* add_obstacles();
inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >&
obstacles() const;
inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >*
mutable_obstacles();
// @@protoc_insertion_point(class_scope:px.ObstacleList)
private:
inline void set_has_utime();
inline void clear_has_utime();
::google::protobuf::UnknownFieldSet _unknown_fields_;
::google::protobuf::uint64 utime_;
::google::protobuf::RepeatedPtrField< ::px::Obstacle > obstacles_;
mutable int _cached_size_;
::google::protobuf::uint32 _has_bits_[(2 + 31) / 32];
friend void protobuf_AddDesc_pixhawk_2eproto();
friend void protobuf_AssignDesc_pixhawk_2eproto();
friend void protobuf_ShutdownFile_pixhawk_2eproto();
void InitAsDefaultInstance();
static ObstacleList* default_instance_;
};
// -------------------------------------------------------------------
class ObstacleMap : public ::google::protobuf::Message {
public:
ObstacleMap();
virtual ~ObstacleMap();
ObstacleMap(const ObstacleMap& from);
inline ObstacleMap& operator=(const ObstacleMap& from) {
CopyFrom(from);
return *this;
}
inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
return _unknown_fields_;
}
inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
return &_unknown_fields_;
}
static const ::google::protobuf::Descriptor* descriptor();
static const ObstacleMap& default_instance();
void Swap(ObstacleMap* other);
// implements Message ----------------------------------------------
ObstacleMap* New() const;
void CopyFrom(const ::google::protobuf::Message& from);
void MergeFrom(const ::google::protobuf::Message& from);
void CopyFrom(const ObstacleMap& from);
void MergeFrom(const ObstacleMap& from);
void Clear();
bool IsInitialized() const;
int ByteSize() const;
bool MergePartialFromCodedStream(
::google::protobuf::io::CodedInputStream* input);
void SerializeWithCachedSizes(
::google::protobuf::io::CodedOutputStream* output) const;
::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
int GetCachedSize() const { return _cached_size_; }
private:
void SharedCtor();
void SharedDtor();
void SetCachedSize(int size) const;
public:
::google::protobuf::Metadata GetMetadata() const;
// nested types ----------------------------------------------------
// accessors -------------------------------------------------------
// required uint64 utime = 1;
inline bool has_utime() const;
inline void clear_utime();
static const int kUtimeFieldNumber = 1;
inline ::google::protobuf::uint64 utime() const;
inline void set_utime(::google::protobuf::uint64 value);
// required int32 type = 2;
inline bool has_type() const;
inline void clear_type();
static const int kTypeFieldNumber = 2;
inline ::google::protobuf::int32 type() const;
inline void set_type(::google::protobuf::int32 value);
// optional float resolution = 3;
inline bool has_resolution() const;
inline void clear_resolution();
static const int kResolutionFieldNumber = 3;
inline float resolution() const;
inline void set_resolution(float value);
// optional int32 rows = 4;
inline bool has_rows() const;
inline void clear_rows();
static const int kRowsFieldNumber = 4;
inline ::google::protobuf::int32 rows() const;
inline void set_rows(::google::protobuf::int32 value);
// optional int32 cols = 5;
inline bool has_cols() const;
inline void clear_cols();
static const int kColsFieldNumber = 5;
inline ::google::protobuf::int32 cols() const;
inline void set_cols(::google::protobuf::int32 value);
// optional int32 mapR0 = 6;
inline bool has_mapr0() const;
inline void clear_mapr0();
static const int kMapR0FieldNumber = 6;
inline ::google::protobuf::int32 mapr0() const;
inline void set_mapr0(::google::protobuf::int32 value);
// optional int32 mapC0 = 7;
inline bool has_mapc0() const;
inline void clear_mapc0();
static const int kMapC0FieldNumber = 7;
inline ::google::protobuf::int32 mapc0() const;
inline void set_mapc0(::google::protobuf::int32 value);
// optional int32 arrayR0 = 8;
inline bool has_arrayr0() const;
inline void clear_arrayr0();
static const int kArrayR0FieldNumber = 8;
inline ::google::protobuf::int32 arrayr0() const;
inline void set_arrayr0(::google::protobuf::int32 value);
// optional int32 arrayC0 = 9;
inline bool has_arrayc0() const;
inline void clear_arrayc0();
static const int kArrayC0FieldNumber = 9;
inline ::google::protobuf::int32 arrayc0() const;
inline void set_arrayc0(::google::protobuf::int32 value);
// optional bytes data = 10;
inline bool has_data() const;
inline void clear_data();
static const int kDataFieldNumber = 10;
inline const ::std::string& data() const;
inline void set_data(const ::std::string& value);
inline void set_data(const char* value);
inline void set_data(const void* value, size_t size);
inline ::std::string* mutable_data();
inline ::std::string* release_data();
// @@protoc_insertion_point(class_scope:px.ObstacleMap)
private:
inline void set_has_utime();
inline void clear_has_utime();
inline void set_has_type();
inline void clear_has_type();
inline void set_has_resolution();
inline void clear_has_resolution();
inline void set_has_rows();
inline void clear_has_rows();
inline void set_has_cols();
inline void clear_has_cols();
inline void set_has_mapr0();
inline void clear_has_mapr0();
inline void set_has_mapc0();
inline void clear_has_mapc0();
inline void set_has_arrayr0();
inline void clear_has_arrayr0();
inline void set_has_arrayc0();
inline void clear_has_arrayc0();
inline void set_has_data();
inline void clear_has_data();
::google::protobuf::UnknownFieldSet _unknown_fields_;
::google::protobuf::uint64 utime_;
::google::protobuf::int32 type_;
float resolution_;
::google::protobuf::int32 rows_;
::google::protobuf::int32 cols_;
::google::protobuf::int32 mapr0_;
::google::protobuf::int32 mapc0_;
::google::protobuf::int32 arrayr0_;
::google::protobuf::int32 arrayc0_;
::std::string* data_;
mutable int _cached_size_;
::google::protobuf::uint32 _has_bits_[(10 + 31) / 32];
friend void protobuf_AddDesc_pixhawk_2eproto();
friend void protobuf_AssignDesc_pixhawk_2eproto();
friend void protobuf_ShutdownFile_pixhawk_2eproto();
void InitAsDefaultInstance();
static ObstacleMap* default_instance_;
};
// ===================================================================
......@@ -1515,6 +1921,453 @@ RGBDImage::mutable_camera_matrix() {
return &camera_matrix_;
}
// -------------------------------------------------------------------
// Obstacle
// optional float x = 1;
inline bool Obstacle::has_x() const {
return (_has_bits_[0] & 0x00000001u) != 0;
}
inline void Obstacle::set_has_x() {
_has_bits_[0] |= 0x00000001u;
}
inline void Obstacle::clear_has_x() {
_has_bits_[0] &= ~0x00000001u;
}
inline void Obstacle::clear_x() {
x_ = 0;
clear_has_x();
}
inline float Obstacle::x() const {
return x_;
}
inline void Obstacle::set_x(float value) {
set_has_x();
x_ = value;
}
// optional float y = 2;
inline bool Obstacle::has_y() const {
return (_has_bits_[0] & 0x00000002u) != 0;
}
inline void Obstacle::set_has_y() {
_has_bits_[0] |= 0x00000002u;
}
inline void Obstacle::clear_has_y() {
_has_bits_[0] &= ~0x00000002u;
}
inline void Obstacle::clear_y() {
y_ = 0;
clear_has_y();
}
inline float Obstacle::y() const {
return y_;
}
inline void Obstacle::set_y(float value) {
set_has_y();
y_ = value;
}
// optional float z = 3;
inline bool Obstacle::has_z() const {
return (_has_bits_[0] & 0x00000004u) != 0;
}
inline void Obstacle::set_has_z() {
_has_bits_[0] |= 0x00000004u;
}
inline void Obstacle::clear_has_z() {
_has_bits_[0] &= ~0x00000004u;
}
inline void Obstacle::clear_z() {
z_ = 0;
clear_has_z();
}
inline float Obstacle::z() const {
return z_;
}
inline void Obstacle::set_z(float value) {
set_has_z();
z_ = value;
}
// optional float length = 4;
inline bool Obstacle::has_length() const {
return (_has_bits_[0] & 0x00000008u) != 0;
}
inline void Obstacle::set_has_length() {
_has_bits_[0] |= 0x00000008u;
}
inline void Obstacle::clear_has_length() {
_has_bits_[0] &= ~0x00000008u;
}
inline void Obstacle::clear_length() {
length_ = 0;
clear_has_length();
}
inline float Obstacle::length() const {
return length_;
}
inline void Obstacle::set_length(float value) {
set_has_length();
length_ = value;
}
// optional float width = 5;
inline bool Obstacle::has_width() const {
return (_has_bits_[0] & 0x00000010u) != 0;
}
inline void Obstacle::set_has_width() {
_has_bits_[0] |= 0x00000010u;
}
inline void Obstacle::clear_has_width() {
_has_bits_[0] &= ~0x00000010u;
}
inline void Obstacle::clear_width() {
width_ = 0;
clear_has_width();
}
inline float Obstacle::width() const {
return width_;
}
inline void Obstacle::set_width(float value) {
set_has_width();
width_ = value;
}
// optional float height = 6;
inline bool Obstacle::has_height() const {
return (_has_bits_[0] & 0x00000020u) != 0;
}
inline void Obstacle::set_has_height() {
_has_bits_[0] |= 0x00000020u;
}
inline void Obstacle::clear_has_height() {
_has_bits_[0] &= ~0x00000020u;
}
inline void Obstacle::clear_height() {
height_ = 0;
clear_has_height();
}
inline float Obstacle::height() const {
return height_;
}
inline void Obstacle::set_height(float value) {
set_has_height();
height_ = value;
}
// -------------------------------------------------------------------
// ObstacleList
// optional uint64 utime = 1;
inline bool ObstacleList::has_utime() const {
return (_has_bits_[0] & 0x00000001u) != 0;
}
inline void ObstacleList::set_has_utime() {
_has_bits_[0] |= 0x00000001u;
}
inline void ObstacleList::clear_has_utime() {
_has_bits_[0] &= ~0x00000001u;
}
inline void ObstacleList::clear_utime() {
utime_ = GOOGLE_ULONGLONG(0);
clear_has_utime();
}
inline ::google::protobuf::uint64 ObstacleList::utime() const {
return utime_;
}
inline void ObstacleList::set_utime(::google::protobuf::uint64 value) {
set_has_utime();
utime_ = value;
}
// repeated .px.Obstacle obstacles = 2;
inline int ObstacleList::obstacles_size() const {
return obstacles_.size();
}
inline void ObstacleList::clear_obstacles() {
obstacles_.Clear();
}
inline const ::px::Obstacle& ObstacleList::obstacles(int index) const {
return obstacles_.Get(index);
}
inline ::px::Obstacle* ObstacleList::mutable_obstacles(int index) {
return obstacles_.Mutable(index);
}
inline ::px::Obstacle* ObstacleList::add_obstacles() {
return obstacles_.Add();
}
inline const ::google::protobuf::RepeatedPtrField< ::px::Obstacle >&
ObstacleList::obstacles() const {
return obstacles_;
}
inline ::google::protobuf::RepeatedPtrField< ::px::Obstacle >*
ObstacleList::mutable_obstacles() {
return &obstacles_;
}
// -------------------------------------------------------------------
// ObstacleMap
// required uint64 utime = 1;
inline bool ObstacleMap::has_utime() const {
return (_has_bits_[0] & 0x00000001u) != 0;
}
inline void ObstacleMap::set_has_utime() {
_has_bits_[0] |= 0x00000001u;
}
inline void ObstacleMap::clear_has_utime() {
_has_bits_[0] &= ~0x00000001u;
}
inline void ObstacleMap::clear_utime() {
utime_ = GOOGLE_ULONGLONG(0);
clear_has_utime();
}
inline ::google::protobuf::uint64 ObstacleMap::utime() const {
return utime_;
}
inline void ObstacleMap::set_utime(::google::protobuf::uint64 value) {
set_has_utime();
utime_ = value;
}
// required int32 type = 2;
inline bool ObstacleMap::has_type() const {
return (_has_bits_[0] & 0x00000002u) != 0;
}
inline void ObstacleMap::set_has_type() {
_has_bits_[0] |= 0x00000002u;
}
inline void ObstacleMap::clear_has_type() {
_has_bits_[0] &= ~0x00000002u;
}
inline void ObstacleMap::clear_type() {
type_ = 0;
clear_has_type();
}
inline ::google::protobuf::int32 ObstacleMap::type() const {
return type_;
}
inline void ObstacleMap::set_type(::google::protobuf::int32 value) {
set_has_type();
type_ = value;
}
// optional float resolution = 3;
inline bool ObstacleMap::has_resolution() const {
return (_has_bits_[0] & 0x00000004u) != 0;
}
inline void ObstacleMap::set_has_resolution() {
_has_bits_[0] |= 0x00000004u;
}
inline void ObstacleMap::clear_has_resolution() {
_has_bits_[0] &= ~0x00000004u;
}
inline void ObstacleMap::clear_resolution() {
resolution_ = 0;
clear_has_resolution();
}
inline float ObstacleMap::resolution() const {
return resolution_;
}
inline void ObstacleMap::set_resolution(float value) {
set_has_resolution();
resolution_ = value;
}
// optional int32 rows = 4;
inline bool ObstacleMap::has_rows() const {
return (_has_bits_[0] & 0x00000008u) != 0;
}
inline void ObstacleMap::set_has_rows() {
_has_bits_[0] |= 0x00000008u;
}
inline void ObstacleMap::clear_has_rows() {
_has_bits_[0] &= ~0x00000008u;
}
inline void ObstacleMap::clear_rows() {
rows_ = 0;
clear_has_rows();
}
inline ::google::protobuf::int32 ObstacleMap::rows() const {
return rows_;
}
inline void ObstacleMap::set_rows(::google::protobuf::int32 value) {
set_has_rows();
rows_ = value;
}
// optional int32 cols = 5;
inline bool ObstacleMap::has_cols() const {
return (_has_bits_[0] & 0x00000010u) != 0;
}
inline void ObstacleMap::set_has_cols() {
_has_bits_[0] |= 0x00000010u;
}
inline void ObstacleMap::clear_has_cols() {
_has_bits_[0] &= ~0x00000010u;
}
inline void ObstacleMap::clear_cols() {
cols_ = 0;
clear_has_cols();
}
inline ::google::protobuf::int32 ObstacleMap::cols() const {
return cols_;
}
inline void ObstacleMap::set_cols(::google::protobuf::int32 value) {
set_has_cols();
cols_ = value;
}
// optional int32 mapR0 = 6;
inline bool ObstacleMap::has_mapr0() const {
return (_has_bits_[0] & 0x00000020u) != 0;
}
inline void ObstacleMap::set_has_mapr0() {
_has_bits_[0] |= 0x00000020u;
}
inline void ObstacleMap::clear_has_mapr0() {
_has_bits_[0] &= ~0x00000020u;
}
inline void ObstacleMap::clear_mapr0() {
mapr0_ = 0;
clear_has_mapr0();
}
inline ::google::protobuf::int32 ObstacleMap::mapr0() const {
return mapr0_;
}
inline void ObstacleMap::set_mapr0(::google::protobuf::int32 value) {
set_has_mapr0();
mapr0_ = value;
}
// optional int32 mapC0 = 7;
inline bool ObstacleMap::has_mapc0() const {
return (_has_bits_[0] & 0x00000040u) != 0;
}
inline void ObstacleMap::set_has_mapc0() {
_has_bits_[0] |= 0x00000040u;
}
inline void ObstacleMap::clear_has_mapc0() {
_has_bits_[0] &= ~0x00000040u;
}
inline void ObstacleMap::clear_mapc0() {
mapc0_ = 0;
clear_has_mapc0();
}
inline ::google::protobuf::int32 ObstacleMap::mapc0() const {
return mapc0_;
}
inline void ObstacleMap::set_mapc0(::google::protobuf::int32 value) {
set_has_mapc0();
mapc0_ = value;
}
// optional int32 arrayR0 = 8;
inline bool ObstacleMap::has_arrayr0() const {
return (_has_bits_[0] & 0x00000080u) != 0;
}
inline void ObstacleMap::set_has_arrayr0() {
_has_bits_[0] |= 0x00000080u;
}
inline void ObstacleMap::clear_has_arrayr0() {
_has_bits_[0] &= ~0x00000080u;
}
inline void ObstacleMap::clear_arrayr0() {
arrayr0_ = 0;
clear_has_arrayr0();
}
inline ::google::protobuf::int32 ObstacleMap::arrayr0() const {
return arrayr0_;
}
inline void ObstacleMap::set_arrayr0(::google::protobuf::int32 value) {
set_has_arrayr0();
arrayr0_ = value;
}
// optional int32 arrayC0 = 9;
inline bool ObstacleMap::has_arrayc0() const {
return (_has_bits_[0] & 0x00000100u) != 0;
}
inline void ObstacleMap::set_has_arrayc0() {
_has_bits_[0] |= 0x00000100u;
}
inline void ObstacleMap::clear_has_arrayc0() {
_has_bits_[0] &= ~0x00000100u;
}
inline void ObstacleMap::clear_arrayc0() {
arrayc0_ = 0;
clear_has_arrayc0();
}
inline ::google::protobuf::int32 ObstacleMap::arrayc0() const {
return arrayc0_;
}
inline void ObstacleMap::set_arrayc0(::google::protobuf::int32 value) {
set_has_arrayc0();
arrayc0_ = value;
}
// optional bytes data = 10;
inline bool ObstacleMap::has_data() const {
return (_has_bits_[0] & 0x00000200u) != 0;
}
inline void ObstacleMap::set_has_data() {
_has_bits_[0] |= 0x00000200u;
}
inline void ObstacleMap::clear_has_data() {
_has_bits_[0] &= ~0x00000200u;
}
inline void ObstacleMap::clear_data() {
if (data_ != &::google::protobuf::internal::kEmptyString) {
data_->clear();
}
clear_has_data();
}
inline const ::std::string& ObstacleMap::data() const {
return *data_;
}
inline void ObstacleMap::set_data(const ::std::string& value) {
set_has_data();
if (data_ == &::google::protobuf::internal::kEmptyString) {
data_ = new ::std::string;
}
data_->assign(value);
}
inline void ObstacleMap::set_data(const char* value) {
set_has_data();
if (data_ == &::google::protobuf::internal::kEmptyString) {
data_ = new ::std::string;
}
data_->assign(value);
}
inline void ObstacleMap::set_data(const void* value, size_t size) {
set_has_data();
if (data_ == &::google::protobuf::internal::kEmptyString) {
data_ = new ::std::string;
}
data_->assign(reinterpret_cast<const char*>(value), size);
}
inline ::std::string* ObstacleMap::mutable_data() {
set_has_data();
if (data_ == &::google::protobuf::internal::kEmptyString) {
data_ = new ::std::string;
}
return data_;
}
inline ::std::string* ObstacleMap::release_data() {
clear_has_data();
if (data_ == &::google::protobuf::internal::kEmptyString) {
return NULL;
} else {
::std::string* temp = data_;
data_ = const_cast< ::std::string*>(&::google::protobuf::internal::kEmptyString);
return temp;
}
}
// @@protoc_insertion_point(namespace_scope)
......
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