WorkingMemoryGuiPlugin.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    MemoryX::WorkingMemoryGui
* @author     Nikolaus Vahrenkamp ( vahrenkamp at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "WorkingMemoryGuiPlugin.h"
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/interface/core/Log.h>
 
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include "WorkingMemoryConfigDialog.h"
#include <MemoryX/gui-plugins/WorkingMemoryGui/ui_WorkingMemoryConfigDialog.h>
 
//Core
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/util/json/JSONObject.h>
 
#include <ArmarXGui/applications/ArmarXGui/ArmarXMainWindow.h>
 
// MemoryX
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/updater/ObjectLocalization/MemoryXUpdaterObjectFactories.h>
 
// Simox-VirtualRobot
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Grasping/Grasp.h>
#include <VirtualRobot/Grasping/GraspSet.h>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
#include <VirtualRobot/XML/ObjectIO.h>
#include <VirtualRobot/XML/RobotIO.h>
 
// Qt headers
#include <QBrush>
#include <QCheckBox>
#include <QDialog>
#include <QDialogButtonBox>
#include <QFileDialog>
#include <QHeaderView>
#include <QMessageBox>
#include <QPushButton>
#include <QSlider>
#include <QSpinBox>
#include <QStringList>
#include <QTableView>
#include <QToolBar>
#include <QTreeWidget>
#include <QTreeWidgetItem>
#include <QWidget>
#include <Qt>
#include <QtGlobal>
 
 
// Coin3D headers
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <Inventor/Qt/SoQt.h>
#include <Inventor/SoDB.h>
#include <Inventor/nodes/SoUnits.h>
#include <MemoryX/components/WorkingMemory/PriorAttributeEnrichmentFusion.h>
 
// For the VRML2.0 export
#include <Inventor/VRMLnodes/SoVRMLGroup.h>
#include <Inventor/actions/SoToVRML2Action.h>
#include <Inventor/actions/SoWriteAction.h>
#include <Inventor/nodes/SoRotation.h>
 
 
// Eigen
#include <eigen3/Eigen/Core>
 
// System
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
 
#include <algorithm>
#include <iostream>
#include <memory>
#include <string>
 
 
Q_DECLARE_METATYPE(::memoryx::EntityBasePtr)
 
using namespace armarx;
using namespace memoryx;
using namespace VirtualRobot;
 
#define DEFAULT_SETTINGS_PLUGIN_NAME "WorkingMemoryGuiPlugin"
#define DEFAULT_SETTINGS_PRIORMEMORY_NAME "PriorKnowledge"
#define DEFAULT_SETTINGS_WORKINGMEMORY_NAME "WorkingMemory"
#define DEFAULT_SETTINGS_WORKINGMEMORY_UPDATESTOPIC "WorkingMemoryUpdates"
#define DEFAULT_SETTINGS_OBJECT_INSTANCES_SEGMENT_NAME "objectInstances"
#define DEFAULT_SETTINGS_AGENT_INSTANCES_SEGMENT_NAME "agentInstances"
#define DEFAULT_SETTINGS_WORLD_STATE_SEGMENT_NAME "worldState"
#define DEFAULT_SETTINGS_ADDITIONAL_PACKAGES "RobotAPI,Armar3,Armar4"
#define DEFAULT_SETTINGS_COMMONSTORAGE_NAME "CommonStorage"
#define MIN_OBJECT_TRANSPARENCY 0.1f
 
/*!
 * \brief The delay between two robot updates. Should be >0
 */
static const int DEFAULT_ROBOT_UPDATE_STEP = 33;
 
namespace TreeItemType
{
    enum TreeItemTypeEnum
    {
        eItemObject = QTreeWidgetItem::UserType + 1,
        eItemAttr = QTreeWidgetItem::UserType + 2,
        eItemValue = QTreeWidgetItem::UserType + 3
    };
} // namespace TreeItemType
 
WorkingMemoryGuiPlugin::WorkingMemoryGuiPlugin()
{
    ARMARX_TRACE;
    addWidget<WorkingMemoryController>();
    qRegisterMetaType<memoryx::EntityBasePtr>("memoryx::EntityBasePtr");
    qRegisterMetaType<Qt::SortOrder>("Qt::SortOrder");
}
 
WorkingMemoryController::WorkingMemoryController()
{
    ARMARX_TRACE;
    rootVisu = nullptr;
    objectsVisu = nullptr;
    agentsVisu = nullptr;
    debugLayerVisu = nullptr;
    show3DViewer = false;
    customToolbar = nullptr;
    mutex3D.reset(new RecursiveMutex());
    debugDrawer = nullptr;
    sceneConfigDialog = nullptr;
    debugDrawerConfigWidget = nullptr;
 
    ui.setupUi(getWidget());
 
    visuSetting_transparentExistanceCertatinty = ui.checkBox_existenceCertainty->isChecked();
    visuSetting_showObjectModels = ui.cbShowObjectModels->isChecked();
 
    showObjInstanceUncertainties = ui.cbShowPositionUncertainty->isChecked();
    getWidget()->setEnabled(false);
 
    ui.tabWidget->removeTab(0);
 
    //add debug layer widget
    debugLayerControlWidget = new DebugLayerControlWidget();
    ui.layerWidget->layout()->addWidget(debugLayerControlWidget);
}
 
WorkingMemoryController::~WorkingMemoryController()
{
    ARMARX_TRACE;
    ARMARX_INFO << "Destructor WorkingMemoryController";
    //removeTabs();
 
    ARMARX_INFO << "removing visu";
    clearEntitiesData();
    clearEntitiesVisu();
 
    ARMARX_INFO << "deleting visualization";
    {
        std::unique_lock lock(*mutex3D);
 
        if (debugLayerVisu)
        {
            debugLayerVisu->removeAllChildren();
            debugLayerVisu->unref();
            debugLayerVisu = nullptr;
        }
 
        if (agentsVisu)
        {
            agentsVisu->removeAllChildren();
            agentsVisu->unref();
            agentsVisu = nullptr;
        }
 
        if (objectsVisu)
        {
            objectsVisu->removeAllChildren();
            objectsVisu->unref();
            objectsVisu = nullptr;
        }
 
        if (rootVisu)
        {
            rootVisu->removeAllChildren();
            //rootVisu->unref();
            //rootVisu = nullptr;
        }
    }
    if (debugDrawer && debugDrawer->getObjectScheduler())
    {
        ARMARX_INFO << "Removing DebugDrawer component...";
        debugDrawer->getObjectScheduler()->terminate();
        ARMARX_INFO << "Removing DebugDrawer component...done";
    }
}
 
void
WorkingMemoryController::onInitComponent()
{
    ARMARX_TRACE;
    verbose = true;
 
    ARMARX_INFO << "Init WorkingMemoryController";
 
    rootVisu = new SoSeparator();
    rootVisu->ref();
 
    // create the debugdrawer component
    std::string debugDrawerComponentName = "WorkingMemoryDebugDrawer_" + getName();
    ARMARX_INFO << "Creating component " << debugDrawerComponentName;
    debugDrawer =
        Component::create<EntityDrawerComponent>(getIceProperties(), debugDrawerComponentName);
 
    if (mutex3D)
    {
        debugDrawer->setMutex(mutex3D);
    }
    else
    {
        ARMARX_ERROR << " No 3d mutex available...";
    }
 
    armarx::ArmarXManagerPtr m = getArmarXManager();
    m->addObject(debugDrawer);
 
    debugLayerControlWidget->setEntityDrawer(debugDrawer);
 
    {
        std::unique_lock lock(*mutex3D);
        debugLayerVisu = new SoSeparator();
        debugLayerVisu->ref();
        debugLayerVisu->addChild(debugDrawer->getVisualization());
 
        objectsVisu = new SoSeparator();
        objectsVisu->ref();
 
        agentsVisu = new SoSeparator();
        agentsVisu->ref();
 
        // for now the debug layer is added, should be a gui swicth later on...
        rootVisu->addChild(objectsVisu);
        rootVisu->addChild(agentsVisu);
        rootVisu->addChild(debugLayerVisu);
        Eigen::Vector3f up;
        up << 0.0f, 0.0f, 1.0f;
        Eigen::Vector3f pos;
        pos << 0, 0, 0;
 
        planeVisu = CoinVisualizationFactory::CreatePlaneVisualization(pos, up, 30000.0f, 0);
        planeVisu->ref();
        rootVisu->addChild(planeVisu);
    }
 
    ARMARX_INFO << "WorkingMemory: " << settings_workingMemoryName;
 
    usingProxy(settings_workingMemoryName);
    usingProxy(settings_priorMemoryName);
    usingProxy(DEFAULT_SETTINGS_COMMONSTORAGE_NAME);
 
    connectSlots();
}
 
void
WorkingMemoryController::onConnectComponent()
{
    ARMARX_TRACE;
    ARMARX_INFO << " Connecting WorkingMemoryController";
 
    try
    {
        std::unique_lock lock(mutexEntities);
        memoryPrx = getProxy<AbstractWorkingMemoryInterfacePrx>(settings_workingMemoryName);
        //memoryPrx->print();
        ARMARX_DEBUG << "Getting object instances segment " << settings_objectInstancesSegmentName;
        objectInstancesPrx = ObjectInstanceMemorySegmentBasePrx::uncheckedCast(
            memoryPrx->getSegment(settings_objectInstancesSegmentName));
        ARMARX_DEBUG << "Getting agent instances segment " << settings_agentInstancesSegmentName;
        agentInstancesProxy = AgentInstancesSegmentBasePrx::uncheckedCast(
            memoryPrx->getSegment(settings_agentInstancesSegmentName));
        ARMARX_DEBUG << "Getting world state segment " << settings_worldStateSegmentName;
        worldStateProxy = WorldStateSegmentBasePrx::uncheckedCast(
            memoryPrx->getSegment(settings_worldStateSegmentName));
 
        ARMARX_DEBUG << "Getting common storage";
        CommonStorageInterfacePrx commonStoragePrx =
            getProxy<CommonStorageInterfacePrx>(DEFAULT_SETTINGS_COMMONSTORAGE_NAME);
        fileManager.reset(new GridFileManager(commonStoragePrx));
        segmentNames = memoryPrx->getSegmentNames();
 
        ARMARX_DEBUG << "Getting prior knowledge " << settings_priorMemoryName;
        priorKnowledgePrx = getProxy<PriorKnowledgeInterfacePrx>(settings_priorMemoryName);
 
        ARMARX_DEBUG << "Getting classes segment";
        classesSegmentPrx = priorKnowledgePrx->getObjectClassesSegment();
        if (!classesSegmentPrx)
        {
            ARMARX_ERROR << "Classes Segment not found in PriorKnowledge";
        }
    }
    catch (...)
    {
        ARMARX_ERROR
            << "Could not connect to other components (WM/PriorKnowledge/CommonStorage)...";
    }
 
 
    usingTopic(settings_workingMemoryUpdatesTopic);
    enableMainWidgetAsync(true);
    QMetaObject::invokeMethod(this, "startTimerInGUIThread");
    ARMARX_INFO << "Reconnected - refetching data";
    QMetaObject::invokeMethod(this, "refetchEntitiesFromMemory");
}
 
void
WorkingMemoryController::onDisconnectComponent()
{
    ARMARX_TRACE;
    ARMARX_INFO << "Disconnected...";
 
    clearEntitiesData();
 
 
    emit signalClearEntities("");
    emit signalRemoveTabs();
 
    enableMainWidgetAsync(false);
    killTimer(robotUpdateTimerId);
 
    // ensure that no timer method is running
    std::unique_lock lock(mutexEntities);
 
    ARMARX_INFO << "Disconnected... done";
}
 
void
WorkingMemoryController::onExitComponent()
{
    ARMARX_TRACE;
    ARMARX_INFO << " Exiting component WorkingMemoryController";
 
    // not allowed to remove tabs here
    //removeTabs();
 
    //ARMARX_INFO << "removing visu";
    clearEntitiesData();
    //clearEntitiesVisu();
 
    /*{
        std::unique_lock lock(*mutex3D);
 
        if (debugLayerVisu)
        {
            debugLayerVisu->removeAllChildren();
            debugLayerVisu->unref();
            debugLayerVisu = nullptr;
        }
 
        if (agentsVisu)
        {
            agentsVisu->removeAllChildren();
            agentsVisu->unref();
            agentsVisu = nullptr;
        }
 
        if (objectsVisu)
        {
            objectsVisu->removeAllChildren();
            objectsVisu->unref();
            objectsVisu = nullptr;
        }
 
        if (rootVisu)
        {
            rootVisu->removeAllChildren();
            rootVisu->unref();
            rootVisu = nullptr;
        }
 
    }*/
 
    // ensure that no timer method is running
    {
        std::unique_lock lock(mutexEntities);
        std::unique_lock lock2(*mutex3D);
    }
    ARMARX_INFO << "exit finshed";
}
 
QPointer<QDialog>
WorkingMemoryController::getConfigDialog(QWidget* parent)
{
    ARMARX_TRACE;
    if (!dialog)
    {
        dialog = new WorkingMemoryConfigDialog(parent);
        dialog->ui->editWorkingMemoryGuiName->setText(
            QString::fromStdString(DEFAULT_SETTINGS_PLUGIN_NAME));
        dialog->ui->editWorkingMemoryName->setText(
            QString::fromStdString(DEFAULT_SETTINGS_WORKINGMEMORY_NAME));
        dialog->ui->editPriorMemoryName->setText(
            QString::fromStdString(DEFAULT_SETTINGS_PRIORMEMORY_NAME));
        dialog->ui->editWorkingMemoryUpdatesTopic->setText(
            QString::fromStdString(DEFAULT_SETTINGS_WORKINGMEMORY_UPDATESTOPIC));
        dialog->ui->editObjectInstancesSegmentName->setText(
            QString::fromStdString(DEFAULT_SETTINGS_OBJECT_INSTANCES_SEGMENT_NAME));
        dialog->ui->editAgentInstancesSegmentName->setText(
            QString::fromStdString(DEFAULT_SETTINGS_AGENT_INSTANCES_SEGMENT_NAME));
        dialog->ui->editWorldStateSegmentName->setText(
            QString::fromStdString(DEFAULT_SETTINGS_WORLD_STATE_SEGMENT_NAME));
        dialog->ui->editAdditionalPackages->setText(
            QString::fromStdString(DEFAULT_SETTINGS_ADDITIONAL_PACKAGES));
    }
 
    return qobject_cast<WorkingMemoryConfigDialog*>(dialog);
}
 
void
WorkingMemoryController::processPackages(const std::string& packages)
{
    ARMARX_TRACE;
    QString addPacksString = QString::fromStdString(packages);
    auto addPacks = addPacksString.split(",", QString::SkipEmptyParts);
 
    for (QString& package : addPacks)
    {
        armarx::CMakePackageFinder finder(package.trimmed().toStdString());
        ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
        armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
    }
}
 
void
WorkingMemoryController::configured()
{
    ARMARX_TRACE;
    settings_priorMemoryName = dialog->ui->editPriorMemoryName->text().toStdString();
    settings_workingMemoryName = dialog->ui->editWorkingMemoryName->text().toStdString();
    settings_objectInstancesSegmentName =
        dialog->ui->editObjectInstancesSegmentName->text().toStdString();
    settings_agentInstancesSegmentName =
        dialog->ui->editAgentInstancesSegmentName->text().toStdString();
    settings_worldStateSegmentName = dialog->ui->editWorldStateSegmentName->text().toStdString();
    settings_workingMemoryUpdatesTopic =
        dialog->ui->editWorkingMemoryUpdatesTopic->text().toStdString();
    settings_packages = dialog->ui->editAdditionalPackages->text().toStdString();
    processPackages(settings_packages);
 
    this->setInstanceName(dialog->ui->editWorkingMemoryGuiName->text());
}
 
void
WorkingMemoryController::loadSettings(QSettings* settings)
{
    ARMARX_TRACE;
    settings_priorMemoryName =
        settings
            ->value("settings_priorMemoryName",
                    QString::fromStdString(DEFAULT_SETTINGS_PRIORMEMORY_NAME))
            .toString()
            .toStdString();
    settings_workingMemoryName =
        settings
            ->value("settings_workingMemoryName",
                    QString::fromStdString(DEFAULT_SETTINGS_WORKINGMEMORY_NAME))
            .toString()
            .toStdString();
    settings_objectInstancesSegmentName =
        settings
            ->value("settings_objectInstancesSegmentName",
                    QString::fromStdString(DEFAULT_SETTINGS_OBJECT_INSTANCES_SEGMENT_NAME))
            .toString()
            .toStdString();
    settings_agentInstancesSegmentName =
        settings
            ->value("settings_agentInstancesSegmentName",
                    QString::fromStdString(DEFAULT_SETTINGS_AGENT_INSTANCES_SEGMENT_NAME))
            .toString()
            .toStdString();
    settings_worldStateSegmentName =
        settings
            ->value("settings_worldStateSegmentName",
                    QString::fromStdString(DEFAULT_SETTINGS_WORLD_STATE_SEGMENT_NAME))
            .toString()
            .toStdString();
    settings_workingMemoryUpdatesTopic =
        settings
            ->value("settings_workingMemoryUpdatesTopic",
                    QString::fromStdString(DEFAULT_SETTINGS_WORKINGMEMORY_UPDATESTOPIC))
            .toString()
            .toStdString();
    settings_packages = settings
                            ->value("settings_packages",
                                    QString::fromStdString(DEFAULT_SETTINGS_ADDITIONAL_PACKAGES))
                            .toString()
                            .toStdString();
 
    visuSetting_transparentExistanceCertatinty =
        settings->value("settings_existenceCertaintyTransparency", true).toBool();
    ui.checkBox_existenceCertainty->setChecked(visuSetting_transparentExistanceCertatinty);
 
    visuSetting_showObjectModels = settings->value("settings_showObjectModels", true).toBool();
    ui.cbShowObjectModels->setChecked(visuSetting_showObjectModels);
 
    QString instanceN =
        settings->value("instanceName", QString::fromStdString(DEFAULT_SETTINGS_PLUGIN_NAME))
            .toString();
 
    processPackages(settings_packages);
    this->setInstanceName(instanceN);
 
    settings->beginGroup("DebugDrawer");
    if (!settings->allKeys().empty())
    {
        getSceneConfigDialog(); // make shure debugDrawerConfigWidget exists
        debugDrawerConfigWidget->loadSettings(settings);
    }
    settings->endGroup();
}
 
void
WorkingMemoryController::saveSettings(QSettings* settings)
{
    ARMARX_TRACE;
    settings->setValue("settings_priorMemoryName",
                       QString::fromStdString(settings_priorMemoryName));
    settings->setValue("settings_workingMemoryName",
                       QString::fromStdString(settings_workingMemoryName));
    settings->setValue("settings_objectInstancesSegmentName",
                       QString::fromStdString(settings_objectInstancesSegmentName));
    settings->setValue("settings_agentInstancesSegmentName",
                       QString::fromStdString(settings_agentInstancesSegmentName));
    settings->setValue("settings_worldStateSegmentName",
                       QString::fromStdString(settings_worldStateSegmentName));
    settings->setValue("settings_workingMemoryUpdatesTopic",
                       QString::fromStdString(settings_workingMemoryUpdatesTopic));
    settings->setValue("settings_packages", QString::fromStdString(settings_packages));
    settings->setValue("instanceName", this->getInstanceName());
    settings->setValue("settings_existenceCertaintyTransparency",
                       ui.checkBox_existenceCertainty->isChecked());
    settings->setValue("settings_showObjectModels", ui.cbShowObjectModels->isChecked());
    settings->beginGroup("DebugDrawer");
    if (debugDrawerConfigWidget)
    {
        debugDrawerConfigWidget->saveSettings(settings);
    }
    settings->endGroup();
}
 
SoNode*
WorkingMemoryController::getScene()
{
    ARMARX_TRACE;
    /*if (rootVisu)
    {
        std::cout << "Returning scene = " << rootVisu->getName() << std::endl;
    }*/
 
    return rootVisu;
}
 
QPointer<QDialog>
WorkingMemoryController::getSceneConfigDialog(QWidget* parent)
{
    ARMARX_TRACE;
    if (!sceneConfigDialog)
    {
        sceneConfigDialog = new QDialog(parent);
        QVBoxLayout* verticalLayout = new QVBoxLayout(sceneConfigDialog);
        sceneConfigDialog->setLayout(verticalLayout);
 
        debugDrawerConfigWidget =
            new armarx::DebugDrawerConfigWidget(debugDrawer, sceneConfigDialog);
        verticalLayout->addWidget(debugDrawerConfigWidget);
 
        QDialogButtonBox* buttonBox = new QDialogButtonBox(sceneConfigDialog);
        buttonBox->setOrientation(Qt::Horizontal);
        buttonBox->setStandardButtons(QDialogButtonBox::Ok);
        verticalLayout->addWidget(buttonBox);
        QObject::connect(buttonBox, SIGNAL(accepted()), sceneConfigDialog, SLOT(accept()));
    }
    else if (!debugDrawerConfigWidget->getDebugDrawer())
    {
        debugDrawerConfigWidget->setDebugDrawer(debugDrawer);
    }
    return sceneConfigDialog;
}
 
void
WorkingMemoryController::connectSlots()
{
 
    connect(ui.btnExport,
            &QPushButton::clicked,
            this,
            &WorkingMemoryController::exportToVRML,
            Qt::QueuedConnection);
 
    ARMARX_TRACE;
    connect(ui.cbShowObjectModels,
            SIGNAL(toggled(bool)),
            this,
            SLOT(setupVisualization()),
            Qt::QueuedConnection);
    connect(ui.cbShowPositionUncertainty,
            SIGNAL(toggled(bool)),
            this,
            SLOT(setupVisualization()),
            Qt::QueuedConnection);
    connect(ui.checkBoxVisuLayers,
            SIGNAL(toggled(bool)),
            this,
            SLOT(showVisuLayers(bool)),
            Qt::QueuedConnection);
    connect(ui.rbEllipse,
            SIGNAL(toggled(bool)),
            this,
            SLOT(uncertaintyVisuToggled(bool)),
            Qt::QueuedConnection);
    connect(ui.rbHeatMap,
            SIGNAL(toggled(bool)),
            this,
            SLOT(uncertaintyVisuToggled(bool)),
            Qt::QueuedConnection);
    connect(ui.rbHeatSurface,
            SIGNAL(toggled(bool)),
            this,
            SLOT(uncertaintyVisuToggled(bool)),
            Qt::QueuedConnection);
    connect(ui.spinEllipseRadius,
            SIGNAL(valueChanged(double)),
            this,
            SLOT(setupVisualization()),
            Qt::QueuedConnection);
    //connect(ui.spinMinVariance, SIGNAL(valueChanmemoryPrxged(double)), this, SLOT(setupVisualization()), Qt::QueuedConnection);
    connect(ui.spinDisplayThreshold,
            SIGNAL(valueChanged(double)),
            this,
            SLOT(setupVisualization()),
            Qt::QueuedConnection);
    connect(ui.spinGridSize,
            SIGNAL(valueChanged(int)),
            this,
            SLOT(setupVisualization()),
            Qt::QueuedConnection);
 
    connect(this,
            SIGNAL(signalRebuildVisualization()),
            this,
            SLOT(rebuildVisualization()),
            Qt::QueuedConnection);
    connect(this,
            SIGNAL(entityChanged(memoryx::EntityBasePtr, QString, bool)),
            this,
            SLOT(updateEntityVisu(memoryx::EntityBasePtr, QString, bool)),
            Qt::QueuedConnection);
    connect(this,
            SIGNAL(entityChanged(memoryx::EntityBasePtr, QString, bool)),
            this,
            SLOT(updateEntityTree(memoryx::EntityBasePtr, QString, bool)),
            Qt::QueuedConnection);
 
    connect(ui.checkBoxAgents,
            SIGNAL(toggled(bool)),
            this,
            SLOT(showAgents(bool)),
            Qt::QueuedConnection);
    connect(ui.checkBox_existenceCertainty,
            SIGNAL(toggled(bool)),
            this,
            SLOT(existenceCertaintyToggled(bool)),
            Qt::QueuedConnection);
 
    connect(ui.checkBoxShowPlane,
            SIGNAL(toggled(bool)),
            this,
            SLOT(showPlane(bool)),
            Qt::QueuedConnection);
 
    connect(this, SIGNAL(signalAgentsRemoved()), this, SLOT(agentsRemoved()), Qt::QueuedConnection);
    connect(
        this, SIGNAL(signalObjectRemoved()), this, SLOT(objectsRemoved()), Qt::QueuedConnection);
    connect(this,
            SIGNAL(signalRefetchEntities()),
            this,
            SLOT(refetchEntitiesFromMemory()),
            Qt::QueuedConnection);
    connect(this,
            SIGNAL(signalClearEntities(const std::string&)),
            this,
            SLOT(clearEntitiesVisu(const std::string&)),
            Qt::QueuedConnection);
    connect(this, SIGNAL(signalRemoveTabs()), this, SLOT(removeTabs()), Qt::QueuedConnection);
}
 
void
WorkingMemoryController::exportToVRML()
{
    QString fi = QFileDialog::getSaveFileName(
        Q_NULLPTR, tr("VRML 2.0 File"), QString(), tr("VRML Files (*.wrl)"));
    std::string s = std::string(fi.toLatin1());
 
    if (s.empty())
    {
        return;
    }
    if (!simox::alg::ends_with(s, ".wrl"))
    {
        s += ".wrl";
    }
 
    std::unique_lock lock(*mutex3D);
 
    SoOutput* so = new SoOutput();
    if (!so->openFile(s.c_str()))
    {
        ARMARX_ERROR << "Could not open file " << s << " for writing." << std::endl;
        return;
    }
 
    so->setHeaderString("#VRML V2.0 utf8");
 
    SoGroup* n = new SoGroup;
    n->ref();
    n->addChild(rootVisu);
    SoGroup* newVisu = CoinVisualizationFactory::convertSoFileChildren(n);
    newVisu->ref();
 
    SoToVRML2Action tovrml2;
    tovrml2.apply(newVisu);
    SoVRMLGroup* newroot = tovrml2.getVRML2SceneGraph();
    newroot->ref();
    SoWriteAction wra(so);
    wra.apply(newroot);
    newroot->unref();
 
    so->closeFile();
 
    newVisu->unref();
    n->unref();
}
 
void
WorkingMemoryController::timerEvent(QTimerEvent*)
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
 
    /* Copy joint values from remote robot to visualization robot. */
    for (const auto& agent : currentAgents)
    {
        updateAgent(agent.second);
    }
}
 
void
WorkingMemoryController::treeItemDoubleClicked(QTreeWidgetItem* item, int column)
{
    ARMARX_TRACE;
    (void)column; // Unused.
    if (item->type() == TreeItemType::eItemObject)
    {
        QMessageBox msgBox;
        msgBox.setText("Entity selected: id = " + item->text(1));
        msgBox.exec();
        const std::string id = item->text(1).toStdString();
        //currentObjects.find(id)->second->getManipulationObject()->highlight();
        //currentObjects.find(id)->second->getManipulationObject()->getVisualization()->ge
    }
}
 
void
WorkingMemoryController::refetchEntitiesFromMemory()
{
    ARMARX_TRACE;
    /* When testing, execution would fail when an object was constantly being updated while entites were being refetched
       from memory. As a workaround to avoid this problem, reportEntityCreated/Updated emits a signal captured by
       updateEntityInTree. This signal is disconnected when refetchEntititesFromMemory is called and reconnected at
       the end. */
 
    QObject::disconnect(this,
                        SIGNAL(entityChanged(memoryx::EntityBasePtr, QString, bool)),
                        this,
                        SLOT(updateEntityTree(memoryx::EntityBasePtr, QString, bool)));
    QObject::disconnect(this,
                        SIGNAL(entityChanged(memoryx::EntityBasePtr, QString, bool)),
                        this,
                        SLOT(updateEntityVisu(memoryx::EntityBasePtr, QString, bool)));
 
    debugDrawer->clearDebugLayer();
    debugDrawer->clearLayer("ObjectUncertainties");
 
    std::string selectedTabName = "";
 
    if (ui.tabWidget->count())
    {
        selectedTabName = ui.tabWidget->tabText(ui.tabWidget->currentIndex()).toStdString();
    }
 
    removeTabs();
 
    ARMARX_INFO << "Refetching data from WorkingmemoryPrxMemory";
 
    try
    {
        ARMARX_TRACE;
 
        clearEntitiesData();
        clearEntitiesVisu();
 
        //build tab-view for all segments
        for (const auto& segmentName : segmentNames)
        {
 
            ARMARX_INFO << "Processing segment " << segmentName;
            WorkingMemoryEntitySegmentBasePrx segmentPrx =
                WorkingMemoryEntitySegmentBasePrx::uncheckedCast(
                    memoryPrx->getSegment(segmentName));
 
            int index = addTab(segmentName);
 
            if (segmentName == selectedTabName)
            {
                ui.tabWidget->setCurrentIndex(index);
            }
 
            //update entity tree for given segment
            for (const EntityBasePtr& entityBase : segmentPrx->getAllEntities())
            {
                ARMARX_INFO << "Processing entity " << entityBase->getName();
 
                if (createEntity(entityBase, segmentName))
                {
                    updateEntityTree(entityBase, QString(segmentName.c_str()), false);
                    updateEntityVisu(entityBase, QString(segmentName.c_str()), false);
                }
            }
        }
    }
    catch (...)
    {
        ARMARX_ERROR << "Exception during refetch: " << armarx::GetHandledExceptionString();
    }
 
    ARMARX_TRACE;
 
    connect(this,
            SIGNAL(entityChanged(memoryx::EntityBasePtr, QString, bool)),
            this,
            SLOT(updateEntityTree(memoryx::EntityBasePtr, QString, bool)),
            Qt::QueuedConnection);
    connect(this,
            SIGNAL(entityChanged(memoryx::EntityBasePtr, QString, bool)),
            this,
            SLOT(updateEntityVisu(memoryx::EntityBasePtr, QString, bool)),
            Qt::QueuedConnection);
 
    // fit scene in view on connect
    std::unique_lock lock(*mutex3D);
    armarx::ArmarXMainWindow* main = dynamic_cast<armarx::ArmarXMainWindow*>(getMainWindow());
 
    if (ui.checkBoxViewAll->isChecked() && main->getViewerWidget())
    {
        main->getViewerWidget()->viewer->viewAll();
    }
 
    emit signalRebuildVisualization();
}
 
void
WorkingMemoryController::startTimerInGUIThread()
{
    ARMARX_TRACE;
    robotUpdateTimerId =
        startTimer((DEFAULT_ROBOT_UPDATE_STEP > 0 ? DEFAULT_ROBOT_UPDATE_STEP : 1));
    ARMARX_CHECK_EXPRESSION(robotUpdateTimerId != 0);
}
 
void
WorkingMemoryController::convertObjectInstanceToGlobalPose(ObjectInstance& obj)
{
    ARMARX_TRACE;
    if (obj.getPosition()->frame != GlobalFrame && !obj.getPosition()->frame.empty())
    {
        if (obj.getPosition()->agent.empty())
        {
            //            ARMARX_WARNING << /*deactivateSpam(1) <<*/ "Could not convert pose of " << obj.getName()
            //                           << " to global pose because agent name is empty: current pose: " << obj.getPose()->output();
            return;
        }
        FramedPosePtr pose = obj.getPose(); // This FramedPose is a copy.
 
        RobotDefinition* agent = nullptr;
 
        for (auto& [name, agentDef] : currentAgents)
        {
            if (agentDef.robot->getName() == pose->agent)
            {
                agent = new RobotDefinition(agentDef); // copy def
            }
        }
        ARMARX_TRACE;
 
        if (!agent)
        {
            ARMARX_WARNING << "Agent " << pose->agent << " not found in current agents.";
            obj.setPose(agentInstancesProxy->convertToWorldPose(pose->agent, obj.getPose()));
        }
        else
        {
            IceUtil::Time localizationTimestamp = obj.getLocalizationTimestamp();
            //ARMARX_INFO << "Agent " << pose->agent << " found in current agents.";
            ARMARX_INFO << deactivateSpam(5, obj.getName() + obj.getId() + "_before") << "Pose of "
                        << obj.getName() << " before: \n"
                        << *pose;
 
            // make sure that the agent has the same timestamp than the localization of the object.
            bool sync_successful = true;
 
            if (localizationTimestamp != agent->timestamp)
            {
                sync_successful = RemoteRobot::synchronizeLocalCloneToTimestamp(
                    agent->timestampedRobot,
                    agent->robotStateComponent,
                    localizationTimestamp.toMicroSeconds());
                agent->timestamp = localizationTimestamp;
            }
 
            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();
 
            if (sync_successful)
            {
                pose->changeToGlobal(agent->timestampedRobot);
                obj.setPose(pose);
            }
            else
            {
                ARMARX_WARNING << "Agent " << pose->agent
                               << " could not successfully be synced. Try to calculate pose with "
                                  "current agent of agentSegment.";
                obj.setPose(agentInstancesProxy->convertToWorldPose(pose->agent, obj.getPose()));
            }
 
            ARMARX_INFO << deactivateSpam(5, obj.getName() + obj.getId() + "_after") << "Pose of "
                        << obj.getName() << " after: \n"
                        << *pose;
        }
 
        delete agent;
        agent = nullptr;
    }
}
 
void
WorkingMemoryController::updateObjectData(const ObjectInstancePtr obj)
{
    ARMARX_TRACE;
    if (!obj)
    {
        return;
    }
 
    std::unique_lock lock(mutexEntities);
 
    ARMARX_DEBUG << "Updating object entity " << obj->getName();
 
    // Convert to global
    try
    {
        if (obj->getPosition()->frame != obj->getOrientation()->frame)
        {
            ARMARX_ERROR << "Frame of pos and orientation are not the same of object "
                         << obj->getName() << " pos frame: " << obj->getPosition()->frame
                         << " ori frame: " << obj->getOrientation()->frame;
        }
 
        IceUtil::Time localizationTimestamp = obj->getLocalizationTimestamp();
        if (lastUpdated.find(obj->getName()) == lastUpdated.end() ||
            localizationTimestamp.toMicroSeconds() > lastUpdated.at(obj->getName()).first)
        {
            // check if localization timestamp is newer than latest update. Otherwise we already converted the object to global
            convertObjectInstanceToGlobalPose(*obj);
            lastUpdated[obj->getName()] =
                std::make_pair(localizationTimestamp.toMicroSeconds(), obj->getPose());
        }
        else
        {
            obj->setPose(lastUpdated.at(obj->getName()).second);
        }
 
        if (showObjInstanceUncertainties)
        {
            ARMARX_TRACE;
            auto uncertainty = obj->getPositionUncertainty();
 
            if (uncertainty)
            {
                float variance = std::min(5000.0f, uncertainty->getVarianceScalar());
                auto pos = obj->getPosition();
 
                if (pos->getFrame() == armarx::GlobalFrame)
                {
                    debugDrawer->setSphereVisu(
                        "ObjectUncertainties",
                        obj->getName() + obj->getId() + "_uncertainty",
                        pos,
                        armarx::DrawColor{std::min(1.0f, variance / 1000),
                                          std::max(0.0f, 1 - variance / 1000),
                                          0,
                                          0.2f},
                        variance);
                }
                else
                {
                    ARMARX_VERBOSE << deactivateSpam(5, obj->getName() + obj->getId())
                                   << "Not global position for " << obj->getName() + obj->getId();
                }
            }
            else
            {
                ARMARX_VERBOSE << deactivateSpam(5, obj->getName() + obj->getId())
                               << "No position uncertainty for " << obj->getName() + obj->getId();
            }
        }
        else
        {
            ARMARX_DEBUG << deactivateSpam(5, obj->getName() + obj->getId())
                         << "Position uncertainty is not shown for "
                         << obj->getName() + obj->getId();
        }
    }
    catch (...)
    {
        ARMARX_ERROR << "Execption during update object data...";
    }
}
 
bool
WorkingMemoryController::createEntity(memoryx::EntityBasePtr entity, const std::string& entityType)
{
    ARMARX_TRACE;
    const std::string id = entity->getId();
    if (entityType == settings_objectInstancesSegmentName ||
        entityType == settings_worldStateSegmentName)
    {
        bool found = false;
        {
            std::unique_lock lock(mutexEntities);
            EntityEntryMap::iterator itEntity = currentEntities.find(id);
            found = itEntity != currentEntities.end();
        }
 
        if (!found)
        {
            ARMARX_TRACE;
            ARMARX_INFO << "Creating entity instance, id:" << id << ", type " << entityType;
            ObjectDefinition o;
 
            EntityPtr e = EntityPtr::dynamicCast(entity);
            // check for entity type
            ObjectInstancePtr obj = ObjectInstancePtr::dynamicCast(e);
            if (!obj)
            {
                ARMARX_WARNING << "Could not cast entitiy to obj";
                return false;
            }
            convertObjectInstanceToGlobalPose(*obj);
 
            // this data could be outdated, if the manipualtion object was updated
            // -> we use the class reference by using the enrichment
            PriorAttributeEnrichmentFusion enrichment(priorKnowledgePrx, true);
            enrichment.initEntityInPlace(obj);
            o.simoxWrapper = obj->addWrapper(new EntityWrappers::SimoxObjectWrapper(fileManager));
 
 
            o.obj = obj;
            if (o.simoxWrapper)
            {
                o.manipObj = o.simoxWrapper->getManipulationObject();
            }
            std::unique_lock lock(mutexEntities);
            currentEntities[id] = o;
        }
        else
        {
            ARMARX_INFO << "Entity already created...";
        }
        return true;
    }
    else if (entityType == settings_agentInstancesSegmentName)
    {
        ARMARX_TRACE;
        bool found = false;
        {
            std::unique_lock lock(mutexEntities);
            AgentEntryMap::iterator itAgent = currentAgents.find(id);
            found = itAgent != currentAgents.end();
        }
        if (!found)
        {
            ARMARX_INFO << "Creating agent entity, id:" << id << " name: " << entity->getName()
                        << ", type " << entity->ice_id();
            AgentInstancePtr agent = AgentInstancePtr::dynamicCast(entity);
            if (!agent || !addAgent(agent))
            {
                ARMARX_WARNING << "Agent loading failed...";
                return false;
            }
        }
        else
        {
            ARMARX_INFO << "Entity already created...";
        }
        return true;
    }
    return false;
}
 
void
WorkingMemoryController::updateEntityVisu(memoryx::EntityBasePtr entity,
                                          QString entityType,
                                          bool isNew)
{
    ARMARX_TRACE;
    const std::string id = entity->getId();
    const std::string typeS = entityType.toStdString();
    ARMARX_DEBUG << "Updating visualization, id:" << id << ", type " << typeS;
 
    std::unique_lock lock1(mutexEntities);
    std::unique_lock lock2(*mutex3D);
 
 
    if (typeS == settings_objectInstancesSegmentName || typeS == settings_worldStateSegmentName)
    {
        ARMARX_TRACE;
        EntityEntryMap::iterator itEntity = currentEntities.find(id);
 
        if (itEntity == currentEntities.end())
        {
            ARMARX_INFO << "Could not find currentEntity entry for object with id = " << id
                        << ", building new entry";
            if (!createEntity(entity, typeS))
            {
                return;
            }
        }
        itEntity = currentEntities.find(id);
 
        //ARMARX_VERBOSE << "Updating visualization for object with id = " << id;
 
        if (isNew)
        {
            createObjectVisualization(itEntity->second);
        }
        else
        {
            ARMARX_TRACE;
            if (itEntity->second.simoxWrapper)
            {
                itEntity->second.simoxWrapper->updateFromEntity(entity);
 
                if (visuSetting_transparentExistanceCertatinty)
                {
                    float t = std::max(MIN_OBJECT_TRANSPARENCY,
                                       itEntity->second.obj->getExistenceCertainty());
                    ARMARX_DEBUG << "transparency for object "
                                 << itEntity->second.manipObj->getName() << ":"
                                 << itEntity->second.obj->getExistenceCertainty();
                    //itEntity->second.visualization->getCoinVisualization(); // ensure that visu is built, not neccessary any more since simox 2.3.22
                    //                    if (t < 0.9)
                    {
                        if (itEntity->second.cv)
                        {
                            itEntity->second.cv->setTransparency(t);
                        }
                    }
                }
            }
        }
    }
    else if (typeS == settings_agentInstancesSegmentName)
    {
        ARMARX_TRACE;
        AgentEntryMap::iterator itAgent = currentAgents.find(id);
 
        if (itAgent == currentAgents.end())
        {
            ARMARX_VERBOSE << "Could not find currentAgents entry for object with id = " << id
                           << ", loading agent data...";
            if (!createEntity(entity, typeS))
            {
                return;
            }
        }
 
        if (isNew)
        {
            createAgentVisualisation(id);
        }
 
        // not needed, is done in timer method...
        //        else
        //            updateAgentVisualisation(entity);
    }
 
    //else
    //ARMARX_WARNING << "updateEntityVisu: entity type not supported: " << typeS;
    ARMARX_DEBUG << "Updating visualization END, id:" << id << ", type " << typeS;
}
 
void
WorkingMemoryController::createObjectVisualization(ObjectDefinition& o)
{
    ARMARX_TRACE;
    if (!o.simoxWrapper || !o.manipObj)
    {
        return;
    }
 
    std::unique_lock lock(*mutex3D);
 
    setupEntityVisu(o);
 
    SoNode* s = nullptr;
    updateVisualization(o.simoxWrapper);
 
 
    o.cv = o.manipObj->getVisualization<VirtualRobot::CoinVisualization>();
 
    if (o.cv)
    {
        if (visuSetting_transparentExistanceCertatinty)
        {
            ARMARX_TRACE;
            //float t = (float)(rand() % 1000) / 1000.0f;
            float t = std::max(MIN_OBJECT_TRANSPARENCY, o.obj->getExistenceCertainty());
            ARMARX_DEBUG << "transparency for object " << o.manipObj->getName() << ":" << t;
            //            cv->getCoinVisualization(); // ensure that visu is built, not neccessary any more since simox 2.3.22
            o.cv->setTransparency(t);
        }
        s = o.cv->getCoinVisualization();
    }
 
    //SoNode* s = VirtualRobot::CoinVisualizationFactory::getCoinVisualization(o.manipObj, SceneObject::Full);
 
    if (!s)
    {
        CoinVisualizationFactoryPtr coinVisFactory(new CoinVisualizationFactory());
        auto name = o.simoxWrapper->getName();
        s = std::dynamic_pointer_cast<VirtualRobot::CoinVisualizationNode>(
                coinVisFactory->createCoordSystem(1.0, &name))
                ->getCoinVisualization();
    }
 
    if (s && objectsVisu)
    {
        o.visualization = s;
 
        if (objectsVisu->findChild(s) < 0)
        {
            objectsVisu->addChild(s);
        }
    }
}
 
void
WorkingMemoryController::updateVisualization(const EntityWrappers::SimoxObjectWrapperPtr objWrapper)
{
    ARMARX_TRACE;
    if (objWrapper && objWrapper->getManipulationObject())
    {
        ARMARX_DEBUG << "updateVisualization, object:"
                     << objWrapper->getManipulationObject()->getName();
    }
    else
    {
        ARMARX_WARNING << "updateVisualization, empty object";
    }
 
    if (!objWrapper)
    {
        return;
    }
 
    std::unique_lock lock(*mutex3D);
 
    /* Update position etc and rebuild visu. */
    objWrapper->refreshVisuPose();
}
 
void
WorkingMemoryController::updateEntityTree(memoryx::EntityBasePtr entity,
                                          QString entityType,
                                          bool isNew)
{
    ARMARX_TRACE;
    (void)isNew; // Unused.
 
    ARMARX_DEBUG << "updateEntityTree:" << entity->getName() << ",type:" << entityType;
 
    EntityPtr obj = EntityPtr::dynamicCast(entity);
    std::string entityTypeS = entityType.toStdString();
 
    if (!segmentTabs.count(entityTypeS))
    {
        ARMARX_INFO << "Segment " << entityTypeS << " not yet displayed";
        return;
    }
 
    QString objName(obj->getName().c_str());
    QString objId = QString::fromStdString(entity->getId());
 
    QList<QTreeWidgetItem*> oldItems;
    QTreeWidgetItem* objItem;
 
    QTreeWidget* tree = segmentTabs.at(entityTypeS).tree;
    oldItems = tree->findItems(objId, Qt::MatchFixedString, 1);
    objItem = (oldItems.size() == 0) ? new QTreeWidgetItem(tree, TreeItemType::eItemObject)
                                     : oldItems.at(0);
    objItem->setText(0, objName);
    objItem->setText(1, objId);
 
    if (!objItem)
    {
        ARMARX_WARNING << "null objItem?!";
        return;
    }
 
    // Clear object node
    QList<QTreeWidgetItem*> oldAttrs = objItem->takeChildren();
 
    while (!oldAttrs.empty())
    {
        delete oldAttrs.takeFirst();
    }
 
    QBrush evenRow(QColor(217, 255, 226));
    QBrush oddRow(Qt::white);
    int i = 0;
    memoryx::NameList attrNames = obj->getAttributeNames();
 
    for (const auto& attrName : attrNames)
    {
        //QList<QTreeWidgetItem *> oldAttrs = objItem->findItems(objId, Qt::MatchFixedString, 0);
        QTreeWidgetItem* attrItem = new QTreeWidgetItem(objItem, TreeItemType::eItemAttr);
        //QString attrName(it->c_str());
        attrItem->setText(0, QString::fromStdString(attrName));
 
        armarx::VariantPtr attrValue = obj->getAttributeValue(attrName);
        auto uncertainty = obj->getAttribute(attrName)->getUncertainty();
 
        if (attrValue)
        {
            QString attrValStr(attrValue->getOutputValueOnly().c_str());
 
            if (uncertainty)
            {
                attrValStr +=
                    QString("\nUncertainty: ") + QString::fromStdString(uncertainty->output());
            }
 
            attrItem->setText(1, attrValStr);
        }
        else
        {
            attrItem->setText(1, "Missing/unsupported value");
        }
 
 
        QBrush& bgBrush = (i % 2 == 0) ? evenRow : oddRow;
        attrItem->setBackground(0, bgBrush);
        attrItem->setBackground(1, bgBrush);
        ++i;
    }
    ARMARX_DEBUG << "updateEntityTree END:" << entity->getName() << ",type:" << entityType;
}
 
void
WorkingMemoryController::clearWorkingMemory()
{
    ARMARX_TRACE;
    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())
    {
        std::unique_lock lock(mutexEntities);
 
        try
        {
            memoryPrx->clear();
        }
        catch (...)
        {
            ARMARX_ERROR << "Execption during clear working memory...";
        }
    }
}
 
void
WorkingMemoryController::showVisuLayers(bool show)
{
    ARMARX_TRACE;
    if (debugDrawer)
    {
        if (show)
        {
            debugDrawer->enableAllLayers();
        }
        else
        {
            debugDrawer->disableAllLayers();
        }
    }
}
 
void
WorkingMemoryController::showAgents(bool show)
{
    ARMARX_TRACE;
    std::unique_lock lock(*mutex3D);
 
    if (!rootVisu || !agentsVisu)
    {
        return;
    }
 
    if (show)
    {
        if (rootVisu->findChild(agentsVisu) < 0)
        {
            rootVisu->addChild(agentsVisu);
        }
    }
    else
    {
        if (rootVisu->findChild(agentsVisu) >= 0)
        {
            rootVisu->removeChild(agentsVisu);
        }
    }
}
 
void
WorkingMemoryController::showPlane(bool show)
{
    ARMARX_TRACE;
    std::unique_lock lock(*mutex3D);
 
    if (!rootVisu || !planeVisu)
    {
        return;
    }
 
    if (show)
    {
        if (rootVisu->findChild(planeVisu) < 0)
        {
            rootVisu->addChild(planeVisu);
        }
    }
    else
    {
        if (rootVisu->findChild(planeVisu) >= 0)
        {
            rootVisu->removeChild(planeVisu);
        }
    }
}
 
void
WorkingMemoryController::clearEntitiesData()
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
    ARMARX_INFO << "Clearing entity data";
 
    /* Clear storage. */
    for (auto& entity : currentEntities)
    {
        deletedEntities[entity.first] = entity.second;
    }
 
    currentEntities.clear();
 
    for (auto& entity : currentAgents)
    {
        deletedAgents[entity.first] = entity.second;
    }
 
    currentAgents.clear();
}
 
void
WorkingMemoryController::clearEntitiesVisu(const std::string& segmentName)
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
    ARMARX_INFO << "Clearing entity visu";
 
    if (segmentName == "" || segmentName == settings_objectInstancesSegmentName)
    {
        ARMARX_TRACE;
        /* Clear storage. */
        for (auto& entity : deletedEntities)
        {
            entity.second.manipObj.reset();
 
            if (entity.second.simoxWrapper)
            {
                entity.second.simoxWrapper->setEntity(EntityBasePtr());
                entity.second.simoxWrapper = nullptr;
            }
        }
 
        deletedEntities.clear();
    }
 
    if (segmentName == "" || segmentName == settings_agentInstancesSegmentName)
    {
        ARMARX_TRACE;
        deletedAgents.clear();
 
        {
            std::unique_lock lock2(*mutex3D);
 
            if (agentsVisu)
            {
                agentsVisu->removeAllChildren();
            }
 
            if (objectsVisu)
            {
                objectsVisu->removeAllChildren();
            }
        }
    }
}
 
void
WorkingMemoryController::rebuildVisualization()
{
    ARMARX_TRACE;
    ARMARX_INFO << "Rebuilding complete visualization";
    std::unique_lock lock1(mutexEntities);
    std::unique_lock lock2(*mutex3D);
 
    if (objectsVisu)
    {
        ARMARX_INFO << "Rebuilding object visualization";
        objectsVisu->removeAllChildren();
 
        /* Entities. */
        ARMARX_INFO << "Rebuilding entity visualization for " << currentEntities.size()
                    << " elements";
 
        for (auto& entity : currentEntities)
        {
            ARMARX_VERBOSE << "Rebuilding entity visualization for: " << entity.first;
            createObjectVisualization(entity.second);
        }
    }
 
    if (agentsVisu)
    {
        ARMARX_INFO << "Rebuilding robot visualization for " << currentAgents.size() << " agents";
        agentsVisu->removeAllChildren();
 
        for (const auto& agent : currentAgents)
        {
            ARMARX_VERBOSE << "Rebuilding robot visualization for: " << agent.first;
            //if (agent.second.visualization)
            //    agent.second.visualization->unref();
            createAgentVisualisation(agent.first);
        }
    }
 
    armarx::ArmarXMainWindow* main = dynamic_cast<armarx::ArmarXMainWindow*>(getMainWindow());
 
    if (main && main->getViewerWidget())
    {
        auto l = main->getViewerWidget()->viewer->getScopedLock();
 
        if (ui.checkBoxViewAll->isChecked() && main->getViewerWidget())
        {
            main->getViewerWidget()->viewer->viewAll();
        }
    }
}
 
void
WorkingMemoryController::uncertaintyVisuToggled(bool isChecked)
{
    ARMARX_TRACE;
    /* Avoid double refresh. */
    if (isChecked)
    {
        setupVisualization();
    }
}
 
void
WorkingMemoryController::existenceCertaintyToggled(bool isChecked)
{
    ARMARX_TRACE;
    visuSetting_transparentExistanceCertatinty = isChecked;
    setupVisualization();
}
 
void
WorkingMemoryController::setupVisualization()
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
 
    for (const auto& entity : currentEntities)
    {
        setupEntityVisu(entity.second);
    }
 
    showObjInstanceUncertainties = ui.cbShowPositionUncertainty->isChecked();
    visuSetting_showObjectModels = ui.cbShowObjectModels->isChecked();
 
    if (!showObjInstanceUncertainties)
    {
        debugDrawer->clearLayer("ObjectUncertainties");
    }
}
 
void
WorkingMemoryController::setupEntityVisu(const ObjectDefinition& objWrapper)
{
    ARMARX_TRACE;
    if (!objWrapper.simoxWrapper)
    {
        return;
    }
    if (objWrapper.manipObj)
    {
        ARMARX_DEBUG << "setupEntityVisu:" << objWrapper.manipObj->getName();
    }
    else
    {
        ARMARX_DEBUG << "setupEntityVisu: no manipObj";
    }
 
    std::unique_lock lock(*mutex3D);
    ObjectInstancePtr obj = ObjectInstancePtr::dynamicCast(objWrapper.simoxWrapper->getEntity());
 
    if (!obj)
    {
        ARMARX_VERBOSE << "Only ObjectInstances are currently supported";
        return;
    }
 
    bool showMainVisu =
        ui.cbShowObjectModels->isChecked() || !obj->getPositionAttribute()->getUncertainty();
    bool showUncertaintyVisu = ui.cbShowPositionUncertainty->isChecked();
 
    if (showUncertaintyVisu)
    {
        ARMARX_TRACE;
        EntityWrappers::SimoxObjectWrapper::UncertaintyVisuType visuType =
            EntityWrappers::SimoxObjectWrapper::eEllipse;
 
        if (ui.rbEllipse->isChecked())
        {
            visuType = EntityWrappers::SimoxObjectWrapper::eEllipse;
        }
        else if (ui.rbHeatMap->isChecked())
        {
            visuType = EntityWrappers::SimoxObjectWrapper::eHeatMap;
        }
        else if (ui.rbHeatSurface->isChecked())
        {
            visuType = EntityWrappers::SimoxObjectWrapper::eHeatSurface;
        }
        objWrapper.simoxWrapper->setUncertaintyVisuType(visuType);
        objWrapper.simoxWrapper->setUncertaintyVisuParams(ui.spinEllipseRadius->value(),
                                                          ui.spinMinVariance->value(),
                                                          expf(-ui.spinDisplayThreshold->value()),
                                                          ui.spinGridSize->value());
        objWrapper.simoxWrapper->refreshVisuPose();
    }
 
    if (objWrapper.manipObj)
    {
        objWrapper.manipObj->setupVisualization(showMainVisu, showUncertaintyVisu);
    }
}
 
void
WorkingMemoryController::reportEntityCreated(const std::string& segmentName,
                                             const EntityBasePtr& entityBase,
                                             const Ice::Current&)
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
 
    try
    {
        ARMARX_VERBOSE << "Entity created. Segment:" << segmentName
                       << ", entity:" << entityBase->getName();
 
        EntityPtr entity = EntityPtr::dynamicCast(entityBase);
 
        //QString entityId = entity->getId().c_str();
        QString entityType(segmentName.c_str());
 
        // check for entity type
        ObjectInstancePtr obj = ObjectInstancePtr::dynamicCast(entity);
 
        if (obj)
        {
            updateObjectData(obj);
        }
        else
        {
            AgentInstancePtr agent = AgentInstancePtr::dynamicCast(entity);
 
            if (agent)
            {
                addAgent(agent);
            }
            else
            {
                ARMARX_VERBOSE << "reportEntityCreated: entity type not supported: " << segmentName;
            }
        }
 
        emit entityChanged(entityBase, entityType, true);
    }
    catch (...)
    {
        ARMARX_ERROR << "Execption during reportEntityCreated...";
    }
}
 
void
WorkingMemoryController::reportEntityUpdated(const std::string& segmentName,
                                             const EntityBasePtr& entityBaseOld,
                                             const EntityBasePtr& entityBaseNew,
                                             const Ice::Current&)
{
    ARMARX_TRACE;
    (void)entityBaseOld; // Unused.
 
    ARMARX_DEBUG << "entity " << entityBaseNew->getName() << " got updated";
    std::unique_lock lock(mutexEntities);
 
    try
    {
        ARMARX_TRACE;
 
        EntityPtr entity = EntityPtr::dynamicCast(entityBaseNew);
 
        QString entityId(entity->getId().c_str());
        QString entityType(segmentName.c_str());
 
        // check for entity type
        ObjectInstancePtr obj = ObjectInstancePtr::dynamicCast(entity);
 
        if (obj)
        {
            updateObjectData(obj);
        }
        else
        {
            AgentInstancePtr agent = AgentInstancePtr::dynamicCast(entity);
 
            if (agent)
            {
                // check if we need to add a new agent
                auto it = currentAgents.find(entityId.toStdString());
 
                if (it == currentAgents.end())
                {
                    addAgent(agent);
                }
            }
        }
 
        emit entityChanged(entityBaseNew, entityType, false);
    }
    catch (...)
    {
        ARMARX_ERROR << "Exception during reportEntityUpdated...";
    }
    ARMARX_DEBUG << "entity " << entityBaseNew->getName() << " got updated END";
}
 
void
WorkingMemoryController::reportEntityRemoved(const std::string& segmentName,
                                             const EntityBasePtr& entity,
                                             const Ice::Current&)
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
 
    try
    {
        std::string entityId = entity->getId();
        ARMARX_DEBUG << "Entity removed: id = " << entityId;
 
        // check for entity type
        ObjectInstancePtr obj = ObjectInstancePtr::dynamicCast(entity);
 
        if (obj)
        {
            ARMARX_TRACE;
            auto it = currentEntities.find(entityId);
 
            if (it != currentEntities.end())
            {
                ARMARX_INFO << "Object removed: id = " << entityId;
                deletedEntities[entityId] = currentEntities[entityId];
                currentEntities.erase(it);
                QMetaObject::invokeMethod(this, "objectsRemoved");
            }
            else
            {
                ARMARX_INFO << "Object not present, id = " << entityId;
            }
        }
        else
        {
            ARMARX_TRACE;
            AgentInstancePtr agent = AgentInstancePtr::dynamicCast(entity);
 
            if (agent)
            {
                // check if we need to add a new agent
                auto it = currentAgents.find(entityId);
 
                if (it != currentAgents.end())
                {
                    ARMARX_INFO << "Object removed (agent): id = " << entityId;
                    deletedAgents[entityId] = currentAgents[entityId];
                    currentAgents.erase(it);
                    QMetaObject::invokeMethod(this, "agentsRemoved");
                }
                else
                {
                    ARMARX_INFO << "agent not present, id = " << entityId;
                }
            }
            else
            {
                QMetaObject::invokeMethod(this,
                                          "removeEntityInTree",
                                          Q_ARG(std::string, entityId),
                                          Q_ARG(std::string, segmentName));
            }
        }
    }
    catch (...)
    {
        ARMARX_ERROR << "Execption during reportEntityRemoved...";
    }
    ARMARX_DEBUG << "Entity removed END";
}
 
void
WorkingMemoryController::objectsRemoved()
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
    std::unique_lock lock2(*mutex3D);
 
    for (auto& e : deletedEntities)
    {
        if (objectsVisu && e.second.visualization &&
            objectsVisu->findChild(e.second.visualization) >= 0)
        {
            objectsVisu->removeChild(e.second.visualization);
        }
 
        removeEntityInTree(e.first, settings_objectInstancesSegmentName);
    }
 
    deletedEntities.clear();
}
 
void
WorkingMemoryController::agentsRemoved()
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
    std::unique_lock lock2(*mutex3D);
    ARMARX_INFO << "Agents removed...";
 
    for (auto& e : deletedAgents)
    {
        ARMARX_INFO << "Agent removed: " << e.first;
        if (agentsVisu && e.second.visualization &&
            agentsVisu->findChild(e.second.visualization) >= 0)
        {
            agentsVisu->removeChild(e.second.visualization);
        }
 
        removeEntityInTree(e.first, settings_agentInstancesSegmentName);
    }
 
    deletedAgents.clear();
}
 
void
WorkingMemoryController::reportSnapshotLoaded(const std::string& segmentName, const Ice::Current&)
{
    ARMARX_TRACE;
    (void)segmentName; // Unused.
}
 
void
WorkingMemoryController::reportSnapshotCompletelyLoaded(const Ice::Current&)
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
 
    {
        emit signalRefetchEntities();
    }
}
 
void
WorkingMemoryController::reportMemoryCleared(const std::string& segmentName, const Ice::Current&)
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
 
 
    ARMARX_INFO << "segment " << segmentName << " cleared";
 
    try
    {
        if (segmentName == settings_objectInstancesSegmentName)
        {
            /* Clear storage. */
            for (auto& entity : currentEntities)
            {
                deletedEntities[entity.first] = entity.second;
            }
 
            currentEntities.clear();
        }
 
        if (segmentName == settings_agentInstancesSegmentName)
        {
            for (auto& entity : currentAgents)
            {
                deletedAgents[entity.first] = entity.second;
            }
 
            currentAgents.clear();
        }
 
 
        //clearEntitiesData();
    }
    catch (...)
    {
        ARMARX_ERROR << "Execption during reportMemoryCleared...";
    }
 
    emit signalClearEntities(segmentName);
}
 
void
WorkingMemoryController::setMutex3D(RecursiveMutexPtr const& mutex3D)
{
    ARMARX_TRACE;
    this->mutex3D = mutex3D;
 
    if (debugDrawer)
    {
        debugDrawer->setMutex(mutex3D);
    }
}
 
void
WorkingMemoryController::removeEntityInTree(const std::string entityId,
                                            const std::string entityType)
{
    ARMARX_INFO << "Remove qt element: " << entityId << ", " << entityType;
    QString entityItemId(entityId.c_str());
    QTreeWidgetItem* entityItem = nullptr;
 
    if (segmentTabs.count(entityType))
    {
        QList<QTreeWidgetItem*> items =
            segmentTabs.at(entityType).tree->findItems(entityItemId, Qt::MatchFixedString, 1);
        if (items.size() > 0)
        {
            entityItem = items.at(0);
        }
    }
    else
    {
        ARMARX_WARNING << "Undefined entity type: " << entityType << std::endl;
        return;
    }
 
    if (!entityItem)
    {
        ARMARX_WARNING << "Could not determine Qt tree item for entity " << entityId << ", type "
                       << entityType;
        return;
    }
 
    QList<QTreeWidgetItem*> entityAttributes = entityItem->takeChildren();
 
    while (!entityAttributes.empty())
    {
        delete entityAttributes.takeFirst();
    }
 
    delete entityItem;
    ARMARX_INFO << "Remove qt element done";
}
 
int
WorkingMemoryController::addTab(std::string segmentName)
{
    ARMARX_TRACE;
    int index;
 
    if (!segmentTabs.count(segmentName))
    {
 
        //create tab
        QWidget* tab = new QWidget();
        QGridLayout* layout = new QGridLayout();
        tab->setLayout(layout);
 
        //create tree
        QTreeWidget* tree = new QTreeWidget();
        tree->setColumnCount(2);
        QStringList header;
        header << "Name"
               << "ID/Value";
        tree->setHeaderLabels(header);
        tree->header()->setDefaultSectionSize(200);
        tree->setSortingEnabled(true);
        tree->sortByColumn(0, Qt::SortOrder::AscendingOrder);
        index = ui.tabWidget->addTab(tab, QString::fromStdString(segmentName));
 
        //add tree to tab
        layout->addWidget(tree);
 
        struct SegmentTab et = {index, tree, tab};
        segmentTabs.insert(std::pair<std::string, struct SegmentTab>(segmentName, et));
 
        connect(tree,
                SIGNAL(itemDoubleClicked(QTreeWidgetItem*, int)),
                this,
                SLOT(treeItemDoubleClicked(QTreeWidgetItem*, int)));
    }
    else
    {
        index = segmentTabs.at(segmentName).tabIndex;
    }
 
    return index;
}
 
void
WorkingMemoryController::removeTab(std::string segmentName)
{
    ARMARX_TRACE;
    if (segmentTabs.count(segmentName))
    {
        ARMARX_INFO << "Removing tab " << segmentName;
        struct SegmentTab tab = segmentTabs.at(segmentName);
        QObject::disconnect(tab.tree,
                            SIGNAL(itemDoubleClicked(QTreeWidgetItem*, int)),
                            this,
                            SLOT(treeItemDoubleClicked(QTreeWidgetItem*, int)));
 
        ui.tabWidget->removeTab(tab.tabIndex);
        tab.tree->~QTreeWidget();
        tab.tab->~QWidget();
        segmentTabs.erase(segmentName);
    }
}
 
void
WorkingMemoryController::removeTabs()
{
    ARMARX_TRACE;
    ARMARX_INFO << "Removing tabs";
    for (auto& entry : segmentTabs)
    {
        removeTab(entry.first);
    }
}
 
QPointer<QWidget>
WorkingMemoryController::getCustomTitlebarWidget(QWidget* parent)
{
    ARMARX_TRACE;
    if (customToolbar)
    {
        if (parent != customToolbar->parent())
        {
            customToolbar->setParent(parent);
        }
 
        return customToolbar;
    }
 
    customToolbar = new QToolBar(parent);
    customToolbar->setIconSize(QSize(16, 16));
    //    customToolbar->addAction(QIcon(":/icons/configure-3.png"), "Configure",this, SLOT(OpenConfigureDialog()));
    customToolbar->addAction(QIcon(":/icons/view-refresh-7.png"),
                             "Refetch Data",
                             this,
                             SLOT(refetchEntitiesFromMemory()));
    customToolbar->addAction(QIcon(":/icons/dialog-close.ico"),
                             "Clear Working Memory",
                             this,
                             SLOT(clearWorkingMemory()));
 
    return customToolbar;
}
 
void
WorkingMemoryController::createAgentVisualisation(const std::string entityId)
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
    if (currentAgents.find(entityId) == currentAgents.end() || !currentAgents[entityId].robot)
    {
        return;
    }
    currentAgents[entityId].visualization =
        VirtualRobot::CoinVisualizationFactory::getCoinVisualization(
            currentAgents[entityId].robot, VirtualRobot::SceneObject::Full, false);
 
    if (!currentAgents[entityId].visualization)
    {
        ARMARX_WARNING << "createAgentVisualisation: could not retrieve visualisation from robot "
                       << currentAgents[entityId].robot->getName() << "; using ellipse";
        currentAgents[entityId].visualization =
            VirtualRobot::CoinVisualizationFactory::CreateEllipse(0.5, 0.5, 1, nullptr, false);
    }
 
    //agents[entityId].visualization->ref();
    if (agentsVisu && agentsVisu->findChild(currentAgents[entityId].visualization) < 0)
    {
        agentsVisu->addChild(currentAgents[entityId].visualization);
    }
 
    updateAgentVisualisation(entityId);
}
 
bool
WorkingMemoryController::addAgent(const AgentInstancePtr agent)
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
 
    try
    {
        if (!agent)
        {
            ARMARX_ERROR << "addAgent: entity is not of type AgentInstance";
            return false;
        }
 
        const std::string agentId = agent->getId();
 
        Ice::CommunicatorPtr iceCommunicator = getIceManager()->getCommunicator();
        if (!agent->hasAttribute("agentFilePath"))
        {
            ARMARX_INFO << "No filepath available for agent " << agent->getName();
            return false;
        }
        std::string agentFilePath = agent->getAgentFilePath();
        ARMARX_INFO << "Adding agent " << agent->getName() << " agentId:" << agentId
                    << ", filepath: " << agentFilePath;
        armarx::SharedRobotInterfacePrx sharedRobotInterfaceProxy = agent->getSharedRobot();
        if (sharedRobotInterfaceProxy)
        {
            ARMARX_INFO << agent->getName()
                        << " proxy: " << sharedRobotInterfaceProxy->ice_toString();
        }
        else
        {
            ARMARX_INFO << agent->getName() << " proxy nullptr";
        }
        try
        {
            if (!sharedRobotInterfaceProxy &&
                !agent->getStringifiedSharedRobotInterfaceProxy().empty())
            {
                sharedRobotInterfaceProxy =
                    armarx::SharedRobotInterfacePrx::checkedCast(iceCommunicator->stringToProxy(
                        agent->getStringifiedSharedRobotInterfaceProxy()));
            }
        }
        catch (std::exception& e)
        {
            ARMARX_ERROR << "Failed to get robot state proxy for agent " << agent->getName() << ": "
                         << e.what();
            return false;
        }
        if (sharedRobotInterfaceProxy)
        {
            ARMARX_TRACE;
            if (sharedRobotInterfaceProxy->getRobotStateComponent())
            {
                auto packages =
                    sharedRobotInterfaceProxy->getRobotStateComponent()->getArmarXPackages();
                ARMARX_INFO << VAROUT(packages);
                for (auto& p : packages)
                {
                    armarx::CMakePackageFinder finder(p);
                    if (finder.packageFound())
                    {
                        armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
                    }
                }
            }
            else
            {
                ARMARX_INFO << "RobotStateComponent proxy is empty";
            }
        }
        if (!armarx::ArmarXDataPath::getAbsolutePath(agentFilePath, agentFilePath))
        {
            ARMARX_TRACE;
            ARMARX_ERROR << "Could not find robot file " << agentFilePath;
 
            ARMARX_VERBOSE << "Searched in " << armarx::ArmarXDataPath::getDataPaths().size()
                           << " additional paths";
 
            for (const auto& path : armarx::ArmarXDataPath::getDataPaths())
            {
                ARMARX_VERBOSE << "\t" << path;
            }
 
            return false;
        }
 
        RobotDefinition robotDefinition;
        try
        {
            ARMARX_TRACE;
            robotDefinition.robot = RemoteRobot::createLocalClone(
                sharedRobotInterfaceProxy, agentFilePath, sharedRobotInterfaceProxy->getScaling());
            robotDefinition.robot->setName(sharedRobotInterfaceProxy->getName());
 
            // TODO: We only need the structure here.
            robotDefinition.timestampedRobot = robotDefinition.robot->clone();
        }
        catch (...)
        {
            robotDefinition.robot.reset();
            ARMARX_ERROR << "addAgent: error while loading robot from file: "
                         << GetHandledExceptionString();
            return false;
        }
 
        ARMARX_VERBOSE << "addAgent: loaded robot from file " << agentFilePath;
 
        robotDefinition.robotStateComponent = sharedRobotInterfaceProxy->getRobotStateComponent();
 
        ARMARX_INFO << "addAgent: created remote robot";
 
        currentAgents[agentId] = robotDefinition;
    }
    catch (...)
    {
        ARMARX_ERROR << "Exception during addAgent:\n" << GetHandledExceptionString();
        return false;
    }
    return true;
}
 
void
WorkingMemoryController::updateAgentVisualisation(const std::string entityId)
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
    auto entity = agentInstancesProxy->getEntityById(entityId);
    updateAgentVisualisation(entity);
}
 
void
WorkingMemoryController::updateAgentVisualisation(const EntityBasePtr entity)
{
    ARMARX_TRACE;
    std::unique_lock lock(*mutex3D);
 
    AgentInstancePtr agent = AgentInstancePtr::dynamicCast(entity);
 
    if (!agent || currentAgents.find(entity->getId()) == currentAgents.end())
    {
        return;
    }
 
    auto entityId = entity->getId();
    Eigen::Matrix4f matrix = Eigen::Matrix4f::Identity();
 
    armarx::FramedPositionPtr agentPosition = agent->getPosition();
 
    if (agentPosition)
    {
        matrix.block(0, 3, 3, 1) = agentPosition->toEigen();
    }
 
    armarx::FramedOrientationPtr agentOrientation = agent->getOrientation();
 
    if (agentOrientation)
    {
        matrix.block(0, 0, 3, 3) = agentOrientation->toEigen();
    }
 
    auto& agentInstance = currentAgents[entityId];
    agentInstance.robot->setGlobalPose(matrix);
}
 
void
WorkingMemoryController::updateAgent(const WorkingMemoryController::RobotDefinition& agent)
{
    ARMARX_TRACE;
    std::unique_lock lock(mutexEntities);
    std::unique_lock lock2(*mutex3D);
 
    if (!agent.robot || !agent.visualization)
    {
        return;
    }
 
    std::map<std::string, float> jointValues;
    std::vector<RobotNodePtr> storeNodesRemote;
 
    try
    {
        armarx::RemoteRobot::synchronizeLocalClone(agent.robot, agent.robotStateComponent);
    }
    catch (...)
    {
        ARMARX_ERROR << deactivateSpam(5) << "Exception during RemoteRobot::synchronizeLocalClone: "
                     << armarx::GetHandledExceptionString();
    }
}