WorkingMemoryController.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 <stdexcept>
#include <string>
#include <vector>
 
#include <IceUtil/UUID.h>
 
// Coin3D
#include <Inventor/SbVec3f.h>
#include <Inventor/SbRotation.h>
 
// MemoryXInterface
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
 
#include "WorkingMemoryController.h"
#include "../controller/AddOperation.h"
#include "../controller/RemoveOperation.h"
#include "../controller/RotateTranslateOperation.h"
 
memoryxcontroller::WorkingMemoryController::WorkingMemoryController(const memoryx::WorkingMemoryInterfacePrx& workingMemoryPrx, const std::string& workingMemoryUpdatesTopic, const std::string& objectInstancesSegmentName, const memoryxcontroller::MemoryXControllerPtr& memoryXController) :
    workingMemoryPrx(workingMemoryPrx),
    workingMemoryUpdatesTopic(workingMemoryUpdatesTopic)
 
{
    this->objectInstancesPrx = memoryx::ObjectInstanceMemorySegmentBasePrx::uncheckedCast(workingMemoryPrx->getSegment(objectInstancesSegmentName));
    this->memoryXController = memoryXController;
 
    std::string s1 = "SceneEditor.WorkingMemoryController";
    std::string s2 = IceUtil::generateUUID();
 
    this->setName(s1 + s2);
}
 
memoryxcontroller::WorkingMemoryController::~WorkingMemoryController()
{
}
 
std::string memoryxcontroller::WorkingMemoryController::addObjectInstance(const std::string& objectName, const memoryx::ObjectClassPtr& objectClassPtr)
{
    memoryx::ObjectInstancePtr newObject = new memoryx::ObjectInstance(objectName);
    memoryx::NameList attributeNames = objectClassPtr->getAttributeNames();
 
    for (memoryx::NameList::const_iterator it = attributeNames.begin(); it != attributeNames.end(); ++it)
    {
        newObject->putAttribute(objectClassPtr->getAttribute(*it));
    }
 
    newObject->setClass(objectClassPtr->getName(), 1.0f);
    newObject->setExistenceCertainty(1.0f);
 
    this->currentChangedInstances.push_back(newObject);
 
    std::string objectID = this->objectInstancesPrx->addEntity(newObject);
    newObject->setId(objectID);
 
    return objectID;
}
 
std::vector<std::string> memoryxcontroller::WorkingMemoryController::addObjectInstances(const std::vector<memoryx::ObjectInstancePtr>& objectInstances)
{
    std::vector<std::string> addedIds;
 
    if (!objectInstances.empty())
    {
        std::shared_ptr<std::vector<controller::OperationPtr> > operations(new std::vector<controller::OperationPtr>());
 
        if (std::shared_ptr<memoryxcontroller::MemoryXController> memXController = this->memoryXController.lock())
        {
            if (controller::ControllerPtr controller = memXController->getController())
            {
                for (std::vector<memoryx::ObjectInstancePtr>::const_iterator it = objectInstances.begin(); it != objectInstances.end(); ++it)
                {
                    memoryx::ObjectInstancePtr objectInstance = *it;
 
                    auto pose = objectInstance->getPose();
 
                    if (pose->getFrame() != armarx::GlobalFrame && !pose->frame.empty())
                    {
                        // FIXME: The localization timestamp is ignored.
                        // This causes stuttering objects during robot motion
                        auto newPose = workingMemoryPrx->getAgentInstancesSegment()->convertToWorldPose(pose->agent, pose);
 
                        if (newPose)
                        {
                            objectInstance->setPose(newPose);
                        }
                        else
                        {
                            ARMARX_ERROR_S << "could not convert local pose to global pose" << std::endl;
                        }
                    }
 
                    this->currentChangedInstances.push_back(objectInstance);
                    std::string objectID = this->objectInstancesPrx->addEntity(objectInstance);
                    objectInstance->setId(objectID);
 
                    addedIds.push_back(objectID);
 
                    controller::OperationPtr operation(new controller::AddOperation(memXController,
                                                       controller->getScene(),
                                                       objectInstance->getName(),
                                                       memXController->getPriorKnowlegdeController()->getCollection(objectInstance),
                                                       getSbVec3fFromInstance(objectInstance),
                                                       getSbRotationFromInstance(objectInstance),
                                                       objectInstance->getId()));
 
                    operations->push_back(operation);
                }
 
                controller->execute(controller::Controller::EXECUTE_ON_SCENE | controller::Controller::UNDOABLE, operations);
            }
        }
    }
 
    return addedIds;
}
 
void memoryxcontroller::WorkingMemoryController::removeObjectInstance(const std::string& id)
{
    if (this->objectInstancesPrx->getEntityById(id))
    {
 
        this->currentChangedInstances.push_back(memoryx::ObjectInstancePtr::dynamicCast(this->objectInstancesPrx->getEntityById(id)));
 
        this->objectInstancesPrx->removeEntity(id);
    }
    else
    {
        throw std::invalid_argument("There is no objectInstance with given ID.");
    }
}
 
void memoryxcontroller::WorkingMemoryController::rotateTranslateObject(const std::string& id, const SbRotation& newOrientation, const SbVec3f& newPosition)
{
    memoryx::ObjectInstanceBasePtr objectBase = this->objectInstancesPrx->getObjectInstanceById(id);
 
    if (objectBase)
    {
 
        memoryx::ObjectInstancePtr object = memoryx::ObjectInstancePtr::dynamicCast(objectBase);
 
        float x, y, z;
        newPosition.getValue(x, y, z);
        // The new position values have to be multiplied with 1000 because in the workingmemory the unit the position is stored is meter
        // and in this local coin-scene the unit is millimeter.
        const Eigen::Vector3f vec(x * 1000.0f, y * 1000.0f, z * 1000.0f);
        armarx::FramedPositionPtr newPos = new armarx::FramedPosition(vec, armarx::GlobalFrame, "");
        object->setPosition(newPos);
 
        float pw, px, py, pz;
        newOrientation.getValue(px, py, pz, pw);
        Eigen::Quaternionf quat(pw, px, py, pz);
        armarx::FramedOrientationPtr newOrient = new armarx::FramedOrientation(quat.toRotationMatrix(), armarx::GlobalFrame, "");
        object->setOrientation(newOrient);
 
        this->currentChangedInstances.push_back(object);
 
        this->objectInstancesPrx->setObjectPoseWithoutMotionModel(id, object->getPose());
    }
    else
    {
        throw std::invalid_argument("There is no objectInstance with given ID.");
    }
}
 
void memoryxcontroller::WorkingMemoryController::addAllInstancesToLocalScene() const
{
    if (std::shared_ptr<memoryxcontroller::MemoryXController> memXController = this->memoryXController.lock())
    {
        if (controller::ControllerPtr controller = memXController->getController())
        {
            std::unique_lock lock(mutexEntities);
 
            memoryx::EntityIdList ids = this->objectInstancesPrx->getAllEntityIds();
            std::shared_ptr<std::vector<controller::OperationPtr> > operations(new std::vector<controller::OperationPtr>());
 
            for (memoryx::EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
            {
                memoryx::ObjectInstancePtr object = memoryx::ObjectInstancePtr::dynamicCast(this->objectInstancesPrx->getObjectInstanceById(*it));
 
                auto pose = object->getPose();
 
                if (pose->getFrame() != armarx::GlobalFrame && !pose->frame.empty())
                {
                    auto newPose = workingMemoryPrx->getAgentInstancesSegment()->convertToWorldPose(pose->agent, pose);
 
                    if (newPose)
                    {
                        object->setPose(newPose);
                    }
                    else
                    {
                        ARMARX_ERROR_S << "could not convert local pose to global pose" << std::endl;
                    }
                }
 
                controller::OperationPtr operation(new controller::AddOperation(memXController,
                                                   controller->getScene(),
                                                   object->getName(),
                                                   memXController->getPriorKnowlegdeController()->getCollection(object),
                                                   getSbVec3fFromInstance(object),
                                                   getSbRotationFromInstance(object),
                                                   object->getId()));
                operations->push_back(operation);
            }
 
            controller->execute(controller::Controller::EXECUTE_ON_SCENE, operations);
        }
    }
}
 
void memoryxcontroller::WorkingMemoryController::reportEntityCreated(const std::string& segmentName, const memoryx::EntityBasePtr& entity,
        const Ice::Current&)
{
    if (0 == objectInstancesPrx->getIceId().name.compare(objectInstancesPrx->getIceId().name.length() - segmentName.length(), segmentName.length(), segmentName))
    {
        memoryx::ObjectInstancePtr ptr = memoryx::ObjectInstancePtr::dynamicCast(entity);
 
        auto pose = ptr->getPose();
 
        if (pose->getFrame() != armarx::GlobalFrame && !pose->frame.empty())
        {
            auto newPose = workingMemoryPrx->getAgentInstancesSegment()->convertToWorldPose(pose->agent, pose);
 
            if (newPose)
            {
                ptr->setPose(newPose);
            }
            else
            {
                ARMARX_ERROR_S << "could not convert local pose to global pose" << std::endl;
            }
        }
 
        if (!(this->findAndRemoveInstanceFromList(this->currentChangedInstances, ptr)))
        {
            std::shared_ptr<std::vector<controller::OperationPtr> > operations(new std::vector<controller::OperationPtr>());
 
            if (std::shared_ptr<MemoryXController> memXController = this->memoryXController.lock())
            {
                if (controller::ControllerPtr controller = memXController->getController())
                {
                    controller::OperationPtr operation(new controller::AddOperation(memXController,
                                                       controller->getScene(),
                                                       ptr->getName(),
                                                       memXController->getPriorKnowlegdeController()->getCollection(ptr),
                                                       getSbVec3fFromInstance(ptr),
                                                       getSbRotationFromInstance(ptr),
                                                       ptr->getId()));
                    operations->push_back(operation);
                    controller->execute(controller::Controller::EXECUTE_ON_SCENE, operations);
                }
            }
        }
    }
}
 
void memoryxcontroller::WorkingMemoryController::reportEntityUpdated(const std::string& segmentName,
        const memoryx::EntityBasePtr& entityOld,
        const memoryx::EntityBasePtr& entityNew, const Ice::Current&)
{
 
 
    if (0 == objectInstancesPrx->getIceId().name.compare(objectInstancesPrx->getIceId().name.length() - segmentName.length(), segmentName.length(), segmentName))
    {
        memoryx::ObjectInstancePtr ptr_new = memoryx::ObjectInstancePtr::dynamicCast(entityNew);
        auto pose = ptr_new->getPose();
 
        if (pose->getFrame() != armarx::GlobalFrame && !pose->frame.empty())
        {
            auto newPose = workingMemoryPrx->getAgentInstancesSegment()->convertToWorldPose(pose->agent, pose);
 
            if (newPose)
            {
                ptr_new->setPose(newPose);
            }
            else
            {
                ARMARX_ERROR_S << "could not convert local pose to global pose" << std::endl;
            }
        }
 
        if (!(this->findAndRemoveInstanceFromList(this->currentChangedInstances, ptr_new)))
        {
            memoryx::ObjectInstancePtr ptr_old = memoryx::ObjectInstancePtr::dynamicCast(entityOld);
 
            auto pose = ptr_old->getPose();
 
            if (pose->getFrame() != armarx::GlobalFrame && !pose->frame.empty())
            {
                auto newPose = workingMemoryPrx->getAgentInstancesSegment()->convertToWorldPose(pose->agent, pose);
 
                if (newPose)
                {
                    ptr_old->setPose(newPose);
                }
                else
                {
                    ARMARX_ERROR_S << "could not convert local pose to global pose" << std::endl;
                }
            }
 
            std::shared_ptr<std::vector<controller::OperationPtr> > operations(new std::vector<controller::OperationPtr>());
 
            if (std::shared_ptr<MemoryXController> memXController = this->memoryXController.lock())
            {
                if (controller::ControllerPtr controller = memXController->getController())
                {
                    controller::OperationPtr operation(new controller::RotateTranslateOperation(memXController,
                                                       controller->getScene(),
                                                       ptr_new->getId(),
                                                       getSbRotationFromInstance(ptr_old),
                                                       getSbRotationFromInstance(ptr_new),
                                                       getSbVec3fFromInstance(ptr_old),
                                                       getSbVec3fFromInstance(ptr_new)));
                    operations->push_back(operation);
                    controller->execute(controller::Controller::EXECUTE_ON_SCENE, operations);
                }
            }
        }
    }
}
 
void memoryxcontroller::WorkingMemoryController::reportEntityRemoved(const std::string& segmentName, const memoryx::EntityBasePtr& entity,
        const Ice::Current&)
{
    if (0 == objectInstancesPrx->getIceId().name.compare(objectInstancesPrx->getIceId().name.length() - segmentName.length(), segmentName.length(), segmentName))
    {
        memoryx::ObjectInstancePtr ptr = memoryx::ObjectInstancePtr::dynamicCast(entity);
 
        if (!(this->findAndRemoveInstanceFromList(this->currentChangedInstances, ptr)))
        {
            std::shared_ptr<std::vector<controller::OperationPtr> > operations(new std::vector<controller::OperationPtr>());
 
            if (std::shared_ptr<MemoryXController> memXController = this->memoryXController.lock())
            {
                if (controller::ControllerPtr controller = memXController->getController())
                {
                    controller::OperationPtr operation(new controller::RemoveOperation(memXController,
                                                       controller->getScene(),
                                                       ptr->getId()));
                    operations->push_back(operation);
                    controller->execute(controller::Controller::EXECUTE_ON_SCENE, operations);
                }
            }
        }
    }
}
 
void memoryxcontroller::WorkingMemoryController::reportMemoryCleared(const std::string& segmentName, const Ice::Current&)
{
}
 
void memoryxcontroller::WorkingMemoryController::reportSnapshotLoaded(const std::string& segmentName, const Ice::Current&)
{
 
}
 
 
void memoryxcontroller::WorkingMemoryController::onInitComponent()
{
    usingTopic(workingMemoryUpdatesTopic);
}
 
void memoryxcontroller::WorkingMemoryController::onConnectComponent()
{
 
}
 
std::string memoryxcontroller::WorkingMemoryController::getDefaultName() const
{
    return "SceneEditor.WorkingMemoryController";
}
 
 
SbRotation memoryxcontroller::WorkingMemoryController::getSbRotationFromInstance(const memoryx::ObjectInstancePtr& objectInstance)
{
    armarx::FramedOrientationPtr ptr = objectInstance->getOrientation();
    SbRotation orientation(SbRotation(ptr->qx, ptr->qy, ptr->qz, ptr->qw));
    return orientation;
}
 
SbVec3f memoryxcontroller::WorkingMemoryController::getSbVec3fFromInstance(const memoryx::ObjectInstancePtr& objectInstance)
{
    armarx::FramedPositionPtr ptr = objectInstance->getPosition();
    // The new position values have to be devided by 1000 because in the workingmemory the unit the position is stored is meter
    // and in this local coin-scene the unit is millimeter.
    SbVec3f pos(SbVec3f(ptr->x * 0.001f, ptr->y * 0.001f, ptr->z * 0.001f));
    return pos;
}
 
bool memoryxcontroller::WorkingMemoryController::findAndRemoveInstanceFromList(std::list<memoryx::ObjectInstancePtr>& instanceList, const memoryx::ObjectInstancePtr& instance) const
{
    for (std::list<memoryx::ObjectInstancePtr>::const_iterator it = instanceList.begin(); it != instanceList.end(); ++it)
    {
        memoryx::ObjectInstancePtr temp = *it;
 
        if (temp->getId() == instance->getId() && temp->getPosition()->toEigen() == instance->getPosition()->toEigen())
        {
            instanceList.remove(temp);
            return true;
        }
    }
 
    return false;
}
 
bool memoryxcontroller::WorkingMemoryController::saveSceneInSnapshot(const std::string& snapshotName, const memoryx::LongtermMemoryInterfacePrx& longtermMemoryPrx) const
{
    return longtermMemoryPrx->saveWorkingMemorySnapshot(snapshotName, this->workingMemoryPrx);
}
 
bool memoryxcontroller::WorkingMemoryController::saveObjectsInSnapshot(const std::string& snapshotName, const memoryx::LongtermMemoryInterfacePrx& longtermMemoryPrx, const std::vector<std::string>& objectIds) const
{
    return longtermMemoryPrx->getWorkingMemorySnapshotListSegment()->createSubsetSnapshot(snapshotName, this->workingMemoryPrx, objectIds);
}