AffordancePipelineGuiWidgetController.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 VisionX::gui-plugins::AffordancePipelineGuiWidgetController
17  * \author Peter Kaiser ( peter dot kaiser at kit dot edu )
18  * \date 2016
19  * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
25 
28 
31 
32 #include <QPushButton>
33 #include <QCheckBox>
34 #include <QListWidget>
35 
36 #include <string>
37 
38 using namespace armarx;
39 
41  customToolbar(NULL)
42 {
43  widget.setupUi(getWidget());
44  updateTimer = new QTimer(this);
45 }
46 
48 {
49  delete updateTimer;
50 }
51 
53 {
54  pointCloudProviderNames.clear();
55  QList<QVariant> names = settings->value("pointCloudProviderNames", QList<QVariant>()).toList();
56  for (int i = 0; i < names.count(); i++)
57  {
58  pointCloudProviderNames.push_back(names[i].toString().toStdString());
59  }
60 
61  filteredPointCloudProviderNames.clear();
62  names = settings->value("filteredPointCloudProviderNames", QList<QVariant>()).toList();
63  for (int i = 0; i < names.count(); i++)
64  {
65  filteredPointCloudProviderNames.push_back(names[i].toString().toStdString());
66  }
67 
68  pointCloudProviderDisplayNames.clear();
69  names = settings->value("pointCloudProviderDisplayNames", QList<QVariant>()).toList();
70  for (int i = 0; i < names.count(); i++)
71  {
72  pointCloudProviderDisplayNames.push_back(names[i].toString().toStdString());
73  }
74 
75  pipelineConfigFiles.clear();
76  names = settings->value("pipelineConfigFiles", QList<QVariant>()).toList();
77  for (int i = 0; i < names.count(); i++)
78  {
79  pipelineConfigFiles.push_back(names[i].toString().toStdString());
80  }
81 
82  pointCloudSegmenterName = settings->value("pointCloudSegmenterName", "").toString().toStdString();
83  primitiveExtractorName = settings->value("primitiveExtractorName", "").toString().toStdString();
84  affordanceExtractionName = settings->value("affordanceExtractionName", "").toString().toStdString();
85  pipelineVisualizationName = settings->value("pipelineVisualizationName", "").toString().toStdString();
86  robotStateComponentName = settings->value("robotStateComponentName", "").toString().toStdString();
87  platform = settings->value("platform", "").toString().toStdString();
88 }
89 
91 {
92  QList<QVariant> names;
93  for (auto& name : pointCloudProviderNames)
94  {
95  names.append(QString::fromStdString(name));
96  }
97  settings->setValue("pointCloudProviderNames", names);
98 
99  names.clear();
100  for (auto& name : filteredPointCloudProviderNames)
101  {
102  names.append(QString::fromStdString(name));
103  }
104  settings->setValue("filteredPointCloudProviderNames", names);
105 
106  names.clear();
107  for (auto& name : pointCloudProviderDisplayNames)
108  {
109  names.append(QString::fromStdString(name));
110  }
111  settings->setValue("pointCloudProviderDisplayNames", names);
112 
113  names.clear();
114  for (auto& filename : pipelineConfigFiles)
115  {
116  names.append(QString::fromStdString(filename));
117  }
118  settings->setValue("pipelineConfigFiles", names);
119 
120  settings->setValue("pointCloudSegmenterName", QString::fromStdString(pointCloudSegmenterName));
121  settings->setValue("primitiveExtractorName", QString::fromStdString(primitiveExtractorName));
122  settings->setValue("affordanceExtractionName", QString::fromStdString(affordanceExtractionName));
123  settings->setValue("pipelineVisualizationName", QString::fromStdString(pipelineVisualizationName));
124  settings->setValue("robotStateComponentName", QString::fromStdString(robotStateComponentName));
125  settings->setValue("platform", QString::fromStdString(platform));
126 }
127 
129 {
130  for (auto& provider : pointCloudProviderNames)
131  {
132  usingProxy(provider);
133  }
134 
135  for (auto& provider : filteredPointCloudProviderNames)
136  {
137  usingProxy(provider);
138  }
139 
140  usingProxy(pointCloudSegmenterName);
141  usingProxy(primitiveExtractorName);
142  usingProxy(affordanceExtractionName);
143  usingProxy(pipelineVisualizationName);
144  usingProxy(robotStateComponentName);
145 }
146 
148 {
149  pointCloudProviders.clear();
150  for (auto& provider : pointCloudProviderNames)
151  {
152  pointCloudProviders.push_back(getProxy<visionx::CapturingPointCloudProviderInterfacePrx>(provider));
153  }
154 
155  filteredPointCloudProviders.clear();
156  for (auto& provider : filteredPointCloudProviderNames)
157  {
158  filteredPointCloudProviders.push_back(getProxy<armarx::PointCloudFilterInterfacePrx>(provider));
159  }
160 
161  pointCloudSegmenter = getProxy<visionx::PointCloudSegmenterInterfacePrx>(pointCloudSegmenterName);
162  primitiveExtractor = getProxy<visionx::PrimitiveMapperInterfacePrx>(primitiveExtractorName);
163  affordanceExtraction = getProxy<AffordanceExtractionInterfacePrx>(affordanceExtractionName);
164  pipelineVisualization = getProxy<AffordancePipelineVisualizationInterfacePrx>(pipelineVisualizationName);
165  robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
166 
167  // Load pipeline config files
168  widget.comboBoxSegmentation->clear();
169  for (unsigned int i = 0; i < pipelineConfigFiles.size(); i++)
170  {
171  widget.comboBoxSegmentation->addItem(QString::fromStdString("C" + std::to_string(i)));
172  }
173 
174  // Initialize provider list in UI
175  widget.comboBoxPointCloudSource->clear();
176  for (auto& provider : pointCloudProviderDisplayNames)
177  {
178  widget.comboBoxPointCloudSource->addItem(QString::fromStdString(provider));
179  }
180  widget.comboBoxPointCloudSource->setCurrentIndex(0);
182 
183  // Clear any previous visualization
184  pipelineVisualization->clearVisualization();
185 
187 
188  connect(widget.pushButtonStart, SIGNAL(clicked()), this, SLOT(startPipeline()));
189  connect(widget.pushButtonPause, SIGNAL(clicked()), this, SLOT(pausePipeline()));
190  connect(widget.pushButtonStep, SIGNAL(clicked()), this, SLOT(stepPipeline()));
191 
192  connect(widget.comboBoxCropping, SIGNAL(currentIndexChanged(int)), this, SLOT(croppingSelected(int)));
193  connect(widget.comboBoxPointCloudSource, SIGNAL(currentIndexChanged(int)), this, SLOT(pointCloudSourceSelected(int)));
194  connect(widget.comboBoxSegmentation, SIGNAL(currentIndexChanged(int)), this, SLOT(segmentationSetupSelected(int)));
195  connect(widget.checkBoxShowPrimitives, SIGNAL(toggled(bool)), this, SLOT(visualizationOptionsChanged(bool)));
196  connect(widget.checkBoxShowAffordances, SIGNAL(toggled(bool)), this, SLOT(visualizationOptionsChanged(bool)));
197 
198  connect(updateTimer, SIGNAL(timeout()), this, SLOT(updatePipelineStatus()));
199  updateTimer->start(500);
200 
201  // Set default cropping setup
202  widget.comboBoxCropping->setCurrentIndex(3);
203 
204  // Set default segmentation setup
205  widget.comboBoxSegmentation->setCurrentIndex(0);
206 
207  // Initially start in paused mode
208  pausePipeline();
209 
210  QMetaObject::invokeMethod(this, "visualizationOptionsChanged", Qt::QueuedConnection, Q_ARG(bool, true));
211 }
212 
214 {
215  QObject::disconnect(this, SLOT(startPipeline()));
216  QObject::disconnect(this, SLOT(pausePipeline()));
217  QObject::disconnect(this, SLOT(stepPipeline()));
218  QObject::disconnect(this, SLOT(croppingSelected(int)));
219  QObject::disconnect(this, SLOT(segmentationSetupSelected(int)));
220  QObject::disconnect(this, SLOT(pointCloudSourceSelected(int)));
221  QObject::disconnect(this, SLOT(visualizationOptionsChanged(bool)));
222 
223  QObject::disconnect(this, SLOT(updatePipelineStatus()));
224  updateTimer->stop();
225 
226  widget.labelProviderStatus->setText("Stopped");
227  widget.labelSegmenterStatus->setText("Stopped");
228  widget.labelPrimitivesStatus->setText("Stopped");
229  widget.labelAffordancesStatus->setText("Stopped");
230 }
231 
233 {
234  try
235  {
236  bool providerStatus = pointCloudProviders[currentPointCloudProvider]->isCaptureEnabled();
237  bool segmenterStatus = pointCloudSegmenter->isPipelineStepEnabled();
238  bool primitivesStatus = primitiveExtractor->isPipelineStepEnabled();
239  bool affordancesStatus = affordanceExtraction->isPipelineStepEnabled();
240 
241  widget.labelProviderStatus->setText(providerStatus ? "Running" : "Paused");
242  widget.labelSegmenterStatus->setText(segmenterStatus ? "Running" : "Paused");
243  widget.labelPrimitivesStatus->setText(primitivesStatus ? "Running" : "Paused");
244  widget.labelAffordancesStatus->setText(affordancesStatus ? "Running" : "Paused");
245 
246  widget.labelProviderTimestamp->setText(formatTimestamp(pointCloudProviders[currentPointCloudProvider]->getPointCloudFormat()->timeProvided));
247  widget.labelSegmenterTimestamp->setText(formatTimestamp(pointCloudSegmenter->getLastProcessedTimestamp()->timestamp));
248  widget.labelPrimitivesTimestamp->setText(formatTimestamp(primitiveExtractor->getLastProcessedTimestamp()->timestamp));
249  widget.labelAffordancesTimestamp->setText(formatTimestamp(affordanceExtraction->getLastProcessedTimestamp()->timestamp));
250 
251  bool status = providerStatus && segmenterStatus && primitivesStatus && affordancesStatus;
252  widget.pushButtonStart->setEnabled(!status);
253  widget.pushButtonPause->setEnabled(status);
254  widget.pushButtonStep->setEnabled(!status);
255  }
256  catch (const Ice::ConnectionRefusedException& e)
257  {
258  ARMARX_INFO << "The pipeline has been killed";
259  }
260 }
261 
263 {
264  ARMARX_INFO << "Starting perception pipeline";
265 
266  pointCloudSegmenter->enablePipelineStep();
267  primitiveExtractor->enablePipelineStep();
268  affordanceExtraction->enablePipelineStep();
269  pointCloudProviders[currentPointCloudProvider]->begin_startCapture();
270 }
271 
273 {
274  ARMARX_INFO << "Pausing perception pipeline";
275 
276  pointCloudProviders[currentPointCloudProvider]->begin_stopCapture();
277  pointCloudSegmenter->disablePipelineStep();
278  primitiveExtractor->disablePipelineStep();
279  affordanceExtraction->disablePipelineStep();
280 }
281 
283 {
284  ARMARX_INFO << "Stepping perception pipeline";
285 
286  pointCloudSegmenter->enablePipelineStep();
287  primitiveExtractor->enablePipelineStep();
288  affordanceExtraction->enablePipelineStep();
289  pointCloudProviders[currentPointCloudProvider]->begin_startCaptureForNumFrames(1);
290 }
291 
293 {
294  pipelineVisualization->begin_enableVisualization(widget.checkBoxShowPrimitives->isChecked(), widget.checkBoxShowAffordances->isChecked(), false);
295 }
296 
298 {
299  std::string filename = QFileDialog::getOpenFileName(getWidget(), tr("Open Affordance Scene"), "~", tr("Scene Files (*.scene)")).toStdString();
300  if (filename == "")
301  {
302  return;
303  }
304 
305  affordanceExtraction->importScene(filename);
306 }
307 
309 {
310  std::string filename = QFileDialog::getSaveFileName(NULL, tr("Save Affordance Scene"), "", tr("Scene Files (*.scene)")).toStdString();
311  if (filename == "")
312  {
313  return;
314  }
315 
316  affordanceExtraction->exportScene(filename);
317 }
318 
320 {
321  Eigen::Vector3f min, max;
322 
323  // Account for robots which don't have their root pose in the ground plane (e.g. ARMAR-4 as opposed to ARMAR-III)
324  float root_z = PosePtr::dynamicCast(robotStateComponent->getSynchronizedRobot()->getGlobalPose())->toEigen()(2, 3);
325 
326  switch (index)
327  {
328  case 0:
329  // Effectively disable cropping
330  min << -10000, -10000, -10000;
331  max << 10000, 10000, 10000;
332  break;
333 
334  case 1:
335  min << -3000, 300, 0 - root_z;
336  max << 3000, 3000, 3000 - root_z;
337  break;
338 
339  case 2:
340  min << -2000, 300, 300 - root_z;
341  max << 2000, 3000, 2500 - root_z;
342  break;
343 
344  case 3:
345  min << -1000, 300, 600 - root_z;
346  max << 1000, 2000, 1500 - root_z;
347  break;
348 
349  case 4:
350  min << -500, 300, 1000 - root_z;
351  max << 500, 2000, 1500 - root_z;
352  break;
353 
354  default:
355  ARMARX_WARNING << "Unknown cropping setup selected";
356  break;
357  }
358 
359  for (auto& provider : filteredPointCloudProviders)
360  {
361  provider->setCroppingParameters(new armarx::Vector3(min), new armarx::Vector3(max), "Root");
362  }
363 }
364 
366 {
367  currentPointCloudProvider = index;
368  pointCloudSegmenter->begin_changePointCloudProvider(filteredPointCloudProviderNames[currentPointCloudProvider] + "Result");
369 
370  segmentationSetupSelected(widget.comboBoxSegmentation->currentIndex());
371 }
372 
374 {
375  if (index < 0 || static_cast<std::size_t>(index) >= pipelineConfigFiles.size())
376  {
377  return;
378  }
379 
380  primitiveExtractor->loadParametersFromFile(pipelineConfigFiles[index]);
381  pointCloudSegmenter->loadLccpParametersFromFile(pipelineConfigFiles[index]);
382 }
383 
384 QString AffordancePipelineGuiWidgetController::formatTimestamp(long timestamp)
385 {
386  IceUtil::Time time = IceUtil::Time::microSeconds(timestamp);
387  return QString(time.toDateTime().substr(time.toDateTime().find(' ') + 1).c_str());
388 }
389 
391 {
392  if (!configDialog)
393  {
394  configDialog = new AffordancePipelineGuiConfigDialog(parent);
395  }
396 
397  return qobject_cast<AffordancePipelineGuiConfigDialog*>(configDialog);
398 }
399 
401 {
402  pointCloudProviderNames = configDialog->getProviderNames();
403  pointCloudProviderDisplayNames = configDialog->getProviderDisplayNames();
404  filteredPointCloudProviderNames = configDialog->getFilteredProviderNames();
405  pipelineConfigFiles = configDialog->getPipelineConfigurationFiles();
406 
407  pointCloudSegmenterName = configDialog->pointCloudSegmenterProxyFinder->getSelectedProxyName().toStdString();
408  primitiveExtractorName = configDialog->primitiveExtractorProxyFinder->getSelectedProxyName().toStdString();
409  affordanceExtractionName = configDialog->affordanceExtractionProxyFinder->getSelectedProxyName().toStdString();
410  pipelineVisualizationName = configDialog->pipelineVisualizationProxyFinder->getSelectedProxyName().toStdString();
411  robotStateComponentName = configDialog->robotStateComponentProxyFinder->getSelectedProxyName().toStdString();
412  platform = configDialog->getPlatform();
413 }
414 
416 {
417  if (customToolbar)
418  {
419  if (parent != customToolbar->parent())
420  {
421  customToolbar->setParent(parent);
422  }
423 
424  return customToolbar;
425  }
426 
427  customToolbar = new QToolBar(parent);
428  customToolbar->setIconSize(QSize(16, 16));
429  customToolbar->addAction(QIcon(":/icons/document-open-folder.png"), "Load Scene File", this, SLOT(loadScene()));
430  customToolbar->addAction(QIcon(":/icons/document-save-5.ico"), "Save Scene File", this, SLOT(saveScene()));
431 
432  return customToolbar;
433 }
armarx::AffordancePipelineGuiWidgetController::updatePipelineStatus
void updatePipelineStatus()
Definition: AffordancePipelineGuiWidgetController.cpp:232
index
uint8_t index
Definition: EtherCATFrame.h:59
Pose.h
armarx::AffordancePipelineGuiWidgetController::configured
void configured() override
This function must be implemented by the user, if he supplies a config dialog.
Definition: AffordancePipelineGuiWidgetController.cpp:400
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:267
armarx::AffordancePipelineGuiWidgetController::loadSettings
void loadSettings(QSettings *settings) override
Definition: AffordancePipelineGuiWidgetController.cpp:52
AffordancePipelineGuiConfigDialog.h
armarx::AffordancePipelineGuiWidgetController::stepPipeline
void stepPipeline()
Definition: AffordancePipelineGuiWidgetController.cpp:282
armarx::AffordancePipelineGuiConfigDialog
Definition: AffordancePipelineGuiConfigDialog.h:40
armarx::AffordancePipelineGuiWidgetController::getConfigDialog
QPointer< QDialog > getConfigDialog(QWidget *parent=0) override
getConfigDialog returns a pointer to the a configuration widget of this controller.
Definition: AffordancePipelineGuiWidgetController.cpp:390
armarx::AffordancePipelineGuiWidgetController::loadScene
void loadScene()
Definition: AffordancePipelineGuiWidgetController.cpp:297
armarx::status
status
Definition: FiniteStateMachine.h:259
armarx::AffordancePipelineGuiWidgetController::segmentationSetupSelected
void segmentationSetupSelected(int index)
Definition: AffordancePipelineGuiWidgetController.cpp:373
armarx::AffordancePipelineGuiWidgetController::getCustomTitlebarWidget
QPointer< QWidget > getCustomTitlebarWidget(QWidget *parent=0) override
getTitleToolbar returns a pointer to the a toolbar widget of this controller.
Definition: AffordancePipelineGuiWidgetController.cpp:415
armarx::AffordancePipelineGuiWidgetController::saveScene
void saveScene()
Definition: AffordancePipelineGuiWidgetController.cpp:308
armarx::AffordancePipelineGuiWidgetController::saveSettings
void saveSettings(QSettings *settings) override
Definition: AffordancePipelineGuiWidgetController.cpp:90
FramedPose.h
armarx::AffordancePipelineGuiWidgetController::pointCloudSourceSelected
void pointCloudSourceSelected(int index)
Definition: AffordancePipelineGuiWidgetController.cpp:365
armarx::AffordancePipelineGuiWidgetController::AffordancePipelineGuiWidgetController
AffordancePipelineGuiWidgetController()
Controller Constructor.
Definition: AffordancePipelineGuiWidgetController.cpp:40
armarx::Vector3
The Vector3 class.
Definition: Pose.h:112
filename
std::string filename
Definition: VisualizationRobot.cpp:83
armarx::AffordancePipelineGuiWidgetController::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: AffordancePipelineGuiWidgetController.cpp:213
armarx::AffordancePipelineGuiWidgetController::pausePipeline
void pausePipeline()
Definition: AffordancePipelineGuiWidgetController.cpp:272
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
AffordancePipelineGuiWidgetController.h
armarx::AffordancePipelineGuiWidgetController::croppingSelected
void croppingSelected(int index)
Definition: AffordancePipelineGuiWidgetController.cpp:319
armarx::viz::data::ElementFlags::names
const simox::meta::IntEnumNames names
Definition: json_elements.cpp:14
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::AffordancePipelineGuiWidgetController::visualizationOptionsChanged
void visualizationOptionsChanged(bool prm=false)
Definition: AffordancePipelineGuiWidgetController.cpp:292
armarx::AffordancePipelineGuiWidgetController::startPipeline
void startPipeline()
Definition: AffordancePipelineGuiWidgetController.cpp:262
armarx::AffordancePipelineGuiWidgetController::~AffordancePipelineGuiWidgetController
virtual ~AffordancePipelineGuiWidgetController()
Controller destructor.
Definition: AffordancePipelineGuiWidgetController.cpp:47
armarx::min
std::vector< T > min(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:294
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
armarx::viz::toString
const char * toString(InteractionFeedbackType type)
Definition: Interaction.h:27
armarx::ArmarXWidgetController::getWidget
virtual QPointer< QWidget > getWidget()
getWidget returns a pointer to the a widget of this controller.
Definition: ArmarXWidgetController.cpp:54
armarx::AffordancePipelineGuiWidgetController::onInitComponent
void onInitComponent() override
Definition: AffordancePipelineGuiWidgetController.cpp:128
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
ArmarXDataPath.h
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:151
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::AffordancePipelineGuiWidgetController::onConnectComponent
void onConnectComponent() override
Definition: AffordancePipelineGuiWidgetController.cpp:147
Application.h