DebugDrawerGuiPluginWidgetController.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * \package RobotAPI::gui-plugins::DebugDrawerGuiPluginWidgetController
17  * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
18  * \date 2019
19  * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include <string>
24 
25 #include <Ice/UUID.h>
26 
27 #include <VirtualRobot/MathTools.h>
28 
30 
32 
33 namespace armarx
34 {
36  _layerName{Ice::generateUUID()},
37  _debugDrawer{_layerName}
38  {
40  _ui.setupUi(getWidget());
41  connect(_ui.pushButtonLayerClear, SIGNAL(clicked()), this, SLOT(on_pushButtonLayerClear_clicked()));
42  connect(_ui.pushButtonPoseDelete, SIGNAL(clicked()), this, SLOT(on_pushButtonPoseDelete_clicked()));
43  connect(_ui.pushButtonArrowDelete, SIGNAL(clicked()), this, SLOT(on_pushButtonArrowDelete_clicked()));
44 
45  connect(_ui.doubleSpinBoxPoseTX, SIGNAL(valueChanged(double)), this, SLOT(updatePose()));
46  connect(_ui.doubleSpinBoxPoseTY, SIGNAL(valueChanged(double)), this, SLOT(updatePose()));
47  connect(_ui.doubleSpinBoxPoseTZ, SIGNAL(valueChanged(double)), this, SLOT(updatePose()));
48  connect(_ui.doubleSpinBoxPoseRX, SIGNAL(valueChanged(double)), this, SLOT(updatePose()));
49  connect(_ui.doubleSpinBoxPoseRY, SIGNAL(valueChanged(double)), this, SLOT(updatePose()));
50  connect(_ui.doubleSpinBoxPoseRZ, SIGNAL(valueChanged(double)), this, SLOT(updatePose()));
51  connect(_ui.doubleSpinBoxPoseScale, SIGNAL(valueChanged(double)), this, SLOT(updatePose()));
52 
53  connect(_ui.doubleSpinBoxArrowFX, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
54  connect(_ui.doubleSpinBoxArrowFY, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
55  connect(_ui.doubleSpinBoxArrowFZ, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
56  connect(_ui.doubleSpinBoxArrowTX, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
57  connect(_ui.doubleSpinBoxArrowTY, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
58  connect(_ui.doubleSpinBoxArrowTZ, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
59  connect(_ui.doubleSpinBoxArrowClrR, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
60  connect(_ui.doubleSpinBoxArrowClrG, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
61  connect(_ui.doubleSpinBoxArrowClrB, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
62  connect(_ui.doubleSpinBoxArrowClrA, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
63  connect(_ui.doubleSpinBoxArrowWidth, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
64  }
65 
66 
68  {
69 
70  }
71 
72 
74  {
75  }
76 
78  {
79  }
80 
81 
83  {
85  offeringTopic(_debugDrawerTopicName);
86  }
87 
88 
90  {
92  _debugDrawer.setDebugDrawer(getTopic<DebugDrawerInterfacePrx>(_debugDrawerTopicName));
93  }
94 
96  {
97  _debugDrawer.clearLayer();
98  }
99 
100  void DebugDrawerGuiPluginWidgetController::on_pushButtonArrowDelete_clicked()
101  {
102  const auto name = _ui.lineEditArrowName->text().toStdString();
103  ARMARX_INFO << "Delete arrow " << name;
104  _debugDrawer.getDebugDrawer()->removePoseVisu(_layerName, name);
105  }
106 
107  void DebugDrawerGuiPluginWidgetController::on_pushButtonPoseDelete_clicked()
108  {
109  const auto name = _ui.lineEditPoseName->text().toStdString();
110  ARMARX_INFO << "Delete pose " << name;
111  _debugDrawer.getDebugDrawer()->removePoseVisu(_layerName, name);
112  }
113 
114  void DebugDrawerGuiPluginWidgetController::on_pushButtonLayerClear_clicked()
115  {
116  _debugDrawer.clearLayer();
117  }
118 
119  void DebugDrawerGuiPluginWidgetController::updatePose()
120  {
121  const auto name = _ui.lineEditPoseName->text().toStdString();
122  ARMARX_INFO << "Updated pose " << name;
123 
124  const float deg2rad = M_PI / 360;
125 
126  _debugDrawer.drawPose(
127  name,
128  VirtualRobot::MathTools::posrpy2eigen4f(
129  _ui.doubleSpinBoxPoseTX->value(),
130  _ui.doubleSpinBoxPoseTY->value(),
131  _ui.doubleSpinBoxPoseTZ->value(),
132  _ui.doubleSpinBoxPoseRX->value() * deg2rad,
133  _ui.doubleSpinBoxPoseRY->value() * deg2rad,
134  _ui.doubleSpinBoxPoseRZ->value() * deg2rad
135  ),
136  _ui.doubleSpinBoxPoseScale->value());
137  }
138 
139  void DebugDrawerGuiPluginWidgetController::updateArrow()
140  {
141  const Eigen::Vector3f from
142  {
143  static_cast<float>(_ui.doubleSpinBoxArrowFX->value()),
144  static_cast<float>(_ui.doubleSpinBoxArrowFY->value()),
145  static_cast<float>(_ui.doubleSpinBoxArrowFZ->value())
146  };
147 
148  const Eigen::Vector3f to
149  {
150  static_cast<float>(_ui.doubleSpinBoxArrowTX->value()),
151  static_cast<float>(_ui.doubleSpinBoxArrowTY->value()),
152  static_cast<float>(_ui.doubleSpinBoxArrowTZ->value())
153  };
154 
155  const Eigen::Vector3f dir = to - from;
156 
157  const float len = dir.norm();
158 
159  const auto name = _ui.lineEditArrowName->text().toStdString();
160  ARMARX_INFO << "Updated arrow " << name;
161 
162  _debugDrawer.drawArrow(
163  name,
164  from,
165  dir.normalized(),
166  {
167  static_cast<float>(_ui.doubleSpinBoxArrowClrR->value()),
168  static_cast<float>(_ui.doubleSpinBoxArrowClrG->value()),
169  static_cast<float>(_ui.doubleSpinBoxArrowClrB->value()),
170  static_cast<float>(_ui.doubleSpinBoxArrowClrA->value())
171  },
172  len,
173  static_cast<float>(_ui.doubleSpinBoxArrowWidth->value()));
174  }
175 }
176 
177 
178 
armarx::DebugDrawerGuiPluginWidgetController::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: DebugDrawerGuiPluginWidgetController.cpp:95
armarx::DebugDrawerHelper::setDebugDrawer
void setDebugDrawer(const DebugDrawerInterfacePrx &drawer)
Definition: DebugDrawerHelper.cpp:363
armarx::DebugDrawerHelper::clearLayer
void clearLayer()
Definition: DebugDrawerHelper.cpp:281
trace.h
armarx::detail::DebugDrawerHelper::FrameView::drawPose
void drawPose(const std::string &name, const Eigen::Matrix4f &pose)
Definition: DebugDrawerHelper.cpp:93
armarx::DebugDrawerGuiPluginWidgetController::~DebugDrawerGuiPluginWidgetController
virtual ~DebugDrawerGuiPluginWidgetController()
Definition: DebugDrawerGuiPluginWidgetController.cpp:67
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::DebugDrawerGuiPluginWidgetController::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: DebugDrawerGuiPluginWidgetController.cpp:89
M_PI
#define M_PI
Definition: MathTools.h:17
DebugDrawerGuiPluginWidgetController.h
armarx::DebugDrawerGuiPluginWidgetController::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: DebugDrawerGuiPluginWidgetController.cpp:82
armarx::DebugDrawerHelper::getDebugDrawer
const DebugDrawerInterfacePrx & getDebugDrawer() const
Definition: DebugDrawerHelper.cpp:358
armarx::DebugDrawerGuiPluginWidgetController::DebugDrawerGuiPluginWidgetController
DebugDrawerGuiPluginWidgetController()
Definition: DebugDrawerGuiPluginWidgetController.cpp:35
armarx::DebugDrawerGuiPluginWidgetController::loadSettings
void loadSettings(QSettings *settings) override
Implement to load the settings that are part of the GUI configuration.
Definition: DebugDrawerGuiPluginWidgetController.cpp:73
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::ManagedIceObject::offeringTopic
void offeringTopic(const std::string &name)
Registers a topic for retrival after initialization.
Definition: ManagedIceObject.cpp:290
armarx::detail::DebugDrawerHelper::FrameView::drawArrow
void drawArrow(const std::string &name, const Eigen::Vector3f &pos, const Eigen::Vector3f &direction, const DrawColor &color, float length, float width)
Definition: DebugDrawerHelper.cpp:128
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::DebugDrawerGuiPluginWidgetController::saveSettings
void saveSettings(QSettings *settings) override
Implement to save the settings as part of the GUI configuration.
Definition: DebugDrawerGuiPluginWidgetController.cpp:77