Controller.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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::gui-plugins::SceneEditor
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include <fstream>
#include <stdexcept>
#include <vector>
 
#include <QtDesigner/abstractpropertyeditor.h>
 
#include <QMetaObject>
#include <QMetaType>
#include <QThread>
#include <QApplication>
 
#include <SimoxUtility/json.h>
 
#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
 
#include "Controller.h"
#include "RemoveOperation.h"
#include "DeselectOperation.h"
#include "RemoveFromGroupOperation.h"
#include "DeleteGroupOperation.h"
 
 
controller::Controller::Controller() :
    QObject(),
    lastSelected(NULL)
{
    qRegisterMetaType<OperationPtrListPtr>("OperationPtrListPtr");
}
 
controller::ControllerPtr controller::Controller::create()
{
    controller::ControllerPtr controller(new Controller());
 
    controller->undoRedoStack.reset(new controller::UndoRedoStack());
    controller->scene = scene3D::Scene::create(controller);
    controller->memoryXController.reset(new memoryxcontroller::MemoryXController());
    controller->shortcutController.reset(new gui::ShortcutController);
 
    return controller;
}
 
controller::Controller::~Controller()
{
}
 
void controller::Controller::execute(int flags, const OperationPtrListPtr& operations, bool blocking)
{
    QMetaObject::invokeMethod(this, "executeQtThread", blocking ? (QApplication::instance()->thread() == QThread::currentThread() || QApplication::instance()->thread() == NULL ? Qt::DirectConnection : Qt::BlockingQueuedConnection) : Qt::QueuedConnection, Q_ARG(int, flags), Q_ARG(OperationPtrListPtr, operations));
}
 
void controller::Controller::executeQtThread(int flags, const OperationPtrListPtr& operations)
{
    std::unique_lock lock(execute_mutex);
    bool failed = false;
    controller::vector_string objectIds;
    std::vector<controller::OperationPtr>::size_type i = 0;
 
    for (; i != operations->size(); i++)
    {
        if (!operations->at(i)->isExecuteable())
        {
            std::unique_lock lock(queue_mutex);
            queuedOperations.push_back(std::make_pair(flags, operations->at(i)));
            continue;
        }
 
        if (controller::Controller::EXECUTE_ON_WM & flags)
        {
            std::string tmp = operations->at(i)->getObjectId();
            OperationPtr op = operations->at(i);
            try
            {
                op->executeOnWorkingMemory();
            }
            catch (std::exception& e)
            {
                ARMARX_ERROR_S << "executeOnWorkingMemory failed: "  << op->getObjectId() << e.what();
                failed = true;
                break;
            }
            catch (...)
            {
                failed = true;
                break;
            }
 
            if (tmp != operations->at(i)->getObjectId())
            {
                if (getScene()->getObjectManager()->getObjectById(operations->at(i)->getObjectId()))
                {
                    // the new id should never be used in the local scene
                    throw std::runtime_error("New id is already used in  local scene.");
                }
 
                std::string newId = operations->at(i)->getObjectId();
                undoRedoStack->updateObjectId(tmp, newId);
 
                // if the current vector is not updated, update it
                if (operations->at(i)->getObjectId() == newId)
                {
                    for (auto ito = operations->begin(); ito != operations->end(); ito++)
                    {
                        if (ito->get()->getObjectId() == tmp)
                        {
                            ito->get()->setObjectId(newId);
                        }
                        else if (ito->get()->getObjectId() == newId)
                        {
                            ito->get()->setObjectId(tmp);
                        }
                    }
                }
 
                // The IDs of all operations in the vector were swapped.
                // We have revert the swap on the current operation, because it was already the new ID.
                operations->at(i)->setObjectId(newId);
            }
        }
 
        if (controller::Controller::EXECUTE_ON_SCENE & flags)
        {
            std::string tmp = operations->at(i)->getObjectId();
            OperationPtr op = operations->at(i);
            try
            {
                op->executeOnScene();
            }
            catch (std::exception& e)
            {
                ARMARX_ERROR_S << "executeOnScene failed: " << op->getObjectId() << e.what();
                failed = true;
                break;
            }
            catch (...)
            {
                failed = true;
                break;
            }
 
            if (tmp != operations->at(i)->getObjectId())
            {
                throw std::runtime_error("ID changed while execution on local Scene.");
            }
        }
 
        objectIds.push_back(operations->at(i)->getObjectId());
    }
 
    if (failed)
    {
        std::cerr << "Could not execute operations. Trying to revert." << std::endl;
 
        // try to revert operations
        if (i > 0)
        {
            int j = i - 1; // i is the failed operation
 
            for (; j >= 0; j--)
            {
                controller::OperationPtr inverse = operations->at(j)->createInverseOperation();
 
                if (controller::Controller::EXECUTE_ON_SCENE & flags)
                {
                    try
                    {
                        inverse->executeOnScene();
                    }
                    catch (...)
                    {
                        std::cerr << "Could not revert operation on the local scene. Trying to revert the other operations." << std::endl;
                    }
                }
 
                if (controller::Controller::EXECUTE_ON_WM & flags)
                {
                    try
                    {
                        inverse->executeOnWorkingMemory();
                    }
                    catch (...)
                    {
                        std::cerr << "Could not revert operation on WorkingMemory. Trying to revert the other operations." << std::endl;
                    }
                }
            }
        }
    }
    else if (controller::Controller::UNDOABLE & flags)
    {
        undoRedoStack->push(operations);
    }
 
    std::vector<scene3D::SceneObjectPtr> allSelected = scene->getSelectionManager()->getAllSelected();
    scene3D::SceneObjectPtr newSelected = allSelected.size() == 0 ? NULL : allSelected[allSelected.size() - 1];
 
    if (newSelected != lastSelected)
    {
        lastSelected = newSelected;
        triggerSceneObjectSelected(newSelected);
    }
 
    qRegisterMetaType<controller::vector_string>("controller::vector_string");
    emit operationExecuted(objectIds);
}
 
void controller::Controller::executeQueuedOperations(bool blocking)
{
    {
        std::unique_lock lock(queue_mutex);
 
        for (auto it = queuedOperations.begin(); it != queuedOperations.end(); ++it)
        {
            controller::OperationPtrListPtr queuedOperationsTmp(new std::vector<controller::OperationPtr>);
            queuedOperationsTmp->push_back((*it).second);
            this->execute((*it).first, queuedOperationsTmp, blocking);
        }
 
        queuedOperations.clear();
    }
}
 
void controller::Controller::reloadLocalScene()
{
    if (!memoryXController->getWorkingMemoryController())
    {
        throw std::runtime_error("Not initialized");
    }
 
    // remove all local objects
    std::vector<scene3D::SceneObjectPtr> currentLocalObjects = scene->getObjectManager()->getAllObjects();
 
    for (scene3D::SceneObjectPtr object : currentLocalObjects)
    {
        scene->getObjectManager()->removeObject(object);
    }
 
    // clear the UndoRedoStack
    undoRedoStack->clear();
    // load the current state of the WorkingMemory
    memoryXController->getWorkingMemoryController()->addAllInstancesToLocalScene();
    emit reloadScene();
 
}
 
const std::shared_ptr<scene3D::Scene> controller::Controller::getScene() const
{
    return scene;
}
 
const std::shared_ptr<memoryxcontroller::MemoryXController> controller::Controller::getMemoryXController() const
{
    return memoryXController;
}
 
const std::shared_ptr<gui::ShortcutController> controller::Controller::getShortcutController() const
{
    return shortcutController;
}
 
void controller::Controller::undo()
{
    execute(controller::Controller::EXECUTE_ON_SCENE | controller::Controller::EXECUTE_ON_WM, undoRedoStack->undo());
}
 
void controller::Controller::redo()
{
    execute(controller::Controller::EXECUTE_ON_SCENE | controller::Controller::EXECUTE_ON_WM, undoRedoStack->redo());
}
 
 
void controller::Controller::triggerObjectClassSelected(const std::string& objectClass, const std::string& collection)
{
    emit objectClassSelected(objectClass, collection);
}
 
void controller::Controller::triggerSceneObjectSelected(scene3D::SceneObjectPtr object)
{
    emit sceneObjectSelected(object);
}
 
void controller::Controller::triggerMinimapClicked()
{
    emit minimapClicked();
}
 
void controller::Controller::triggerObjectsChanged(controller::vector_string objectIds)
{
    emit objectsChanged(objectIds);
}
 
void controller::Controller::clearScene()
{
    std::shared_ptr<std::vector<controller::OperationPtr> > operations(new std::vector<controller::OperationPtr>());
    std::vector<scene3D::SceneObjectPtr> selectedObjects = getScene()->getSelectionManager()->getAllSelected();
 
    for (auto it = selectedObjects.rbegin(); it != selectedObjects.rend(); ++it)
    {
        controller::OperationPtr operation(new controller::DeselectOperation(getMemoryXController(), getScene(), (*it)->getObjectId()));
        operations->push_back(operation);
    }
 
    std::vector<scene3D::SceneObjectPtr> allObjects = getScene()->getObjectManager()->getAllObjects();
    std::vector<scene3D::SceneGroupPtr> groups = getScene()->getGroupManager()->getAllGroups();
 
    for (auto itG = groups.begin(); itG != groups.end(); ++itG)
    {
        std::vector<scene3D::SceneObjectPtr> groupObjects = (*itG)->getAllObjects();
 
        for (auto it = groupObjects.rbegin(); it != groupObjects.rend(); ++it)
        {
            controller::OperationPtr removeFromGroupOperation(new controller::RemoveFromGroupOperation(getMemoryXController(), getScene(), (*itG)->getGroupId(), (*it)->getObjectId()));
            operations->push_back(removeFromGroupOperation);
        }
 
        controller::OperationPtr deleteGroupOperation(new controller::DeleteGroupOperation(getMemoryXController(), getScene(), (*itG)->getGroupId()));
        operations->push_back(deleteGroupOperation);
    }
 
    for (auto it = allObjects.rbegin(); it != allObjects.rend(); ++it)
    {
        controller::OperationPtr operation(new controller::RemoveOperation(getMemoryXController(), getScene(), (*it)->getObjectId()));
        operations->push_back(operation);
    }
 
    execute(controller::Controller::EXECUTE_ON_SCENE | controller::Controller::EXECUTE_ON_WM | controller::Controller::UNDOABLE, operations);
}
 
void controller::Controller::saveSnapshotAsJSON(std::string const& snapshotName)
{
    std::shared_ptr<scene3D::Scene> scene = getScene();
    scene3D::SceneObjectManagerPtr objectManager = scene->getObjectManager();
    std::vector<scene3D::SceneObjectPtr> objects = objectManager->getAllObjects();
 
    armarx::objects::Scene data;
    for (scene3D::SceneObjectPtr const& object : objects)
    {
        armarx::objects::SceneObject& objData = data.objects.emplace_back();
 
        SbVec3f translation = object->getTranslation();
        objData.position.x() = translation[0];
        objData.position.y() = translation[1];
        objData.position.z() = translation[2];
        objData.position *= 1000;  // m to mm
 
        SbRotation rotation = object->getRotation();
        objData.orientation.x() = rotation[0];
        objData.orientation.y() = rotation[1];
        objData.orientation.z() = rotation[2];
        objData.orientation.w() = rotation[3];
 
        objData.collection = object->getCollection();
 
        objData.className = object->getClassId();
    }
    nlohmann::json j = data;
 
    // TODO: We have to be able to define the folder somehow
    const std::string filename = snapshotName + ".json";
    try
    {
        nlohmann::write_json(filename, j, 2);
        ARMARX_INFO << "Saved snapshot as JSON at \n" << std::filesystem::absolute(filename);
    }
    catch (const std::ios_base::failure& e)
    {
        ARMARX_WARNING << "Failed to save snapshot as JSON at \n"
                       << std::filesystem::absolute(filename) << ":\n" << e.what();
    }
}