diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index f9cabce16a25689bc510ddeb0139a387109debea..94227c4faf7005b9e93672133a642b5aef828541 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -31,10 +31,6 @@ #include "QGCMAVLinkUASFactory.h" #include "QGC.h" -#ifdef QGC_PROTOBUF_ENABLED -#include "mavlink_protobuf_manager.hpp" -#endif - /** * The default constructor will create a new MAVLink object sending heartbeats at * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links. @@ -189,7 +185,28 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) #ifdef QGC_PROTOBUF_ENABLED if (message.msgid == MAVLINK_MSG_ID_EXTENDED_MESSAGE) { - mavlink::ProtobufManager::instance(); + mavlink_extended_message_t extended_message; + + extended_message.base_msg = message; + + // read extended header + uint8_t* payload = reinterpret_cast(message.payload64); + memcpy(&extended_message.extended_payload_len, payload + 3, 4); + + const uint8_t* extended_payload = reinterpret_cast(b.constData()) + MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_EXTENDED_HEADER_LEN; + + // copy extended payload data + memcpy(extended_message.extended_payload, extended_payload, extended_message.extended_payload_len); + + if (protobufManager.cacheFragment(extended_message)) + { + std::tr1::shared_ptr protobuf_msg; + + if (protobufManager.getMessage(protobuf_msg)) + { + emit extendedMessageReceived(link, protobuf_msg); + } + } } #endif diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 57e80c86614acf30a1b3f2add002afa53587d3cf..32a3755217aed6f1054ac4421f51efca371b5f14 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -42,6 +42,11 @@ This file is part of the QGROUNDCONTROL project #include "QGCMAVLink.h" #include "QGC.h" +#ifdef QGC_PROTOBUF_ENABLED +#include +#endif + + /** * @brief MAVLink micro air vehicle protocol reference implementation. * @@ -197,10 +202,17 @@ protected: int currLossCounter; bool versionMismatchIgnore; int systemId; +#ifdef QGC_PROTOBUF_ENABLED + mavlink::ProtobufManager protobufManager; +#endif signals: /** @brief Message received and directly copied via signal */ void messageReceived(LinkInterface* link, mavlink_message_t message); +#ifdef QGC_PROTOBUF_ENABLED + /** @brief Message received via signal */ + void extendedMessageReceived(LinkInterface *link, std::tr1::shared_ptr message); +#endif /** @brief Emitted if heartbeat emission mode is changed */ void heartbeatChanged(bool heartbeats); /** @brief Emitted if logging is started / stopped */ diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 57271b35da03595f2020276f79c05dadaa415e72..80b11a81e0861564ef3c521ddc15bc52d7af68dd 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -206,6 +206,14 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) #endif } +#ifdef QGC_PROTOBUF_ENABLED +void PxQuadMAV::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr message) +{ + UAS::receiveExtendedMessage(link, message); +} + +#endif + void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command) { #ifdef MAVLINK_ENABLED_PIXHAWK diff --git a/src/uas/PxQuadMAV.h b/src/uas/PxQuadMAV.h index cac17453e5365e6c2de5d4906f84b6efb18e0850..b52ea999ea02094523ff3e700c9490169f649716 100644 --- a/src/uas/PxQuadMAV.h +++ b/src/uas/PxQuadMAV.h @@ -35,6 +35,10 @@ public: public slots: /** @brief Receive a MAVLink message from this MAV */ void receiveMessage(LinkInterface* link, mavlink_message_t message); +#ifdef QGC_PROTOBUF_ENABLED + /** @brief Receive a Protobuf message from this MAV */ + void receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr message); +#endif /** @brief Send a command to an onboard process */ void sendProcessCommand(int watchdogId, int processId, unsigned int command); signals: diff --git a/src/uas/QGCMAVLinkUASFactory.cc b/src/uas/QGCMAVLinkUASFactory.cc index 0902ba07abd3ebf047499f9709a5597dcaf59291..6b1b9967e24df72acb5755f6a7fb8c64c9234ea0 100644 --- a/src/uas/QGCMAVLinkUASFactory.cc +++ b/src/uas/QGCMAVLinkUASFactory.cc @@ -30,6 +30,9 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte mav->setSystemType((int)heartbeat->type); // Connect this robot to the UAS object connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); +#ifdef QGC_PROTOBUF_ENABLED + connect(mavlink, SIGNAL(extendedMessageReceived(LinkInterface*, std::tr1::shared_ptr)), mav, SLOT(receiveExtendedMessage(LinkInterface*, std::tr1::shared_ptr))); +#endif uas = mav; } break; @@ -43,6 +46,9 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte // else the slot of the parent object is called (and thus the special // packets never reach their goal) connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); +#ifdef QGC_PROTOBUF_ENABLED + connect(mavlink, SIGNAL(extendedMessageReceived(LinkInterface*, std::tr1::shared_ptr)), mav, SLOT(receiveExtendedMessage(LinkInterface*, std::tr1::shared_ptr))); +#endif uas = mav; } break; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 89c403e0b58656bd4f5567124e26185f0b143f0b..e14eb5a6d6d2fdc46b949679695c67041dc180b3 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -970,6 +970,23 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } } +#ifdef QGC_PROTOBUF_ENABLED +void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr message) +{ + if (!link) return; + if (!links->contains(link)) + { + addLink(link); + } + + if (message->GetTypeName() == pointCloud.GetTypeName()) + { + pointCloud.CopyFrom(*message); + } +} + +#endif + void UAS::setHomePosition(double lat, double lon, double alt) { QMessageBox msgBox; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 63e5c5a924876792fa0a1d0b99f741c44b8e8de2..b7583d9de7be04ebbbc675e66dcea70b71c9d137 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -128,6 +128,12 @@ public: } bool getSelected() const; +#ifdef QGC_PROTOBUF_ENABLED + px::PointCloudXYZRGB getPointCloud() const { + return pointCloud; + } +#endif + friend class UASWaypointManager; protected: //COMMENTS FOR TEST UNIT @@ -208,6 +214,10 @@ protected: //COMMENTS FOR TEST UNIT QImage image; ///< Image data of last completely transmitted image quint64 imageStart; +#ifdef QGC_PROTOBUF_ENABLED + px::PointCloudXYZRGB pointCloud; +#endif + QMap* > parameters; ///< All parameters bool paramsOnceRequested; ///< If the parameter list has been read at least once int airframe; ///< The airframe type @@ -451,6 +461,11 @@ public slots: /** @brief Receive a message from one of the communication links. */ virtual void receiveMessage(LinkInterface* link, mavlink_message_t message); +#ifdef QGC_PROTOBUF_ENABLED + /** @brief Receive a message from one of the communication links. */ + virtual void receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr message); +#endif + /** @brief Send a message over this link (to this or to all UAS on this link) */ void sendMessage(LinkInterface* link, mavlink_message_t message); /** @brief Send a message over all links this UAS can be reached with (!= all links) */ diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 96a8d1f315febd230a44f714cd4d55590f36d777..c2ccd1a1629528e4e76657a4277165f81bdb44b4 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -44,6 +44,11 @@ This file is part of the QGROUNDCONTROL project #include "QGCUASParamManager.h" #include "RadioCalibration/RadioCalibrationData.h" +#ifdef QGC_PROTOBUF_ENABLED +#include +#include +#endif + /** * @brief Interface for all robots. * @@ -89,6 +94,10 @@ public: virtual bool getSelected() const = 0; +#ifdef QGC_PROTOBUF_ENABLED + virtual px::PointCloudXYZRGB getPointCloud() const = 0; +#endif + virtual bool isArmed() const = 0; /** @brief Set the airframe of this MAV */ diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index 743ef330c2e4ff6fc15f7ad306d5309934e8cf1b..1c08d0315200270fd4e71d0ca6830a86c7e92370 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -44,6 +44,11 @@ #include "QGC.h" +#ifdef QGC_PROTOBUF_ENABLED +#include +#include +#endif + Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) : Q3DWidget(parent) , uas(NULL) @@ -58,7 +63,6 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) , enableRGBDColor(true) , enableTarget(false) , followCamera(true) - , enableFreenect(false) , frame(MAV_FRAME_GLOBAL) , lastRobotX(0.0f) , lastRobotY(0.0f) @@ -92,16 +96,9 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) targetNode = createTarget(); rollingMap->addChild(targetNode); -#ifdef QGC_LIBFREENECT_ENABLED - freenect.reset(new Freenect()); - enableFreenect = freenect->init(); -#endif - // generate RGBD model - if (enableFreenect) { - rgbd3DNode = createRGBD3D(); - egocentricMap->addChild(rgbd3DNode); - } + rgbd3DNode = createRGBD3D(); + egocentricMap->addChild(rgbd3DNode); setupHUD(); @@ -123,7 +120,8 @@ Pixhawk3DWidget::~Pixhawk3DWidget() * * @param uas the UAS/MAV to monitor/display with the HUD */ -void Pixhawk3DWidget::setActiveUAS(UASInterface* uas) +void +Pixhawk3DWidget::setActiveUAS(UASInterface* uas) { if (this->uas != NULL && this->uas != uas) { // Disconnect any previously connected active MAV @@ -555,11 +553,12 @@ Pixhawk3DWidget::display(void) updateTarget(robotX, robotY); } -#ifdef QGC_LIBFREENECT_ENABLED - if (enableFreenect && (displayRGBD2D || displayRGBD3D)) { - updateRGBD(); +#ifdef QGC_PROTOBUF_ENABLED + if (displayRGBD2D || displayRGBD3D) { + updateRGBD(robotX, robotY, robotZ); } #endif + updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone); // set node visibility @@ -569,9 +568,7 @@ Pixhawk3DWidget::display(void) rollingMap->setChildValue(mapNode, displayImagery); rollingMap->setChildValue(waypointGroupNode, displayWaypoints); rollingMap->setChildValue(targetNode, enableTarget); - if (enableFreenect) { - egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D); - } + egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D); hudGroup->setChildValue(rgb2DGeode, displayRGBD2D); hudGroup->setChildValue(depth2DGeode, displayRGBD2D); @@ -788,7 +785,7 @@ Pixhawk3DWidget::createMap(void) osg::ref_ptr Pixhawk3DWidget::createRGBD3D(void) { - int frameSize = 640 * 480; + int frameSize = 752 * 480; osg::ref_ptr geode(new osg::Geode); osg::ref_ptr geometry(new osg::Geometry); @@ -1218,46 +1215,51 @@ float colormap_jet[128][3] = { {0.5f,0.0f,0.0f} }; -#ifdef QGC_LIBFREENECT_ENABLED +#ifdef QGC_PROTOBUF_ENABLED void -Pixhawk3DWidget::updateRGBD(void) +Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ) { - QSharedPointer rgb = freenect->getRgbData(); - if (!rgb.isNull()) { - rgbImage->setImage(640, 480, 1, - GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, - reinterpret_cast(rgb->data()), - osg::Image::NO_DELETE); - rgbImage->dirty(); - } - - QSharedPointer coloredDepth = freenect->getColoredDepthData(); - if (!coloredDepth.isNull()) { - depthImage->setImage(640, 480, 1, - GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, - reinterpret_cast(coloredDepth->data()), - osg::Image::NO_DELETE); - depthImage->dirty(); - } + px::PointCloudXYZRGB pointCloud = uas->getPointCloud(); + +// QSharedPointer rgb = freenect->getRgbData(); +// if (!rgb.isNull()) { +// rgbImage->setImage(640, 480, 1, +// GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, +// reinterpret_cast(rgb->data()), +// osg::Image::NO_DELETE); +// rgbImage->dirty(); +// } - QSharedPointer< QVector > pointCloud = - freenect->get6DPointCloudData(); +// QSharedPointer coloredDepth = freenect->getColoredDepthData(); +// if (!coloredDepth.isNull()) { +// depthImage->setImage(640, 480, 1, +// GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, +// reinterpret_cast(coloredDepth->data()), +// osg::Image::NO_DELETE); +// depthImage->dirty(); +// } osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry(); osg::Vec3Array* vertices = static_cast(geometry->getVertexArray()); osg::Vec4Array* colors = static_cast(geometry->getColorArray()); - for (int i = 0; i < pointCloud->size(); ++i) { - double x = pointCloud->at(i).x; - double y = pointCloud->at(i).y; - double z = pointCloud->at(i).z; - (*vertices)[i].set(x, z, -y); + for (int i = 0; i < pointCloud.points_size(); ++i) { + const px::PointCloudXYZRGB_PointXYZRGB& p = pointCloud.points(i); + + double x = p.x() - robotX; + double y = p.y() - robotY; + double z = p.z() - robotZ; + + (*vertices)[i].set(y, x, -z); if (enableRGBDColor) { - (*colors)[i].set(pointCloud->at(i).r / 255.0f, - pointCloud->at(i).g / 255.0f, - pointCloud->at(i).b / 255.0f, - 1.0f); + float rgb = p.rgb(); + + float b = *(reinterpret_cast(&rgb)) / 255.0f; + float g = *(1 + reinterpret_cast(&rgb)) / 255.0f; + float r = *(2 + reinterpret_cast(&rgb)) / 255.0f; + + (*colors)[i].set(r, g, b, 1.0f); } else { double dist = sqrt(x * x + y * y + z * z); int colorIndex = static_cast(fmin(dist / 7.0 * 127.0, 127.0)); @@ -1270,10 +1272,10 @@ Pixhawk3DWidget::updateRGBD(void) if (geometry->getNumPrimitiveSets() == 0) { geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS, - 0, pointCloud->size())); + 0, pointCloud.points_size())); } else { osg::DrawArrays* drawarrays = static_cast(geometry->getPrimitiveSet(0)); - drawarrays->setCount(pointCloud->size()); + drawarrays->setCount(pointCloud.points_size()); } } #endif diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index 1cc5072adccb6bc5a4bc87ee7ed356b45a59d7f3..6773bab0063b4c97f383748dcf8b89b5beefbb52 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -39,10 +39,6 @@ #include "ImageWindowGeode.h" #include "WaypointGroupNode.h" -#ifdef QGC_LIBFREENECT_ENABLED -#include "Freenect.h" -#endif - #include "Q3DWidget.h" class UASInterface; @@ -115,8 +111,8 @@ private: const QString& zone); void updateWaypoints(void); void updateTarget(double robotX, double robotY); -#ifdef QGC_LIBFREENECT_ENABLED - void updateRGBD(void); +#ifdef QGC_PROTOBUF_ENABLED + void updateRGBD(double robotX, double robotY, double robotZ); #endif int findWaypoint(int mouseX, int mouseY); @@ -161,10 +157,6 @@ private: osg::ref_ptr waypointGroupNode; osg::ref_ptr targetNode; osg::ref_ptr rgbd3DNode; -#ifdef QGC_LIBFREENECT_ENABLED - QScopedPointer freenect; -#endif - bool enableFreenect; QVector< osg::ref_ptr > vehicleModels;