PriorKnowledgeController.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 <math.h>
 
#include <algorithm>
#include <exception>
#include <map>
#include <string>
#include <vector>
 
// Coin3D
#include <Inventor/nodes/SoNode.h>
 
// MemoryXInterface
#include <MemoryX/interface/components/CommonStorageInterface.h>
#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
 
// MemoryX
#include <ArmarXCore/core/application/Application.h>
 
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
 
// Simox-VirtualRobot
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
 
#include "MemoryXController.h"
#include "PriorKnowledgeController.h"
 
memoryxcontroller::PriorKnowledgeController::PriorKnowledgeController(
    const memoryx::PriorKnowledgeInterfacePrx& priorKnowledgePrx)
{
    this->priorKnowledgePrx = priorKnowledgePrx;
    classesSegmentPrx = this->priorKnowledgePrx->getObjectClassesSegment();
    databasePrx = this->priorKnowledgePrx->getCommonStorage();
 
    try
    {
        fileManager.reset(new memoryx::GridFileManager(databasePrx));
    }
    catch (const armarx::LocalException&)
    {
        fileManager.reset(new memoryx::GridFileManager(databasePrx, "./TempCacheForTesting/"));
    }
}
 
memoryxcontroller::PriorKnowledgeController::PriorKnowledgeController(
    const PriorKnowledgeController& other)
{
    std::unique_lock lock(mutexEntities);
    this->classesSegmentPrx = other.classesSegmentPrx;
    this->databasePrx = other.databasePrx;
    this->memoryXController = other.memoryXController;
    this->priorKnowledgePrx = other.priorKnowledgePrx;
    this->fileManager = other.fileManager;
}
 
memoryxcontroller::PriorKnowledgeController::~PriorKnowledgeController()
{
}
 
std::vector<std::string>
memoryxcontroller::PriorKnowledgeController::getCollectionNames() const
{
    std::unique_lock lock(mutexEntities);
    return classesSegmentPrx->getReadCollectionsNS();
}
 
std::vector<memoryx::ObjectClassPtr>
memoryxcontroller::PriorKnowledgeController::getAllObjectClassesPtr(
    const std::string& collection) const
{
    std::vector<memoryx::ObjectClassPtr> classPtr;
    std::unique_lock lock(mutexEntities);
 
    try
    {
        memoryx::CollectionInterfacePrx collectionInterface =
            this->databasePrx->requestCollection(collection);
        ARMARX_CHECK_EXPRESSION(collectionInterface);
 
        memoryx::EntityIdList ids = collectionInterface->findAllIds();
 
        for (memoryx::EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
        {
            ARMARX_CHECK_EXPRESSION(classesSegmentPrx);
 
            const memoryx::EntityBasePtr entity = classesSegmentPrx->getEntityById(*it);
            const memoryx::ObjectClassPtr objClass = memoryx::ObjectClassPtr::dynamicCast(entity);
 
            if (objClass)
            {
                classPtr.push_back(objClass);
            }
        }
    }
    catch (const memoryx::DBNotSpecifiedException&)
    {
        ARMARX_WARNING_S << "Tried to acces non-existing collection.";
        return {};
    }
 
    return classPtr;
}
 
memoryx::ObjectClassPtr
memoryxcontroller::PriorKnowledgeController::getObjectClassPtr(const std::string& className,
                                                               const std::string& collection) const
{
    std::unique_lock lock(mutexEntities);
    std::vector<memoryx::ObjectClassPtr> objectClasses = this->getAllObjectClassesPtr(collection);
 
    if (!objectClasses.empty())
    {
        for (memoryx::ObjectClassPtr ptr : objectClasses)
        {
            ARMARX_CHECK_EXPRESSION(ptr);
 
            if (ptr->getName() == className)
            {
                return ptr;
            }
        }
    }
 
    return NULL;
}
 
SoNode*
memoryxcontroller::PriorKnowledgeController::getCoinVisualisation(
    const memoryx::ObjectClassPtr& objectClass,
    const bool& collisionModel) const
{
    std::unique_lock lock(mutexEntities);
    memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper =
        objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
 
    simoxWrapper->getManipulationObject()->setGlobalPose(Eigen::Matrix4f::Identity());
 
    VirtualRobot::VisualizationNodePtr visNode;
    bool hasModel = true;
 
    if (collisionModel)
    {
        VirtualRobot::CollisionModelPtr collisionModel = simoxWrapper->getCollisionModel();
 
        if (!collisionModel)
        {
            hasModel = false;
        }
        else
        {
            visNode = collisionModel->getVisualization();
        }
    }
    else
    {
        visNode = simoxWrapper->getVisualization();
    }
 
    if (!hasModel)
    {
        return NULL;
    }
 
    return VirtualRobot::CoinVisualizationFactory::getCoinVisualization(visNode);
}
 
std::map<std::string, std::string>
memoryxcontroller::PriorKnowledgeController::getAllAttributes(
    const memoryx::ObjectClassPtr& objectClass) const
{
    std::unique_lock lock(mutexEntities);
    std::map<std::string, std::string> allAttributes;
 
    if (!objectClass)
    {
        return allAttributes;
    }
 
    memoryx::NameList attributeNames = objectClass->getAttributeNames();
 
    memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper =
        objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
 
    for (memoryx::NameList::const_iterator it = attributeNames.begin(); it != attributeNames.end();
         ++it)
    {
        memoryx::EntityAttributeBasePtr attrValue = objectClass->getAttribute(*it);
        armarx::VariantPtr attributeValue = armarx::VariantPtr::dynamicCast(attrValue->getValue());
 
        if (attributeValue && (attributeValue->getOutputValueOnly() != ""))
        {
            if (*it == "ManipulationFile")
            {
                allAttributes.insert(std::pair<std::string, std::string>(
                    *it, simoxWrapper->getManipulationObjectFileName()));
            }
            else if (*it == "IvFile" || *it == "IvFileCollision" || *it == "texFiles")
            {
                // Since the class SimoxObjectWrapper does not provide a way to get the file path of the IvFile, we have to use this workaround
                try
                {
                    std::string maniPath = simoxWrapper->getManipulationObjectFileName();
                    memoryx::MongoDBRefPtr mongoPtr =
                        attributeValue->getClass<memoryx::MongoDBRef>();
                    const memoryx::GridFileInterfacePrx prx =
                        databasePrx->getFileProxyById(mongoPtr->dbName, mongoPtr->docId);
                    if (!prx)
                    {
                        ARMARX_WARNING_S << "Could not find IvFile of objectClass "
                                         << objectClass->getName() << VAROUT(mongoPtr->dbName)
                                         << VAROUT(mongoPtr->docId);
                        allAttributes.insert(std::pair<std::string, std::string>(*it, ""));
                        continue;
                    }
 
                    std::string ivFilePath =
                        maniPath.substr(0, maniPath.rfind("/") + 1) + prx->getFilename();
 
                    allAttributes.insert(std::pair<std::string, std::string>(*it, ivFilePath));
                }
                catch (const armarx::InvalidTypeException&)
                {
                    ARMARX_WARNING_S << "Could not find IvFile of objectClass "
                                     << objectClass->getName();
                    allAttributes.insert(std::pair<std::string, std::string>(*it, ""));
                }
            }
            else if (*it == "featureFile")
            {
                memoryx::EntityWrappers::TexturedRecognitionWrapperPtr texturedRecognitionWrapper =
                    objectClass->addWrapper(
                        new memoryx::EntityWrappers::TexturedRecognitionWrapper(fileManager));
                allAttributes.insert(std::pair<std::string, std::string>(
                    *it, texturedRecognitionWrapper->getFeatureFile()));
            }
            else
            {
                allAttributes.insert(
                    std::pair<std::string, std::string>(*it, attributeValue->getOutputValueOnly()));
            }
        }
        else
        {
            allAttributes.insert(std::pair<std::string, std::string>(*it, ""));
        }
    }
 
    return allAttributes;
}
 
std::string
memoryxcontroller::PriorKnowledgeController::getCollection(
    const memoryx::ObjectInstancePtr& objectInstance) const
{
    std::unique_lock lock(mutexEntities);
 
    memoryx::NameList collectionNames = this->getCollectionNames();
    std::map<memoryx::ObjectClassPtr, std::string> possibleObjectClasses;
    std::string className = objectInstance->getMostProbableClass();
 
    for (std::vector<std::string>::const_iterator it = collectionNames.begin();
         it != collectionNames.end();
         ++it)
    {
        memoryx::ObjectClassPtr ptr = this->getObjectClassPtr(className, *it);
 
        if (ptr)
        {
            possibleObjectClasses.insert(std::pair<memoryx::ObjectClassPtr, std::string>(ptr, *it));
        }
    }
 
    if (possibleObjectClasses.size() > 1)
    {
        // this should never happen, in all collections the names of the object-class shuold be unique.
        // if there are for some reason two or more object-classes with the same name. one of these will be returned "at random"
 
        return possibleObjectClasses.begin()->second;
    }
    else if (possibleObjectClasses.size() == 1)
    {
        return possibleObjectClasses.begin()->second;
    }
    else
    {
        return "";
    }
}