From 2e26be8a48e14e9b2e4dfc455a23df031e14d156 Mon Sep 17 00:00:00 2001 From: hengli Date: Wed, 18 Jan 2012 13:48:46 +0100 Subject: [PATCH 1/2] Enabled real-time mouse-based selection of target and waypoint headings. --- src/ui/map3D/Pixhawk3DWidget.cc | 130 ++++++++++++++++++++++++++------------ src/ui/map3D/Pixhawk3DWidget.h | 12 ++-- src/ui/map3D/WaypointGroupNode.cc | 45 +++++++++++-- 3 files changed, 136 insertions(+), 51 deletions(-) diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index 420b37c..e294edf 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -114,7 +114,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) buildLayout(); - updateHUD(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, "132N"); + updateHUD(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, "32N"); connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); @@ -292,7 +292,8 @@ Pixhawk3DWidget::selectTarget(void) double altitude = uas->getAltitude(); std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); + getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), + altitude); target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0); } @@ -301,14 +302,14 @@ Pixhawk3DWidget::selectTarget(void) double z = uas->getLocalZ(); std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), -z); + getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z); target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0); } enableTarget = true; - mode = SELECT_TARGET_YAW_MODE; + mode = SELECT_TARGET_HEADING_MODE; } void @@ -316,7 +317,8 @@ Pixhawk3DWidget::setTarget(void) { selectTargetHeading(); - uas->setTargetPosition(target.x(), target.y(), 0.0, target.z()); + uas->setTargetPosition(target.x(), target.y(), 0.0, + osg::RadiansToDegrees(target.z())); } void @@ -338,7 +340,8 @@ Pixhawk3DWidget::insertWaypoint(void) Imagery::LLtoUTM(latitude, longitude, x, y, utmZone); std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); + getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), + altitude); Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone, latitude, longitude); @@ -350,7 +353,7 @@ Pixhawk3DWidget::insertWaypoint(void) double z = uas->getLocalZ(); std::pair cursorWorldCoords = - getGlobalCursorPosition(getMouseX(), getMouseY(), -z); + getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z); wp = new Waypoint(0, cursorWorldCoords.first, cursorWorldCoords.second, z, 0.0, 0.25); @@ -361,17 +364,20 @@ Pixhawk3DWidget::insertWaypoint(void) wp->setFrame(frame); uas->getWaypointManager()->addWaypointEditable(wp); } -} -void -Pixhawk3DWidget::moveWaypoint(void) -{ - mode = MOVE_WAYPOINT_MODE; + selectedWpIndex = wp->getId(); + mode = MOVE_WAYPOINT_HEADING_MODE; } void -Pixhawk3DWidget::setWaypoint(void) +Pixhawk3DWidget::moveWaypointPosition(void) { + if (mode != MOVE_WAYPOINT_POSITION_MODE) + { + mode = MOVE_WAYPOINT_POSITION_MODE; + return; + } + if (!uas) { return; @@ -393,12 +399,11 @@ Pixhawk3DWidget::setWaypoint(void) std::pair cursorWorldCoords = getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); - Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone, - latitude, longitude); + 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) { @@ -409,11 +414,53 @@ Pixhawk3DWidget::setWaypoint(void) waypoint->setX(cursorWorldCoords.first); waypoint->setY(cursorWorldCoords.second); - waypoint->setZ(z); } } void +Pixhawk3DWidget::moveWaypointHeading(void) +{ + 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) @@ -738,27 +785,23 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event) { if (event->button() == Qt::LeftButton) { - if (mode == MOVE_WAYPOINT_MODE) + if (mode == SELECT_TARGET_HEADING_MODE) { - setWaypoint(); - mode = DEFAULT_MODE; - - return; + setTarget(); } - if (mode == SELECT_TARGET_YAW_MODE) + if (mode != DEFAULT_MODE) { - setTarget(); mode = DEFAULT_MODE; - - return; } if (event->modifiers() == Qt::ShiftModifier) { - selectedWpIndex = findWaypoint(event->x(), event->y()); + selectedWpIndex = findWaypoint(event->pos()); if (selectedWpIndex == -1) { + cachedMousePos = event->pos(); + showInsertWaypointMenu(event->globalPos()); } else @@ -776,10 +819,18 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event) void Pixhawk3DWidget::mouseMoveEvent(QMouseEvent* event) { - if (mode == SELECT_TARGET_YAW_MODE) + if (mode == SELECT_TARGET_HEADING_MODE) { selectTargetHeading(); } + if (mode == MOVE_WAYPOINT_POSITION_MODE) + { + moveWaypointPosition(); + } + if (mode == MOVE_WAYPOINT_HEADING_MODE) + { + moveWaypointHeading(); + } Q3DWidget::mouseMoveEvent(event); } @@ -986,7 +1037,7 @@ Pixhawk3DWidget::createTarget(void) pat->setPosition(osg::Vec3d(0.0, 0.0, 0.0)); - osg::ref_ptr cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.1f, 0.4f); + 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); @@ -1285,14 +1336,9 @@ Pixhawk3DWidget::updateTarget(double robotX, double robotY) osg::Geode* geode = dynamic_cast(pat->getChild(0)); osg::ShapeDrawable* sd = dynamic_cast(geode->getDrawable(0)); - if (mode == SELECT_TARGET_YAW_MODE) - { - sd->setColor(osg::Vec4f(1.0f, 0.8f, 0.0f, 1.0f)); - } - else - { - sd->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f)); - } + + + sd->setColor(osg::Vec4f(1.0f, 0.8f, 0.0f, 1.0f)); } float colormap_jet[128][3] = { @@ -1526,13 +1572,14 @@ Pixhawk3DWidget::updateObstacles(void) #endif int -Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY) +Pixhawk3DWidget::findWaypoint(const QPoint& mousePos) { 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++) @@ -1596,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 032d7b4..326fac8 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -74,8 +74,8 @@ private slots: 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); @@ -123,15 +123,16 @@ private: 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, - SELECT_TARGET_YAW_MODE + MOVE_WAYPOINT_POSITION_MODE, + MOVE_WAYPOINT_HEADING_MODE, + SELECT_TARGET_HEADING_MODE }; Mode mode; int selectedWpIndex; @@ -175,6 +176,7 @@ private: MAV_FRAME frame; osg::Vec3d target; + QPoint cachedMousePos; double lastRobotX, lastRobotY, lastRobotZ; }; diff --git a/src/ui/map3D/WaypointGroupNode.cc b/src/ui/map3D/WaypointGroupNode.cc index 246bf04..d19ea36 100644 --- a/src/ui/map3D/WaypointGroupNode.cc +++ b/src/ui/map3D/WaypointGroupNode.cc @@ -89,8 +89,42 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas) double wpX, wpY, wpZ; getPosition(wp, wpX, wpY, wpZ); + double wpYaw = osg::DegreesToRadians(wp->getYaw()); + osg::ref_ptr group = new osg::Group; + + // 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); + + sd->setShape(cone); + 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 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(), @@ -108,12 +142,13 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas) sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f)); } - osg::ref_ptr geode = new osg::Geode; + geode = new osg::Geode; geode->addDrawable(sd); + group->addChild(geode); char wpLabel[10]; sprintf(wpLabel, "wp%d", i); - geode->setName(wpLabel); + group->setName(wpLabel); if (i < list.size() - 1) { @@ -143,15 +178,13 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas) geode->addDrawable(geometry); } - osg::ref_ptr pat = - new osg::PositionAttitudeTransform; - + pat = new osg::PositionAttitudeTransform; pat->setPosition(osg::Vec3d(wpY - robotY, wpX - robotX, robotZ)); addChild(pat); - pat->addChild(geode); + pat->addChild(group); } } From 7f07336a959cbb559418b47e8635661f75bce73e Mon Sep 17 00:00:00 2001 From: hengli Date: Wed, 18 Jan 2012 16:00:59 +0100 Subject: [PATCH 2/2] Fixed long command bug: sends the required 7 parameters instead of 4 parameters. --- src/uas/UAS.cc | 21 +-------------------- src/uas/UAS.h | 2 -- src/uas/UASInterface.h | 2 +- src/ui/designer/QGCCommandButton.cc | 6 ++++-- 4 files changed, 6 insertions(+), 25 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 3807404..b5b5509 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1911,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; @@ -2196,7 +2177,7 @@ void UAS::shutdown() void UAS::setTargetPosition(float x, float y, float z, float yaw) { mavlink_message_t msg; - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 2, 2, 0, yaw, x, y, z); + 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 ea19081..5216711 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -411,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 */ diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index f54f1af..371341e 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -215,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/designer/QGCCommandButton.cc b/src/ui/designer/QGCCommandButton.cc index ad50897..e979af8 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";