From 15852ad56856fb005d1292c2141aedbb25ca8e92 Mon Sep 17 00:00:00 2001 From: hengli Date: Mon, 3 Jan 2011 18:32:59 +0100 Subject: [PATCH] Added selection of target for path planner. --- src/ui/map3D/Pixhawk3DWidget.cc | 98 ++++++++++++++++++++++++++++++++- src/ui/map3D/Pixhawk3DWidget.h | 7 +++ 2 files changed, 104 insertions(+), 1 deletion(-) diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index da453ba92..a3ed15568 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -56,6 +56,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) , displayRGBD2D(false) , displayRGBD3D(false) , enableRGBDColor(true) + , enableTarget(false) , followCamera(true) , enableFreenect(false) , frame(MAV_FRAME_GLOBAL) @@ -87,6 +88,10 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) waypointGroupNode->init(); rollingMap->addChild(waypointGroupNode); + // generate target model + targetNode = createTarget(); + rollingMap->addChild(targetNode); + #ifdef QGC_LIBFREENECT_ENABLED freenect.reset(new Freenect()); enableFreenect = freenect->init(); @@ -236,6 +241,36 @@ Pixhawk3DWidget::toggleFollowCamera(int32_t state) } } +void +Pixhawk3DWidget::selectTarget(void) +{ + if (uas) + { + if (frame == MAV_FRAME_GLOBAL) + { + double altitude = uas->getAltitude(); + + std::pair cursorWorldCoords = + getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); + + target.set(cursorWorldCoords.first, cursorWorldCoords.second); + } + else if (frame == MAV_FRAME_LOCAL) + { + double z = uas->getLocalZ(); + + std::pair cursorWorldCoords = + getGlobalCursorPosition(getMouseX(), getMouseY(), -z); + + target.set(cursorWorldCoords.first, cursorWorldCoords.second); + } + + uas->setTargetPosition(target.x(), target.y(), 0.0, 0.0); + + enableTarget = true; + } +} + void Pixhawk3DWidget::insertWaypoint(void) { @@ -572,6 +607,11 @@ Pixhawk3DWidget::display(void) updateWaypoints(); } + if (enableTarget) + { + updateTarget(robotX, robotY); + } + #ifdef QGC_LIBFREENECT_ENABLED if (enableFreenect && (displayRGBD2D || displayRGBD3D)) { @@ -586,6 +626,7 @@ Pixhawk3DWidget::display(void) rollingMap->setChildValue(trailNode, displayTrail); rollingMap->setChildValue(mapNode, displayImagery); rollingMap->setChildValue(waypointGroupNode, displayWaypoints); + rollingMap->setChildValue(targetNode, enableTarget); if (enableFreenect) { egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D); @@ -843,6 +884,26 @@ Pixhawk3DWidget::createRGBD3D(void) return geode; } +osg::ref_ptr +Pixhawk3DWidget::createTarget(void) +{ + osg::ref_ptr pat = + new osg::PositionAttitudeTransform; + + 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"); + + pat->addChild(sphereGeode); + + return pat; +} + void Pixhawk3DWidget::setupHUD(void) { @@ -1118,6 +1179,14 @@ Pixhawk3DWidget::updateWaypoints(void) waypointGroupNode->update(frame, uas); } +void +Pixhawk3DWidget::updateTarget(double robotX, double robotY) +{ + osg::PositionAttitudeTransform* pat = + static_cast(targetNode.get()); + pat->setPosition(osg::Vec3d(target.y() - robotY, target.x() - robotX, 0.0)); +} + float colormap_jet[128][3] = { {0.0f,0.0f,0.53125f}, @@ -1336,7 +1405,6 @@ Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY) std::string nodeName = it->nodePath[i]->getName(); if (nodeName.substr(0, 2).compare("wp") == 0) { - qDebug() << nodeName.c_str() << "Got!!"; return atoi(nodeName.substr(2).c_str()); } } @@ -1347,12 +1415,40 @@ Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY) return -1; } +bool +Pixhawk3DWidget::findTarget(int mouseX, int mouseY) +{ + if (getSceneData() != NULL) + { + osgUtil::LineSegmentIntersector::Intersections 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) + { + std::string nodeName = it->nodePath[i]->getName(); + if (nodeName.compare("Target") == 0) + { + return true; + } + } + } + } + } + + return false; +} + void Pixhawk3DWidget::showInsertWaypointMenu(const QPoint &cursorPos) { QMenu menu; menu.addAction("Insert new waypoint", this, SLOT(insertWaypoint())); menu.addAction("Clear all waypoints", this, SLOT(clearAllWaypoints())); + menu.addAction("Select target", this, SLOT(selectTarget())); menu.exec(cursorPos); } diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index 2d3b1a57b..1cc5072ad 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -71,6 +71,7 @@ private slots: void recenter(void); void toggleFollowCamera(int state); + void selectTarget(void); void insertWaypoint(void); void moveWaypoint(void); void setWaypoint(void); @@ -101,6 +102,7 @@ private: osg::ref_ptr createTrail(void); osg::ref_ptr createMap(void); osg::ref_ptr createRGBD3D(void); + osg::ref_ptr createTarget(void); void setupHUD(void); void resizeHUD(void); @@ -112,11 +114,13 @@ private: void updateImagery(double originX, double originY, double originZ, const QString& zone); void updateWaypoints(void); + void updateTarget(double robotX, double robotY); #ifdef QGC_LIBFREENECT_ENABLED void updateRGBD(void); #endif int findWaypoint(int mouseX, int mouseY); + bool findTarget(int mouseX, int mouseY); void showInsertWaypointMenu(const QPoint& cursorPos); void showEditWaypointMenu(const QPoint& cursorPos); @@ -134,6 +138,7 @@ private: bool displayRGBD2D; bool displayRGBD3D; bool enableRGBDColor; + bool enableTarget; bool followCamera; @@ -154,6 +159,7 @@ private: osg::ref_ptr trailDrawArrays; osg::ref_ptr mapNode; osg::ref_ptr waypointGroupNode; + osg::ref_ptr targetNode; osg::ref_ptr rgbd3DNode; #ifdef QGC_LIBFREENECT_ENABLED QScopedPointer freenect; @@ -163,6 +169,7 @@ private: QVector< osg::ref_ptr > vehicleModels; MAV_FRAME frame; + osg::Vec2d target; double lastRobotX, lastRobotY, lastRobotZ; }; -- 2.22.0