diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index b5b5509..4e3f155 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -998,6 +998,11 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptrGetTypeName() == path.GetTypeName()) + { + path.CopyFrom(*message); + emit pathChanged(this); + } } #endif diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 5216711..290025b 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -146,6 +146,10 @@ public: px::ObstacleList getObstacleList() const { return obstacleList; } + + px::Path getPath() const { + return path; + } #endif friend class UASWaypointManager; @@ -235,6 +239,7 @@ protected: //COMMENTS FOR TEST UNIT px::PointCloudXYZRGB pointCloud; px::RGBDImage rgbdImage; px::ObstacleList obstacleList; + px::Path path; #endif QMap* > parameters; ///< All parameters @@ -573,6 +578,8 @@ signals: void rgbdImageChanged(UASInterface* uas); /** @brief Obstacle list data has been changed */ void obstacleListChanged(UASInterface* uas); + /** @brief Path data has been changed */ + void pathChanged(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 e3cbacf..37df27f 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -98,6 +98,7 @@ public: virtual px::PointCloudXYZRGB getPointCloud() const = 0; virtual px::RGBDImage getRGBDImage() const = 0; virtual px::ObstacleList getObstacleList() const = 0; + virtual px::Path getPath() const = 0; #endif virtual bool isArmed() const = 0; diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index e8fdf53..71ca8c1 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -61,6 +61,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) , displayRGBD2D(false) , displayRGBD3D(true) , displayObstacleList(true) + , displayPath(true) , enableRGBDColor(false) , enableTarget(false) , followCamera(true) @@ -81,7 +82,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) rollingMap->addChild(gridNode); // generate empty trail model - trailNode = createTrail(); + trailNode = createTrail(osg::Vec4(1.0f, 0.0f, 0.0f, 1.0f)); rollingMap->addChild(trailNode); // generate map model @@ -105,6 +106,10 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) obstacleGroupNode = new ObstacleGroupNode; obstacleGroupNode->init(); rollingMap->addChild(obstacleGroupNode); + + // generate path model + pathNode = createTrail(osg::Vec4(1.0f, 0.8f, 0.0f, 1.0f)); + rollingMap->addChild(pathNode); #endif setupHUD(); @@ -675,6 +680,7 @@ Pixhawk3DWidget::display(void) rollingMap->setChildValue(targetNode, enableTarget); #ifdef QGC_PROTOBUF_ENABLED rollingMap->setChildValue(obstacleGroupNode, displayObstacleList); + rollingMap->setChildValue(pathNode, displayPath); #endif rollingMap->setChildValue(rgbd3DNode, displayRGBD3D); hudGroup->setChildValue(rgb2DGeode, displayRGBD2D); @@ -742,6 +748,11 @@ Pixhawk3DWidget::display(void) { updateObstacles(); } + + if (displayPath) + { + updatePath(robotX, robotY, robotZ); + } #endif updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw, utmZone); @@ -774,6 +785,10 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event) case 'O': displayObstacleList = !displayObstacleList; break; + case 'p': + case 'P': + displayPath = !displayPath; + break; } } @@ -974,30 +989,30 @@ Pixhawk3DWidget::createGrid(void) } osg::ref_ptr -Pixhawk3DWidget::createTrail(void) +Pixhawk3DWidget::createTrail(const osg::Vec4& color) { osg::ref_ptr geode(new osg::Geode()); - trailGeometry = new osg::Geometry(); - trailGeometry->setUseDisplayList(false); - geode->addDrawable(trailGeometry.get()); + osg::ref_ptr geometry(new osg::Geometry()); + geometry->setUseDisplayList(false); + geode->addDrawable(geometry.get()); - trailVertices = new osg::Vec3dArray; - trailGeometry->setVertexArray(trailVertices); + osg::ref_ptr vertices(new osg::Vec3dArray()); + geometry->setVertexArray(vertices); - trailDrawArrays = new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP); - trailGeometry->addPrimitiveSet(trailDrawArrays); + osg::ref_ptr drawArrays(new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP)); + geometry->addPrimitiveSet(drawArrays); - osg::ref_ptr color(new osg::Vec4Array); - color->push_back(osg::Vec4(1.0f, 0.0f, 0.0f, 1.0f)); - trailGeometry->setColorArray(color); - trailGeometry->setColorBinding(osg::Geometry::BIND_OVERALL); + osg::ref_ptr colorArray(new osg::Vec4Array); + colorArray->push_back(color); + geometry->setColorArray(colorArray); + geometry->setColorBinding(osg::Geometry::BIND_OVERALL); osg::ref_ptr stateset(new osg::StateSet); osg::ref_ptr linewidth(new osg::LineWidth()); linewidth->setWidth(1.0f); stateset->setAttributeAndModes(linewidth, osg::StateAttribute::ON); stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF); - trailGeometry->setStateSet(stateset); + geometry->setStateSet(stateset); return geode; } @@ -1227,17 +1242,20 @@ Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ) } } - trailVertices->clear(); + osg::Geometry* geometry = reinterpret_cast(trailNode->getDrawable(0)); + osg::Vec3dArray* vertices = reinterpret_cast(geometry->getVertexArray()); + osg::DrawArrays* drawArrays = reinterpret_cast(geometry->getPrimitiveSet(0)); + vertices->clear(); for (int i = 0; i < trail.size(); ++i) { - trailVertices->push_back(osg::Vec3d(trail[i].y() - robotY, - trail[i].x() - robotX, - -(trail[i].z() - robotZ))); + vertices->push_back(osg::Vec3d(trail[i].y() - robotY, + trail[i].x() - robotX, + -(trail[i].z() - robotZ))); } - trailDrawArrays->setFirst(0); - trailDrawArrays->setCount(trailVertices->size()); - trailGeometry->dirtyBound(); + drawArrays->setFirst(0); + drawArrays->setCount(vertices->size()); + geometry->dirtyBound(); } void @@ -1569,6 +1587,29 @@ Pixhawk3DWidget::updateObstacles(void) obstacleGroupNode->update(frame, uas); } +void +Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ) +{ + px::Path path = uas->getPath(); + + osg::Geometry* geometry = reinterpret_cast(pathNode->getDrawable(0)); + osg::Vec3dArray* vertices = reinterpret_cast(geometry->getVertexArray()); + osg::DrawArrays* drawArrays = reinterpret_cast(geometry->getPrimitiveSet(0)); + vertices->clear(); + for (int i = 0; i < path.waypoints_size(); ++i) + { + const px::Waypoint& wp = path.waypoints(i); + + vertices->push_back(osg::Vec3d(wp.y() - robotY, + wp.x() - robotX, + -(wp.z() - robotZ))); + } + + drawArrays->setFirst(0); + drawArrays->setCount(vertices->size()); + geometry->dirtyBound(); +} + #endif int diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index fe8f255..cc9933d 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -116,7 +116,7 @@ private: void getPosition(double& x, double& y, double& z); osg::ref_ptr createGrid(void); - osg::ref_ptr createTrail(void); + osg::ref_ptr createTrail(const osg::Vec4& color); osg::ref_ptr createMap(void); osg::ref_ptr createRGBD3D(void); osg::ref_ptr createTarget(void); @@ -135,6 +135,7 @@ private: #ifdef QGC_PROTOBUF_ENABLED void updateRGBD(double robotX, double robotY, double robotZ); void updateObstacles(void); + void updatePath(double robotX, double robotY, double robotZ); #endif int findWaypoint(const QPoint& mousePos); @@ -158,12 +159,12 @@ private: bool displayRGBD2D; bool displayRGBD3D; bool displayObstacleList; + bool displayPath; bool enableRGBDColor; bool enableTarget; bool followCamera; - osg::ref_ptr trailVertices; QVarLengthArray trail; osg::ref_ptr vehicleModel; @@ -176,14 +177,13 @@ private: osg::ref_ptr depthImage; osg::ref_ptr gridNode; osg::ref_ptr trailNode; - osg::ref_ptr trailGeometry; - osg::ref_ptr trailDrawArrays; osg::ref_ptr mapNode; osg::ref_ptr waypointGroupNode; osg::ref_ptr targetNode; osg::ref_ptr rgbd3DNode; #ifdef QGC_PROTOBUF_ENABLED osg::ref_ptr obstacleGroupNode; + osg::ref_ptr pathNode; #endif QVector< osg::ref_ptr > vehicleModels;