From 15852ad56856fb005d1292c2141aedbb25ca8e92 Mon Sep 17 00:00:00 2001 From: hengli Date: Mon, 3 Jan 2011 18:32:59 +0100 Subject: [PATCH 1/3] 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 da453ba..a3ed155 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(); @@ -237,6 +242,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) { if (uas) @@ -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 2d3b1a5..1cc5072 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; }; From 3b7e0db1c7da7eb7c3df8a9f1da292ed0c36b0ad Mon Sep 17 00:00:00 2001 From: Lionel Heng Date: Tue, 4 Jan 2011 00:08:41 +0100 Subject: [PATCH 2/3] Improved custom widget functionality. --- src/uas/UAS.cc | 13 +++++++ src/uas/UAS.h | 3 ++ src/uas/UASInterface.h | 3 ++ src/ui/designer/QGCActionButton.cc | 76 +++++++++++++++++++++++++++++++++++++- src/ui/designer/QGCActionButton.h | 8 ++++ src/ui/designer/QGCActionButton.ui | 34 +++++------------ 6 files changed, 111 insertions(+), 26 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 7fc1a66..2110411 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1291,6 +1291,19 @@ void UAS::setParameter(int component, QString id, float value) } /** + * Sets an action + * + **/ +void UAS::setAction(MAV_ACTION action) +{ + mavlink_message_t msg; + mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, action); + // Send message twice to increase chance that it reaches its goal + sendMessage(msg); + sendMessage(msg); +} + +/** * Launches the system * **/ diff --git a/src/uas/UAS.h b/src/uas/UAS.h index d246b67..410b15e 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -174,6 +174,9 @@ public: void setAutopilotType(int apType) { autopilot = apType;} public slots: + /** @brief Sets an action **/ + void setAction(MAV_ACTION action); + /** @brief Launches the system **/ void launch(); /** @brief Write this waypoint to the list of waypoints */ diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index ae57720..8bbc716 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -162,6 +162,9 @@ public: public slots: + /** @brief Sets an action **/ + virtual void setAction(MAV_ACTION action) = 0; + /** @brief Launches the system/Liftof **/ virtual void launch() = 0; /** @brief Set a new waypoint **/ diff --git a/src/ui/designer/QGCActionButton.cc b/src/ui/designer/QGCActionButton.cc index 84c5f2f..fe59d62 100644 --- a/src/ui/designer/QGCActionButton.cc +++ b/src/ui/designer/QGCActionButton.cc @@ -1,13 +1,66 @@ #include "QGCActionButton.h" #include "ui_QGCActionButton.h" +#include "MAVLinkProtocol.h" +#include "UASManager.h" + +const char* kActionLabels[MAV_ACTION_NB] = +{"HOLD", + "START MOTORS", + "LAUNCH", + "RETURN", + "EMERGENCY LAND", + "EMERGENCY KILL", + "CONFIRM KILL", + "CONTINUE", + "STOP MOTORS", + "HALT", + "SHUTDOWN", + "REBOOT", + "SET MANUAL", + "SET AUTO", + "READ STORAGE", + "WRITE STORAGE", + "CALIBRATE RC", + "CALIBRATE GYRO", + "CALIBRATE MAG", + "CALIBRATE PRESSURE", + "START REC", + "PAUSE REC", + "STOP REC", + "TAKEOFF", + "NAVIGATE", + "LAND", + "LOITER", + "SET ORIGIN", + "RELAY ON", + "RELAY OFF", + "GET IMAGE", + "START VIDEO", + "STOP VIDEO", + "RESET MAP"}; + QGCActionButton::QGCActionButton(QWidget *parent) : QGCToolWidgetItem(parent), - ui(new Ui::QGCActionButton) + ui(new Ui::QGCActionButton), + uas(NULL) { ui->setupUi(this); + + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + this, SLOT(setActiveUAS(UASInterface*))); + + connect(ui->actionButton, SIGNAL(clicked()), this, SLOT(sendAction())); connect(ui->editFinishButton, SIGNAL(clicked()), this, SLOT(endEditMode())); + connect(ui->editButtonName, SIGNAL(textChanged(QString)), this, SLOT(setActionButtonName(QString))); + connect(ui->editActionComboBox, SIGNAL(currentIndexChanged(QString)), ui->nameLabel, SLOT(setText(QString))); endEditMode(); + + // add action labels to combobox + for (int i = 0; i < MAV_ACTION_NB; i++) + { + ui->editActionComboBox->addItem(kActionLabels[i]); + } } QGCActionButton::~QGCActionButton() @@ -15,6 +68,22 @@ QGCActionButton::~QGCActionButton() delete ui; } +void QGCActionButton::sendAction() +{ + if (uas) + { + MAV_ACTION action = static_cast( + ui->editActionComboBox->currentIndex()); + + uas->setAction(action); + } +} + +void QGCActionButton::setActionButtonName(QString text) +{ + ui->actionButton->setText(text); +} + void QGCActionButton::startEditMode() { ui->editActionComboBox->show(); @@ -30,3 +99,8 @@ void QGCActionButton::endEditMode() ui->editFinishButton->hide(); isInEditMode = false; } + +void QGCActionButton::setActiveUAS(UASInterface *uas) +{ + this->uas = uas; +} diff --git a/src/ui/designer/QGCActionButton.h b/src/ui/designer/QGCActionButton.h index 63556d6..9f21b73 100644 --- a/src/ui/designer/QGCActionButton.h +++ b/src/ui/designer/QGCActionButton.h @@ -7,6 +7,8 @@ namespace Ui { class QGCActionButton; } +class UASInterface; + class QGCActionButton : public QGCToolWidgetItem { Q_OBJECT @@ -16,11 +18,17 @@ public: ~QGCActionButton(); public slots: + void sendAction(); + void setActionButtonName(QString text); void startEditMode(); void endEditMode(); +private slots: + void setActiveUAS(UASInterface* uas); + private: Ui::QGCActionButton *ui; + UASInterface* uas; }; #endif // QGCACTIONBUTTON_H diff --git a/src/ui/designer/QGCActionButton.ui b/src/ui/designer/QGCActionButton.ui index 8622481..2d04693 100644 --- a/src/ui/designer/QGCActionButton.ui +++ b/src/ui/designer/QGCActionButton.ui @@ -21,13 +21,6 @@ - - - - Button name - - - @@ -59,38 +52,29 @@ + + + + Button name + + + - editButtonName - textChanged(QString) - actionButton - setWindowTitle(QString) - - - 310 - 22 - - - 310 - 55 - - - - editNameLabel textChanged(QString) nameLabel setText(QString) - 116 + 114 22 - 116 + 114 55 From 2f721788a5e9fdba7c96c0fa291f9e331009abe9 Mon Sep 17 00:00:00 2001 From: Lionel Heng Date: Wed, 5 Jan 2011 12:19:52 +0100 Subject: [PATCH 3/3] Fixed bugs in custom widget and 3D view. --- src/ui/designer/QGCActionButton.cc | 1 + src/ui/map3D/Pixhawk3DWidget.cc | 2 -- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/ui/designer/QGCActionButton.cc b/src/ui/designer/QGCActionButton.cc index fe59d62..3bb9671 100644 --- a/src/ui/designer/QGCActionButton.cc +++ b/src/ui/designer/QGCActionButton.cc @@ -24,6 +24,7 @@ const char* kActionLabels[MAV_ACTION_NB] = "CALIBRATE RC", "CALIBRATE GYRO", "CALIBRATE MAG", + "CALIBRATE ACC", "CALIBRATE PRESSURE", "START REC", "PAUSE REC", diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index a3ed155..27dc8ac 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -574,8 +574,6 @@ Pixhawk3DWidget::display(void) lastRobotZ = robotZ; recenterCamera(robotY, robotX, -robotZ); - - return; } if (followCamera)