diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index 37add31ca3270f6c5a62f5290e35ed80e6b8e37b..dacd2ac3a39beecc608c04e6abfcf7b7cc8b4e78 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -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) { diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index f93cab1095e1b93c44322252928d5e5f992c3214..dec0ea3d4803a7b452f5deb604643ef6ad8db06a 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -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") diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 8053f13e539ded1d2009958511b84bc1849bd279..b5b55096274428c579f72718306a8d7444bb2509 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -993,6 +993,11 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptrGetTypeName() == 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); } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index c5149734f9367307f4495cf164ba281d50321974..5216711a8f31e9fe17f036516c9973b6d7d8b331 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -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* > 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); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index f929af84b15da06c17ad03709d7a366197aaa2b3..e3cbacfe27acc07b8b2f69d6d52d7b67d329c62e 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -46,7 +46,7 @@ This file is part of the QGROUNDCONTROL project #ifdef QGC_PROTOBUF_ENABLED #include -#include +#include #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; diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 39e0be4d7e9b94e2bf16507c15d4eb8c0abb2797..90898ee234d3d5064f0aa60d2b7c3fe108c88479 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -1431,10 +1431,13 @@ void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int s void HUD::copyImage() { - qDebug() << "HUD::copyImage()"; - UAS* u = dynamic_cast(this->uas); - if (u) + if (isVisible()) { - this->glImage = QGLWidget::convertToGLFormat(u->getImage()); + qDebug() << "HUD::copyImage()"; + UAS* u = dynamic_cast(this->uas); + if (u) + { + this->glImage = QGLWidget::convertToGLFormat(u->getImage()); + } } } diff --git a/src/ui/MAVLinkDecoder.cc b/src/ui/MAVLinkDecoder.cc index b1ba65fdd64b6e319b66ea93fee11935c8a2e85f..091b2f4474eb16f2e8165a4e091efebf839775a3 100644 --- a/src/ui/MAVLinkDecoder.cc +++ b/src/ui/MAVLinkDecoder.cc @@ -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); diff --git a/src/ui/QGCFirmwareUpdate.h b/src/ui/QGCFirmwareUpdate.h index 858650bab8ea1f0547d88e907a29d2a438ae784f..852c977c90e51f140f3ee7e1c9ee41b4c37d6ff1 100644 --- a/src/ui/QGCFirmwareUpdate.h +++ b/src/ui/QGCFirmwareUpdate.h @@ -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 diff --git a/src/ui/QGCToolBar.cc b/src/ui/QGCToolBar.cc index c786f14a9c01c7f5775ecbfcdd1b3b106ae01b1b..106725ec0bae32653113d936f9f83c0adbc93a68 100644 --- a/src/ui/QGCToolBar.cc +++ b/src/ui/QGCToolBar.cc @@ -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(); } diff --git a/src/ui/designer/QGCCommandButton.cc b/src/ui/designer/QGCCommandButton.cc index ad50897cbf38969946d84babce5ddb6dfecfb376..e979af8d7cd422ea7797f58c82cddea436643ded 100644 --- a/src/ui/designer/QGCCommandButton.cc +++ b/src/ui/designer/QGCCommandButton.cc @@ -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(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"; diff --git a/src/ui/map/QGCMapWidget.cc b/src/ui/map/QGCMapWidget.cc index 2c5a7aa3e7e3034170388080c187bebc11b79f97..d0c95be23ffc6d1a45c14d276383d140d5089bde 100644 --- a/src/ui/map/QGCMapWidget.cc +++ b/src/ui/map/QGCMapWidget.cc @@ -284,6 +284,68 @@ void QGCMapWidget::updateGlobalPosition() } } +void QGCMapWidget::updateLocalPosition() +{ + QList 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 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) { diff --git a/src/ui/map/QGCMapWidget.h b/src/ui/map/QGCMapWidget.h index 32d2e04734b0ef5efcd6212a880edf3fb8f42cc2..1909ed6abf5ac7286ae61d1fb065d73d09a1fbc8 100644 --- a/src/ui/map/QGCMapWidget.h +++ b/src/ui/map/QGCMapWidget.h @@ -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 */ diff --git a/src/ui/map3D/HUDScaleGeode.cc b/src/ui/map3D/HUDScaleGeode.cc index 0c32c7ab45d635c5692f81b872111c9845ebad69..6ece68bc83fb02d584f7d3490f1a04f38f3f8864 100644 --- a/src/ui/map3D/HUDScaleGeode.cc +++ b/src/ui/map3D/HUDScaleGeode.cc @@ -40,7 +40,7 @@ HUDScaleGeode::HUDScaleGeode() } void -HUDScaleGeode::init(void) +HUDScaleGeode::init(osg::ref_ptr& font) { osg::ref_ptr 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)); diff --git a/src/ui/map3D/HUDScaleGeode.h b/src/ui/map3D/HUDScaleGeode.h index 7f2a370887fcc6ccffcfef97ab9c1caf1fc754b5..0db2809113b37ebec9474143454e0dc0cb9f96b0 100644 --- a/src/ui/map3D/HUDScaleGeode.h +++ b/src/ui/map3D/HUDScaleGeode.h @@ -41,7 +41,7 @@ class HUDScaleGeode : public osg::Geode public: HUDScaleGeode(); - void init(void); + void init(osg::ref_ptr& font); void update(int windowHeight, float cameraFov, float cameraDistance, bool darkBackground); diff --git a/src/ui/map3D/ImageWindowGeode.cc b/src/ui/map3D/ImageWindowGeode.cc index 19a7f38a3d5c0ad64d2d3254427b3845f9fcacbc..3f4d33050870594e3037e5b5978e77eaba8f2ab8 100644 --- a/src/ui/map3D/ImageWindowGeode.cc +++ b/src/ui/map3D/ImageWindowGeode.cc @@ -31,11 +31,17 @@ #include "ImageWindowGeode.h" -ImageWindowGeode::ImageWindowGeode(const QString& caption, - const osg::Vec4& backgroundColor, - osg::ref_ptr& image) +ImageWindowGeode::ImageWindowGeode() : border(5) { + +} + +void +ImageWindowGeode::init(const QString& caption, const osg::Vec4& backgroundColor, + osg::ref_ptr& image, + osg::ref_ptr& font) +{ // image osg::ref_ptr 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)); diff --git a/src/ui/map3D/ImageWindowGeode.h b/src/ui/map3D/ImageWindowGeode.h index 87281d860b68070391903269366c3a0487dce945..5ebad10da0dd1016d6acfadbb8c888ae68e3811b 100644 --- a/src/ui/map3D/ImageWindowGeode.h +++ b/src/ui/map3D/ImageWindowGeode.h @@ -40,8 +40,10 @@ class ImageWindowGeode : public osg::Geode { public: - ImageWindowGeode(const QString& caption, const osg::Vec4& backgroundColor, - osg::ref_ptr& image); + ImageWindowGeode(); + void init(const QString& caption, const osg::Vec4& backgroundColor, + osg::ref_ptr& image, + osg::ref_ptr& font); void setAttributes(int x, int y, int width, int height); diff --git a/src/ui/map3D/ObstacleGroupNode.cc b/src/ui/map3D/ObstacleGroupNode.cc new file mode 100644 index 0000000000000000000000000000000000000000..72120fcea2d58a73c79272beeb396bdd1ef6e7d0 --- /dev/null +++ b/src/ui/map3D/ObstacleGroupNode.cc @@ -0,0 +1,89 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +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 . + +======================================================================*/ + +/** + * @file + * @brief Definition of the class ObstacleGroupNode. + * + * @author Lionel Heng + * + */ + +#include "ObstacleGroupNode.h" + +#include +#include + +#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 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 box = + new osg::Box(obsPos, obs.width(), obs.width(), obs.height()); + osg::ref_ptr 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); +} + diff --git a/src/ui/map3D/ObstacleGroupNode.h b/src/ui/map3D/ObstacleGroupNode.h new file mode 100644 index 0000000000000000000000000000000000000000..0c965cef0d94ee150b1d14ec3715dbae3255ebef --- /dev/null +++ b/src/ui/map3D/ObstacleGroupNode.h @@ -0,0 +1,48 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +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 . + +======================================================================*/ + +/** + * @file + * @brief Definition of the class ObstacleGroupNode. + * + * @author Lionel Heng + * + */ + +#ifndef OBSTACLEGROUPNODE_H +#define OBSTACLEGROUPNODE_H + +#include + +#include "UASInterface.h" + +class ObstacleGroupNode : public osg::Group +{ +public: + ObstacleGroupNode(); + + void init(void); + void update(MAV_FRAME frame, UASInterface* uas); +}; + +#endif // OBSTACLEGROUPNODE_H diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index b212e5102a3274ad6270fa8f6bb0008fb9873350..e8fdf5355b93974133f2b4524902e5ecb9b9c806 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -25,7 +25,7 @@ * @file * @brief Definition of the class Pixhawk3DWidget. * - * @author Lionel Heng + * @author Lionel Heng * */ @@ -46,7 +46,7 @@ #ifdef QGC_PROTOBUF_ENABLED #include -#include +#include #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(index)); - if (mapNode->getImageryType() == Imagery::BLANK_MAP) { + if (mapNode->getImageryType() == Imagery::BLANK_MAP) + { displayImagery = false; - } else { + } + else + { displayImagery = true; } } @@ -211,124 +237,234 @@ 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) { - double altitude = uas->getAltitude(); + if (!uas) + { + return; + } + + osg::Vec2d p; - std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); + if (frame == MAV_FRAME_GLOBAL) + { + double altitude = uas->getAltitude(); - target.set(cursorWorldCoords.first, cursorWorldCoords.second); - } else if (frame == MAV_FRAME_LOCAL_NED) { - double z = uas->getLocalZ(); + std::pair cursorWorldCoords = + getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); - std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), -z); + p.set(cursorWorldCoords.first, cursorWorldCoords.second); + } + else if (frame == MAV_FRAME_LOCAL_NED) + { + double z = uas->getLocalZ(); - target.set(cursorWorldCoords.first, cursorWorldCoords.second); - } + std::pair cursorWorldCoords = + getGlobalCursorPosition(getMouseX(), getMouseY(), -z); + + 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 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 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) { - Waypoint* wp = NULL; - if (frame == MAV_FRAME_GLOBAL) { - double latitude = uas->getLatitude(); - double longitude = uas->getLongitude(); - double altitude = uas->getAltitude(); - double x, y; - QString utmZone; - Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); - - std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); - - Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone, - latitude, longitude); - - wp = new Waypoint(0, longitude, latitude, altitude); - } else if (frame == MAV_FRAME_LOCAL_NED) { - double z = uas->getLocalZ(); - - std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), -z); - - wp = new Waypoint(0, cursorWorldCoords.first, - cursorWorldCoords.second, z); - } + if (!uas) + { + return; + } - if (wp) { - wp->setFrame(frame); - uas->getWaypointManager()->addWaypointEditable(wp); - } + Waypoint* wp = NULL; + if (frame == MAV_FRAME_GLOBAL) + { + double latitude = uas->getLatitude(); + double longitude = uas->getLongitude(); + double altitude = uas->getAltitude(); + double x, y; + QString utmZone; + Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); + + std::pair cursorWorldCoords = + getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), + altitude); + + Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone, + latitude, longitude); + + wp = new Waypoint(0, longitude, latitude, altitude, 0.0, 0.25); + } + else if (frame == MAV_FRAME_LOCAL_NED) + { + double z = uas->getLocalZ(); + + std::pair cursorWorldCoords = + getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z); + + wp = new Waypoint(0, cursorWorldCoords.first, + cursorWorldCoords.second, z, 0.0, 0.25); } + + if (wp) + { + wp->setFrame(frame); + uas->getWaypointManager()->addWaypointEditable(wp); + } + + selectedWpIndex = wp->getId(); + mode = MOVE_WAYPOINT_HEADING_MODE; } void -Pixhawk3DWidget::moveWaypoint(void) +Pixhawk3DWidget::moveWaypointPosition(void) { - mode = MOVE_WAYPOINT_MODE; + if (mode != MOVE_WAYPOINT_POSITION_MODE) + { + mode = MOVE_WAYPOINT_POSITION_MODE; + return; + } + + if (!uas) + { + return; + } + + const QVector waypoints = + uas->getWaypointManager()->getWaypointEditableList(); + Waypoint* waypoint = waypoints.at(selectedWpIndex); + + if (frame == MAV_FRAME_GLOBAL) + { + double latitude = uas->getLatitude(); + double longitude = uas->getLongitude(); + double altitude = uas->getAltitude(); + double x, y; + QString utmZone; + Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); + + std::pair cursorWorldCoords = + getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); + + Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, + utmZone, latitude, longitude); + + waypoint->setX(longitude); + waypoint->setY(latitude); + } + else if (frame == MAV_FRAME_LOCAL_NED) + { + double z = uas->getLocalZ(); + + std::pair cursorWorldCoords = + getGlobalCursorPosition(getMouseX(), getMouseY(), -z); + + waypoint->setX(cursorWorldCoords.first); + waypoint->setY(cursorWorldCoords.second); + } } void -Pixhawk3DWidget::setWaypoint(void) +Pixhawk3DWidget::moveWaypointHeading(void) { - if (uas) { - const QVector waypoints = - uas->getWaypointManager()->getWaypointEditableList(); - Waypoint* waypoint = waypoints.at(selectedWpIndex); - - if (frame == MAV_FRAME_GLOBAL) { - double latitude = uas->getLatitude(); - double longitude = uas->getLongitude(); - double altitude = uas->getAltitude(); - double x, y; - QString utmZone; - Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); - - std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); - - 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) { - double z = uas->getLocalZ(); - - std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), -z); - - waypoint->setX(cursorWorldCoords.first); - waypoint->setY(cursorWorldCoords.second); - waypoint->setZ(z); - } + if (mode != MOVE_WAYPOINT_HEADING_MODE) + { + mode = MOVE_WAYPOINT_HEADING_MODE; + return; + } + + if (!uas) + { + return; + } + + const QVector 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 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) { - bool ok; - const QVector waypoints = - uas->getWaypointManager()->getWaypointEditableList(); - Waypoint* waypoint = waypoints.at(selectedWpIndex); + if (!uas) + { + return; + } - double altitude = waypoint->getZ(); - if (frame == MAV_FRAME_LOCAL_NED) { - altitude = -altitude; - } + bool ok; + const QVector waypoints = + uas->getWaypointManager()->getWaypointEditableList(); + Waypoint* waypoint = waypoints.at(selectedWpIndex); - 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) { - waypoint->setZ(newAltitude); - } else if (frame == MAV_FRAME_LOCAL_NED) { - waypoint->setZ(-newAltitude); - } + double altitude = waypoint->getZ(); + 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) + { + 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 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 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,29 +816,54 @@ 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) { - 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) { - x = uas->getLocalX(); - y = uas->getLocalY(); - z = uas->getLocalZ(); - } + if (!uas) + { + return; + } - roll = uas->getRoll(); - pitch = uas->getPitch(); - yaw = uas->getYaw(); + 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) + { + x = uas->getLocalX(); + y = uas->getLocalY(); + z = uas->getLocalZ(); + } + + roll = uas->getRoll(); + pitch = uas->getPitch(); + yaw = uas->getYaw(); } void @@ -663,23 +878,25 @@ void Pixhawk3DWidget::getPosition(double& x, double& y, double& z, QString& utmZone) { - if (uas) + if (!uas) { - if (frame == MAV_FRAME_GLOBAL) - { - double latitude = uas->getLatitude(); - double longitude = uas->getLongitude(); - double altitude = uas->getAltitude(); + return; + } - Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); - z = -altitude; - } - else if (frame == MAV_FRAME_LOCAL_NED) - { - x = uas->getLocalX(); - y = uas->getLocalY(); - z = uas->getLocalZ(); - } + 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) + { + x = uas->getLocalX(); + y = uas->getLocalY(); + z = uas->getLocalZ(); } } @@ -820,14 +1037,15 @@ Pixhawk3DWidget::createTarget(void) pat->setPosition(osg::Vec3d(0.0, 0.0, 0.0)); - osg::ref_ptr sphere = new osg::Sphere(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.1f); - osg::ref_ptr sphereDrawable = new osg::ShapeDrawable(sphere); - sphereDrawable->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f)); - osg::ref_ptr sphereGeode = new osg::Geode; - sphereGeode->addDrawable(sphereDrawable); - sphereGeode->setName("Target"); + osg::ref_ptr cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.2f, 0.6f); + osg::ref_ptr 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 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 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 cursorPosition = getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ); @@ -1059,7 +1273,7 @@ Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ, maxResolution = 0.25; break; default: - {} + {} } double resolution = minResolution; @@ -1113,8 +1327,18 @@ void Pixhawk3DWidget::updateTarget(double robotX, double robotY) { osg::PositionAttitudeTransform* pat = - static_cast(targetNode.get()); + dynamic_cast(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(pat->getChild(0)); + osg::ShapeDrawable* sd = dynamic_cast(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(geometry->getVertexArray()); osg::Vec4Array* colors = static_cast(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(&rgb)) / 255.0f; @@ -1313,7 +1539,9 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ) float r = *(2 + reinterpret_cast(&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(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(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())); diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index 6773bab0063b4c97f383748dcf8b89b5beefbb52..fe8f255f25fb6844c8f02459cc4915ba4945e3ba 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -25,7 +25,7 @@ * @file * @brief Definition of the class Pixhawk3DWidget. * - * @author Lionel Heng + * @author Lionel Heng * */ @@ -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 > 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; osg::ref_ptr targetNode; osg::ref_ptr rgbd3DNode; +#ifdef QGC_PROTOBUF_ENABLED + osg::ref_ptr obstacleGroupNode; +#endif QVector< osg::ref_ptr > vehicleModels; MAV_FRAME frame; - osg::Vec2d target; + osg::Vec3d target; + QPoint cachedMousePos; double lastRobotX, lastRobotY, lastRobotZ; }; diff --git a/src/ui/map3D/Q3DWidget.cc b/src/ui/map3D/Q3DWidget.cc index a14fd11fe6a356e24cfe784ec15d248db42d810f..1e3f7f079855d43d1084676c444bd687226e462b 100644 --- a/src/ui/map3D/Q3DWidget.cc +++ b/src/ui/map3D/Q3DWidget.cc @@ -35,6 +35,9 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#ifdef QGC_OSG_QT_ENABLED +#include +#endif #ifdef Q_OS_MACX #include #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 fontImpl; + fontImpl = new osgQt::QFontImplementation(QFont(":/general/vera.ttf")); +#else + osg::ref_ptr 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() diff --git a/src/ui/map3D/Q3DWidget.h b/src/ui/map3D/Q3DWidget.h index 1b4c83cbf7df47d717030479b0a8b6f63c4d444b..f3b257a33fe339597eeccdf5c08a3d7f81558eb1 100644 --- a/src/ui/map3D/Q3DWidget.h +++ b/src/ui/map3D/Q3DWidget.h @@ -38,6 +38,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #include #include "GCManipulator.h" @@ -259,6 +260,8 @@ protected: CameraParams cameraParams; /**< Struct representing camera parameters. */ float fps; + + osg::ref_ptr font; }; #endif // Q3DWIDGET_H diff --git a/src/ui/map3D/WaypointGroupNode.cc b/src/ui/map3D/WaypointGroupNode.cc index 4d56ac4888a04bbf08b1f56d611d76c2f4fd8a27..d19ea365efc0b0b32bdc9769069e4b1e3ebf3d67 100644 --- a/src/ui/map3D/WaypointGroupNode.cc +++ b/src/ui/map3D/WaypointGroupNode.cc @@ -25,7 +25,7 @@ This file is part of the QGROUNDCONTROL project * @file * @brief Definition of the class WaypointGroupNode. * - * @author Lionel Heng + * @author Lionel Heng * */ @@ -51,102 +51,148 @@ 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 (!uas) + { + return; + } - if (getNumChildren() > 0) { - removeChild(0, getNumChildren()); - } + 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(); - const QVector& list = uas->getWaypointManager()->getWaypointEditableList(); + 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(); + } - for (int i = 0; i < list.size(); i++) { - Waypoint* wp = list.at(i); + if (getNumChildren() > 0) + { + removeChild(0, getNumChildren()); + } - double wpX, wpY, wpZ; - getPosition(wp, wpX, wpY, wpZ); + const QVector& list = uas->getWaypointManager()->getWaypointEditableList(); - osg::ref_ptr sd = new osg::ShapeDrawable; - osg::ref_ptr cylinder = - new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0), - wp->getAcceptanceRadius(), - fabs(wpZ)); + for (int i = 0; i < list.size(); i++) + { + Waypoint* wp = list.at(i); - sd->setShape(cylinder); - sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); + double wpX, wpY, wpZ; + getPosition(wp, wpX, wpY, wpZ); + double wpYaw = osg::DegreesToRadians(wp->getYaw()); - 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 group = new osg::Group; - osg::ref_ptr geode = new osg::Geode; - geode->addDrawable(sd); + // cone indicates waypoint orientation + osg::ref_ptr sd = new osg::ShapeDrawable; + double coneRadius = wp->getAcceptanceRadius() / 2.0; + osg::ref_ptr cone = + new osg::Cone(osg::Vec3d(wpZ, 0.0, 0.0), + coneRadius, wp->getAcceptanceRadius() * 2.0); - char wpLabel[10]; - sprintf(wpLabel, "wp%d", i); - geode->setName(wpLabel); + sd->setShape(cone); + 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 geode = new osg::Geode; + geode->addDrawable(sd); + + osg::ref_ptr 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 cylinder = + new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0), + wp->getAcceptanceRadius(), + 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 geometry = new osg::Geometry; - osg::ref_ptr 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 = new osg::Geode; + geode->addDrawable(sd); + group->addChild(geode); - osg::ref_ptr 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); + group->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 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 geometry = new osg::Geometry; + osg::ref_ptr 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 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 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 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); } + + pat = new osg::PositionAttitudeTransform; + pat->setPosition(osg::Vec3d(wpY - robotY, + wpX - robotX, + robotZ)); + + addChild(pat); + 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(); diff --git a/src/ui/map3D/WaypointGroupNode.h b/src/ui/map3D/WaypointGroupNode.h index 1c1c4de8ec46e55c2f8ccb0bdfd1bd1e8d360f91..e4df6049df80eabd5918fd8ca95a452460727123 100644 --- a/src/ui/map3D/WaypointGroupNode.h +++ b/src/ui/map3D/WaypointGroupNode.h @@ -25,7 +25,7 @@ This file is part of the QGROUNDCONTROL project * @file * @brief Definition of the class WaypointGroupNode. * - * @author Lionel Heng + * @author Lionel Heng * */ diff --git a/thirdParty/mavlink/include/mavlink_protobuf_manager.hpp b/thirdParty/mavlink/include/mavlink_protobuf_manager.hpp index fad1956440ee9c1cf75892f00eb71dad9ecdf2ca..c25baa650e91d553258b9984b7956785699eef8f 100644 --- a/thirdParty/mavlink/include/mavlink_protobuf_manager.hpp +++ b/thirdParty/mavlink/include/mavlink_protobuf_manager.hpp @@ -42,6 +42,18 @@ public: registerType(msg); } + // register ObstacleList + { + std::tr1::shared_ptr msg(new px::ObstacleList); + registerType(msg); + } + + // register ObstacleMap + { + std::tr1::shared_ptr 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 { diff --git a/thirdParty/mavlink/include/mavlink_types.h b/thirdParty/mavlink/include/mavlink_types.h index 1060be832700ff81b5ea8091a5394969e55ef040..cb82024543bb8f4e71ba54fbaf476b38089af7a6 100644 --- a/thirdParty/mavlink/include/mavlink_types.h +++ b/thirdParty/mavlink/include/mavlink_types.h @@ -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 { diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.pb.h b/thirdParty/mavlink/include/pixhawk/pixhawk.pb.h index 5ca63c9314520ddb82d9649f0eabe2ae1a38027c..9244a7318ac9dc5425addaec10105aceef8076f7 100644 --- a/thirdParty/mavlink/include/pixhawk/pixhawk.pb.h +++ b/thirdParty/mavlink/include/pixhawk/pixhawk.pb.h @@ -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(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)