GraspCandidateViewerWidgetController.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::GraspCandidateViewerWidgetController
17  * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
18  * \date 2020
19  * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include <string>
24 
25 #include <SimoxUtility/math/convert/quat_to_rpy.h>
26 
29 
30 //setting management
31 namespace armarx
32 {
34  {
36  std::lock_guard g{_mutex};
37  getRobotStateComponentPlugin().setRobotStateComponentName(settings->value("rsc", "Armar6StateComponent").toString().toStdString());
38  getGraspCandidateObserverComponentPlugin().setGraspCandidateObserverName(settings->value("gco", "GraspCandidateObserver").toString().toStdString());
39 
40  }
42  {
44  std::lock_guard g{_mutex};
45  settings->setValue("rsc", QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
46  settings->setValue("rsc", QString::fromStdString(getGraspCandidateObserverComponentPlugin().getGraspCandidateObserverName()));
47 
48  }
49  QPointer<QDialog> GraspCandidateViewerWidgetController::getConfigDialog(QWidget* parent)
50  {
52  std::lock_guard g{_mutex};
53  if (!_dialog)
54  {
55  _dialog = new SimpleConfigDialog(parent);
56  _dialog->addProxyFinder<RobotStateComponentInterfacePrx>("rsc", "Robot State Component", "*Component");
57  _dialog->addProxyFinder<grasping::GraspCandidateObserverInterfacePrx>("gco", "Grasp Candidate Observer", "*Observer");
58  }
59  return qobject_cast<SimpleConfigDialog*>(_dialog);
60  }
62  {
64  std::lock_guard g{_mutex};
65  getRobotStateComponentPlugin().setRobotStateComponentName(_dialog->getProxyName("rsc"));
67  }
68 }
69 //ctor
70 namespace armarx
71 {
73  {
75  std::lock_guard g{_mutex};
76  _ui.setupUi(getWidget());
77 
78  connect(_ui.pushButtonUpdateGC, &QPushButton::clicked,
80  connect(_ui.treeWidgetGC, &QTreeWidget::itemSelectionChanged,
82 
83  connect(_ui.treeWidgetGC, SIGNAL(itemChanged(QTreeWidgetItem*, int)),
84  this, SLOT(check_state_changed()));
85  }
86 
88  {}
89 
91  {
93  std::lock_guard g{_mutex};
95  {
96  return;
97  }
98  //clean
99  {
100  _ui.treeWidgetGC->clear();
101  for (auto& [prname, pr] : _providers)
102  {
103  pr.candidates.clear();
104  pr.item = nullptr;
105  }
106  _tree_item_to_gc.clear();
107  }
108  //fill
109  {
110  for (const auto& gc : getGraspCandidateObserver()->getAllCandidates())
111  {
113  const auto& pname = gc->providerName;
114  auto& pr = _providers[pname];
115  pr.provider_name = pname;
116  if (!pr.item)
117  {
118  pr.item = new QTreeWidgetItem;
119  pr.item->setCheckState(0, Qt::Unchecked);
120  _ui.treeWidgetGC-> addTopLevelItem(pr.item);
121  pr.item->setText(0, QString::fromStdString(pname));
122  }
123 
124  auto& gcdata = pr.candidates.emplace_back();
125  gcdata.gc = gc;
126  gcdata.idx = pr.candidates.size() - 1;
127  gcdata.provider = &pr;
128  gcdata.item = new QTreeWidgetItem;
129  gcdata.item->setCheckState(0, Qt::Unchecked);
130  pr.item->addChild(gcdata.item);
131  gcdata.item->setText(0, QString::fromStdString(gcdata.name()));
132  _tree_item_to_gc[gcdata.item] = &gcdata;
133  }
134  }
135  }
136 
138  {
139  ARMARX_TRACE;
140  std::lock_guard g{_mutex};
141  synchronizeLocalClone(_robot);
143 
144  std::vector<viz::Layer> layers;
145  for (const auto& [pname, pr] : _providers)
146  {
147  layers.emplace_back(getArvizClient().layer(pname));
148  auto& l = layers.back();
149  for (const auto& gc : pr.candidates)
150  {
151  if (gc.item->checkState(0) == Qt::Unchecked)
152  {
153  gc_drawer.draw(gc.gc, l);
154  }
155  }
156  }
157  getArvizClient().commit(layers);
158  }
160  {
161  ARMARX_TRACE;
162  std::lock_guard g{_mutex};
163  const auto& selected = _ui.treeWidgetGC->selectedItems();
164  ARMARX_CHECK_GREATER(2, selected.size());
165  if (selected.empty())
166  {
167  show_entry(nullptr);
168  //clear
169  }
170  else if (_tree_item_to_gc.count(selected.value(0)))
171  {
172  show_entry(_tree_item_to_gc.at(selected.value(0)));
173  }
174  //provider item selected -> do nothing
175  }
176 
177  void GraspCandidateViewerWidgetController::show_entry(entry_gc* e)
178  {
179  if (!e)
180  {
181  _ui.labelGCIdx ->setText("-");
182  _ui.labelNumGC ->setText("-");
183 
184  _ui.labelGPoseTX ->setText("-");
185  _ui.labelGPoseTY ->setText("-");
186  _ui.labelGPoseTZ ->setText("-");
187  _ui.labelGPoseRX ->setText("-");
188  _ui.labelGPoseRY ->setText("-");
189  _ui.labelGPoseRZ ->setText("-");
190 
191  _ui.labelRPoseTX ->setText("-");
192  _ui.labelRPoseTY ->setText("-");
193  _ui.labelRPoseTZ ->setText("-");
194  _ui.labelRPoseRX ->setText("-");
195  _ui.labelRPoseRY ->setText("-");
196  _ui.labelRPoseRZ ->setText("-");
197 
198  _ui.labelGApprTX ->setText("-");
199  _ui.labelGApprTY ->setText("-");
200  _ui.labelGApprTZ ->setText("-");
201 
202  _ui.labelSrcFrame ->setText("-");
203  _ui.labelTrgFrame ->setText("-");
204  _ui.labelSide ->setText("-");
205  _ui.labelGraspProb ->setText("-");
206  _ui.labelGraspgroupNr ->setText("-");
207  _ui.labelGraspObjType ->setText("-");
208  _ui.labelGraspProviderName->setText("-");
209  return;
210  }
211  ARMARX_CHECK_NOT_NULL(e->gc);
212  ARMARX_CHECK_NOT_NULL(e->gc->graspPose->position);
213  ARMARX_CHECK_NOT_NULL(e->gc->robotPose->position);
214  ARMARX_CHECK_NOT_NULL(e->gc->graspPose->orientation);
215  ARMARX_CHECK_NOT_NULL(e->gc->robotPose->orientation);
216  ARMARX_CHECK_NOT_NULL(e->gc->approachVector);
217  _ui.labelGCIdx ->setText(QString::number(e->idx + 1));
218  _ui.labelNumGC ->setText(QString::number(e->provider->candidates.size()));
219 
220  _ui.labelGPoseTX ->setText(QString::number(e->gc->graspPose->position->x));
221  _ui.labelGPoseTY ->setText(QString::number(e->gc->graspPose->position->y));
222  _ui.labelGPoseTZ ->setText(QString::number(e->gc->graspPose->position->z));
223  const Eigen::Vector3f vec_g = simox::math::quat_to_rpy(
224  {
225  e->gc->graspPose->orientation->qw,
226  e->gc->graspPose->orientation->qx,
227  e->gc->graspPose->orientation->qy,
228  e->gc->graspPose->orientation->qz
229 
230  }
231  );
232  _ui.labelGPoseRX ->setText(QString::number(vec_g(0)));
233  _ui.labelGPoseRY ->setText(QString::number(vec_g(1)));
234  _ui.labelGPoseRZ ->setText(QString::number(vec_g(2)));
235 
236  _ui.labelRPoseTX ->setText(QString::number(e->gc->robotPose->position->x));
237  _ui.labelRPoseTY ->setText(QString::number(e->gc->robotPose->position->y));
238  _ui.labelRPoseTZ ->setText(QString::number(e->gc->robotPose->position->z));
239  const Eigen::Vector3f vec_r = simox::math::quat_to_rpy(
240  {
241  e->gc->robotPose->orientation->qw,
242  e->gc->robotPose->orientation->qx,
243  e->gc->robotPose->orientation->qy,
244  e->gc->robotPose->orientation->qz
245 
246  }
247  );
248  _ui.labelRPoseRX ->setText(QString::number(vec_r(0)));
249  _ui.labelRPoseRY ->setText(QString::number(vec_r(1)));
250  _ui.labelRPoseRZ ->setText(QString::number(vec_r(2)));
251 
252  _ui.labelGApprTX ->setText(QString::number(e->gc->approachVector->x));
253  _ui.labelGApprTY ->setText(QString::number(e->gc->approachVector->y));
254  _ui.labelGApprTZ ->setText(QString::number(e->gc->approachVector->z));
255 
256  _ui.labelSrcFrame ->setText(QString::fromStdString(e->gc->sourceFrame));
257  _ui.labelTrgFrame ->setText(QString::fromStdString(e->gc->targetFrame));
258  _ui.labelSide ->setText(QString::fromStdString(e->gc->side));
259  _ui.labelGraspProb ->setText(QString::number(e->gc->graspSuccessProbability));
260  _ui.labelGraspgroupNr ->setText(QString::number(e->gc->groupNr));
261  switch (e->gc->objectType)
262  {
263  case objpose::ObjectType::AnyObject :
264  _ui.labelGraspObjType->setText("AnyObject");
265  break;
266  case objpose::ObjectType::KnownObject :
267  _ui.labelGraspObjType->setText("KnownObject");
268  break;
269  case objpose::ObjectType::UnknownObject :
270  _ui.labelGraspObjType->setText("UnknownObject");
271  break;
272  }
273  _ui.labelGraspProviderName->setText(QString::fromStdString(e->gc->providerName));
274  }
275 
276 }
armarx::viz::Client::commit
CommitResult commit(StagedCommit const &commit)
Definition: Client.cpp:80
armarx::ArVizComponentPluginUser::getArvizClient
armarx::viz::Client & getArvizClient()
Definition: ArVizComponentPlugin.h:45
armarx::RobotStateComponentPluginUser::getRobotNameHelper
RobotNameHelperPtr getRobotNameHelper() const
Definition: RobotStateComponentPlugin.cpp:321
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
armarx::GraspCandidateViewerWidgetController::~GraspCandidateViewerWidgetController
virtual ~GraspCandidateViewerWidgetController()
Definition: GraspCandidateViewerWidgetController.cpp:87
armarx::GraspCandidateViewerWidgetController::getConfigDialog
QPointer< QDialog > getConfigDialog(QWidget *parent) override
getConfigDialog returns a pointer to the a configuration widget of this controller.
Definition: GraspCandidateViewerWidgetController.cpp:49
ARMARX_CHECK_GREATER
#define ARMARX_CHECK_GREATER(lhs, rhs)
This macro evaluates whether lhs is greater (>) than rhs and if it turns out to be false it will thro...
Definition: ExpressionException.h:116
armarx::GraspCandidateObserverComponentPluginUser::getGraspCandidateObserverComponentPlugin
armarx::plugins::GraspCandidateObserverComponentPlugin & getGraspCandidateObserverComponentPlugin()
Definition: GraspCandidateObserverComponentPlugin.cpp:111
armarx::GraspCandidateViewerWidgetController::loadSettings
void loadSettings(QSettings *settings) override
Implement to load the settings that are part of the GUI configuration.
Definition: GraspCandidateViewerWidgetController.cpp:33
armarx::plugins::GraspCandidateObserverComponentPlugin::setGraspCandidateObserverName
void setGraspCandidateObserverName(const std::string &name)
Definition: GraspCandidateObserverComponentPlugin.cpp:92
armarx::GraspCandidateViewerWidgetController::saveSettings
void saveSettings(QSettings *settings) override
Implement to save the settings as part of the GUI configuration.
Definition: GraspCandidateViewerWidgetController.cpp:41
armarx::GraspCandidateViewerWidgetController::configured
void configured() override
This function must be implemented by the user, if he supplies a config dialog.
Definition: GraspCandidateViewerWidgetController.cpp:61
armarx::GraspCandidateViewerWidgetController::update_gc
void update_gc()
Definition: GraspCandidateViewerWidgetController.cpp:90
armarx::plugins::RobotStateComponentPlugin::setRobotStateComponentName
void setRobotStateComponentName(const std::string &name)
Definition: RobotStateComponentPlugin.cpp:272
armarx::RobotStateComponentPluginUser::synchronizeLocalClone
bool synchronizeLocalClone(const VirtualRobot::RobotPtr &robot) const
Definition: RobotStateComponentPlugin.cpp:371
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::GraspCandidateObserverComponentPluginUser::getGraspCandidateObserver
grasping::GraspCandidateObserverInterfacePrx getGraspCandidateObserver()
Definition: GraspCandidateObserverComponentPlugin.cpp:106
armarx::RobotStateComponentPluginUser::getRobotStateComponentPlugin
const RobotStateComponentPlugin & getRobotStateComponentPlugin() const
Definition: RobotStateComponentPlugin.cpp:307
armarx::GraspCandidateViewerWidgetController::selected_item_changed
void selected_item_changed()
Definition: GraspCandidateViewerWidgetController.cpp:159
grasp_candidate_drawer.h
GraspCandidateViewerWidgetController.h
armarx::GraspCandidateViewerWidgetController::GraspCandidateViewerWidgetController
GraspCandidateViewerWidgetController()
Definition: GraspCandidateViewerWidgetController.cpp:72
armarx::GraspCandidateViewerWidgetController::check_state_changed
void check_state_changed()
Definition: GraspCandidateViewerWidgetController.cpp:137
IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface >
armarx::grasp_candidate_drawer
Definition: grasp_candidate_drawer.h:14
armarx::ArmarXWidgetController::getWidget
virtual QPointer< QWidget > getWidget()
getWidget returns a pointer to the a widget of this controller.
Definition: ArmarXWidgetController.cpp:54
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::SimpleConfigDialog
A config-dialog containing one (or multiple) proxy finders.
Definition: SimpleConfigDialog.h:84