Skip to content

Q
qgroundcontrol
Commit 2e26be8a authored by hengli's avatar hengli
Browse files
Options

Enabled real-time mouse-based selection of target and waypoint headings.

parent b81bdcfe
Hide whitespace changes
Inline Side-by-side
Showing with 136 additions and 51 deletions
src/ui/map3D/Pixhawk3DWidget.cc
View file @ 2e26be8a
......@@ -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<double,double> 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<double,double> 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<double,double> 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<double,double> 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<double,double> 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,10 +414,52 @@ 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<Waypoint *> 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<double,double> 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)
{
......@@ -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<osg::Cone> cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.1f, 0.4f);
osg::ref_ptr<osg::Cone> cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.2f, 0.6f);
osg::ref_ptr<osg::ShapeDrawable> 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<osg::Geode*>(pat->getChild(0));
osg::ShapeDrawable* sd = dynamic_cast<osg::ShapeDrawable*>(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()));
......
src/ui/map3D/Pixhawk3DWidget.h
View file @ 2e26be8a
......@@ -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;
};
......
src/ui/map3D/WaypointGroupNode.cc
View file @ 2e26be8a
......@@ -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<osg::Group> group = new osg::Group;
// cone indicates waypoint orientation
osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
double coneRadius = wp->getAcceptanceRadius() / 2.0;
osg::ref_ptr<osg::Cone> 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<osg::Geode> geode = new osg::Geode;
geode->addDrawable(sd);
osg::ref_ptr<osg::PositionAttitudeTransform> 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<osg::Cylinder> 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<osg::Geode> 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<osg::PositionAttitudeTransform> 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);
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment