WorkingMemoryEntitySegment.h

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* 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::WorkingMemory
* @author     ALexey Kozlov ( kozlov at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <MemoryX/core/entity/Entity.h>
#include <MemoryX/core/entity/EntityRef.h>
#include <MemoryX/libraries/workingmemory/fusion/EntityFusionMethod.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/core/memory/SegmentUtilImplementations.h>
#include <MemoryX/core/memory/SegmentedMemory.h>
#include <ArmarXCore/util/json/JSONObject.h>
 
#include <Ice/ObjectAdapter.h>
 
namespace memoryx
{
 
    /*! This class represents segments in the \ref WorkingMemory
     *
     */
    template <class T>
    class ARMARXCOMPONENT_IMPORT_EXPORT WorkingMemoryEntitySegment :
        virtual public WorkingMemoryEntitySegmentBase,
        public virtual SegmentUtilImplementations
    {
        friend class SegmentedMemory;
 
    public:
        WorkingMemoryEntitySegment():
            lastEntityId(0)
        {
            this->objectTypeId = T::ice_staticId();
            //            this->fusionMethod = fusionMethod ? fusionMethod : new AttributeEnrichementFusion();
        }
 
        /**
         * @brief getObjectTypeId returns the ice_staticId() of the entities stored inside this WorkingMemorySegment
         * @return
         */
        std::string getObjectTypeId(const ::Ice::Current& c = Ice::emptyCurrent) const override
        {
            return objectTypeId;
        }
 
        void addFusionMethod(const ::memoryx::EntityFusionMethodBasePtr& fusionMethod, const ::Ice::Current& c = Ice::emptyCurrent) override
        {
            if (!fusionMethod)
            {
                throw armarx::LocalException("FusionMethod Ptr null");
            }
 
            fusionMethods.push_back(fusionMethod);
        }
 
    protected:
        void fuseEntity(const EntityBasePtr& entity)
        {
            // check whether entity is not null
            if (!entity)
            {
                throw InvalidEntityException("entity is null");
            }
 
            // check whether entity is of correct type
            if (!entity->ice_isA(objectTypeId))
            {
                throw InvalidEntityException("invalid object id");
            }
 
            // init fusion method(s)
            EntityBasePtr newEntity = entity;
 
            for (FusionMethodList::iterator it = fusionMethods.begin(); it != fusionMethods.end(); ++it)
            {
                newEntity = (*it)->initEntity(newEntity);
            }
 
        }
 
        /**
         * @brief adds the entity to the segment.
         * @note Does not lock the segment! use AddEntity instead.
         * @param newEntity
         * @return id of new entity
         * @see addEntity
         */
        std::string addEntityThreadUnsafe(const EntityBasePtr& newEntity)
        {
 
            // add to memory
            std::string id = newEntity->getId();
            {
 
 
                // this dirty hack is needed to preserve ids when loading snapshots:
                // - if entity has an ID set, use it and adjust lastEntityId accordingly (entity from snapshot)
                // - otherwise, use lastEntityId+1 as new ID (normal case)
                if (id.empty() || entityMap.find(id) != entityMap.end())
                {
                    ++lastEntityId;
                    id = std::to_string(lastEntityId);
                    newEntity->setId(id);
                }
                else
                {
                    try
                    {
                        const int intId = std::stoi(id);
 
                        if (lastEntityId < intId)
                        {
                            lastEntityId = intId;
                        }
                    }
                    catch (std::exception const&) {}
                }
 
                entityMap[id] = newEntity;
 
                // inform listeners
                if (listenerProxy)
                {
                    listenerProxy->reportEntityCreated(segmentName, newEntity);
                }
            }
 
            // return id of created object
            return id;
        }
 
 
    public:
        /*!
         * \brief addEntity addes an entity to this segment. The registered fusion methods are applied in order to fuse the entity entry.
         *
         * When ObjectInstances are used as template parameter the \see PriorAttributeEnrichmentFusion is used to enrich the
         * attributes by prior knowledge data (i.e. 3D models etc)
         *
         * \param entity The entity to add. The attributes of the added entry may be enriched by fusion methods (e.g. PriorAttributeEnrichmentFusion)
         * \return The id of the newly created entry.
         */
        std::string addEntity(const EntityBasePtr& entity, const ::Ice::Current& c = Ice::emptyCurrent) override
        {
            fuseEntity(entity);
 
            std::scoped_lock<std::mutex> lock(mutex);
            return addEntityThreadUnsafe(entity);
        }
 
 
 
        EntityIdList addEntityList(const EntityBaseList& entityList, const ::Ice::Current& c) override
        {
            EntityIdList entityIds(entityList.size(), "");
 
            for (size_t i = 0; i < entityList.size(); i++)
            {
                try
                {
                    entityIds[i] = addEntity(entityList[i], c);
                }
                catch (const memoryx::InvalidEntityException& e)
                {
                    // don't handle this exception but return an empty string as ID for this specific entity
                }
            }
 
            return entityIds;
        }
 
 
        std::string upsertEntity(const std::string& entityId, const EntityBasePtr& newEntity, const ::Ice::Current& c = Ice::emptyCurrent) override
        {
            std::scoped_lock<std::mutex> lock(mutex);
            if (hasEntityByIdThreadUnsafe(entityId))
            {
                updateEntityThreadUnsafe(entityId, newEntity, Ice::StringSeq());
                return entityId;
            }
            else
            {
                return addEntityThreadUnsafe(newEntity);
            }
        }
 
 
        EntityIdList upsertEntityList(const EntityBaseList& entityList, const ::Ice::Current& c) override
        {
            EntityIdList entityIds(entityList.size(), "");
 
            for (size_t i = 0; i < entityList.size(); i++)
            {
                try
                {
                    entityIds[i] = upsertEntity(entityList[i]->getId(), entityList[i], c);
                }
                catch (const memoryx::InvalidEntityException& e)
                {
                    // don't handle this exception but return an empty string as ID for this specific entity
                }
            }
 
            return entityIds;
        }
 
 
        std::string upsertEntityByName(const std::string& entityName, const EntityBasePtr& newEntity, const ::Ice::Current& c = Ice::emptyCurrent) override
        {
            std::scoped_lock<std::mutex> lock(mutex);
            auto oldEntity = getEntityByNameThreadUnsafe(entityName);
            if (oldEntity)
            {
                updateEntityThreadUnsafe(oldEntity->getId(), newEntity, Ice::StringSeq());
                return oldEntity->getId();
            }
            else
            {
                return addEntityThreadUnsafe(newEntity);
            }
        }
 
 
 
        void updateEntity(const std::string& entityId, const EntityBasePtr& update, const ::Ice::Current& c = Ice::emptyCurrent) override
        {
            updateEntity(entityId, update, Ice::StringSeq(), c);
        }
 
 
        void updateEntity(const std::string& entityId, const EntityBasePtr& update, const Ice::StringSeq& deactivatedFusionMethods, const ::Ice::Current& c = Ice::emptyCurrent)
        {
            std::scoped_lock<std::mutex> lock(mutex);
            updateEntityThreadUnsafe(entityId, update, deactivatedFusionMethods);
        }
    protected:
 
        void updateEntityThreadUnsafe(const std::string& entityId, const EntityBasePtr& update, const Ice::StringSeq& deactivatedFusionMethods)
        {
 
            // check whether entity is valid
            if (!update)
            {
                ARMARX_FATAL_S << "invalid update";
                throw InvalidEntityException();
            }
 
            // check whether id is known
            EntityBasePtr oldEntity = getEntityByIdUnsafe(entityId);
 
            if (!oldEntity)
            {
                throw EntityNotFoundException();
            }
 
            // assure same name
            if (update->getName() != oldEntity->getName())
            {
                ARMARX_FATAL_S << "Entity names do not match: New name = '" << update->getName() << "', Old name = '" << oldEntity->getName() << "', Entity id = " << entityId;
                throw InvalidEntityException();
            }
 
            // assure correct id
            update->setId(entityId);
 
            // call fusion method(s)
            EntityBasePtr newEntity = update;
 
 
            for (FusionMethodList::iterator it = fusionMethods.begin(); it != fusionMethods.end(); ++it)
            {
                if (std::find(deactivatedFusionMethods.begin(), deactivatedFusionMethods.end(), (*it)->getMethodName()) == deactivatedFusionMethods.end())
                {
                    newEntity = (*it)->fuseEntity(oldEntity, newEntity);
                }
            }
 
            entityMap[entityId] = newEntity;
 
            // inform listener
            if (listenerProxy)
            {
                listenerProxy->reportEntityUpdated(segmentName,
                                                   EntityPtr::dynamicCast(oldEntity->ice_clone()), // not safe if not cloned!
                                                   EntityPtr::dynamicCast(newEntity->ice_clone()));
            }
 
        }
    public:
 
        void removeEntity(const ::std::string& id, const ::Ice::Current& c = Ice::emptyCurrent) override
        {
            std::scoped_lock<std::mutex> lock(mutex);
            IdEntityMap::iterator it = entityMap.find(id);
 
            if (it == entityMap.end())
            {
                throw EntityNotFoundException();
            }
 
            EntityBasePtr entity = it->second;
            entityMap.erase(it);
 
            if (listenerProxy)
            {
                listenerProxy->reportEntityRemoved(segmentName, entity);
            }
        }
 
        void removeAllEntities(const ::Ice::Current& c = Ice::emptyCurrent) override
        {
            clear(c);
        }
 
        void setEntityAttribute(const std::string& entityId, const EntityAttributeBasePtr& attribute, const ::Ice::Current& c) override
        {
            std::scoped_lock<std::mutex> lock(mutex);
            IdEntityMap::iterator it = entityMap.find(entityId);
 
            if (it == entityMap.end())
            {
                throw EntityNotFoundException();
            }
 
            it->second->putAttribute(attribute);
        }
 
        void setEntityAttributes(const std::string& entityId, const EntityAttributeList& attributeMap, const ::Ice::Current& c) override
        {
            std::scoped_lock<std::mutex> lock(mutex);
            IdEntityMap::iterator it = entityMap.find(entityId);
 
            if (it == entityMap.end())
            {
                throw EntityNotFoundException();
            }
 
            for (auto attribute : attributeMap)
            {
                it->second->putAttribute(attribute);
            }
        }
 
        void clear(const ::Ice::Current& c = Ice::emptyCurrent) override
        {
            {
                std::scoped_lock<std::mutex> lock(mutex);
 
                for (auto entityMapIter = entityMap.begin(); entityMapIter != entityMap.end();)
                {
                    EntityBasePtr entity = entityMapIter->second;
 
                    if (listenerProxy)
                    {
                        listenerProxy->reportEntityRemoved(segmentName, entity);
                    }
 
                    entityMapIter = entityMap.erase(entityMapIter);
                }
 
                lastEntityId = 0;
            }
 
            if (listenerProxy)
            {
                listenerProxy->reportMemoryCleared(segmentName);
            }
        }
 
        Ice::Int size(const ::Ice::Current& c = Ice::emptyCurrent) const override
        {
            std::scoped_lock<std::mutex> lock(mutex);
            return (Ice::Int) entityMap.size();
        }
 
        bool hasEntityById(const std::string& entityId, const ::Ice::Current& c) const override
        {
            std::scoped_lock<std::mutex> lock(mutex);
            return hasEntityByIdThreadUnsafe(entityId);
        }
    protected:
        bool hasEntityByIdThreadUnsafe(const std::string& entityId) const
        {
            IdEntityMap::const_iterator it = entityMap.find(entityId);
            return it != entityMap.end();
        }
    public:
 
        bool hasEntityByName(const std::string& entityName, const ::Ice::Current& c) const override
        {
            std::scoped_lock<std::mutex> lock(mutex);
 
            for (const auto& entity : entityMap)
            {
                if (entity.second && entity.second->getName() == entityName)
                {
                    return true;
                }
            }
 
            return false;
        }
 
 
 
        EntityBasePtr getEntityById(const ::std::string& id) const
        {
            std::scoped_lock<std::mutex> lock(mutex);
            return getEntityByIdUnsafe(id);
        }
 
        EntityBasePtr getEntityById(const ::std::string& id, const ::Ice::Current& c) const override
        {
            std::scoped_lock<std::mutex> lock(mutex);
            IdEntityMap::const_iterator it = entityMap.find(id);
 
            if (it != entityMap.end())
            {
                // clone before return it because it could be manipulated afterwards
                return EntityBasePtr::dynamicCast(it->second->ice_clone());
                //                return EntityBasePtr::dynamicCast(it->second);
            }
            else
            {
 
                return EntityBasePtr();
            }
        }
 
        EntityBasePtr getEntityByName(const ::std::string& name) const
        {
            std::scoped_lock<std::mutex> lock(mutex);
            return getEntityByNameThreadUnsafe(name);
        }
    protected:
        EntityBasePtr getEntityByNameThreadUnsafe(const ::std::string& name) const
        {
            for (IdEntityMap::const_iterator it = entityMap.begin(); it != entityMap.end(); ++it)
            {
 
                if (it->second->getName() == name)
                {
                    return EntityBasePtr::dynamicCast(it->second);
                }
            }
 
            std::string entities;
 
            for (auto e : entityMap)
            {
                entities += e.second->getName() + " ";
            }
 
            ARMARX_INFO_S << "Could not find entity named " << name << " in  entity list { " << entities << " }";
            return EntityBasePtr();
        }
    public:
 
        EntityBasePtr getEntityByName(const ::std::string& name, const ::Ice::Current& c) const override
        {
            std::scoped_lock<std::mutex> lock(mutex);
 
            for (IdEntityMap::const_iterator it = entityMap.begin(); it != entityMap.end(); ++it)
            {
                // clone before giving it to ice because it could be manipulated afterwards
                if (it->second->getName() == name)
                {
                    return EntityBasePtr::dynamicCast(it->second->ice_clone());
                }
 
                //                    return EntityBasePtr::dynamicCast(it->second);
            }
 
            std::string entities;
 
            for (auto e : entityMap)
            {
                entities += e.second->getName() + " ";
            }
 
            ARMARX_INFO_S << "Could not find entity named " << name << " in  entity list { " << entities << " }";
            return EntityBasePtr();
        }
 
 
        EntityBaseList getEntitiesByAttrValue(const ::std::string& attrName, const ::std::string& attrValue, const ::Ice::Current& c) const override
        {
            NameList attrValueList;
            attrValueList.push_back(attrValue);
            return getEntitiesByAttrValueList(attrName, attrValueList, c);
        }
 
        EntityBaseList getEntitiesByAttrValue(const ::std::string& attrName, const ::std::string& attrValue) const
        {
            NameList attrValueList;
            attrValueList.push_back(attrValue);
            return getEntitiesByAttrValueList(attrName, attrValueList);
        }
 
        EntityBaseList getEntitiesByAttrValueList(const ::std::string& attrName, const NameList& attrValueList) const
        {
            EntityBaseList result;
 
            std::scoped_lock<std::mutex> lock(mutex);
            for (IdEntityMap::const_iterator it = entityMap.begin(); it != entityMap.end(); ++it)
            {
                const EntityAttributeBasePtr attr = it->second->getAttribute(attrName);
 
                if (attr)
                {
                    for (NameList::const_iterator itVal = attrValueList.begin(); itVal != attrValueList.end(); ++itVal)
                    {
                        if (attr->hasValue(new armarx::Variant(*itVal)))
                        {
                            result.push_back(EntityBasePtr::dynamicCast(it->second));
                            break;
                        }
                    }
                }
            }
 
            return result;
        }
 
        EntityBaseList getEntitiesByAttrValueList(const ::std::string& attrName, const NameList& attrValueList, const ::Ice::Current& c) const override
        {
            EntityBaseList result;
 
            std::scoped_lock<std::mutex> lock(mutex);
 
            for (IdEntityMap::const_iterator it = entityMap.begin(); it != entityMap.end(); ++it)
            {
                const EntityAttributeBasePtr attr = it->second->getAttribute(attrName);
 
                if (attr)
                {
                    for (NameList::const_iterator itVal = attrValueList.begin(); itVal != attrValueList.end(); ++itVal)
                    {
                        if (attr->hasValue(new armarx::Variant(*itVal)))
                        {
                            // clone before giving it to ice because it could be manipulated afterwards
                            result.push_back(EntityBasePtr::dynamicCast(it->second->ice_clone()));
                            break;
                        }
                    }
                }
            }
 
            return result;
        }
 
        EntityIdList getAllEntityIds(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            EntityIdList result;
            std::scoped_lock<std::mutex> lock(mutex);
            for (IdEntityMap::const_iterator it = entityMap.begin(); it != entityMap.end(); ++it)
            {
                result.push_back(it->first);
            }
 
            return result;
        }
 
        EntityBaseList getAllEntities(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            EntityBaseList result;
            std::scoped_lock<std::mutex> lock(mutex);
            result.reserve(entityMap.size());
 
            for (const auto& entry : entityMap)
            {
                result.push_back(EntityBasePtr::dynamicCast(entry.second->ice_clone()));
            }
 
            return result;
        }
 
        IdEntityMap getIdEntityMap(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            IdEntityMap result;
            std::scoped_lock<std::mutex> lock(mutex);
 
            for (const auto& entry : entityMap)
            {
                result.insert({entry.first, EntityBasePtr::dynamicCast(entry.second->ice_clone())});
            }
 
            return result;
        }
 
        void print(const ::Ice::Current& c = Ice::emptyCurrent) const override
        {
            std::scoped_lock<std::mutex> lock(mutex);
 
            for (IdEntityMap::const_iterator it = entityMap.begin(); it != entityMap.end(); ++it)
            {
                ARMARX_INFO_S << EntityPtr::dynamicCast(it->second) << std::endl;
            }
        }
 
        std::string getSegmentName(const ::Ice::Current& c = Ice::emptyCurrent) const override
        {
            return segmentName;
        }
 
        /*!
         * \brief getListener The topic on which changes are reported.
         * \return NULL if disabled.
         */
        WorkingMemoryListenerInterfacePrx getListenerProxy(const ::Ice::Current& c = Ice::emptyCurrent) override
        {
            return this->listenerProxy;
        }
 
    protected:
        IdEntityMap entityMap;
        int lastEntityId;
 
    protected:
        // protected methods
        void setListenerProxy(const WorkingMemoryListenerInterfacePrx& listenerProxy, const ::Ice::Current& c = Ice::emptyCurrent) override
        {
            this->listenerProxy = listenerProxy;
        }
 
        // set segment name
        void setSegmentName(const std::string& segmentName, const ::Ice::Current& c = Ice::emptyCurrent) override
        {
            this->segmentName = segmentName;
        }
 
        void setParentMemory(const MemoryInterfacePtr& memory, const ::Ice::Current& c) override
        {
            this->parentMemory = memory;
        }
 
        EntityFusionMethodPtr fusionMethod;
        std::string segmentName;
        MemoryInterfacePtr parentMemory;
 
        // EntityMemorySegmentInterface interface
    public:
        EntityRefBasePtr getEntityRefById(const std::string& id, const Ice::Current& c) const override
        {
            auto entity = getEntityById(id, c);
 
            if (!entity)
            {
                return NULL;
            }
 
            auto proxy = EntityMemorySegmentInterfacePrx::uncheckedCast(c.adapter->createProxy(c.id));
            SegmentedMemoryPtr mem = SegmentedMemoryPtr::dynamicCast(parentMemory);
            std::string memName = mem->getMemoryName();
            auto memoryProxy = MemoryInterfacePrx::checkedCast(c.adapter->getCommunicator()->stringToProxy(memName));
            EntityRefPtr ref = new EntityRef(entity,
                                             segmentName,
                                             memName,
                                             memoryProxy,
                                             proxy);
            return ref;
        }
        EntityRefBasePtr getEntityRefByName(const std::string& name, const Ice::Current& c) const override
        {
            if (!c.adapter)
            {
                ARMARX_ERROR_S << "adapter is NULL";
            }
 
            auto entity = getEntityByName(name, c);
 
            if (!entity)
            {
                return NULL;
            }
 
            SegmentedMemoryPtr mem = SegmentedMemoryPtr::dynamicCast(parentMemory);
 
            if (!mem)
            {
                ARMARX_ERROR_S << "non mem ptr";
            }
 
 
            std::string memName = mem->getMemoryName();
            auto proxy = EntityMemorySegmentInterfacePrx::uncheckedCast(c.adapter->createProxy(c.id));
            auto memoryProxy = MemoryInterfacePrx::checkedCast(c.adapter->getCommunicator()->stringToProxy(memName));
            EntityRefPtr ref = new EntityRef(entity,
                                             segmentName,
                                             memName,
                                             memoryProxy,
                                             proxy);
            return ref;
        }
 
        // AbstractMemorySegment interface
    public:
        Ice::Identity getIceId(const ::Ice::Current& c) const override
        {
            Ice::Identity id;
            id.name = parentMemory->getMemoryName() + "_" + segmentName;
            return id;
        }
    protected:
        EntityBasePtr getEntityByIdUnsafe(const ::std::string& id) const
        {
            IdEntityMap::const_iterator it = entityMap.find(id);
 
            if (it != entityMap.end())
            {
                return EntityBasePtr::dynamicCast(it->second);
            }
            else
            {
 
                return EntityBasePtr();
            }
        }
    };
}