ObjectPoseGuiWidgetController.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    RobotAPI::gui-plugins::ObjectPoseGuiWidgetController
 * \author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * \date       2020
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ObjectPoseGuiWidgetController.h"
 
#include <string>
 
#include <QMenu>
#include <QTimer>
 
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
 
namespace armarx
{
 
    static const int OBJECTS_COLUMN_ATTACHMENT = 6;
 
    ObjectPoseGuiWidgetController::ObjectPoseGuiWidgetController()
    {
        widget.setupUi(getWidget());
 
        widget.objectsTree->clear();
        widget.objectsTree->sortItems(0, Qt::SortOrder::AscendingOrder);
        widget.objectsTree->setContextMenuPolicy(Qt::CustomContextMenu);
 
        widget.requestTree->clear();
        widget.requestTree->sortItems(0, Qt::SortOrder::AscendingOrder);
 
 
        using This = ObjectPoseGuiWidgetController;
 
        connect(widget.updateButton, &QPushButton::pressed, this, &This::updateTab);
        connect(widget.tabWidget, &QTabWidget::currentChanged, this, &This::updateTab);
        connect(widget.detchAllButton,
                &QPushButton::pressed,
                [this]() {
                    this->detachAllObjectsFromRobotNodes(
                        widget.detchAllCommitAttachedCheckBox->isChecked());
                });
 
        connect(widget.objectsTree,
                &QTreeWidget::customContextMenuRequested,
                this,
                &This::prepareObjectContextMenu);
 
        connect(widget.requestButton, &QPushButton::pressed, this, &This::requestSelectedObjects);
 
        QTimer* timer = new QTimer(this);
        timer->setInterval(500);
        connect(timer, &QTimer::timeout, this, &This::updateTab);
        connect(widget.autoUpdateCheckBox,
                &QCheckBox::toggled,
                this,
                [timer](bool checked)
                {
                    if (checked)
                    {
                        timer->start();
                    }
                    else
                    {
                        timer->stop();
                    }
                });
    }
 
    ObjectPoseGuiWidgetController::~ObjectPoseGuiWidgetController()
    {
    }
 
    void
    ObjectPoseGuiWidgetController::loadSettings(QSettings* settings)
    {
        (void)settings;
    }
 
    void
    ObjectPoseGuiWidgetController::saveSettings(QSettings* settings)
    {
        (void)settings;
    }
 
    QString
    ObjectPoseGuiWidgetController::GetWidgetName()
    {
        return "ArMem.ObjectPoseGui";
    }
 
    static const std::string CONFIG_KEY_OBJECT_POSE_STORAGE = "ObjectPoseStorage";
 
    QPointer<QDialog>
    ObjectPoseGuiWidgetController::getConfigDialog(QWidget* parent)
    {
        if (!configDialog)
        {
            configDialog = new SimpleConfigDialog(parent);
            configDialog->addProxyFinder<armarx::objpose::ObjectPoseStorageInterfacePrx>(
                {CONFIG_KEY_OBJECT_POSE_STORAGE, "Object pose storage", "Object*"});
        }
        return qobject_cast<QDialog*>(configDialog);
    }
 
    void
    ObjectPoseGuiWidgetController::configured()
    {
        if (configDialog)
        {
            objectPoseStorageName = configDialog->getProxyName(CONFIG_KEY_OBJECT_POSE_STORAGE);
            if (objectPoseStorageName.empty())
            {
                objectPoseStorageName = "ObjectMemory";
            }
        }
    }
 
    void
    ObjectPoseGuiWidgetController::onInitComponent()
    {
        if (!objectPoseStorageName.empty())
        {
            usingProxy(objectPoseStorageName);
        }
    }
 
    void
    ObjectPoseGuiWidgetController::onConnectComponent()
    {
        if (!objectPoseStorageName.empty())
        {
            getProxy(ObjectPoseStorage, objectPoseStorageName);
        }
 
        this->attachableFrames = ObjectPoseStorage->getAttachableFrames();
        std::sort(attachableFrames.begin(),
                  attachableFrames.end(),
                  [](const auto& lhs, const auto& rhs) { return lhs.agent < rhs.agent; });
        for (objpose::AgentFrames& frames : attachableFrames)
        {
            std::sort(frames.frames.begin(), frames.frames.end());
        }
 
        // Update once.
        updateTab();
    }
 
    void
    ObjectPoseGuiWidgetController::onDisconnectComponent()
    {
        ObjectPoseStorage = nullptr;
    }
 
    void
    ObjectPoseGuiWidgetController::updateTab()
    {
        if (widget.tabWidget->currentWidget() == widget.tabObjects)
        {
            updateObjectsTab();
        }
        else if (widget.tabWidget->currentWidget() == widget.tabRequest)
        {
            updateRequestTab();
        }
    }
 
    void
    ObjectPoseGuiWidgetController::updateObjectsTab()
    {
        if (!ObjectPoseStorage)
        {
            // Probably disconnected.
            ARMARX_VERBOSE << "No object pose storage.";
            return;
        }
 
        IceUtil::Time start = IceUtil::Time::now();
        ARMARX_VERBOSE << "Getting object poses...";
        const objpose::data::ObjectPoseSeq objectPosesIce = ObjectPoseStorage->getObjectPoses();
        ARMARX_VERBOSE << "Got " << objectPosesIce.size() << " object poses. "
                       << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble()
                       << " ms.)";
 
        const objpose::ObjectPoseSeq objectPoses = objpose::fromIce(objectPosesIce);
 
        std::map<std::string, objpose::ObjectPoseSeq> objectPosesByProvider;
        for (const auto& pose : objectPoses)
        {
            objectPosesByProvider[pose.providerName].push_back(pose);
        }
 
        start = IceUtil::Time::now();
 
        QTreeWidget* tree = widget.objectsTree;
 
        MapTreeWidgetBuilder builder(objectPosesByProvider);
        builder.setMakeItemFn(
            [](const std::string& provider, const objpose::ObjectPoseSeq&)
            {
                QTreeWidgetItem* item = new QTreeWidgetItem({QString::fromStdString(provider)});
                return item;
            });
        builder.setUpdateItemFn(
            [](const std::string&, const objpose::ObjectPoseSeq& objectPoses, QTreeWidgetItem* item)
            {
                bool expand = item->childCount() == 0;
 
                TreeWidgetBuilder<objpose::ObjectPose> builder;
                builder.setNameFn([](const objpose::ObjectPose& pose)
                                  { return pose.objectID.str(); });
                builder.setMakeItemFn(
                    [](const objpose::ObjectPose&)
                    {
                        QTreeWidgetItem* item = new QTreeWidgetItem(QStringList{});
                        return item;
                    });
                builder.setUpdateItemFn(
                    [](const objpose::ObjectPose& pose, QTreeWidgetItem* item)
                    {
                        int col = 0;
                        item->setText(col++, QString::fromStdString(pose.objectID.str()));
                        item->setText(col++, QString::fromStdString(pose.providerName));
                        item->setText(col++,
                                      QString::fromStdString(
                                          objpose::ObjectTypeNames.to_name(pose.objectType)));
 
                        {
                            std::stringstream ss;
                            if (pose.localOOBB)
                            {
                                static const Eigen::IOFormat iof(5, 0, "", " x ", "", "", "", "");
                                ss << pose.localOOBB->dimensions().format(iof);
                            }
                            else
                            {
                                ss << "None";
                            }
                            item->setText(col++, QString::fromStdString(ss.str()));
                        }
                        item->setText(col++, QString::number(double(pose.confidence), 'g', 2));
                        item->setText(col++,
                                      QString::fromStdString(pose.timestamp.toDateTimeString()));
 
                        {
                            std::stringstream ss;
                            if (pose.attachment)
                            {
                                ss << pose.attachment->frameName << " ("
                                   << pose.attachment->agentName << ")";
                            }
                            item->setText(col++, QString::fromStdString(ss.str()));
                        }
 
                        return true;
                    });
                builder.updateTreeWithContainer(item, objectPoses);
 
                if (expand)
                {
                    item->setExpanded(true);
                }
 
                return true;
            });
        builder.updateTree(tree, objectPosesByProvider);
 
        ARMARX_VERBOSE << "Gui update took "
                       << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
    }
 
    void
    ObjectPoseGuiWidgetController::updateRequestTab()
    {
        if (!ObjectPoseStorage)
        {
            // Probably disconnected.
            ARMARX_VERBOSE << "No object pose storage.";
            return;
        }
 
        IceUtil::Time start = IceUtil::Time::now();
        objpose::ProviderInfoMap availableProvidersInfo =
            ObjectPoseStorage->getAvailableProvidersInfo();
        ARMARX_VERBOSE << "Got infos of " << availableProvidersInfo.size()
                       << " object pose providers. "
                       << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble()
                       << " ms.)";
 
 
        // Restructure data.
        std::map<std::string, std::set<std::pair<std::string, std::string>>> data;
        for (const auto& [providerName, info] : availableProvidersInfo)
        {
            for (const auto& id : info.supportedObjects)
            {
                data[id.dataset].insert(std::make_pair(id.className, providerName));
            }
        }
 
        start = IceUtil::Time::now();
 
        QTreeWidget* tree = widget.requestTree;
 
        MapTreeWidgetBuilder builder(data);
        builder.setMakeItemFn(
            [](const std::string& dataset, const auto&)
            {
                QTreeWidgetItem* item = new QTreeWidgetItem({QString::fromStdString(dataset)});
                return item;
            });
        builder.setUpdateItemFn(
            [tree](
                const std::string& dataset, const auto& datasetData, QTreeWidgetItem* datasetItem)
            {
                (void)dataset;
 
                TreeWidgetBuilder<std::pair<std::string, std::string>> builder;
                builder.setCompareFn(
                    [](const std::pair<std::string, std::string>& lhs, QTreeWidgetItem* item)
                    {
                        auto rhs = std::make_pair(item->text(0).toStdString(),
                                                  item->text(1).toStdString());
                        if (lhs < rhs)
                        {
                            return -1;
                        }
                        return lhs == rhs ? 0 : 1;
                    });
                builder.setMakeItemFn(
                    [](const std::pair<std::string, std::string>& element)
                    {
                        QTreeWidgetItem* item =
                            new QTreeWidgetItem({QString::fromStdString(element.first),
                                                 QString::fromStdString(element.second)});
                        return item;
                    });
                builder.setUpdateItemFn(
                    [tree](const std::pair<std::string, std::string>& element,
                           QTreeWidgetItem* item)
                    {
                        (void)element;
                        if (!tree->itemWidget(item, 2))
                        {
                            QCheckBox* requestCheckBox = new QCheckBox();
                            tree->setItemWidget(item, 2, requestCheckBox);
                        }
                        return true;
                    });
                builder.updateTreeWithContainer(datasetItem, datasetData);
 
                return true;
            });
        builder.updateTree(tree, data);
 
        ARMARX_VERBOSE << "Gui update took "
                       << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
    }
 
    void
    ObjectPoseGuiWidgetController::prepareObjectContextMenu(const QPoint& pos)
    {
        QTreeWidget* tree = widget.objectsTree;
        QTreeWidgetItem* item = tree->itemAt(pos);
 
        if (item == nullptr || item->parent() == nullptr)
        {
            // No item or top level item => no context menu.
            return;
        }
 
        QString providerName = item->parent()->text(0);
        QString objectID = item->text(0);
 
        QMenu* attachMenu = new QMenu("Attach to robot node", tree);
        for (const objpose::AgentFrames& agentFrames : attachableFrames)
        {
            QMenu* agentMenu = new QMenu(QString::fromStdString(agentFrames.agent), tree);
 
            for (const std::string& frame : agentFrames.frames)
            {
                QAction* attachAgentAction = new QAction(QString::fromStdString(frame), tree);
                // attachAgentAction->setStatusTip(tr("Attach object rigidly to a robot node"));
                connect(attachAgentAction,
                        &QAction::triggered,
                        [=, this]() {
                            this->attachObjectToRobotNode(
                                providerName, objectID, agentFrames.agent, frame);
                        });
                agentMenu->addAction(attachAgentAction);
            }
            attachMenu->addMenu(agentMenu);
        }
 
        QAction* detachAction = new QAction(tr("Detach from to robot node"), tree);
        detachAction->setEnabled(!item->text(OBJECTS_COLUMN_ATTACHMENT).isEmpty());
        connect(detachAction,
                &QAction::triggered,
                [=, this]() { this->detachObjectFromRobotNode(providerName, objectID); });
 
        QMenu menu(tree);
        menu.addMenu(attachMenu);
        menu.addAction(detachAction);
 
        menu.exec(tree->mapToGlobal(pos));
    }
 
    void
    ObjectPoseGuiWidgetController::attachObjectToRobotNode(QString providerName,
                                                           QString objectID,
                                                           const std::string& agentName,
                                                           const std::string& frameName)
    {
        ARMARX_VERBOSE << "Attaching " << objectID << " by '" << providerName << "' to robot node '"
                       << frameName << "' of agent '" << agentName << "'.";
 
        objpose::AttachObjectToRobotNodeInput input;
        input.providerName = providerName.toStdString();
        input.objectID = armarx::toIce(armarx::ObjectID(objectID.toStdString()));
        input.agentName = agentName;
        input.frameName = frameName;
 
        try
        {
            objpose::AttachObjectToRobotNodeOutput output =
                ObjectPoseStorage->attachObjectToRobotNode(input);
            ARMARX_VERBOSE << "Success of attaching: " << output.success;
        }
        catch (const IceUtil::Exception& e)
        {
            ARMARX_WARNING << "Failed to attach object '" << input.objectID << "' to robot node '"
                           << input.frameName << "' of agent '" << input.agentName << "'."
                           << "\nReason: " << e.what();
        }
    }
 
    void
    ObjectPoseGuiWidgetController::detachObjectFromRobotNode(QString providerName, QString objectID)
    {
        ARMARX_VERBOSE << "Detaching " << objectID << " by '" << providerName
                       << "' from robot node.";
 
        objpose::DetachObjectFromRobotNodeInput input;
        input.providerName = providerName.toStdString();
        input.objectID = armarx::toIce(armarx::ObjectID(objectID.toStdString()));
 
        try
        {
            objpose::DetachObjectFromRobotNodeOutput output =
                ObjectPoseStorage->detachObjectFromRobotNode(input);
            ARMARX_VERBOSE << "Was attached: " << output.wasAttached;
        }
        catch (const IceUtil::Exception& e)
        {
            ARMARX_WARNING << "Failed to detach object '" << input.objectID
                           << "' from a robot node."
                           << "\nReason: " << e.what();
        }
    }
 
    void
    ObjectPoseGuiWidgetController::detachAllObjectsFromRobotNodes(bool commitAttachedPose)
    {
 
        objpose::DetachAllObjectsFromRobotNodesInput input;
        input.commitAttachedPose = commitAttachedPose;
 
        try
        {
            objpose::DetachAllObjectsFromRobotNodesOutput output =
                ObjectPoseStorage->detachAllObjectsFromRobotNodes(input);
            ARMARX_VERBOSE << "Detached " << output.numDetached << " objects from robot nodes.";
        }
        catch (const IceUtil::Exception& e)
        {
            ARMARX_WARNING << "Failed to detach all objects from robot nodes.";
        }
    }
 
    void
    ObjectPoseGuiWidgetController::requestSelectedObjects()
    {
        std::map<std::string, objpose::observer::RequestObjectsInput> requestsPerProvider;
 
        QTreeWidget* tree = widget.requestTree;
        for (int i = 0; i < tree->topLevelItemCount(); ++i)
        {
            QTreeWidgetItem* datasetItem = tree->topLevelItem(i);
            for (int j = 0; j < datasetItem->childCount(); ++j)
            {
                QTreeWidgetItem* classItem = datasetItem->child(j);
                QCheckBox* selected = dynamic_cast<QCheckBox*>(tree->itemWidget(classItem, 2));
                ARMARX_CHECK_NOT_NULL(selected);
                if (selected->isChecked())
                {
                    std::string providerName = classItem->text(1).toStdString();
                    objpose::observer::RequestObjectsInput& requests =
                        requestsPerProvider[providerName];
                    data::ObjectID& id = requests.request.objectIDs.emplace_back();
                    id.dataset = datasetItem->text(0).toStdString();
                    id.className = classItem->text(0).toStdString();
                }
            }
        }
 
        long timeoutMS = -1;
        if (!widget.requestInfiniteCheckBox->isChecked())
        {
            timeoutMS = long(widget.requestTimeoutSpinBox->value() * 1000);
        }
 
        for (auto& [providerName, request] : requestsPerProvider)
        {
            request.provider = providerName;
            request.request.relativeTimeoutMS = timeoutMS;
 
            ARMARX_INFO << "Requesting " << request.request.objectIDs.size() << " objects for "
                        << request.request.relativeTimeoutMS << " ms.";
            objpose::observer::RequestObjectsOutput output =
                ObjectPoseStorage->requestObjects(request);
            int successful = 0;
            for (const auto& [id, result] : output.results)
            {
                successful += int(!result.providerName.empty() && result.result.success);
            }
            ARMARX_INFO << successful << " of " << request.request.objectIDs.size()
                        << " object request successful.";
        }
    }
 
} // namespace armarx