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