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 <QTimer>
#include <QMenu>
 
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.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.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 "MemoryX.ObjectPoseGui";
    }
 
    static const std::string CONFIG_KEY_OBJECT_POSE_OBSERVER = "ObjectPoseStorage";
 
    QPointer<QDialog> ObjectPoseGuiWidgetController::getConfigDialog(QWidget* parent)
    {
        if (!configDialog)
        {
            configDialog = new SimpleConfigDialog(parent);
            configDialog->addProxyFinder<armarx::objpose::ObjectPoseStorageInterfacePrx>({CONFIG_KEY_OBJECT_POSE_OBSERVER, "Object pose observer.", "*"});
        }
        return qobject_cast<QDialog*>(configDialog);
    }
 
    void ObjectPoseGuiWidgetController::configured()
    {
        if (configDialog)
        {
            ObjectPoseStorageName = configDialog->getProxyName(CONFIG_KEY_OBJECT_POSE_OBSERVER);
        }
    }
 
    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());
        }
    }
 
    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 observer.";
            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 observer.";
            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::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.";
        }
    }
 
}