EntityDrawerComponent.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::ArmarXObjects::EntityDrawerComponent
 * @author     Nikolaus Vahrenkamp ( vahrenkamp at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "EntityDrawerComponent.h"
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
 
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoUnits.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
 
using namespace VirtualRobot;
 
namespace memoryx
{
 
    static const std::string DEBUG_LAYER_NAME{"debug"};
 
    EntityDrawerComponent::EntityDrawerComponent() : DebugDrawerComponent()
    {
    }
 
    EntityDrawerComponent::~EntityDrawerComponent()
    {
    }
 
    void
    EntityDrawerComponent::onUpdateVisualization()
    {
        // update custom visualizations
        auto l1 = getScopedAccumulatedDataLock();
        auto l2 = getScopedVisuLock();
 
        for (auto i = accumulatedCustomUpdateData.entities.begin();
             i != accumulatedCustomUpdateData.entities.end();
             i++)
        {
            drawEntity(i->second);
        }
 
        accumulatedCustomUpdateData.entities.clear();
    }
 
    void
    EntityDrawerComponent::removeCustomVisu(const std::string& layerName, const std::string& name)
    {
        removeObjectVisu(layerName, name);
    }
 
    void
    EntityDrawerComponent::drawEntity(const EntityDrawerComponent::EntityData& d)
    {
        auto l = getScopedVisuLock();
 
        auto& layer = requestLayer(d.layerName);
 
        if (!d.active)
        {
            ARMARX_INFO << "Removing entity " << d.name;
            removeEntity(d.layerName, d.name);
            return;
        }
 
        // load or get object
        VirtualRobot::SceneObjectPtr obj = requestEntity(d);
 
        if (!obj)
        {
            ARMARX_ERROR << deactivateSpam() << "Could not determine object " << d.name
                         << " at layer " << d.layerName;
            return;
        }
 
        if (d.updatePose)
        {
            obj->setGlobalPose(d.globalPose);
        }
 
        if (d.updateColor)
        {
            SoSeparator* sep = layer.addedCustomVisualizations[d.name];
 
            if (!sep || sep->getNumChildren() < 2)
            {
                ARMARX_ERROR << "Internal entity layer error1";
                return;
            }
 
            SoMaterial* m = dynamic_cast<SoMaterial*>(sep->getChild(0));
 
            if (!m)
            {
                ARMARX_ERROR << "Internal entity layer error2";
                return;
            }
 
            if (d.color.isNone())
            {
                m->setOverride(false);
            }
            else
            {
                if (d.color.r < 0 || d.color.g < 0 || d.color.b < 0)
                {
                    // transparency mode
                    if (d.color.transparency == 1)
                    {
                        m->diffuseColor.setIgnored(FALSE);
                        m->setOverride(FALSE);
                    }
                    else
                    {
                        m->transparency = d.color.transparency;
                        m->diffuseColor.setIgnored(TRUE);
                        m->setOverride(TRUE);
                    }
                }
                else
                {
                    // color & transparency mode
                    m->diffuseColor = SbColor(d.color.r, d.color.g, d.color.b);
                    m->ambientColor = SbColor(0, 0, 0);
                    m->transparency = std::max(0.0f, d.color.transparency);
                    m->setOverride(true);
                }
            }
        }
    }
 
    void
    EntityDrawerComponent::removeEntity(const std::string& layerName, const std::string& name)
    {
        auto l = getScopedVisuLock();
 
        // process active entities
        std::string entryName = "__" + layerName + "__" + name + "__";
 
        if (activeObjects.find(entryName) != activeObjects.end())
        {
            activeObjects.erase(entryName);
        }
 
        // process visualizations
        if (!hasLayer(layerName))
        {
            return;
        }
 
        auto& layer = layers.at(layerName);
 
        if (layer.addedCustomVisualizations.find(name) == layer.addedCustomVisualizations.end())
        {
            return;
        }
 
        layer.mainNode->removeChild(layer.addedCustomVisualizations[name]);
        layer.addedCustomVisualizations.erase(name);
    }
 
    void
    EntityDrawerComponent::onInitComponent()
    {
        DebugDrawerComponent::onInitComponent();
 
        std::string commonStorageName = getProperty<std::string>("CommonStorageName").getValue();
        usingProxy(commonStorageName);
 
        std::string priorKnowledgeName = getProperty<std::string>("PriorKnowledgeName").getValue();
        usingProxy(priorKnowledgeName);
    }
 
    void
    EntityDrawerComponent::onConnectComponent()
    {
        DebugDrawerComponent::onConnectComponent();
 
        std::string commonStorageName = getProperty<std::string>("CommonStorageName").getValue();
        CommonStorageInterfacePrx commonStoragePrx =
            getProxy<CommonStorageInterfacePrx>(commonStorageName);
 
        if (commonStoragePrx)
        {
            fileManager.reset(new GridFileManager(commonStoragePrx));
        }
        else
        {
            ARMARX_ERROR << "Could not get CommonStorage Proxy";
        }
 
        std::string priorKnowledgeName = getProperty<std::string>("PriorKnowledgeName").getValue();
        ARMARX_INFO << "Connecting to prior knowldege with name:" << priorKnowledgeName;
        priorKnowledgePrx = getProxy<PriorKnowledgeInterfacePrx>(priorKnowledgeName);
        objectClassSegment = priorKnowledgePrx->getObjectClassesSegment();
    }
 
    void
    EntityDrawerComponent::onDisconnectComponent()
    {
        DebugDrawerComponent::onDisconnectComponent();
    }
 
    void
    EntityDrawerComponent::onExitComponent()
    {
        DebugDrawerComponent::onExitComponent();
    }
 
    void
    EntityDrawerComponent::removeObjectVisu(const std::string& layerName,
                                            const std::string& objectName,
                                            const Ice::Current& c)
    {
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = "__" + layerName + "__" + objectName + "__";
        EntityData& d = accumulatedCustomUpdateData.entities[entryName];
        d.layerName = layerName;
        d.name = objectName;
        d.active = false;
    }
 
    void
    EntityDrawerComponent::updateObjectColor(const std::string& layerName,
                                             const std::string& objectName,
                                             const armarx::DrawColor& color,
                                             const Ice::Current& c)
    {
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = "__" + layerName + "__" + objectName + "__";
        EntityData& d = accumulatedCustomUpdateData.entities[entryName];
 
        // update data
        d.update = true;
        d.updateColor = true;
        d.layerName = layerName;
        d.name = objectName;
 
        if (color.a == 0 && color.b == 0 && color.r == 0 && color.g == 0)
        {
            d.color = VirtualRobot::VisualizationFactory::Color::None();
        }
        else
        {
            d.color =
                VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
        }
 
        d.active = true;
    }
 
    void
    EntityDrawerComponent::updateObjectTransparency(const std::string& layerName,
                                                    const std::string& objectName,
                                                    float alpha,
                                                    const Ice::Current& c)
    {
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = "__" + layerName + "__" + objectName + "__";
        EntityData& d = accumulatedCustomUpdateData.entities[entryName];
 
        if (alpha < 0)
        {
            alpha = 0;
        }
        if (alpha > 1.0f)
        {
            alpha = 1.0;
        }
 
        // update data
        d.update = true;
        d.updateColor = true;
        d.layerName = layerName;
        d.name = objectName;
        d.color = VirtualRobot::VisualizationFactory::Color(-1, -1, -1, 1 - alpha);
        d.active = true;
    }
 
    void
    EntityDrawerComponent::updateObjectPose(const std::string& layerName,
                                            const std::string& objectName,
                                            const armarx::PoseBasePtr& globalPose,
                                            const Ice::Current& c)
    {
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = "__" + layerName + "__" + objectName + "__";
        EntityData& d = accumulatedCustomUpdateData.entities[entryName];
 
        // update data
        d.update = true;
        d.updatePose = true;
        d.globalPose = armarx::PosePtr::dynamicCast(globalPose)->toEigen();
 
        d.layerName = layerName;
        d.name = objectName;
        d.active = true;
    }
 
    void
    EntityDrawerComponent::setObjectVisu(const std::string& layerName,
                                         const std::string& objectName,
                                         const ObjectClassBasePtr& objectClassBase,
                                         const armarx::PoseBasePtr& globalPose,
                                         const Ice::Current& c)
    {
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = "__" + layerName + "__" + objectName + "__";
        EntityData& d = accumulatedCustomUpdateData.entities[entryName];
 
        d.objectClass = ObjectClassPtr::dynamicCast(objectClassBase);
        d.globalPose = armarx::PosePtr::dynamicCast(globalPose)->toEigen();
        d.layerName = layerName;
        d.name = objectName;
        d.active = true;
        d.update = true;
        d.updatePose = true;
    }
 
    void
    EntityDrawerComponent::setObjectVisuByName(const std::string& layerName,
                                               const std::string& customObjectName,
                                               const std::string& objectClassName,
                                               const armarx::PoseBasePtr& globalPose,
                                               const Ice::Current& c)
    {
        auto objectClassBase = objectClassSegment->getObjectClassByName(objectClassName);
        if (!objectClassBase)
        {
            ARMARX_WARNING << "An object with class name '" << objectClassName
                           << "' does not exist!";
            return;
        }
        auto l = getScopedAccumulatedDataLock();
        std::string entryName = "__" + layerName + "__" + customObjectName + "__";
        EntityData& d = accumulatedCustomUpdateData.entities[entryName];
 
        d.objectClass = ObjectClassPtr::dynamicCast(objectClassBase);
        d.globalPose = armarx::PosePtr::dynamicCast(globalPose)->toEigen();
        d.layerName = layerName;
        d.name = customObjectName;
        d.active = true;
        d.update = true;
        d.updatePose = true;
    }
 
    VirtualRobot::SceneObjectPtr
    EntityDrawerComponent::requestEntity(const EntityData& d)
    {
        auto l = getScopedVisuLock();
 
        auto& layer = requestLayer(d.layerName);
        std::string entryName = "__" + d.layerName + "__" + d.name + "__";
 
        if (activeObjects.find(entryName) != activeObjects.end())
        {
            return activeObjects[entryName];
        }
 
        VirtualRobot::SceneObjectPtr result;
 
        try
        {
 
            memoryx::ObjectClassPtr objectClass = d.objectClass;
 
            if (!objectClass)
            {
                ARMARX_ERROR << "Invalid object class; could not create object with instance name "
                             << d.name;
                return result;
            }
            if (objectCache.count(objectClass->getId()) == 0)
            {
                memoryx::EntityWrappers::SimoxObjectWrapperPtr sw =
                    objectClass->addWrapper(new EntityWrappers::SimoxObjectWrapper(fileManager));
 
                VirtualRobot::ManipulationObjectPtr mo = sw->getManipulationObject();
 
                if (!mo)
                {
                    ARMARX_ERROR << "Could not retrieve manipulation object of object class "
                                 << objectClass->getName();
                    return result;
                }
 
                //ARMARX_INFO << "Filename mo:" << mo->getVisualization()->getFilename();
                //ARMARX_INFO << "Filename sw:" << sw->getManipulationObjectFileName();
 
                mo->setName(d.name);
                result = mo;
                objectCache[objectClass->getId()] = mo;
                ARMARX_LOG << "Loaded manipulation object " << mo->getName() << " of class "
                           << objectClass->getName();
            }
            else
            {
                result = objectCache[objectClass->getId()]->clone(d.name);
                ARMARX_LOG << "Using manipulation object " << result->getName() << " of class "
                           << objectClass->getName() << " from cache";
            }
        }
        catch (...)
        {
            ARMARX_WARNING << "Loading object " << d.objectClass->getName() << "failed";
            return result;
        }
 
 
        ARMARX_INFO << "Loaded object " << d.name;
 
        SoSeparator* sep = new SoSeparator;
        layer.mainNode->addChild(sep);
 
        SoMaterial* m = new SoMaterial;
        m->setOverride(false);
        sep->addChild(m);
 
        auto objVisu = result->getVisualization<CoinVisualization>(VirtualRobot::SceneObject::Full);
        SoNode* sepo = objVisu->getCoinVisualization();
        sep->addChild(sepo);
 
        activeObjects[entryName] = result;
        layer.addedCustomVisualizations[d.name] = sep;
 
        return result;
    }
 
    void
    EntityDrawerComponent::onRemoveAccumulatedData(const std::string& layerName)
    {
        auto lockData = getScopedAccumulatedDataLock();
        std::string entryName = "__" + layerName + "__";
        {
            auto i1 = accumulatedCustomUpdateData.entities.begin();
 
            while (i1 != accumulatedCustomUpdateData.entities.end())
            {
                if (simox::alg::starts_with(i1->first, entryName))
                {
                    i1 = accumulatedCustomUpdateData.entities.erase(i1);
                }
                else
                {
                    i1++;
                }
            }
        }
    }
 
} // namespace memoryx