Set gui camera pose

src/gui/plugins/view_angle/ViewAngle.cc

@@ -18,6 +18,7 @@
 #include "ViewAngle.hh"
 
 #include <ignition/msgs/boolean.pb.h>
+#include <ignition/msgs/gui_camera.pb.h>
 #include <ignition/msgs/vector3d.pb.h>
 
 #include <iostream>
@@ -38,7 +39,13 @@ namespace ignition::gazebo
     public: std::mutex mutex;
 
     /// \brief View Angle service name
-    public: std::string service;
+    public: std::string viewAngleService;
+
+    /// \brief Move gui camera to pose service name
+    public: std::string moveToPoseService;
+
+    /// \brief gui camera pose
+    public: math::Pose3d camPose;
   };
 }
 
@@ -61,7 +68,15 @@ void ViewAngle::LoadConfig(const tinyxml2::XMLElement *)
     this->title = "View Angle";
 
   // For view angle requests
-  this->dataPtr->service = "/gui/view_angle";
+  this->dataPtr->viewAngleService = "/gui/view_angle";
+
+  // Subscribe to camera pose
+  std::string topic = "/gui/camera/pose";
+  this->dataPtr->node.Subscribe(
+    topic, &ViewAngle::CamPoseCb, this);
+
+  // Move to pose service
+  this->dataPtr->moveToPoseService = "/gui/move_to/pose";
 }
 
 /////////////////////////////////////////////////
@@ -79,7 +94,52 @@ void ViewAngle::OnAngleMode(int _x, int _y, int _z)
   req.set_y(_y);
   req.set_z(_z);
 
-  this->dataPtr->node.Request(this->dataPtr->service, req, cb);
+  this->dataPtr->node.Request(this->dataPtr->viewAngleService, req, cb);
+}
+
+/////////////////////////////////////////////////
+QList<double> ViewAngle::CamPose() const
+{
+  return QList({
+    this->dataPtr->camPose.Pos().X(),
+    this->dataPtr->camPose.Pos().Y(),
+    this->dataPtr->camPose.Pos().Z(),
+    this->dataPtr->camPose.Rot().Roll(),
+    this->dataPtr->camPose.Rot().Pitch(),
+    this->dataPtr->camPose.Rot().Yaw()
+  });
+}
+
+/////////////////////////////////////////////////
+void ViewAngle::SetCamPose(double _x, double _y, double _z,
+                           double _roll, double _pitch, double _yaw)
+{
+  this->dataPtr->camPose.Set(_x, _y, _z, _roll, _pitch, _yaw);
+
+  std::function<void(const ignition::msgs::Boolean &, const bool)> cb =
+      [](const ignition::msgs::Boolean &/*_rep*/, const bool _result)
+  {
+    if (!_result)
+      ignerr << "Error sending move camera to pose request" << std::endl;
+  };
+
+  ignition::msgs::GUICamera req;
+  msgs::Set(req.mutable_pose(), this->dataPtr->camPose);
+
+  this->dataPtr->node.Request(this->dataPtr->moveToPoseService, req, cb);
+}
+
+/////////////////////////////////////////////////
+void ViewAngle::CamPoseCb(const msgs::Pose &_msg)
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+  math::Pose3d pose = msgs::Convert(_msg);
+
+  if (pose != this->dataPtr->camPose)
+  {
+    this->dataPtr->camPose = pose;
+    this->CamPoseChanged();
+  }
 }
 
 // Register this plugin

src/gui/plugins/view_angle/ViewAngle.hh

@@ -18,6 +18,8 @@
 #ifndef IGNITION_GAZEBO_GUI_VIEWANGLE_HH_
 #define IGNITION_GAZEBO_GUI_VIEWANGLE_HH_
 
+#include <ignition/msgs/pose.pb.h>
+
 #include <memory>
 
 #include <ignition/gui/Plugin.hh>
@@ -36,6 +38,13 @@ namespace gazebo
   {
     Q_OBJECT
 
+    /// \brief gui camera pose (QList order is x, y, z, roll, pitch, yaw)
+    Q_PROPERTY(
+      QList<double> camPose
+      READ CamPose
+      NOTIFY CamPoseChanged
+    )
+
     /// \brief Constructor
     public: ViewAngle();
 
@@ -54,6 +63,22 @@ namespace gazebo
     /// to assume.  All 0s for x, y, and z indicate the initial camera pose.
     public slots: void OnAngleMode(int _x, int _y, int _z);
 
+    /// \brief Get the current gui camera pose.
+    public: Q_INVOKABLE QList<double> CamPose() const;
+
+    /// \brief Notify that the gui camera pose has changed.
+    signals: void CamPoseChanged();
+
+    /// \brief Callback to update gui camera pose
+    /// \param[in] _x, _y, _z cartesion coordinates
+    /// \param[in] _roll, _pitch, _yaw principal coordinates
+    public slots: void SetCamPose(double _x, double _y, double _z,
+                               double _roll, double _pitch, double _yaw);
+
+    /// \brief Callback for retrieving gui camera pose
+    /// \param[in] _msg Pose message
+    public: void CamPoseCb(const msgs::Pose &_msg);
+
     /// \internal
     /// \brief Pointer to private data.
     private: std::unique_ptr<ViewAnglePrivate> dataPtr;

src/gui/plugins/view_angle/ViewAngle.qml

@@ -20,10 +20,12 @@ import QtQuick.Controls.Material 2.2
 import QtQuick.Controls.Material.impl 2.2
 import QtQuick.Layouts 1.3
 import QtQuick.Controls.Styles 1.4
+import "qrc:/qml"
 
 ToolBar {
-  Layout.minimumWidth: 200
-  Layout.minimumHeight: 200
+  Layout.minimumWidth: 320
+  Layout.minimumHeight: 380
+  anchors.fill: parent
 
   background: Rectangle {
     color: "transparent"
@@ -34,7 +36,9 @@ ToolBar {
   }
 
   GridLayout {
-    columns: 4
+    id: views
+    anchors.horizontalCenter: parent.horizontalCenter
+    columns: 8
     ToolButton {
       id: top
       checkable: true
@@ -183,4 +187,132 @@ ToolBar {
       }
     }
   }
+
+  // set camera pose
+  Rectangle {
+    y: views.height + 10
+    width: parent.width
+    color: "transparent"
+
+    GridLayout {
+      width: parent.width
+      columns: 6
+
+      Text {
+        text: "X (m)"
+        color: "dimgrey"
+        Layout.row: 4
+        Layout.column: 1
+        leftPadding: 5
+      }
+      IgnSpinBox {
+        id: x
+        Layout.fillWidth: true
+        Layout.row: 4
+        Layout.column: 2
+        value: ViewAngle.camPose[0]
+        maximumValue: 1000000
+        minimumValue: -1000000
+        decimals: 6
+        stepSize: 0.01
+        onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value)
+      }
+      Text {
+        text: "Y (m)"
+        color: "dimgrey"
+        Layout.row: 5
+        Layout.column: 1
+        leftPadding: 5
+      }
+      IgnSpinBox {
+        id: y
+        Layout.fillWidth: true
+        Layout.row: 5
+        Layout.column: 2
+        value: ViewAngle.camPose[1]
+        maximumValue: 1000000
+        minimumValue: -1000000
+        decimals: 6
+        stepSize: 0.01
+        onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value)
+      }
+      Text {
+        text: "Z (m)"
+        color: "dimgrey"
+        Layout.row: 6
+        Layout.column: 1
+        leftPadding: 5
+      }
+      IgnSpinBox {
+        id: z
+        Layout.fillWidth: true
+        Layout.row: 6
+        Layout.column: 2
+        value: ViewAngle.camPose[2]
+        maximumValue: 1000000
+        minimumValue: -1000000
+        decimals: 6
+        stepSize: 0.01
+        onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value)
+      }
+
+      Text {
+        text: "Roll (rad)"
+        color: "dimgrey"
+        Layout.row: 4
+        Layout.column: 3
+        leftPadding: 5
+      }
+      IgnSpinBox {
+        id: roll
+        Layout.fillWidth: true
+        Layout.row: 4
+        Layout.column: 4
+        value: ViewAngle.camPose[3]
+        maximumValue: 6.28
+        minimumValue: -6.28
+        decimals: 6
+        stepSize: 0.01
+        onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value)
+      }
+      Text {
+        text: "Pitch (rad)"
+        color: "dimgrey"
+        Layout.row: 5
+        Layout.column: 3
+        leftPadding: 5
+      }
+      IgnSpinBox {
+        id: pitch
+        Layout.fillWidth: true
+        Layout.row: 5
+        Layout.column: 4
+        value: ViewAngle.camPose[4]
+        maximumValue: 6.28
+        minimumValue: -6.28
+        decimals: 6
+        stepSize: 0.01
+        onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value)
+      }
+      Text {
+        text: "Yaw (rad)"
+        color: "dimgrey"
+        Layout.row: 6
+        Layout.column: 3
+        leftPadding: 5
+      }
+      IgnSpinBox {
+        id: yaw
+        Layout.fillWidth: true
+        Layout.row: 6
+        Layout.column: 4
+        value: ViewAngle.camPose[5]
+        maximumValue: 6.28
+        minimumValue: -6.28
+        decimals: 6
+        stepSize: 0.01
+        onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value)
+      }
+    }
+  }
 }