Added yaw selection into target selection.

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);
 }
 

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<double,double> 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<double,double> 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<double,double> 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<double,double> 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());
@@ -725,6 +773,17 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
     Q3DWidget::mousePressEvent(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,
@@ -927,14 +986,15 @@ Pixhawk3DWidget::createTarget(void)
 
     pat->setPosition(osg::Vec3d(0.0, 0.0, 0.0));
 
-    osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.1f);
-    osg::ref_ptr<osg::ShapeDrawable> sphereDrawable = new osg::ShapeDrawable(sphere);
-    sphereDrawable->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f));
-    osg::ref_ptr<osg::Geode> sphereGeode = new osg::Geode;
-    sphereGeode->addDrawable(sphereDrawable);
-    sphereGeode->setName("Target");
+    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::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);
+    osg::ref_ptr<osg::Geode> 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<osg::PositionAttitudeTransform*>(targetNode.get());
+        dynamic_cast<osg::PositionAttitudeTransform*>(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<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));
+    }
 }
 
 float colormap_jet[128][3] = {

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<osg::Node> > vehicleModels;
 
     MAV_FRAME frame;
-    osg::Vec2d target;
+    osg::Vec3d target;
     double lastRobotX, lastRobotY, lastRobotZ;
 };
 

src/ui/map3D/Q3DWidget.cc

@@ -67,6 +67,7 @@ Q3DWidget::Q3DWidget(QWidget* parent)
     setThreadingModel(osgViewer::Viewer::SingleThreaded);
 
     setFocusPolicy(Qt::StrongFocus);
+    setMouseTracking(true);
 }
 
 Q3DWidget::~Q3DWidget()