From 483684f885bb8975063a22a510bb114033a9faaa Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Fri, 18 Jun 2021 16:38:21 -0700 Subject: [PATCH] Set gui camera pose (#863) Signed-off-by: Jenn Nguyen Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- src/gui/plugins/view_angle/ViewAngle.cc | 66 +++++++++- src/gui/plugins/view_angle/ViewAngle.hh | 25 ++++ src/gui/plugins/view_angle/ViewAngle.qml | 149 ++++++++++++++++++++++- 3 files changed, 234 insertions(+), 6 deletions(-) diff --git a/src/gui/plugins/view_angle/ViewAngle.cc b/src/gui/plugins/view_angle/ViewAngle.cc index eae23f7d4f..9de7fddf4f 100644 --- a/src/gui/plugins/view_angle/ViewAngle.cc +++ b/src/gui/plugins/view_angle/ViewAngle.cc @@ -18,6 +18,7 @@ #include "ViewAngle.hh" #include +#include #include #include @@ -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 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 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 lock(this->dataPtr->mutex); + math::Pose3d pose = msgs::Convert(_msg); + + if (pose != this->dataPtr->camPose) + { + this->dataPtr->camPose = pose; + this->CamPoseChanged(); + } } // Register this plugin diff --git a/src/gui/plugins/view_angle/ViewAngle.hh b/src/gui/plugins/view_angle/ViewAngle.hh index 02cf34dae9..d58b064aed 100644 --- a/src/gui/plugins/view_angle/ViewAngle.hh +++ b/src/gui/plugins/view_angle/ViewAngle.hh @@ -18,6 +18,8 @@ #ifndef IGNITION_GAZEBO_GUI_VIEWANGLE_HH_ #define IGNITION_GAZEBO_GUI_VIEWANGLE_HH_ +#include + #include #include @@ -36,6 +38,13 @@ namespace gazebo { Q_OBJECT + /// \brief gui camera pose (QList order is x, y, z, roll, pitch, yaw) + Q_PROPERTY( + QList 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 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 dataPtr; diff --git a/src/gui/plugins/view_angle/ViewAngle.qml b/src/gui/plugins/view_angle/ViewAngle.qml index a0ea12e017..311ca12b3e 100644 --- a/src/gui/plugins/view_angle/ViewAngle.qml +++ b/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,143 @@ ToolBar { } } } + + // set camera pose + Rectangle { + y: views.height + 10 + width: parent.width + color: "transparent" + + ColumnLayout { + width: parent.width + Text { + text: "Camera Pose" + color: Material.Grey + Layout.row: 4 + Layout.column: 1 + leftPadding: 5 + } + + 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) + } + } + } + } }