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 <Ice/ObjectAdapter.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/util/json/JSONObject.h>
 
#include <MemoryX/core/entity/Entity.h>
#include <MemoryX/core/entity/EntityRef.h>
#include <MemoryX/core/memory/SegmentUtilImplementations.h>
#include <MemoryX/core/memory/SegmentedMemory.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/libraries/workingmemory/fusion/EntityFusionMethod.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();
            }
        }
    };
} // namespace memoryx