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/SbRotation.h>
#include <Inventor/SbVec3f.h>
 
// MemoryXInterface
#include "../controller/AddOperation.h"
#include "../controller/RemoveOperation.h"
#include "../controller/RotateTranslateOperation.h"
#include "WorkingMemoryController.h"
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.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);
}