diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 8d04d39..3807404 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2196,7 +2196,7 @@ void UAS::shutdown() void UAS::setTargetPosition(float x, float y, float z, float yaw) { mavlink_message_t msg; - mavlink_msg_set_local_position_setpoint_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_FRAME_LOCAL_NED, x, y, z, yaw); + 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); sendMessage(msg); } diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index 9341fc5..420b37c 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -248,13 +248,15 @@ Pixhawk3DWidget::toggleFollowCamera(int32_t state) } void -Pixhawk3DWidget::selectTarget(void) +Pixhawk3DWidget::selectTargetHeading(void) { if (!uas) { return; } + osg::Vec2d p; + if (frame == MAV_FRAME_GLOBAL) { double altitude = uas->getAltitude(); @@ -262,7 +264,7 @@ Pixhawk3DWidget::selectTarget(void) std::pair cursorWorldCoords = getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); - target.set(cursorWorldCoords.first, cursorWorldCoords.second); + p.set(cursorWorldCoords.first, cursorWorldCoords.second); } else if (frame == MAV_FRAME_LOCAL_NED) { @@ -271,12 +273,50 @@ Pixhawk3DWidget::selectTarget(void) std::pair cursorWorldCoords = getGlobalCursorPosition(getMouseX(), getMouseY(), -z); - target.set(cursorWorldCoords.first, cursorWorldCoords.second); + p.set(cursorWorldCoords.first, cursorWorldCoords.second); } - uas->setTargetPosition(target.x(), target.y(), 0.0, 0.0); + target.z() = atan2(p.y() - target.y(), p.x() - target.x()); +} + +void +Pixhawk3DWidget::selectTarget(void) +{ + if (!uas) + { + return; + } + + if (frame == MAV_FRAME_GLOBAL) + { + double altitude = uas->getAltitude(); + + std::pair cursorWorldCoords = + getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); + + target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0); + } + else if (frame == MAV_FRAME_LOCAL_NED) + { + double z = uas->getLocalZ(); + + std::pair cursorWorldCoords = + getGlobalCursorPosition(getMouseX(), getMouseY(), -z); + + target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0); + } enableTarget = true; + + mode = SELECT_TARGET_YAW_MODE; +} + +void +Pixhawk3DWidget::setTarget(void) +{ + selectTargetHeading(); + + uas->setTargetPosition(target.x(), target.y(), 0.0, target.z()); } void @@ -303,7 +343,7 @@ Pixhawk3DWidget::insertWaypoint(void) Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone, latitude, longitude); - wp = new Waypoint(0, longitude, latitude, altitude); + wp = new Waypoint(0, longitude, latitude, altitude, 0.0, 0.25); } else if (frame == MAV_FRAME_LOCAL_NED) { @@ -313,7 +353,7 @@ Pixhawk3DWidget::insertWaypoint(void) getGlobalCursorPosition(getMouseX(), getMouseY(), -z); wp = new Waypoint(0, cursorWorldCoords.first, - cursorWorldCoords.second, z); + cursorWorldCoords.second, z, 0.0, 0.25); } if (wp) @@ -706,6 +746,14 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event) return; } + if (mode == SELECT_TARGET_YAW_MODE) + { + setTarget(); + mode = DEFAULT_MODE; + + return; + } + if (event->modifiers() == Qt::ShiftModifier) { selectedWpIndex = findWaypoint(event->x(), event->y()); @@ -726,6 +774,17 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event) } void +Pixhawk3DWidget::mouseMoveEvent(QMouseEvent* event) +{ + if (mode == SELECT_TARGET_YAW_MODE) + { + selectTargetHeading(); + } + + Q3DWidget::mouseMoveEvent(event); +} + +void Pixhawk3DWidget::getPose(double& x, double& y, double& z, double& roll, double& pitch, double& yaw, QString& utmZone) @@ -927,14 +986,15 @@ Pixhawk3DWidget::createTarget(void) 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"); + osg::ref_ptr cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.1f, 0.4f); + 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); + osg::ref_ptr coneGeode = new osg::Geode; + coneGeode->addDrawable(coneDrawable); + coneGeode->setName("Target"); - pat->addChild(sphereGeode); + pat->addChild(coneGeode); return pat; } @@ -1216,8 +1276,23 @@ void Pixhawk3DWidget::updateTarget(double robotX, double robotY) { osg::PositionAttitudeTransform* pat = - static_cast(targetNode.get()); + dynamic_cast(targetNode.get()); + pat->setPosition(osg::Vec3d(target.y() - robotY, target.x() - robotX, 0.0)); + pat->setAttitude(osg::Quat(target.z() - 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))); + + 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)); + } } float colormap_jet[128][3] = { diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index c411654..032d7b4 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -70,7 +70,9 @@ private slots: void recenter(void); void toggleFollowCamera(int state); + void selectTargetHeading(void); void selectTarget(void); + void setTarget(void); void insertWaypoint(void); void moveWaypoint(void); void setWaypoint(void); @@ -85,6 +87,7 @@ protected: virtual void display(void); virtual void keyPressEvent(QKeyEvent* event); virtual void mousePressEvent(QMouseEvent* event); + virtual void mouseMoveEvent(QMouseEvent* event); UASInterface* uas; @@ -127,7 +130,8 @@ private: enum Mode { DEFAULT_MODE, - MOVE_WAYPOINT_MODE + MOVE_WAYPOINT_MODE, + SELECT_TARGET_YAW_MODE }; Mode mode; int selectedWpIndex; @@ -170,7 +174,7 @@ private: QVector< osg::ref_ptr > vehicleModels; MAV_FRAME frame; - osg::Vec2d target; + osg::Vec3d target; double lastRobotX, lastRobotY, lastRobotZ; }; diff --git a/src/ui/map3D/Q3DWidget.cc b/src/ui/map3D/Q3DWidget.cc index af37b35..b5dbeb5 100644 --- a/src/ui/map3D/Q3DWidget.cc +++ b/src/ui/map3D/Q3DWidget.cc @@ -67,6 +67,7 @@ Q3DWidget::Q3DWidget(QWidget* parent) setThreadingModel(osgViewer::Viewer::SingleThreaded); setFocusPolicy(Qt::StrongFocus); + setMouseTracking(true); } Q3DWidget::~Q3DWidget()