Skip to content

Commit

Permalink
Set gui camera pose (#863)
Browse files Browse the repository at this point in the history 
Signed-off-by: Jenn Nguyen <jenn@openrobotics.org>
Signed-off-by: Nate Koenig <nate@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>
  • Loading branch information
@jennuine
jennuine and Nate Koenig authored Jun 18, 2021
1 parent fada73c commit 483684f
Show file tree
Hide file tree
Showing 3 changed files with 234 additions and 6 deletions.
66 changes: 63 additions & 3 deletions src/gui/plugins/view_angle/ViewAngle.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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;
};
}

Expand All @@ -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";
}

/////////////////////////////////////////////////
Expand All @@ -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
Expand Down
25 changes: 25 additions & 0 deletions src/gui/plugins/view_angle/ViewAngle.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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();

Expand All @@ -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;
Expand Down
149 changes: 146 additions & 3 deletions src/gui/plugins/view_angle/ViewAngle.qml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -34,7 +36,9 @@ ToolBar {
}

GridLayout {
columns: 4
id: views
anchors.horizontalCenter: parent.horizontalCenter
columns: 8
ToolButton {
id: top
checkable: true
Expand Down Expand Up @@ -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)
}
}
}
}
}

0 comments on commit 483684f

Please sign in to comment.