WorkingMemoryGuiPlugin.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 MemoryX::WorkingMemoryGui
17 * @author Nikolaus Vahrenkamp ( vahrenkamp at kit dot edu)
18 * @date 2012
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 #include "WorkingMemoryGuiPlugin.h"
24 
25 #include <ArmarXCore/core/ArmarXManager.h>
26 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
27 #include <ArmarXCore/core/system/ArmarXDataPath.h>
28 #include <ArmarXCore/interface/core/Log.h>
29 
30 #include <RobotAPI/interface/core/RobotState.h>
31 #include <RobotAPI/libraries/core/Pose.h>
32 
33 #include "WorkingMemoryConfigDialog.h"
34 #include <MemoryX/gui-plugins/WorkingMemoryGui/ui_WorkingMemoryConfigDialog.h>
35 
36 //Core
37 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
38 #include <ArmarXCore/util/CPPUtility/trace.h>
39 #include <ArmarXCore/util/json/JSONObject.h>
40 
41 #include <ArmarXGui/applications/ArmarXGui/ArmarXMainWindow.h>
42 
43 // MemoryX
44 #include <MemoryX/core/MemoryXCoreObjectFactories.h>
45 #include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
46 #include <MemoryX/libraries/updater/ObjectLocalization/MemoryXUpdaterObjectFactories.h>
47 
48 // Simox-VirtualRobot
49 #include <SimoxUtility/algorithm/string/string_tools.h>
50 #include <VirtualRobot/Grasping/Grasp.h>
51 #include <VirtualRobot/Grasping/GraspSet.h>
52 #include <VirtualRobot/ManipulationObject.h>
53 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
54 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
55 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
56 #include <VirtualRobot/XML/ObjectIO.h>
57 #include <VirtualRobot/XML/RobotIO.h>
58 
59 // Qt headers
60 #include <QBrush>
61 #include <QCheckBox>
62 #include <QDialog>
63 #include <QDialogButtonBox>
64 #include <QFileDialog>
65 #include <QHeaderView>
66 #include <QMessageBox>
67 #include <QPushButton>
68 #include <QSlider>
69 #include <QSpinBox>
70 #include <QStringList>
71 #include <QTableView>
72 #include <QToolBar>
73 #include <QTreeWidget>
74 #include <QTreeWidgetItem>
75 #include <QWidget>
76 #include <Qt>
77 #include <QtGlobal>
78 
79 
80 // Coin3D headers
81 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
82 
83 #include <Inventor/Qt/SoQt.h>
84 #include <Inventor/SoDB.h>
85 #include <Inventor/nodes/SoUnits.h>
86 #include <MemoryX/components/WorkingMemory/PriorAttributeEnrichmentFusion.h>
87 
88 // For the VRML2.0 export
89 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
90 #include <Inventor/actions/SoToVRML2Action.h>
91 #include <Inventor/actions/SoWriteAction.h>
92 #include <Inventor/nodes/SoRotation.h>
93 
94 
95 // Eigen
96 #include <eigen3/Eigen/Core>
97 
98 // System
99 #include <math.h>
100 #include <stdio.h>
101 #include <stdlib.h>
102 #include <string.h>
103 
104 #include <algorithm>
105 #include <iostream>
106 #include <memory>
107 #include <string>
108 
109 
110 Q_DECLARE_METATYPE(::memoryx::EntityBasePtr)
111 
112 using namespace armarx;
113 using namespace memoryx;
114 using namespace VirtualRobot;
115 
116 #define DEFAULT_SETTINGS_PLUGIN_NAME "WorkingMemoryGuiPlugin"
117 #define DEFAULT_SETTINGS_PRIORMEMORY_NAME "PriorKnowledge"
118 #define DEFAULT_SETTINGS_WORKINGMEMORY_NAME "WorkingMemory"
119 #define DEFAULT_SETTINGS_WORKINGMEMORY_UPDATESTOPIC "WorkingMemoryUpdates"
120 #define DEFAULT_SETTINGS_OBJECT_INSTANCES_SEGMENT_NAME "objectInstances"
121 #define DEFAULT_SETTINGS_AGENT_INSTANCES_SEGMENT_NAME "agentInstances"
122 #define DEFAULT_SETTINGS_WORLD_STATE_SEGMENT_NAME "worldState"
123 #define DEFAULT_SETTINGS_ADDITIONAL_PACKAGES "RobotAPI,Armar3,Armar4"
124 #define DEFAULT_SETTINGS_COMMONSTORAGE_NAME "CommonStorage"
125 #define MIN_OBJECT_TRANSPARENCY 0.1f
126 
127 /*!
128  * \brief The delay between two robot updates. Should be >0
129  */
130 static const int DEFAULT_ROBOT_UPDATE_STEP = 33;
131 
132 namespace TreeItemType
133 {
134  enum TreeItemTypeEnum
135  {
136  eItemObject = QTreeWidgetItem::UserType + 1,
137  eItemAttr = QTreeWidgetItem::UserType + 2,
138  eItemValue = QTreeWidgetItem::UserType + 3
139  };
140 } // namespace TreeItemType
141 
142 WorkingMemoryGuiPlugin::WorkingMemoryGuiPlugin()
143 {
144  ARMARX_TRACE;
145  addWidget<WorkingMemoryController>();
146  qRegisterMetaType<memoryx::EntityBasePtr>("memoryx::EntityBasePtr");
147  qRegisterMetaType<Qt::SortOrder>("Qt::SortOrder");
148 }
149 
150 WorkingMemoryController::WorkingMemoryController()
151 {
152  ARMARX_TRACE;
153  rootVisu = nullptr;
154  objectsVisu = nullptr;
155  agentsVisu = nullptr;
156  debugLayerVisu = nullptr;
157  show3DViewer = false;
158  customToolbar = nullptr;
159  mutex3D.reset(new RecursiveMutex());
160  debugDrawer = nullptr;
161  sceneConfigDialog = nullptr;
162  debugDrawerConfigWidget = nullptr;
163 
164  ui.setupUi(getWidget());
165 
166  visuSetting_transparentExistanceCertatinty = ui.checkBox_existenceCertainty->isChecked();
167  visuSetting_showObjectModels = ui.cbShowObjectModels->isChecked();
168 
169  showObjInstanceUncertainties = ui.cbShowPositionUncertainty->isChecked();
170  getWidget()->setEnabled(false);
171 
172  ui.tabWidget->removeTab(0);
173 
174  //add debug layer widget
175  debugLayerControlWidget = new DebugLayerControlWidget();
176  ui.layerWidget->layout()->addWidget(debugLayerControlWidget);
177 }
178 
179 WorkingMemoryController::~WorkingMemoryController()
180 {
181  ARMARX_TRACE;
182  ARMARX_INFO << "Destructor WorkingMemoryController";
183  //removeTabs();
184 
185  ARMARX_INFO << "removing visu";
186  clearEntitiesData();
187  clearEntitiesVisu();
188 
189  ARMARX_INFO << "deleting visualization";
190  {
191  std::unique_lock lock(*mutex3D);
192 
193  if (debugLayerVisu)
194  {
195  debugLayerVisu->removeAllChildren();
196  debugLayerVisu->unref();
197  debugLayerVisu = nullptr;
198  }
199 
200  if (agentsVisu)
201  {
202  agentsVisu->removeAllChildren();
203  agentsVisu->unref();
204  agentsVisu = nullptr;
205  }
206 
207  if (objectsVisu)
208  {
209  objectsVisu->removeAllChildren();
210  objectsVisu->unref();
211  objectsVisu = nullptr;
212  }
213 
214  if (rootVisu)
215  {
216  rootVisu->removeAllChildren();
217  //rootVisu->unref();
218  //rootVisu = nullptr;
219  }
220  }
221  if (debugDrawer && debugDrawer->getObjectScheduler())
222  {
223  ARMARX_INFO << "Removing DebugDrawer component...";
224  debugDrawer->getObjectScheduler()->terminate();
225  ARMARX_INFO << "Removing DebugDrawer component...done";
226  }
227 }
228 
229 void
230 WorkingMemoryController::onInitComponent()
231 {
232  ARMARX_TRACE;
233  verbose = true;
234 
235  ARMARX_INFO << "Init WorkingMemoryController";
236 
237  rootVisu = new SoSeparator();
238  rootVisu->ref();
239 
240  // create the debugdrawer component
241  std::string debugDrawerComponentName = "WorkingMemoryDebugDrawer_" + getName();
242  ARMARX_INFO << "Creating component " << debugDrawerComponentName;
243  debugDrawer =
244  Component::create<EntityDrawerComponent>(getIceProperties(), debugDrawerComponentName);
245 
246  if (mutex3D)
247  {
248  debugDrawer->setMutex(mutex3D);
249  }
250  else
251  {
252  ARMARX_ERROR << " No 3d mutex available...";
253  }
254 
255  armarx::ArmarXManagerPtr m = getArmarXManager();
256  m->addObject(debugDrawer);
257 
258  debugLayerControlWidget->setEntityDrawer(debugDrawer);
259 
260  {
261  std::unique_lock lock(*mutex3D);
262  debugLayerVisu = new SoSeparator();
263  debugLayerVisu->ref();
264  debugLayerVisu->addChild(debugDrawer->getVisualization());
265 
266  objectsVisu = new SoSeparator();
267  objectsVisu->ref();
268 
269  agentsVisu = new SoSeparator();
270  agentsVisu->ref();
271 
272  // for now the debug layer is added, should be a gui swicth later on...
273  rootVisu->addChild(objectsVisu);
274  rootVisu->addChild(agentsVisu);
275  rootVisu->addChild(debugLayerVisu);
276  Eigen::Vector3f up;
277  up << 0.0f, 0.0f, 1.0f;
278  Eigen::Vector3f pos;
279  pos << 0, 0, 0;
280 
281  planeVisu = CoinVisualizationFactory::CreatePlaneVisualization(pos, up, 30000.0f, 0);
282  planeVisu->ref();
283  rootVisu->addChild(planeVisu);
284  }
285 
286  ARMARX_INFO << "WorkingMemory: " << settings_workingMemoryName;
287 
288  usingProxy(settings_workingMemoryName);
289  usingProxy(settings_priorMemoryName);
290  usingProxy(DEFAULT_SETTINGS_COMMONSTORAGE_NAME);
291 
292  connectSlots();
293 }
294 
295 void
296 WorkingMemoryController::onConnectComponent()
297 {
298  ARMARX_TRACE;
299  ARMARX_INFO << " Connecting WorkingMemoryController";
300 
301  try
302  {
303  std::unique_lock lock(mutexEntities);
304  memoryPrx = getProxy<AbstractWorkingMemoryInterfacePrx>(settings_workingMemoryName);
305  //memoryPrx->print();
306  ARMARX_DEBUG << "Getting object instances segment " << settings_objectInstancesSegmentName;
307  objectInstancesPrx = ObjectInstanceMemorySegmentBasePrx::uncheckedCast(
308  memoryPrx->getSegment(settings_objectInstancesSegmentName));
309  ARMARX_DEBUG << "Getting agent instances segment " << settings_agentInstancesSegmentName;
310  agentInstancesProxy = AgentInstancesSegmentBasePrx::uncheckedCast(
311  memoryPrx->getSegment(settings_agentInstancesSegmentName));
312  ARMARX_DEBUG << "Getting world state segment " << settings_worldStateSegmentName;
313  worldStateProxy = WorldStateSegmentBasePrx::uncheckedCast(
314  memoryPrx->getSegment(settings_worldStateSegmentName));
315 
316  ARMARX_DEBUG << "Getting common storage";
317  CommonStorageInterfacePrx commonStoragePrx =
318  getProxy<CommonStorageInterfacePrx>(DEFAULT_SETTINGS_COMMONSTORAGE_NAME);
319  fileManager.reset(new GridFileManager(commonStoragePrx));
320  segmentNames = memoryPrx->getSegmentNames();
321 
322  ARMARX_DEBUG << "Getting prior knowledge " << settings_priorMemoryName;
323  priorKnowledgePrx = getProxy<PriorKnowledgeInterfacePrx>(settings_priorMemoryName);
324 
325  ARMARX_DEBUG << "Getting classes segment";
326  classesSegmentPrx = priorKnowledgePrx->getObjectClassesSegment();
327  if (!classesSegmentPrx)
328  {
329  ARMARX_ERROR << "Classes Segment not found in PriorKnowledge";
330  }
331  }
332  catch (...)
333  {
334  ARMARX_ERROR
335  << "Could not connect to other components (WM/PriorKnowledge/CommonStorage)...";
336  }
337 
338 
339  usingTopic(settings_workingMemoryUpdatesTopic);
340  enableMainWidgetAsync(true);
341  QMetaObject::invokeMethod(this, "startTimerInGUIThread");
342  ARMARX_INFO << "Reconnected - refetching data";
343  QMetaObject::invokeMethod(this, "refetchEntitiesFromMemory");
344 }
345 
346 void
347 WorkingMemoryController::onDisconnectComponent()
348 {
349  ARMARX_TRACE;
350  ARMARX_INFO << "Disconnected...";
351 
352  clearEntitiesData();
353 
354 
355  emit signalClearEntities("");
356  emit signalRemoveTabs();
357 
358  enableMainWidgetAsync(false);
359  killTimer(robotUpdateTimerId);
360 
361  // ensure that no timer method is running
362  std::unique_lock lock(mutexEntities);
363 
364  ARMARX_INFO << "Disconnected... done";
365 }
366 
367 void
368 WorkingMemoryController::onExitComponent()
369 {
370  ARMARX_TRACE;
371  ARMARX_INFO << " Exiting component WorkingMemoryController";
372 
373  // not allowed to remove tabs here
374  //removeTabs();
375 
376  //ARMARX_INFO << "removing visu";
377  clearEntitiesData();
378  //clearEntitiesVisu();
379 
380  /*{
381  std::unique_lock lock(*mutex3D);
382 
383  if (debugLayerVisu)
384  {
385  debugLayerVisu->removeAllChildren();
386  debugLayerVisu->unref();
387  debugLayerVisu = nullptr;
388  }
389 
390  if (agentsVisu)
391  {
392  agentsVisu->removeAllChildren();
393  agentsVisu->unref();
394  agentsVisu = nullptr;
395  }
396 
397  if (objectsVisu)
398  {
399  objectsVisu->removeAllChildren();
400  objectsVisu->unref();
401  objectsVisu = nullptr;
402  }
403 
404  if (rootVisu)
405  {
406  rootVisu->removeAllChildren();
407  rootVisu->unref();
408  rootVisu = nullptr;
409  }
410 
411  }*/
412 
413  // ensure that no timer method is running
414  {
415  std::unique_lock lock(mutexEntities);
416  std::unique_lock lock2(*mutex3D);
417  }
418  ARMARX_INFO << "exit finshed";
419 }
420 
421 QPointer<QDialog>
422 WorkingMemoryController::getConfigDialog(QWidget* parent)
423 {
424  ARMARX_TRACE;
425  if (!dialog)
426  {
427  dialog = new WorkingMemoryConfigDialog(parent);
428  dialog->ui->editWorkingMemoryGuiName->setText(
429  QString::fromStdString(DEFAULT_SETTINGS_PLUGIN_NAME));
430  dialog->ui->editWorkingMemoryName->setText(
431  QString::fromStdString(DEFAULT_SETTINGS_WORKINGMEMORY_NAME));
432  dialog->ui->editPriorMemoryName->setText(
433  QString::fromStdString(DEFAULT_SETTINGS_PRIORMEMORY_NAME));
434  dialog->ui->editWorkingMemoryUpdatesTopic->setText(
435  QString::fromStdString(DEFAULT_SETTINGS_WORKINGMEMORY_UPDATESTOPIC));
436  dialog->ui->editObjectInstancesSegmentName->setText(
437  QString::fromStdString(DEFAULT_SETTINGS_OBJECT_INSTANCES_SEGMENT_NAME));
438  dialog->ui->editAgentInstancesSegmentName->setText(
439  QString::fromStdString(DEFAULT_SETTINGS_AGENT_INSTANCES_SEGMENT_NAME));
440  dialog->ui->editWorldStateSegmentName->setText(
441  QString::fromStdString(DEFAULT_SETTINGS_WORLD_STATE_SEGMENT_NAME));
442  dialog->ui->editAdditionalPackages->setText(
443  QString::fromStdString(DEFAULT_SETTINGS_ADDITIONAL_PACKAGES));
444  }
445 
446  return qobject_cast<WorkingMemoryConfigDialog*>(dialog);
447 }
448 
449 void
450 WorkingMemoryController::processPackages(const std::string& packages)
451 {
452  ARMARX_TRACE;
453  QString addPacksString = QString::fromStdString(packages);
454  auto addPacks = addPacksString.split(",", QString::SkipEmptyParts);
455 
456  for (QString& package : addPacks)
457  {
458  armarx::CMakePackageFinder finder(package.trimmed().toStdString());
459  ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
460  armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
461  }
462 }
463 
464 void
465 WorkingMemoryController::configured()
466 {
467  ARMARX_TRACE;
468  settings_priorMemoryName = dialog->ui->editPriorMemoryName->text().toStdString();
469  settings_workingMemoryName = dialog->ui->editWorkingMemoryName->text().toStdString();
470  settings_objectInstancesSegmentName =
471  dialog->ui->editObjectInstancesSegmentName->text().toStdString();
472  settings_agentInstancesSegmentName =
473  dialog->ui->editAgentInstancesSegmentName->text().toStdString();
474  settings_worldStateSegmentName = dialog->ui->editWorldStateSegmentName->text().toStdString();
475  settings_workingMemoryUpdatesTopic =
476  dialog->ui->editWorkingMemoryUpdatesTopic->text().toStdString();
477  settings_packages = dialog->ui->editAdditionalPackages->text().toStdString();
478  processPackages(settings_packages);
479 
480  this->setInstanceName(dialog->ui->editWorkingMemoryGuiName->text());
481 }
482 
483 void
484 WorkingMemoryController::loadSettings(QSettings* settings)
485 {
486  ARMARX_TRACE;
487  settings_priorMemoryName =
488  settings
489  ->value("settings_priorMemoryName",
490  QString::fromStdString(DEFAULT_SETTINGS_PRIORMEMORY_NAME))
491  .toString()
492  .toStdString();
493  settings_workingMemoryName =
494  settings
495  ->value("settings_workingMemoryName",
496  QString::fromStdString(DEFAULT_SETTINGS_WORKINGMEMORY_NAME))
497  .toString()
498  .toStdString();
499  settings_objectInstancesSegmentName =
500  settings
501  ->value("settings_objectInstancesSegmentName",
502  QString::fromStdString(DEFAULT_SETTINGS_OBJECT_INSTANCES_SEGMENT_NAME))
503  .toString()
504  .toStdString();
505  settings_agentInstancesSegmentName =
506  settings
507  ->value("settings_agentInstancesSegmentName",
508  QString::fromStdString(DEFAULT_SETTINGS_AGENT_INSTANCES_SEGMENT_NAME))
509  .toString()
510  .toStdString();
511  settings_worldStateSegmentName =
512  settings
513  ->value("settings_worldStateSegmentName",
514  QString::fromStdString(DEFAULT_SETTINGS_WORLD_STATE_SEGMENT_NAME))
515  .toString()
516  .toStdString();
517  settings_workingMemoryUpdatesTopic =
518  settings
519  ->value("settings_workingMemoryUpdatesTopic",
520  QString::fromStdString(DEFAULT_SETTINGS_WORKINGMEMORY_UPDATESTOPIC))
521  .toString()
522  .toStdString();
523  settings_packages = settings
524  ->value("settings_packages",
525  QString::fromStdString(DEFAULT_SETTINGS_ADDITIONAL_PACKAGES))
526  .toString()
527  .toStdString();
528 
529  visuSetting_transparentExistanceCertatinty =
530  settings->value("settings_existenceCertaintyTransparency", true).toBool();
531  ui.checkBox_existenceCertainty->setChecked(visuSetting_transparentExistanceCertatinty);
532 
533  visuSetting_showObjectModels = settings->value("settings_showObjectModels", true).toBool();
534  ui.cbShowObjectModels->setChecked(visuSetting_showObjectModels);
535 
536  QString instanceN =
537  settings->value("instanceName", QString::fromStdString(DEFAULT_SETTINGS_PLUGIN_NAME))
538  .toString();
539 
540  processPackages(settings_packages);
541  this->setInstanceName(instanceN);
542 
543  settings->beginGroup("DebugDrawer");
544  if (!settings->allKeys().empty())
545  {
546  getSceneConfigDialog(); // make shure debugDrawerConfigWidget exists
547  debugDrawerConfigWidget->loadSettings(settings);
548  }
549  settings->endGroup();
550 }
551 
552 void
553 WorkingMemoryController::saveSettings(QSettings* settings)
554 {
555  ARMARX_TRACE;
556  settings->setValue("settings_priorMemoryName",
557  QString::fromStdString(settings_priorMemoryName));
558  settings->setValue("settings_workingMemoryName",
559  QString::fromStdString(settings_workingMemoryName));
560  settings->setValue("settings_objectInstancesSegmentName",
561  QString::fromStdString(settings_objectInstancesSegmentName));
562  settings->setValue("settings_agentInstancesSegmentName",
563  QString::fromStdString(settings_agentInstancesSegmentName));
564  settings->setValue("settings_worldStateSegmentName",
565  QString::fromStdString(settings_worldStateSegmentName));
566  settings->setValue("settings_workingMemoryUpdatesTopic",
567  QString::fromStdString(settings_workingMemoryUpdatesTopic));
568  settings->setValue("settings_packages", QString::fromStdString(settings_packages));
569  settings->setValue("instanceName", this->getInstanceName());
570  settings->setValue("settings_existenceCertaintyTransparency",
571  ui.checkBox_existenceCertainty->isChecked());
572  settings->setValue("settings_showObjectModels", ui.cbShowObjectModels->isChecked());
573  settings->beginGroup("DebugDrawer");
574  if (debugDrawerConfigWidget)
575  {
576  debugDrawerConfigWidget->saveSettings(settings);
577  }
578  settings->endGroup();
579 }
580 
581 SoNode*
582 WorkingMemoryController::getScene()
583 {
584  ARMARX_TRACE;
585  /*if (rootVisu)
586  {
587  std::cout << "Returning scene = " << rootVisu->getName() << std::endl;
588  }*/
589 
590  return rootVisu;
591 }
592 
593 QPointer<QDialog>
594 WorkingMemoryController::getSceneConfigDialog(QWidget* parent)
595 {
596  ARMARX_TRACE;
597  if (!sceneConfigDialog)
598  {
599  sceneConfigDialog = new QDialog(parent);
600  QVBoxLayout* verticalLayout = new QVBoxLayout(sceneConfigDialog);
601  sceneConfigDialog->setLayout(verticalLayout);
602 
603  debugDrawerConfigWidget =
604  new armarx::DebugDrawerConfigWidget(debugDrawer, sceneConfigDialog);
605  verticalLayout->addWidget(debugDrawerConfigWidget);
606 
607  QDialogButtonBox* buttonBox = new QDialogButtonBox(sceneConfigDialog);
608  buttonBox->setOrientation(Qt::Horizontal);
609  buttonBox->setStandardButtons(QDialogButtonBox::Ok);
610  verticalLayout->addWidget(buttonBox);
611  QObject::connect(buttonBox, SIGNAL(accepted()), sceneConfigDialog, SLOT(accept()));
612  }
613  else if (!debugDrawerConfigWidget->getDebugDrawer())
614  {
615  debugDrawerConfigWidget->setDebugDrawer(debugDrawer);
616  }
617  return sceneConfigDialog;
618 }
619 
620 void
621 WorkingMemoryController::connectSlots()
622 {
623 
624  connect(ui.btnExport,
625  &QPushButton::clicked,
626  this,
627  &WorkingMemoryController::exportToVRML,
628  Qt::QueuedConnection);
629 
630  ARMARX_TRACE;
631  connect(ui.cbShowObjectModels,
632  SIGNAL(toggled(bool)),
633  this,
634  SLOT(setupVisualization()),
635  Qt::QueuedConnection);
636  connect(ui.cbShowPositionUncertainty,
637  SIGNAL(toggled(bool)),
638  this,
639  SLOT(setupVisualization()),
640  Qt::QueuedConnection);
641  connect(ui.checkBoxVisuLayers,
642  SIGNAL(toggled(bool)),
643  this,
644  SLOT(showVisuLayers(bool)),
645  Qt::QueuedConnection);
646  connect(ui.rbEllipse,
647  SIGNAL(toggled(bool)),
648  this,
649  SLOT(uncertaintyVisuToggled(bool)),
650  Qt::QueuedConnection);
651  connect(ui.rbHeatMap,
652  SIGNAL(toggled(bool)),
653  this,
654  SLOT(uncertaintyVisuToggled(bool)),
655  Qt::QueuedConnection);
656  connect(ui.rbHeatSurface,
657  SIGNAL(toggled(bool)),
658  this,
659  SLOT(uncertaintyVisuToggled(bool)),
660  Qt::QueuedConnection);
661  connect(ui.spinEllipseRadius,
662  SIGNAL(valueChanged(double)),
663  this,
664  SLOT(setupVisualization()),
665  Qt::QueuedConnection);
666  //connect(ui.spinMinVariance, SIGNAL(valueChanmemoryPrxged(double)), this, SLOT(setupVisualization()), Qt::QueuedConnection);
667  connect(ui.spinDisplayThreshold,
668  SIGNAL(valueChanged(double)),
669  this,
670  SLOT(setupVisualization()),
671  Qt::QueuedConnection);
672  connect(ui.spinGridSize,
673  SIGNAL(valueChanged(int)),
674  this,
675  SLOT(setupVisualization()),
676  Qt::QueuedConnection);
677 
678  connect(this,
679  SIGNAL(signalRebuildVisualization()),
680  this,
681  SLOT(rebuildVisualization()),
682  Qt::QueuedConnection);
683  connect(this,
684  SIGNAL(entityChanged(memoryx::EntityBasePtr, QString, bool)),
685  this,
686  SLOT(updateEntityVisu(memoryx::EntityBasePtr, QString, bool)),
687  Qt::QueuedConnection);
688  connect(this,
689  SIGNAL(entityChanged(memoryx::EntityBasePtr, QString, bool)),
690  this,
691  SLOT(updateEntityTree(memoryx::EntityBasePtr, QString, bool)),
692  Qt::QueuedConnection);
693 
694  connect(ui.checkBoxAgents,
695  SIGNAL(toggled(bool)),
696  this,
697  SLOT(showAgents(bool)),
698  Qt::QueuedConnection);
699  connect(ui.checkBox_existenceCertainty,
700  SIGNAL(toggled(bool)),
701  this,
702  SLOT(existenceCertaintyToggled(bool)),
703  Qt::QueuedConnection);
704 
705  connect(ui.checkBoxShowPlane,
706  SIGNAL(toggled(bool)),
707  this,
708  SLOT(showPlane(bool)),
709  Qt::QueuedConnection);
710 
711  connect(this, SIGNAL(signalAgentsRemoved()), this, SLOT(agentsRemoved()), Qt::QueuedConnection);
712  connect(
713  this, SIGNAL(signalObjectRemoved()), this, SLOT(objectsRemoved()), Qt::QueuedConnection);
714  connect(this,
715  SIGNAL(signalRefetchEntities()),
716  this,
717  SLOT(refetchEntitiesFromMemory()),
718  Qt::QueuedConnection);
719  connect(this,
720  SIGNAL(signalClearEntities(const std::string&)),
721  this,
722  SLOT(clearEntitiesVisu(const std::string&)),
723  Qt::QueuedConnection);
724  connect(this, SIGNAL(signalRemoveTabs()), this, SLOT(removeTabs()), Qt::QueuedConnection);
725 }
726 
727 void
728 WorkingMemoryController::exportToVRML()
729 {
730  QString fi = QFileDialog::getSaveFileName(
731  Q_NULLPTR, tr("VRML 2.0 File"), QString(), tr("VRML Files (*.wrl)"));
732  std::string s = std::string(fi.toLatin1());
733 
734  if (s.empty())
735  {
736  return;
737  }
738  if (!simox::alg::ends_with(s, ".wrl"))
739  {
740  s += ".wrl";
741  }
742 
743  std::unique_lock lock(*mutex3D);
744 
745  SoOutput* so = new SoOutput();
746  if (!so->openFile(s.c_str()))
747  {
748  ARMARX_ERROR << "Could not open file " << s << " for writing." << std::endl;
749  return;
750  }
751 
752  so->setHeaderString("#VRML V2.0 utf8");
753 
754  SoGroup* n = new SoGroup;
755  n->ref();
756  n->addChild(rootVisu);
757  SoGroup* newVisu = CoinVisualizationFactory::convertSoFileChildren(n);
758  newVisu->ref();
759 
760  SoToVRML2Action tovrml2;
761  tovrml2.apply(newVisu);
762  SoVRMLGroup* newroot = tovrml2.getVRML2SceneGraph();
763  newroot->ref();
764  SoWriteAction wra(so);
765  wra.apply(newroot);
766  newroot->unref();
767 
768  so->closeFile();
769 
770  newVisu->unref();
771  n->unref();
772 }
773 
774 void
775 WorkingMemoryController::timerEvent(QTimerEvent*)
776 {
777  ARMARX_TRACE;
778  std::unique_lock lock(mutexEntities);
779 
780  /* Copy joint values from remote robot to visualization robot. */
781  for (const auto& agent : currentAgents)
782  {
783  updateAgent(agent.second);
784  }
785 }
786 
787 void
788 WorkingMemoryController::treeItemDoubleClicked(QTreeWidgetItem* item, int column)
789 {
790  ARMARX_TRACE;
791  (void)column; // Unused.
792  if (item->type() == TreeItemType::eItemObject)
793  {
794  QMessageBox msgBox;
795  msgBox.setText("Entity selected: id = " + item->text(1));
796  msgBox.exec();
797  const std::string id = item->text(1).toStdString();
798  //currentObjects.find(id)->second->getManipulationObject()->highlight();
799  //currentObjects.find(id)->second->getManipulationObject()->getVisualization()->ge
800  }
801 }
802 
803 void
804 WorkingMemoryController::refetchEntitiesFromMemory()
805 {
806  ARMARX_TRACE;
807  /* When testing, execution would fail when an object was constantly being updated while entites were being refetched
808  from memory. As a workaround to avoid this problem, reportEntityCreated/Updated emits a signal captured by
809  updateEntityInTree. This signal is disconnected when refetchEntititesFromMemory is called and reconnected at
810  the end. */
811 
812  QObject::disconnect(this,
813  SIGNAL(entityChanged(memoryx::EntityBasePtr, QString, bool)),
814  this,
815  SLOT(updateEntityTree(memoryx::EntityBasePtr, QString, bool)));
816  QObject::disconnect(this,
817  SIGNAL(entityChanged(memoryx::EntityBasePtr, QString, bool)),
818  this,
819  SLOT(updateEntityVisu(memoryx::EntityBasePtr, QString, bool)));
820 
821  debugDrawer->clearDebugLayer();
822  debugDrawer->clearLayer("ObjectUncertainties");
823 
824  std::string selectedTabName = "";
825 
826  if (ui.tabWidget->count())
827  {
828  selectedTabName = ui.tabWidget->tabText(ui.tabWidget->currentIndex()).toStdString();
829  }
830 
831  removeTabs();
832 
833  ARMARX_INFO << "Refetching data from WorkingmemoryPrxMemory";
834 
835  try
836  {
837  ARMARX_TRACE;
838 
839  clearEntitiesData();
840  clearEntitiesVisu();
841 
842  //build tab-view for all segments
843  for (const auto& segmentName : segmentNames)
844  {
845 
846  ARMARX_INFO << "Processing segment " << segmentName;
847  WorkingMemoryEntitySegmentBasePrx segmentPrx =
848  WorkingMemoryEntitySegmentBasePrx::uncheckedCast(
849  memoryPrx->getSegment(segmentName));
850 
851  int index = addTab(segmentName);
852 
853  if (segmentName == selectedTabName)
854  {
855  ui.tabWidget->setCurrentIndex(index);
856  }
857 
858  //update entity tree for given segment
859  for (const EntityBasePtr& entityBase : segmentPrx->getAllEntities())
860  {
861  ARMARX_INFO << "Processing entity " << entityBase->getName();
862 
863  if (createEntity(entityBase, segmentName))
864  {
865  updateEntityTree(entityBase, QString(segmentName.c_str()), false);
866  updateEntityVisu(entityBase, QString(segmentName.c_str()), false);
867  }
868  }
869  }
870  }
871  catch (...)
872  {
873  ARMARX_ERROR << "Exception during refetch: " << armarx::GetHandledExceptionString();
874  }
875 
876  ARMARX_TRACE;
877 
878  connect(this,
879  SIGNAL(entityChanged(memoryx::EntityBasePtr, QString, bool)),
880  this,
881  SLOT(updateEntityTree(memoryx::EntityBasePtr, QString, bool)),
882  Qt::QueuedConnection);
883  connect(this,
884  SIGNAL(entityChanged(memoryx::EntityBasePtr, QString, bool)),
885  this,
886  SLOT(updateEntityVisu(memoryx::EntityBasePtr, QString, bool)),
887  Qt::QueuedConnection);
888 
889  // fit scene in view on connect
890  std::unique_lock lock(*mutex3D);
891  armarx::ArmarXMainWindow* main = dynamic_cast<armarx::ArmarXMainWindow*>(getMainWindow());
892 
893  if (ui.checkBoxViewAll->isChecked() && main->getViewerWidget())
894  {
895  main->getViewerWidget()->viewer->viewAll();
896  }
897 
898  emit signalRebuildVisualization();
899 }
900 
901 void
902 WorkingMemoryController::startTimerInGUIThread()
903 {
904  ARMARX_TRACE;
905  robotUpdateTimerId =
906  startTimer((DEFAULT_ROBOT_UPDATE_STEP > 0 ? DEFAULT_ROBOT_UPDATE_STEP : 1));
907  ARMARX_CHECK_EXPRESSION(robotUpdateTimerId != 0);
908 }
909 
910 void
911 WorkingMemoryController::convertObjectInstanceToGlobalPose(ObjectInstance& obj)
912 {
913  ARMARX_TRACE;
914  if (obj.getPosition()->frame != GlobalFrame && !obj.getPosition()->frame.empty())
915  {
916  if (obj.getPosition()->agent.empty())
917  {
918  // ARMARX_WARNING << /*deactivateSpam(1) <<*/ "Could not convert pose of " << obj.getName()
919  // << " to global pose because agent name is empty: current pose: " << obj.getPose()->output();
920  return;
921  }
922  FramedPosePtr pose = obj.getPose(); // This FramedPose is a copy.
923 
924  RobotDefinition* agent = nullptr;
925 
926  for (auto& [name, agentDef] : currentAgents)
927  {
928  if (agentDef.robot->getName() == pose->agent)
929  {
930  agent = new RobotDefinition(agentDef); // copy def
931  }
932  }
933  ARMARX_TRACE;
934 
935  if (!agent)
936  {
937  ARMARX_WARNING << "Agent " << pose->agent << " not found in current agents.";
938  obj.setPose(agentInstancesProxy->convertToWorldPose(pose->agent, obj.getPose()));
939  }
940  else
941  {
942  IceUtil::Time localizationTimestamp = obj.getLocalizationTimestamp();
943  //ARMARX_INFO << "Agent " << pose->agent << " found in current agents.";
944  ARMARX_INFO << deactivateSpam(5, obj.getName() + obj.getId() + "_before") << "Pose of "
945  << obj.getName() << " before: \n"
946  << *pose;
947 
948  // make sure that the agent has the same timestamp than the localization of the object.
949  bool sync_successful = true;
950 
951  if (localizationTimestamp != agent->timestamp)
952  {
953  sync_successful = RemoteRobot::synchronizeLocalCloneToTimestamp(
954  agent->timestampedRobot,
955  agent->robotStateComponent,
956  localizationTimestamp.toMicroSeconds());
957  agent->timestamp = localizationTimestamp;
958  }
959 
960  ARMARX_INFO << deactivateSpam(5, obj.getName() + obj.getId() + "_timestamps")
961  << "Updating Pose of object with ts " << localizationTimestamp
962  << " with agent " << pose->agent << "with pose: \n"
963  << agent->timestampedRobot->getRootNode()->getGlobalPose();
964 
965  if (sync_successful)
966  {
967  pose->changeToGlobal(agent->timestampedRobot);
968  obj.setPose(pose);
969  }
970  else
971  {
972  ARMARX_WARNING << "Agent " << pose->agent
973  << " could not successfully be synced. Try to calculate pose with "
974  "current agent of agentSegment.";
975  obj.setPose(agentInstancesProxy->convertToWorldPose(pose->agent, obj.getPose()));
976  }
977 
978  ARMARX_INFO << deactivateSpam(5, obj.getName() + obj.getId() + "_after") << "Pose of "
979  << obj.getName() << " after: \n"
980  << *pose;
981  }
982 
983  delete agent;
984  agent = nullptr;
985  }
986 }
987 
988 void
989 WorkingMemoryController::updateObjectData(const ObjectInstancePtr obj)
990 {
991  ARMARX_TRACE;
992  if (!obj)
993  {
994  return;
995  }
996 
997  std::unique_lock lock(mutexEntities);
998 
999  ARMARX_DEBUG << "Updating object entity " << obj->getName();
1000 
1001  // Convert to global
1002  try
1003  {
1004  if (obj->getPosition()->frame != obj->getOrientation()->frame)
1005  {
1006  ARMARX_ERROR << "Frame of pos and orientation are not the same of object "
1007  << obj->getName() << " pos frame: " << obj->getPosition()->frame
1008  << " ori frame: " << obj->getOrientation()->frame;
1009  }
1010 
1011  IceUtil::Time localizationTimestamp = obj->getLocalizationTimestamp();
1012  if (lastUpdated.find(obj->getName()) == lastUpdated.end() ||
1013  localizationTimestamp.toMicroSeconds() > lastUpdated.at(obj->getName()).first)
1014  {
1015  // check if localization timestamp is newer than latest update. Otherwise we already converted the object to global
1016  convertObjectInstanceToGlobalPose(*obj);
1017  lastUpdated[obj->getName()] =
1018  std::make_pair(localizationTimestamp.toMicroSeconds(), obj->getPose());
1019  }
1020  else
1021  {
1022  obj->setPose(lastUpdated.at(obj->getName()).second);
1023  }
1024 
1025  if (showObjInstanceUncertainties)
1026  {
1027  ARMARX_TRACE;
1028  auto uncertainty = obj->getPositionUncertainty();
1029 
1030  if (uncertainty)
1031  {
1032  float variance = std::min(5000.0f, uncertainty->getVarianceScalar());
1033  auto pos = obj->getPosition();
1034 
1035  if (pos->getFrame() == armarx::GlobalFrame)
1036  {
1037  debugDrawer->setSphereVisu(
1038  "ObjectUncertainties",
1039  obj->getName() + obj->getId() + "_uncertainty",
1040  pos,
1041  armarx::DrawColor{std::min(1.0f, variance / 1000),
1042  std::max(0.0f, 1 - variance / 1000),
1043  0,
1044  0.2f},
1045  variance);
1046  }
1047  else
1048  {
1049  ARMARX_VERBOSE << deactivateSpam(5, obj->getName() + obj->getId())
1050  << "Not global position for " << obj->getName() + obj->getId();
1051  }
1052  }
1053  else
1054  {
1055  ARMARX_VERBOSE << deactivateSpam(5, obj->getName() + obj->getId())
1056  << "No position uncertainty for " << obj->getName() + obj->getId();
1057  }
1058  }
1059  else
1060  {
1061  ARMARX_DEBUG << deactivateSpam(5, obj->getName() + obj->getId())
1062  << "Position uncertainty is not shown for "
1063  << obj->getName() + obj->getId();
1064  }
1065  }
1066  catch (...)
1067  {
1068  ARMARX_ERROR << "Execption during update object data...";
1069  }
1070 }
1071 
1072 bool
1073 WorkingMemoryController::createEntity(memoryx::EntityBasePtr entity, const std::string& entityType)
1074 {
1075  ARMARX_TRACE;
1076  const std::string id = entity->getId();
1077  if (entityType == settings_objectInstancesSegmentName ||
1078  entityType == settings_worldStateSegmentName)
1079  {
1080  bool found = false;
1081  {
1082  std::unique_lock lock(mutexEntities);
1083  EntityEntryMap::iterator itEntity = currentEntities.find(id);
1084  found = itEntity != currentEntities.end();
1085  }
1086 
1087  if (!found)
1088  {
1089  ARMARX_TRACE;
1090  ARMARX_INFO << "Creating entity instance, id:" << id << ", type " << entityType;
1091  ObjectDefinition o;
1092 
1093  EntityPtr e = EntityPtr::dynamicCast(entity);
1094  // check for entity type
1095  ObjectInstancePtr obj = ObjectInstancePtr::dynamicCast(e);
1096  if (!obj)
1097  {
1098  ARMARX_WARNING << "Could not cast entitiy to obj";
1099  return false;
1100  }
1101  convertObjectInstanceToGlobalPose(*obj);
1102 
1103  // this data could be outdated, if the manipualtion object was updated
1104  // -> we use the class reference by using the enrichment
1105  PriorAttributeEnrichmentFusion enrichment(priorKnowledgePrx, true);
1106  enrichment.initEntityInPlace(obj);
1107  o.simoxWrapper = obj->addWrapper(new EntityWrappers::SimoxObjectWrapper(fileManager));
1108 
1109 
1110  o.obj = obj;
1111  if (o.simoxWrapper)
1112  {
1113  o.manipObj = o.simoxWrapper->getManipulationObject();
1114  }
1115  std::unique_lock lock(mutexEntities);
1116  currentEntities[id] = o;
1117  }
1118  else
1119  {
1120  ARMARX_INFO << "Entity already created...";
1121  }
1122  return true;
1123  }
1124  else if (entityType == settings_agentInstancesSegmentName)
1125  {
1126  ARMARX_TRACE;
1127  bool found = false;
1128  {
1129  std::unique_lock lock(mutexEntities);
1130  AgentEntryMap::iterator itAgent = currentAgents.find(id);
1131  found = itAgent != currentAgents.end();
1132  }
1133  if (!found)
1134  {
1135  ARMARX_INFO << "Creating agent entity, id:" << id << " name: " << entity->getName()
1136  << ", type " << entity->ice_id();
1137  AgentInstancePtr agent = AgentInstancePtr::dynamicCast(entity);
1138  if (!agent || !addAgent(agent))
1139  {
1140  ARMARX_WARNING << "Agent loading failed...";
1141  return false;
1142  }
1143  }
1144  else
1145  {
1146  ARMARX_INFO << "Entity already created...";
1147  }
1148  return true;
1149  }
1150  return false;
1151 }
1152 
1153 void
1154 WorkingMemoryController::updateEntityVisu(memoryx::EntityBasePtr entity,
1155  QString entityType,
1156  bool isNew)
1157 {
1158  ARMARX_TRACE;
1159  const std::string id = entity->getId();
1160  const std::string typeS = entityType.toStdString();
1161  ARMARX_DEBUG << "Updating visualization, id:" << id << ", type " << typeS;
1162 
1163  std::unique_lock lock1(mutexEntities);
1164  std::unique_lock lock2(*mutex3D);
1165 
1166 
1167  if (typeS == settings_objectInstancesSegmentName || typeS == settings_worldStateSegmentName)
1168  {
1169  ARMARX_TRACE;
1170  EntityEntryMap::iterator itEntity = currentEntities.find(id);
1171 
1172  if (itEntity == currentEntities.end())
1173  {
1174  ARMARX_INFO << "Could not find currentEntity entry for object with id = " << id
1175  << ", building new entry";
1176  if (!createEntity(entity, typeS))
1177  {
1178  return;
1179  }
1180  }
1181  itEntity = currentEntities.find(id);
1182 
1183  //ARMARX_VERBOSE << "Updating visualization for object with id = " << id;
1184 
1185  if (isNew)
1186  {
1187  createObjectVisualization(itEntity->second);
1188  }
1189  else
1190  {
1191  ARMARX_TRACE;
1192  if (itEntity->second.simoxWrapper)
1193  {
1194  itEntity->second.simoxWrapper->updateFromEntity(entity);
1195 
1196  if (visuSetting_transparentExistanceCertatinty)
1197  {
1198  float t = std::max(MIN_OBJECT_TRANSPARENCY,
1199  itEntity->second.obj->getExistenceCertainty());
1200  ARMARX_DEBUG << "transparency for object "
1201  << itEntity->second.manipObj->getName() << ":"
1202  << itEntity->second.obj->getExistenceCertainty();
1203  //itEntity->second.visualization->getCoinVisualization(); // ensure that visu is built, not neccessary any more since simox 2.3.22
1204  // if (t < 0.9)
1205  {
1206  if (itEntity->second.cv)
1207  {
1208  itEntity->second.cv->setTransparency(t);
1209  }
1210  }
1211  }
1212  }
1213  }
1214  }
1215  else if (typeS == settings_agentInstancesSegmentName)
1216  {
1217  ARMARX_TRACE;
1218  AgentEntryMap::iterator itAgent = currentAgents.find(id);
1219 
1220  if (itAgent == currentAgents.end())
1221  {
1222  ARMARX_VERBOSE << "Could not find currentAgents entry for object with id = " << id
1223  << ", loading agent data...";
1224  if (!createEntity(entity, typeS))
1225  {
1226  return;
1227  }
1228  }
1229 
1230  if (isNew)
1231  {
1232  createAgentVisualisation(id);
1233  }
1234 
1235  // not needed, is done in timer method...
1236  // else
1237  // updateAgentVisualisation(entity);
1238  }
1239 
1240  //else
1241  //ARMARX_WARNING << "updateEntityVisu: entity type not supported: " << typeS;
1242  ARMARX_DEBUG << "Updating visualization END, id:" << id << ", type " << typeS;
1243 }
1244 
1245 void
1246 WorkingMemoryController::createObjectVisualization(ObjectDefinition& o)
1247 {
1248  ARMARX_TRACE;
1249  if (!o.simoxWrapper || !o.manipObj)
1250  {
1251  return;
1252  }
1253 
1254  std::unique_lock lock(*mutex3D);
1255 
1256  setupEntityVisu(o);
1257 
1258  SoNode* s = nullptr;
1259  updateVisualization(o.simoxWrapper);
1260 
1261 
1262  o.cv = o.manipObj->getVisualization<VirtualRobot::CoinVisualization>();
1263 
1264  if (o.cv)
1265  {
1266  if (visuSetting_transparentExistanceCertatinty)
1267  {
1268  ARMARX_TRACE;
1269  //float t = (float)(rand() % 1000) / 1000.0f;
1270  float t = std::max(MIN_OBJECT_TRANSPARENCY, o.obj->getExistenceCertainty());
1271  ARMARX_DEBUG << "transparency for object " << o.manipObj->getName() << ":" << t;
1272  // cv->getCoinVisualization(); // ensure that visu is built, not neccessary any more since simox 2.3.22
1273  o.cv->setTransparency(t);
1274  }
1275  s = o.cv->getCoinVisualization();
1276  }
1277 
1278  //SoNode* s = VirtualRobot::CoinVisualizationFactory::getCoinVisualization(o.manipObj, SceneObject::Full);
1279 
1280  if (!s)
1281  {
1282  CoinVisualizationFactoryPtr coinVisFactory(new CoinVisualizationFactory());
1283  auto name = o.simoxWrapper->getName();
1284  s = std::dynamic_pointer_cast<VirtualRobot::CoinVisualizationNode>(
1285  coinVisFactory->createCoordSystem(1.0, &name))
1286  ->getCoinVisualization();
1287  }
1288 
1289  if (s && objectsVisu)
1290  {
1291  o.visualization = s;
1292 
1293  if (objectsVisu->findChild(s) < 0)
1294  {
1295  objectsVisu->addChild(s);
1296  }
1297  }
1298 }
1299 
1300 void
1301 WorkingMemoryController::updateVisualization(const EntityWrappers::SimoxObjectWrapperPtr objWrapper)
1302 {
1303  ARMARX_TRACE;
1304  if (objWrapper && objWrapper->getManipulationObject())
1305  {
1306  ARMARX_DEBUG << "updateVisualization, object:"
1307  << objWrapper->getManipulationObject()->getName();
1308  }
1309  else
1310  {
1311  ARMARX_WARNING << "updateVisualization, empty object";
1312  }
1313 
1314  if (!objWrapper)
1315  {
1316  return;
1317  }
1318 
1319  std::unique_lock lock(*mutex3D);
1320 
1321  /* Update position etc and rebuild visu. */
1322  objWrapper->refreshVisuPose();
1323 }
1324 
1325 void
1326 WorkingMemoryController::updateEntityTree(memoryx::EntityBasePtr entity,
1327  QString entityType,
1328  bool isNew)
1329 {
1330  ARMARX_TRACE;
1331  (void)isNew; // Unused.
1332 
1333  ARMARX_DEBUG << "updateEntityTree:" << entity->getName() << ",type:" << entityType;
1334 
1335  EntityPtr obj = EntityPtr::dynamicCast(entity);
1336  std::string entityTypeS = entityType.toStdString();
1337 
1338  if (!segmentTabs.count(entityTypeS))
1339  {
1340  ARMARX_INFO << "Segment " << entityTypeS << " not yet displayed";
1341  return;
1342  }
1343 
1344  QString objName(obj->getName().c_str());
1345  QString objId = QString::fromStdString(entity->getId());
1346 
1347  QList<QTreeWidgetItem*> oldItems;
1348  QTreeWidgetItem* objItem;
1349 
1350  QTreeWidget* tree = segmentTabs.at(entityTypeS).tree;
1351  oldItems = tree->findItems(objId, Qt::MatchFixedString, 1);
1352  objItem = (oldItems.size() == 0) ? new QTreeWidgetItem(tree, TreeItemType::eItemObject)
1353  : oldItems.at(0);
1354  objItem->setText(0, objName);
1355  objItem->setText(1, objId);
1356 
1357  if (!objItem)
1358  {
1359  ARMARX_WARNING << "null objItem?!";
1360  return;
1361  }
1362 
1363  // Clear object node
1364  QList<QTreeWidgetItem*> oldAttrs = objItem->takeChildren();
1365 
1366  while (!oldAttrs.empty())
1367  {
1368  delete oldAttrs.takeFirst();
1369  }
1370 
1371  QBrush evenRow(QColor(217, 255, 226));
1372  QBrush oddRow(Qt::white);
1373  int i = 0;
1374  memoryx::NameList attrNames = obj->getAttributeNames();
1375 
1376  for (const auto& attrName : attrNames)
1377  {
1378  //QList<QTreeWidgetItem *> oldAttrs = objItem->findItems(objId, Qt::MatchFixedString, 0);
1379  QTreeWidgetItem* attrItem = new QTreeWidgetItem(objItem, TreeItemType::eItemAttr);
1380  //QString attrName(it->c_str());
1381  attrItem->setText(0, QString::fromStdString(attrName));
1382 
1383  armarx::VariantPtr attrValue = obj->getAttributeValue(attrName);
1384  auto uncertainty = obj->getAttribute(attrName)->getUncertainty();
1385 
1386  if (attrValue)
1387  {
1388  QString attrValStr(attrValue->getOutputValueOnly().c_str());
1389 
1390  if (uncertainty)
1391  {
1392  attrValStr +=
1393  QString("\nUncertainty: ") + QString::fromStdString(uncertainty->output());
1394  }
1395 
1396  attrItem->setText(1, attrValStr);
1397  }
1398  else
1399  {
1400  attrItem->setText(1, "Missing/unsupported value");
1401  }
1402 
1403 
1404  QBrush& bgBrush = (i % 2 == 0) ? evenRow : oddRow;
1405  attrItem->setBackground(0, bgBrush);
1406  attrItem->setBackground(1, bgBrush);
1407  ++i;
1408  }
1409  ARMARX_DEBUG << "updateEntityTree END:" << entity->getName() << ",type:" << entityType;
1410 }
1411 
1412 void
1413 WorkingMemoryController::clearWorkingMemory()
1414 {
1415  ARMARX_TRACE;
1416  if (QMessageBox::Yes ==
1417  QMessageBox(QMessageBox::Information,
1418  "Clearing Working Memory",
1419  "Do you really want to clear the content of Working Memory?",
1420  QMessageBox::Yes | QMessageBox::No)
1421  .exec())
1422  {
1423  std::unique_lock lock(mutexEntities);
1424 
1425  try
1426  {
1427  memoryPrx->clear();
1428  }
1429  catch (...)
1430  {
1431  ARMARX_ERROR << "Execption during clear working memory...";
1432  }
1433  }
1434 }
1435 
1436 void
1437 WorkingMemoryController::showVisuLayers(bool show)
1438 {
1439  ARMARX_TRACE;
1440  if (debugDrawer)
1441  {
1442  if (show)
1443  {
1444  debugDrawer->enableAllLayers();
1445  }
1446  else
1447  {
1448  debugDrawer->disableAllLayers();
1449  }
1450  }
1451 }
1452 
1453 void
1454 WorkingMemoryController::showAgents(bool show)
1455 {
1456  ARMARX_TRACE;
1457  std::unique_lock lock(*mutex3D);
1458 
1459  if (!rootVisu || !agentsVisu)
1460  {
1461  return;
1462  }
1463 
1464  if (show)
1465  {
1466  if (rootVisu->findChild(agentsVisu) < 0)
1467  {
1468  rootVisu->addChild(agentsVisu);
1469  }
1470  }
1471  else
1472  {
1473  if (rootVisu->findChild(agentsVisu) >= 0)
1474  {
1475  rootVisu->removeChild(agentsVisu);
1476  }
1477  }
1478 }
1479 
1480 void
1481 WorkingMemoryController::showPlane(bool show)
1482 {
1483  ARMARX_TRACE;
1484  std::unique_lock lock(*mutex3D);
1485 
1486  if (!rootVisu || !planeVisu)
1487  {
1488  return;
1489  }
1490 
1491  if (show)
1492  {
1493  if (rootVisu->findChild(planeVisu) < 0)
1494  {
1495  rootVisu->addChild(planeVisu);
1496  }
1497  }
1498  else
1499  {
1500  if (rootVisu->findChild(planeVisu) >= 0)
1501  {
1502  rootVisu->removeChild(planeVisu);
1503  }
1504  }
1505 }
1506 
1507 void
1508 WorkingMemoryController::clearEntitiesData()
1509 {
1510  ARMARX_TRACE;
1511  std::unique_lock lock(mutexEntities);
1512  ARMARX_INFO << "Clearing entity data";
1513 
1514  /* Clear storage. */
1515  for (auto& entity : currentEntities)
1516  {
1517  deletedEntities[entity.first] = entity.second;
1518  }
1519 
1520  currentEntities.clear();
1521 
1522  for (auto& entity : currentAgents)
1523  {
1524  deletedAgents[entity.first] = entity.second;
1525  }
1526 
1527  currentAgents.clear();
1528 }
1529 
1530 void
1531 WorkingMemoryController::clearEntitiesVisu(const std::string& segmentName)
1532 {
1533  ARMARX_TRACE;
1534  std::unique_lock lock(mutexEntities);
1535  ARMARX_INFO << "Clearing entity visu";
1536 
1537  if (segmentName == "" || segmentName == settings_objectInstancesSegmentName)
1538  {
1539  ARMARX_TRACE;
1540  /* Clear storage. */
1541  for (auto& entity : deletedEntities)
1542  {
1543  entity.second.manipObj.reset();
1544 
1545  if (entity.second.simoxWrapper)
1546  {
1547  entity.second.simoxWrapper->setEntity(EntityBasePtr());
1548  entity.second.simoxWrapper = nullptr;
1549  }
1550  }
1551 
1552  deletedEntities.clear();
1553  }
1554 
1555  if (segmentName == "" || segmentName == settings_agentInstancesSegmentName)
1556  {
1557  ARMARX_TRACE;
1558  deletedAgents.clear();
1559 
1560  {
1561  std::unique_lock lock2(*mutex3D);
1562 
1563  if (agentsVisu)
1564  {
1565  agentsVisu->removeAllChildren();
1566  }
1567 
1568  if (objectsVisu)
1569  {
1570  objectsVisu->removeAllChildren();
1571  }
1572  }
1573  }
1574 }
1575 
1576 void
1577 WorkingMemoryController::rebuildVisualization()
1578 {
1579  ARMARX_TRACE;
1580  ARMARX_INFO << "Rebuilding complete visualization";
1581  std::unique_lock lock1(mutexEntities);
1582  std::unique_lock lock2(*mutex3D);
1583 
1584  if (objectsVisu)
1585  {
1586  ARMARX_INFO << "Rebuilding object visualization";
1587  objectsVisu->removeAllChildren();
1588 
1589  /* Entities. */
1590  ARMARX_INFO << "Rebuilding entity visualization for " << currentEntities.size()
1591  << " elements";
1592 
1593  for (auto& entity : currentEntities)
1594  {
1595  ARMARX_VERBOSE << "Rebuilding entity visualization for: " << entity.first;
1596  createObjectVisualization(entity.second);
1597  }
1598  }
1599 
1600  if (agentsVisu)
1601  {
1602  ARMARX_INFO << "Rebuilding robot visualization for " << currentAgents.size() << " agents";
1603  agentsVisu->removeAllChildren();
1604 
1605  for (const auto& agent : currentAgents)
1606  {
1607  ARMARX_VERBOSE << "Rebuilding robot visualization for: " << agent.first;
1608  //if (agent.second.visualization)
1609  // agent.second.visualization->unref();
1610  createAgentVisualisation(agent.first);
1611  }
1612  }
1613 
1614  armarx::ArmarXMainWindow* main = dynamic_cast<armarx::ArmarXMainWindow*>(getMainWindow());
1615 
1616  if (main && main->getViewerWidget())
1617  {
1618  auto l = main->getViewerWidget()->viewer->getScopedLock();
1619 
1620  if (ui.checkBoxViewAll->isChecked() && main->getViewerWidget())
1621  {
1622  main->getViewerWidget()->viewer->viewAll();
1623  }
1624  }
1625 }
1626 
1627 void
1628 WorkingMemoryController::uncertaintyVisuToggled(bool isChecked)
1629 {
1630  ARMARX_TRACE;
1631  /* Avoid double refresh. */
1632  if (isChecked)
1633  {
1634  setupVisualization();
1635  }
1636 }
1637 
1638 void
1639 WorkingMemoryController::existenceCertaintyToggled(bool isChecked)
1640 {
1641  ARMARX_TRACE;
1642  visuSetting_transparentExistanceCertatinty = isChecked;
1643  setupVisualization();
1644 }
1645 
1646 void
1647 WorkingMemoryController::setupVisualization()
1648 {
1649  ARMARX_TRACE;
1650  std::unique_lock lock(mutexEntities);
1651 
1652  for (const auto& entity : currentEntities)
1653  {
1654  setupEntityVisu(entity.second);
1655  }
1656 
1657  showObjInstanceUncertainties = ui.cbShowPositionUncertainty->isChecked();
1658  visuSetting_showObjectModels = ui.cbShowObjectModels->isChecked();
1659 
1660  if (!showObjInstanceUncertainties)
1661  {
1662  debugDrawer->clearLayer("ObjectUncertainties");
1663  }
1664 }
1665 
1666 void
1667 WorkingMemoryController::setupEntityVisu(const ObjectDefinition& objWrapper)
1668 {
1669  ARMARX_TRACE;
1670  if (!objWrapper.simoxWrapper)
1671  {
1672  return;
1673  }
1674  if (objWrapper.manipObj)
1675  {
1676  ARMARX_DEBUG << "setupEntityVisu:" << objWrapper.manipObj->getName();
1677  }
1678  else
1679  {
1680  ARMARX_DEBUG << "setupEntityVisu: no manipObj";
1681  }
1682 
1683  std::unique_lock lock(*mutex3D);
1684  ObjectInstancePtr obj = ObjectInstancePtr::dynamicCast(objWrapper.simoxWrapper->getEntity());
1685 
1686  if (!obj)
1687  {
1688  ARMARX_VERBOSE << "Only ObjectInstances are currently supported";
1689  return;
1690  }
1691 
1692  bool showMainVisu =
1693  ui.cbShowObjectModels->isChecked() || !obj->getPositionAttribute()->getUncertainty();
1694  bool showUncertaintyVisu = ui.cbShowPositionUncertainty->isChecked();
1695 
1696  if (showUncertaintyVisu)
1697  {
1698  ARMARX_TRACE;
1699  EntityWrappers::SimoxObjectWrapper::UncertaintyVisuType visuType =
1700  EntityWrappers::SimoxObjectWrapper::eEllipse;
1701 
1702  if (ui.rbEllipse->isChecked())
1703  {
1704  visuType = EntityWrappers::SimoxObjectWrapper::eEllipse;
1705  }
1706  else if (ui.rbHeatMap->isChecked())
1707  {
1708  visuType = EntityWrappers::SimoxObjectWrapper::eHeatMap;
1709  }
1710  else if (ui.rbHeatSurface->isChecked())
1711  {
1712  visuType = EntityWrappers::SimoxObjectWrapper::eHeatSurface;
1713  }
1714  objWrapper.simoxWrapper->setUncertaintyVisuType(visuType);
1715  objWrapper.simoxWrapper->setUncertaintyVisuParams(ui.spinEllipseRadius->value(),
1716  ui.spinMinVariance->value(),
1717  expf(-ui.spinDisplayThreshold->value()),
1718  ui.spinGridSize->value());
1719  objWrapper.simoxWrapper->refreshVisuPose();
1720  }
1721 
1722  if (objWrapper.manipObj)
1723  {
1724  objWrapper.manipObj->setupVisualization(showMainVisu, showUncertaintyVisu);
1725  }
1726 }
1727 
1728 void
1729 WorkingMemoryController::reportEntityCreated(const std::string& segmentName,
1730  const EntityBasePtr& entityBase,
1731  const Ice::Current&)
1732 {
1733  ARMARX_TRACE;
1734  std::unique_lock lock(mutexEntities);
1735 
1736  try
1737  {
1738  ARMARX_VERBOSE << "Entity created. Segment:" << segmentName
1739  << ", entity:" << entityBase->getName();
1740 
1741  EntityPtr entity = EntityPtr::dynamicCast(entityBase);
1742 
1743  //QString entityId = entity->getId().c_str();
1744  QString entityType(segmentName.c_str());
1745 
1746  // check for entity type
1747  ObjectInstancePtr obj = ObjectInstancePtr::dynamicCast(entity);
1748 
1749  if (obj)
1750  {
1751  updateObjectData(obj);
1752  }
1753  else
1754  {
1755  AgentInstancePtr agent = AgentInstancePtr::dynamicCast(entity);
1756 
1757  if (agent)
1758  {
1759  addAgent(agent);
1760  }
1761  else
1762  {
1763  ARMARX_VERBOSE << "reportEntityCreated: entity type not supported: " << segmentName;
1764  }
1765  }
1766 
1767  emit entityChanged(entityBase, entityType, true);
1768  }
1769  catch (...)
1770  {
1771  ARMARX_ERROR << "Execption during reportEntityCreated...";
1772  }
1773 }
1774 
1775 void
1776 WorkingMemoryController::reportEntityUpdated(const std::string& segmentName,
1777  const EntityBasePtr& entityBaseOld,
1778  const EntityBasePtr& entityBaseNew,
1779  const Ice::Current&)
1780 {
1781  ARMARX_TRACE;
1782  (void)entityBaseOld; // Unused.
1783 
1784  ARMARX_DEBUG << "entity " << entityBaseNew->getName() << " got updated";
1785  std::unique_lock lock(mutexEntities);
1786 
1787  try
1788  {
1789  ARMARX_TRACE;
1790 
1791  EntityPtr entity = EntityPtr::dynamicCast(entityBaseNew);
1792 
1793  QString entityId(entity->getId().c_str());
1794  QString entityType(segmentName.c_str());
1795 
1796  // check for entity type
1797  ObjectInstancePtr obj = ObjectInstancePtr::dynamicCast(entity);
1798 
1799  if (obj)
1800  {
1801  updateObjectData(obj);
1802  }
1803  else
1804  {
1805  AgentInstancePtr agent = AgentInstancePtr::dynamicCast(entity);
1806 
1807  if (agent)
1808  {
1809  // check if we need to add a new agent
1810  auto it = currentAgents.find(entityId.toStdString());
1811 
1812  if (it == currentAgents.end())
1813  {
1814  addAgent(agent);
1815  }
1816  }
1817  }
1818 
1819  emit entityChanged(entityBaseNew, entityType, false);
1820  }
1821  catch (...)
1822  {
1823  ARMARX_ERROR << "Exception during reportEntityUpdated...";
1824  }
1825  ARMARX_DEBUG << "entity " << entityBaseNew->getName() << " got updated END";
1826 }
1827 
1828 void
1829 WorkingMemoryController::reportEntityRemoved(const std::string& segmentName,
1830  const EntityBasePtr& entity,
1831  const Ice::Current&)
1832 {
1833  ARMARX_TRACE;
1834  std::unique_lock lock(mutexEntities);
1835 
1836  try
1837  {
1838  std::string entityId = entity->getId();
1839  ARMARX_DEBUG << "Entity removed: id = " << entityId;
1840 
1841  // check for entity type
1842  ObjectInstancePtr obj = ObjectInstancePtr::dynamicCast(entity);
1843 
1844  if (obj)
1845  {
1846  ARMARX_TRACE;
1847  auto it = currentEntities.find(entityId);
1848 
1849  if (it != currentEntities.end())
1850  {
1851  ARMARX_INFO << "Object removed: id = " << entityId;
1852  deletedEntities[entityId] = currentEntities[entityId];
1853  currentEntities.erase(it);
1854  QMetaObject::invokeMethod(this, "objectsRemoved");
1855  }
1856  else
1857  {
1858  ARMARX_INFO << "Object not present, id = " << entityId;
1859  }
1860  }
1861  else
1862  {
1863  ARMARX_TRACE;
1864  AgentInstancePtr agent = AgentInstancePtr::dynamicCast(entity);
1865 
1866  if (agent)
1867  {
1868  // check if we need to add a new agent
1869  auto it = currentAgents.find(entityId);
1870 
1871  if (it != currentAgents.end())
1872  {
1873  ARMARX_INFO << "Object removed (agent): id = " << entityId;
1874  deletedAgents[entityId] = currentAgents[entityId];
1875  currentAgents.erase(it);
1876  QMetaObject::invokeMethod(this, "agentsRemoved");
1877  }
1878  else
1879  {
1880  ARMARX_INFO << "agent not present, id = " << entityId;
1881  }
1882  }
1883  else
1884  {
1885  QMetaObject::invokeMethod(this,
1886  "removeEntityInTree",
1887  Q_ARG(std::string, entityId),
1888  Q_ARG(std::string, segmentName));
1889  }
1890  }
1891  }
1892  catch (...)
1893  {
1894  ARMARX_ERROR << "Execption during reportEntityRemoved...";
1895  }
1896  ARMARX_DEBUG << "Entity removed END";
1897 }
1898 
1899 void
1900 WorkingMemoryController::objectsRemoved()
1901 {
1902  ARMARX_TRACE;
1903  std::unique_lock lock(mutexEntities);
1904  std::unique_lock lock2(*mutex3D);
1905 
1906  for (auto& e : deletedEntities)
1907  {
1908  if (objectsVisu && e.second.visualization &&
1909  objectsVisu->findChild(e.second.visualization) >= 0)
1910  {
1911  objectsVisu->removeChild(e.second.visualization);
1912  }
1913 
1914  removeEntityInTree(e.first, settings_objectInstancesSegmentName);
1915  }
1916 
1917  deletedEntities.clear();
1918 }
1919 
1920 void
1921 WorkingMemoryController::agentsRemoved()
1922 {
1923  ARMARX_TRACE;
1924  std::unique_lock lock(mutexEntities);
1925  std::unique_lock lock2(*mutex3D);
1926  ARMARX_INFO << "Agents removed...";
1927 
1928  for (auto& e : deletedAgents)
1929  {
1930  ARMARX_INFO << "Agent removed: " << e.first;
1931  if (agentsVisu && e.second.visualization &&
1932  agentsVisu->findChild(e.second.visualization) >= 0)
1933  {
1934  agentsVisu->removeChild(e.second.visualization);
1935  }
1936 
1937  removeEntityInTree(e.first, settings_agentInstancesSegmentName);
1938  }
1939 
1940  deletedAgents.clear();
1941 }
1942 
1943 void
1944 WorkingMemoryController::reportSnapshotLoaded(const std::string& segmentName, const Ice::Current&)
1945 {
1946  ARMARX_TRACE;
1947  (void)segmentName; // Unused.
1948 }
1949 
1950 void
1951 WorkingMemoryController::reportSnapshotCompletelyLoaded(const Ice::Current&)
1952 {
1953  ARMARX_TRACE;
1954  std::unique_lock lock(mutexEntities);
1955 
1956  {
1957  emit signalRefetchEntities();
1958  }
1959 }
1960 
1961 void
1962 WorkingMemoryController::reportMemoryCleared(const std::string& segmentName, const Ice::Current&)
1963 {
1964  ARMARX_TRACE;
1965  std::unique_lock lock(mutexEntities);
1966 
1967 
1968  ARMARX_INFO << "segment " << segmentName << " cleared";
1969 
1970  try
1971  {
1972  if (segmentName == settings_objectInstancesSegmentName)
1973  {
1974  /* Clear storage. */
1975  for (auto& entity : currentEntities)
1976  {
1977  deletedEntities[entity.first] = entity.second;
1978  }
1979 
1980  currentEntities.clear();
1981  }
1982 
1983  if (segmentName == settings_agentInstancesSegmentName)
1984  {
1985  for (auto& entity : currentAgents)
1986  {
1987  deletedAgents[entity.first] = entity.second;
1988  }
1989 
1990  currentAgents.clear();
1991  }
1992 
1993 
1994  //clearEntitiesData();
1995  }
1996  catch (...)
1997  {
1998  ARMARX_ERROR << "Execption during reportMemoryCleared...";
1999  }
2000 
2001  emit signalClearEntities(segmentName);
2002 }
2003 
2004 void
2005 WorkingMemoryController::setMutex3D(RecursiveMutexPtr const& mutex3D)
2006 {
2007  ARMARX_TRACE;
2008  this->mutex3D = mutex3D;
2009 
2010  if (debugDrawer)
2011  {
2012  debugDrawer->setMutex(mutex3D);
2013  }
2014 }
2015 
2016 void
2017 WorkingMemoryController::removeEntityInTree(const std::string entityId,
2018  const std::string entityType)
2019 {
2020  ARMARX_INFO << "Remove qt element: " << entityId << ", " << entityType;
2021  QString entityItemId(entityId.c_str());
2022  QTreeWidgetItem* entityItem = nullptr;
2023 
2024  if (segmentTabs.count(entityType))
2025  {
2026  QList<QTreeWidgetItem*> items =
2027  segmentTabs.at(entityType).tree->findItems(entityItemId, Qt::MatchFixedString, 1);
2028  if (items.size() > 0)
2029  {
2030  entityItem = items.at(0);
2031  }
2032  }
2033  else
2034  {
2035  ARMARX_WARNING << "Undefined entity type: " << entityType << std::endl;
2036  return;
2037  }
2038 
2039  if (!entityItem)
2040  {
2041  ARMARX_WARNING << "Could not determine Qt tree item for entity " << entityId << ", type "
2042  << entityType;
2043  return;
2044  }
2045 
2046  QList<QTreeWidgetItem*> entityAttributes = entityItem->takeChildren();
2047 
2048  while (!entityAttributes.empty())
2049  {
2050  delete entityAttributes.takeFirst();
2051  }
2052 
2053  delete entityItem;
2054  ARMARX_INFO << "Remove qt element done";
2055 }
2056 
2057 int
2058 WorkingMemoryController::addTab(std::string segmentName)
2059 {
2060  ARMARX_TRACE;
2061  int index;
2062 
2063  if (!segmentTabs.count(segmentName))
2064  {
2065 
2066  //create tab
2067  QWidget* tab = new QWidget();
2068  QGridLayout* layout = new QGridLayout();
2069  tab->setLayout(layout);
2070 
2071  //create tree
2072  QTreeWidget* tree = new QTreeWidget();
2073  tree->setColumnCount(2);
2074  QStringList header;
2075  header << "Name"
2076  << "ID/Value";
2077  tree->setHeaderLabels(header);
2078  tree->header()->setDefaultSectionSize(200);
2079  tree->setSortingEnabled(true);
2080  tree->sortByColumn(0, Qt::SortOrder::AscendingOrder);
2081  index = ui.tabWidget->addTab(tab, QString::fromStdString(segmentName));
2082 
2083  //add tree to tab
2084  layout->addWidget(tree);
2085 
2086  struct SegmentTab et = {index, tree, tab};
2087  segmentTabs.insert(std::pair<std::string, struct SegmentTab>(segmentName, et));
2088 
2089  connect(tree,
2090  SIGNAL(itemDoubleClicked(QTreeWidgetItem*, int)),
2091  this,
2092  SLOT(treeItemDoubleClicked(QTreeWidgetItem*, int)));
2093  }
2094  else
2095  {
2096  index = segmentTabs.at(segmentName).tabIndex;
2097  }
2098 
2099  return index;
2100 }
2101 
2102 void
2103 WorkingMemoryController::removeTab(std::string segmentName)
2104 {
2105  ARMARX_TRACE;
2106  if (segmentTabs.count(segmentName))
2107  {
2108  ARMARX_INFO << "Removing tab " << segmentName;
2109  struct SegmentTab tab = segmentTabs.at(segmentName);
2110  QObject::disconnect(tab.tree,
2111  SIGNAL(itemDoubleClicked(QTreeWidgetItem*, int)),
2112  this,
2113  SLOT(treeItemDoubleClicked(QTreeWidgetItem*, int)));
2114 
2115  ui.tabWidget->removeTab(tab.tabIndex);
2116  tab.tree->~QTreeWidget();
2117  tab.tab->~QWidget();
2118  segmentTabs.erase(segmentName);
2119  }
2120 }
2121 
2122 void
2123 WorkingMemoryController::removeTabs()
2124 {
2125  ARMARX_TRACE;
2126  ARMARX_INFO << "Removing tabs";
2127  for (auto& entry : segmentTabs)
2128  {
2129  removeTab(entry.first);
2130  }
2131 }
2132 
2133 QPointer<QWidget>
2134 WorkingMemoryController::getCustomTitlebarWidget(QWidget* parent)
2135 {
2136  ARMARX_TRACE;
2137  if (customToolbar)
2138  {
2139  if (parent != customToolbar->parent())
2140  {
2141  customToolbar->setParent(parent);
2142  }
2143 
2144  return customToolbar;
2145  }
2146 
2147  customToolbar = new QToolBar(parent);
2148  customToolbar->setIconSize(QSize(16, 16));
2149  // customToolbar->addAction(QIcon(":/icons/configure-3.png"), "Configure",this, SLOT(OpenConfigureDialog()));
2150  customToolbar->addAction(QIcon(":/icons/view-refresh-7.png"),
2151  "Refetch Data",
2152  this,
2153  SLOT(refetchEntitiesFromMemory()));
2154  customToolbar->addAction(QIcon(":/icons/dialog-close.ico"),
2155  "Clear Working Memory",
2156  this,
2157  SLOT(clearWorkingMemory()));
2158 
2159  return customToolbar;
2160 }
2161 
2162 void
2163 WorkingMemoryController::createAgentVisualisation(const std::string entityId)
2164 {
2165  ARMARX_TRACE;
2166  std::unique_lock lock(mutexEntities);
2167  if (currentAgents.find(entityId) == currentAgents.end() || !currentAgents[entityId].robot)
2168  {
2169  return;
2170  }
2171  currentAgents[entityId].visualization =
2172  VirtualRobot::CoinVisualizationFactory::getCoinVisualization(
2173  currentAgents[entityId].robot, VirtualRobot::SceneObject::Full, false);
2174 
2175  if (!currentAgents[entityId].visualization)
2176  {
2177  ARMARX_WARNING << "createAgentVisualisation: could not retrieve visualisation from robot "
2178  << currentAgents[entityId].robot->getName() << "; using ellipse";
2179  currentAgents[entityId].visualization =
2180  VirtualRobot::CoinVisualizationFactory::CreateEllipse(0.5, 0.5, 1, nullptr, false);
2181  }
2182 
2183  //agents[entityId].visualization->ref();
2184  if (agentsVisu && agentsVisu->findChild(currentAgents[entityId].visualization) < 0)
2185  {
2186  agentsVisu->addChild(currentAgents[entityId].visualization);
2187  }
2188 
2189  updateAgentVisualisation(entityId);
2190 }
2191 
2192 bool
2193 WorkingMemoryController::addAgent(const AgentInstancePtr agent)
2194 {
2195  ARMARX_TRACE;
2196  std::unique_lock lock(mutexEntities);
2197 
2198  try
2199  {
2200  if (!agent)
2201  {
2202  ARMARX_ERROR << "addAgent: entity is not of type AgentInstance";
2203  return false;
2204  }
2205 
2206  const std::string agentId = agent->getId();
2207 
2208  Ice::CommunicatorPtr iceCommunicator = getIceManager()->getCommunicator();
2209  if (!agent->hasAttribute("agentFilePath"))
2210  {
2211  ARMARX_INFO << "No filepath available for agent " << agent->getName();
2212  return false;
2213  }
2214  std::string agentFilePath = agent->getAgentFilePath();
2215  ARMARX_INFO << "Adding agent " << agent->getName() << " agentId:" << agentId
2216  << ", filepath: " << agentFilePath;
2217  armarx::SharedRobotInterfacePrx sharedRobotInterfaceProxy = agent->getSharedRobot();
2218  if (sharedRobotInterfaceProxy)
2219  {
2220  ARMARX_INFO << agent->getName()
2221  << " proxy: " << sharedRobotInterfaceProxy->ice_toString();
2222  }
2223  else
2224  {
2225  ARMARX_INFO << agent->getName() << " proxy nullptr";
2226  }
2227  try
2228  {
2229  if (!sharedRobotInterfaceProxy &&
2230  !agent->getStringifiedSharedRobotInterfaceProxy().empty())
2231  {
2232  sharedRobotInterfaceProxy =
2233  armarx::SharedRobotInterfacePrx::checkedCast(iceCommunicator->stringToProxy(
2234  agent->getStringifiedSharedRobotInterfaceProxy()));
2235  }
2236  }
2237  catch (std::exception& e)
2238  {
2239  ARMARX_ERROR << "Failed to get robot state proxy for agent " << agent->getName() << ": "
2240  << e.what();
2241  return false;
2242  }
2243  if (sharedRobotInterfaceProxy)
2244  {
2245  ARMARX_TRACE;
2246  if (sharedRobotInterfaceProxy->getRobotStateComponent())
2247  {
2248  auto packages =
2249  sharedRobotInterfaceProxy->getRobotStateComponent()->getArmarXPackages();
2250  ARMARX_INFO << VAROUT(packages);
2251  for (auto& p : packages)
2252  {
2253  armarx::CMakePackageFinder finder(p);
2254  if (finder.packageFound())
2255  {
2256  armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
2257  }
2258  }
2259  }
2260  else
2261  {
2262  ARMARX_INFO << "RobotStateComponent proxy is empty";
2263  }
2264  }
2265  if (!armarx::ArmarXDataPath::getAbsolutePath(agentFilePath, agentFilePath))
2266  {
2267  ARMARX_TRACE;
2268  ARMARX_ERROR << "Could not find robot file " << agentFilePath;
2269 
2270  ARMARX_VERBOSE << "Searched in " << armarx::ArmarXDataPath::getDataPaths().size()
2271  << " additional paths";
2272 
2273  for (const auto& path : armarx::ArmarXDataPath::getDataPaths())
2274  {
2275  ARMARX_VERBOSE << "\t" << path;
2276  }
2277 
2278  return false;
2279  }
2280 
2281  RobotDefinition robotDefinition;
2282  try
2283  {
2284  ARMARX_TRACE;
2285  robotDefinition.robot = RemoteRobot::createLocalClone(
2286  sharedRobotInterfaceProxy, agentFilePath, sharedRobotInterfaceProxy->getScaling());
2287  robotDefinition.robot->setName(sharedRobotInterfaceProxy->getName());
2288 
2289  // TODO: We only need the structure here.
2290  robotDefinition.timestampedRobot = robotDefinition.robot->clone();
2291  }
2292  catch (...)
2293  {
2294  robotDefinition.robot.reset();
2295  ARMARX_ERROR << "addAgent: error while loading robot from file: "
2296  << GetHandledExceptionString();
2297  return false;
2298  }
2299 
2300  ARMARX_VERBOSE << "addAgent: loaded robot from file " << agentFilePath;
2301 
2302  robotDefinition.robotStateComponent = sharedRobotInterfaceProxy->getRobotStateComponent();
2303 
2304  ARMARX_INFO << "addAgent: created remote robot";
2305 
2306  currentAgents[agentId] = robotDefinition;
2307  }
2308  catch (...)
2309  {
2310  ARMARX_ERROR << "Exception during addAgent:\n" << GetHandledExceptionString();
2311  return false;
2312  }
2313  return true;
2314 }
2315 
2316 void
2317 WorkingMemoryController::updateAgentVisualisation(const std::string entityId)
2318 {
2319  ARMARX_TRACE;
2320  std::unique_lock lock(mutexEntities);
2321  auto entity = agentInstancesProxy->getEntityById(entityId);
2322  updateAgentVisualisation(entity);
2323 }
2324 
2325 void
2326 WorkingMemoryController::updateAgentVisualisation(const EntityBasePtr entity)
2327 {
2328  ARMARX_TRACE;
2329  std::unique_lock lock(*mutex3D);
2330 
2331  AgentInstancePtr agent = AgentInstancePtr::dynamicCast(entity);
2332 
2333  if (!agent || currentAgents.find(entity->getId()) == currentAgents.end())
2334  {
2335  return;
2336  }
2337 
2338  auto entityId = entity->getId();
2339  Eigen::Matrix4f matrix = Eigen::Matrix4f::Identity();
2340 
2341  armarx::FramedPositionPtr agentPosition = agent->getPosition();
2342 
2343  if (agentPosition)
2344  {
2345  matrix.block(0, 3, 3, 1) = agentPosition->toEigen();
2346  }
2347 
2348  armarx::FramedOrientationPtr agentOrientation = agent->getOrientation();
2349 
2350  if (agentOrientation)
2351  {
2352  matrix.block(0, 0, 3, 3) = agentOrientation->toEigen();
2353  }
2354 
2355  auto& agentInstance = currentAgents[entityId];
2356  agentInstance.robot->setGlobalPose(matrix);
2357 }
2358 
2359 void
2360 WorkingMemoryController::updateAgent(const WorkingMemoryController::RobotDefinition& agent)
2361 {
2362  ARMARX_TRACE;
2363  std::unique_lock lock(mutexEntities);
2364  std::unique_lock lock2(*mutex3D);
2365 
2366  if (!agent.robot || !agent.visualization)
2367  {
2368  return;
2369  }
2370 
2371  std::map<std::string, float> jointValues;
2372  std::vector<RobotNodePtr> storeNodesRemote;
2373 
2374  try
2375  {
2376  armarx::RemoteRobot::synchronizeLocalClone(agent.robot, agent.robotStateComponent);
2377  }
2378  catch (...)
2379  {
2380  ARMARX_ERROR << deactivateSpam(5) << "Exception during RemoteRobot::synchronizeLocalClone: "
2381  << armarx::GetHandledExceptionString();
2382  }
2383 }
memoryx::WorkingMemoryController::SegmentTab::tabIndex
int tabIndex
Definition: WorkingMemoryGuiPlugin.h:376
armarx::viz::coin::exportToVRML
void exportToVRML(SoNode *node, std::string const &exportFilePath)
Definition: ExportVRML.cpp:70
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
TreeItemType::TreeItemTypeEnum
TreeItemTypeEnum
Definition: PriorMemoryEditorPlugin.cpp:97
DEFAULT_SETTINGS_COMMONSTORAGE_NAME
#define DEFAULT_SETTINGS_COMMONSTORAGE_NAME
Definition: WorkingMemoryGuiPlugin.cpp:124
TreeItemType::eItemObject
@ eItemObject
Definition: PriorMemoryEditorPlugin.cpp:99
memoryx::WorkingMemoryController::RobotDefinition::robot
VirtualRobot::RobotPtr robot
Robot used for visualisation purposes.
Definition: WorkingMemoryGuiPlugin.h:355
armarx::CMakePackageFinder::packageFound
bool packageFound() const
Returns whether or not this package was found with cmake.
Definition: CMakePackageFinder.cpp:511
DEFAULT_SETTINGS_AGENT_INSTANCES_SEGMENT_NAME
#define DEFAULT_SETTINGS_AGENT_INSTANCES_SEGMENT_NAME
Definition: WorkingMemoryGuiPlugin.cpp:121
JSONObject.h
index
uint8_t index
Definition: EtherCATFrame.h:59
memoryx::Entity::getName
::std::string getName(const ::Ice::Current &=Ice::emptyCurrent) const override
Retrieve name of this entity.
Definition: Entity.cpp:181
armarx::RemoteRobot::synchronizeLocalClone
static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
Definition: RemoteRobot.cpp:522
ArmarXManager.h
VirtualRobot
Definition: FramedPose.h:42
memoryx::Entity::getId
::std::string getId(const ::Ice::Current &=Ice::emptyCurrent) const override
Retrieve id of this entity which is an integer in string representation.
Definition: Entity.cpp:167
PriorAttributeEnrichmentFusion.h
string.h
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
Pose.h
DEFAULT_SETTINGS_WORLD_STATE_SEGMENT_NAME
#define DEFAULT_SETTINGS_WORLD_STATE_SEGMENT_NAME
Definition: WorkingMemoryGuiPlugin.cpp:122
TreeItemType::eItemAttr
@ eItemAttr
Definition: PriorMemoryEditorPlugin.cpp:100
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:65
armarx::ArmarXMainWindow
The ArmarXMainWindow class.
Definition: ArmarXMainWindow.h:76
trace.h
memoryx::WorkingMemoryController::RobotDefinition
Definition: WorkingMemoryGuiPlugin.h:353
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:52
armarx::ArmarXDataPath::getDataPaths
static std::vector< std::string > getDataPaths()
Definition: ArmarXDataPath.cpp:615
memoryx::PriorAttributeEnrichmentFusion::initEntityInPlace
void initEntityInPlace(const EntityBasePtr &updateEntity)
Definition: PriorAttributeEnrichmentFusion.cpp:42
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
memoryx::PriorAttributeEnrichmentFusion
Definition: PriorAttributeEnrichmentFusion.h:38
memoryx::ObjectInstance
Definition: ObjectInstance.h:48
so
linux gnu libIceDB so
Definition: CMakeLists.txt:7
memoryx::ObjectInstance::setPose
void setPose(const armarx::FramedPoseBasePtr &newPose)
Convenience function to set position and orientation attributes at once.
Definition: ObjectInstance.cpp:207
DEFAULT_SETTINGS_ADDITIONAL_PACKAGES
#define DEFAULT_SETTINGS_ADDITIONAL_PACKAGES
Definition: WorkingMemoryGuiPlugin.cpp:123
memoryx::WorkingMemoryController::ObjectDefinition::cv
VirtualRobot::CoinVisualizationPtr cv
Definition: WorkingMemoryGuiPlugin.h:371
DEFAULT_SETTINGS_WORKINGMEMORY_NAME
#define DEFAULT_SETTINGS_WORKINGMEMORY_NAME
Definition: WorkingMemoryGuiPlugin.cpp:118
memoryx::WorkingMemoryController::ObjectDefinition::obj
ObjectInstancePtr obj
Definition: WorkingMemoryGuiPlugin.h:367
IceInternal::Handle< FramedPose >
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:75
armarx::GetHandledExceptionString
std::string GetHandledExceptionString()
Definition: Exception.cpp:165
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:77
DEFAULT_SETTINGS_WORKINGMEMORY_UPDATESTOPIC
#define DEFAULT_SETTINGS_WORKINGMEMORY_UPDATESTOPIC
Definition: WorkingMemoryGuiPlugin.cpp:119
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
memoryx::WorkingMemoryController::RobotDefinition::robotStateComponent
armarx::RobotStateComponentInterfacePrx robotStateComponent
Definition: WorkingMemoryGuiPlugin.h:357
armarx::ArmarXWidgetController::RecursiveMutexPtr
std::shared_ptr< RecursiveMutex > RecursiveMutexPtr
Definition: ArmarXWidgetController.h:262
memoryx::ObjectInstance::getPosition
armarx::FramedPositionPtr getPosition() const
Retrieve position of the instance.
Definition: ObjectInstance.cpp:88
ArmarXMainWindow.h
DebugLayerControlWidget
Definition: DebugLayerControlWidget.h:54
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
MIN_OBJECT_TRANSPARENCY
#define MIN_OBJECT_TRANSPARENCY
Definition: WorkingMemoryGuiPlugin.cpp:125
memoryx::WorkingMemoryController::SegmentTab::tab
QWidget * tab
Definition: WorkingMemoryGuiPlugin.h:378
armarx::CMakePackageFinder::getDataDir
std::string getDataDir() const
Definition: CMakePackageFinder.h:194
memoryx::WorkingMemoryController::RobotDefinition::timestamp
IceUtil::Time timestamp
Time when timestamped robot was updated.
Definition: WorkingMemoryGuiPlugin.h:360
memoryx::ObjectInstance::getLocalizationTimestamp
IceUtil::Time getLocalizationTimestamp() const
Definition: ObjectInstance.cpp:138
memoryx::WorkingMemoryController::ObjectDefinition::manipObj
VirtualRobot::ManipulationObjectPtr manipObj
Object used for visualisation purposes.
Definition: WorkingMemoryGuiPlugin.h:369
MemoryXCoreObjectFactories.h
ArmarXObjectScheduler.h
max
T max(T t1, T t2)
Definition: gdiam.h:51
memoryx::EntityWrappers::SimoxObjectWrapper::UncertaintyVisuType
UncertaintyVisuType
Definition: SimoxObjectWrapper.h:49
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
memoryx::EntityWrappers::SimoxObjectWrapper
SimoxObjectWrapper offers a simplified access to the Simox ManipulationObject (i.e visualization,...
Definition: SimoxObjectWrapper.h:46
memoryx::ObjectInstance::getPose
armarx::FramedPosePtr getPose() const
Convenience function to get position and orientation attributes at once.
Definition: ObjectInstance.cpp:198
TreeItemType::eItemValue
@ eItemValue
Definition: PriorMemoryEditorPlugin.cpp:101
armarx::DebugDrawerConfigWidget
Brief description of class DebugDrawerConfigWidget.
Definition: DebugDrawerConfigWidget.h:44
CMakePackageFinder.h
memoryx::WorkingMemoryController::SegmentTab
Definition: WorkingMemoryGuiPlugin.h:374
No
Introduction Thank you for taking interest in our work and downloading this software This library implements the algorithm described in the paper R R R Klein Efficient RANSAC for Point Cloud Shape in Computer Graphics No
Definition: ReadMe.txt:21
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
WorkingMemoryConfigDialog.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::ArmarXDataPath::addDataPaths
static void addDataPaths(const std::string &dataPathList)
Definition: ArmarXDataPath.cpp:554
armarx::RemoteRobot::createLocalClone
VirtualRobot::RobotPtr createLocalClone()
Clones the structure of this remote robot to a local instance.
Definition: RemoteRobot.cpp:381
memoryx::WorkingMemoryConfigDialog
Definition: WorkingMemoryConfigDialog.h:36
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
memoryx::WorkingMemoryController::SegmentTab::tree
QTreeWidget * tree
Definition: WorkingMemoryGuiPlugin.h:377
IceUtil::Handle< ArmarXManager >
armarx::RemoteRobot::synchronizeLocalCloneToTimestamp
static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
Synchronizes a local robot to a robot state at timestamp.
Definition: RemoteRobot.cpp:566
armarx::ends_with
bool ends_with(const std::string &haystack, const std::string &needle)
Definition: StringHelpers.cpp:53
IceInternal::ProxyHandle<::IceProxy::armarx::SharedRobotInterface >
GfxTL::Yes
OnOff< true > Yes
Definition: OnOff.h:14
memoryx::GridFileManager
GridFileManager provides utility functions for working with files in Mongo GridFS and links to them s...
Definition: GridFileManager.h:41
MemoryXTypesObjectFactories.h
DEFAULT_SETTINGS_OBJECT_INSTANCES_SEGMENT_NAME
#define DEFAULT_SETTINGS_OBJECT_INSTANCES_SEGMENT_NAME
Definition: WorkingMemoryGuiPlugin.cpp:120
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition: ArmarXDataPath.cpp:109
DEFAULT_SETTINGS_PRIORMEMORY_NAME
#define DEFAULT_SETTINGS_PRIORMEMORY_NAME
Definition: WorkingMemoryGuiPlugin.cpp:117
memoryx::WorkingMemoryController::ObjectDefinition::visualization
SoNode * visualization
Object visualisation.
Definition: WorkingMemoryGuiPlugin.h:370
min
T min(T t1, T t2)
Definition: gdiam.h:44
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
memoryx::WorkingMemoryController::ObjectDefinition::simoxWrapper
EntityWrappers::SimoxObjectWrapperPtr simoxWrapper
Definition: WorkingMemoryGuiPlugin.h:366
memoryx::WorkingMemoryController::RobotDefinition::timestampedRobot
VirtualRobot::RobotPtr timestampedRobot
Definition: WorkingMemoryGuiPlugin.h:361
ArmarXDataPath.h
memoryx::WorkingMemoryController::RobotDefinition::visualization
SoNode * visualization
Robot visualisation.
Definition: WorkingMemoryGuiPlugin.h:356
TreeItemType
Definition: PriorMemoryEditorPlugin.cpp:95
MemoryXUpdaterObjectFactories.h
memoryx::WorkingMemoryController::ObjectDefinition
Definition: WorkingMemoryGuiPlugin.h:364
main
int main(int argc, char *argv[])
Definition: Admin.cpp:45
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx::RecursiveMutex
boost::recursive_mutex RecursiveMutex
Definition: Synchronization.h:159
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
WorkingMemoryGuiPlugin.h
DEFAULT_SETTINGS_PLUGIN_NAME
#define DEFAULT_SETTINGS_PLUGIN_NAME
Definition: WorkingMemoryGuiPlugin.cpp:116