SimpleEpisodicMemoryWorkingMemoryConnector.cpp

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::ArmarXObjects::SimpleEpisodicMemoryWorkingMemoryConnector
 * @author     fabian.peller-konrad@kit.edu ( fabian dot peller-konrad at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SimpleEpisodicMemoryWorkingMemoryConnector.h"
 
#include <ArmarXCore/util/json/JSONObject.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/memorytypes/entity/Relation.h>
 
namespace memoryx
{
 
    armarx::PropertyDefinitionsPtr
    SimpleEpisodicMemoryWorkingMemoryConnector::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def{
            new armarx::ComponentPropertyDefinitions{getConfigIdentifier()}};
 
        def->topic<WorkingMemoryListenerInterface>("WorkingMemory", "WorkingMemoryName");
        def->optional(minOrientationDistance,
                      "MinOrientationDistance",
                      "Min Distance an object has to rotate until the memory receives an update");
        def->optional(minPositionDistance,
                      "MinPositrionDistance",
                      "Min Distance an object has to move until the memory receives an update");
        return def;
    }
 
    SimpleEpisodicMemoryWorkingMemoryConnector::SimpleEpisodicMemoryWorkingMemoryConnector() :
        minOrientationDistance(0.25 * M_PI), minPositionDistance(100)
    {
    }
 
    std::string
    SimpleEpisodicMemoryWorkingMemoryConnector::getDefaultName() const
    {
        return "SimpleEpisodicMemoryWorkingMemoryConnector";
    }
 
    void
    SimpleEpisodicMemoryWorkingMemoryConnector::onInitComponent()
    {
        usingProxy(m_simple_episodic_memory_proxy_name);
        usingProxy("WorkingMemory");
        usingProxy("RobotStateComponent");
    }
 
    void
    SimpleEpisodicMemoryWorkingMemoryConnector::onConnectComponent()
    {
        getProxy(m_simple_episodic_memory, m_simple_episodic_memory_proxy_name);
        memoryPrx = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
    }
 
    void
    SimpleEpisodicMemoryWorkingMemoryConnector::onDisconnectComponent()
    {
    }
 
    void
    SimpleEpisodicMemoryWorkingMemoryConnector::onExitComponent()
    {
    }
 
    void
    SimpleEpisodicMemoryWorkingMemoryConnector::reportEntityCreated(
        const std::string& segmentName,
        const memoryx::EntityBasePtr& newEntity,
        const Ice::Current&)
    {
        if (segmentName != "objectInstances")
        {
            return;
        }
 
        EntityPtr entity = EntityPtr::dynamicCast(newEntity);
 
        //ARMARX_DEBUG << "Entity created in segment " << segmentName << ": " << entity;
        std::map<int, memoryx::EntityBasePtr> entitiesForSegment = entityMap[segmentName];
        entitiesForSegment[std::stoi(entity->getId())] = entity;
 
        EntityAttributePtr pos = EntityAttributePtr::dynamicCast(entity->getAttribute("position"));
        armarx::VariantPtr posValue = armarx::VariantPtr::dynamicCast(pos->getValue());
        auto framedPos = posValue->getClass<armarx::FramedPositionBase>();
 
        memoryx::ObjectPoseEvent o;
        o.objectName = entity->getName();
        o.x = framedPos->x;
        o.y = framedPos->y;
        o.z = framedPos->z;
        o.frame = framedPos->frame;
        o.type = memoryx::ObjectPoseEventType::NEW_OBJECT_RECOGNIZED;
        o.receivedInMs = IceUtil::Time::now().toMilliSecondsDouble();
        m_simple_episodic_memory->registerObjectPoseEvent(o);
    }
 
    void
    SimpleEpisodicMemoryWorkingMemoryConnector::reportEntityUpdated(
        const std::string& segmentName,
        const memoryx::EntityBasePtr& entityOld,
        const memoryx::EntityBasePtr& entityNew,
        const Ice::Current&)
    {
        //ARMARX_DEBUG << "Entity changed: ";
        //ARMARX_DEBUG << "Old: " << EntityPtr::dynamicCast(entityOld);
        //ARMARX_DEBUG << "New: " << EntityPtr::dynamicCast(entityNew);
        if (segmentName != "objectInstances")
        {
            return;
        }
 
        EntityPtr prevEntity = EntityPtr::dynamicCast(entityOld);
        EntityPtr currEntity = EntityPtr::dynamicCast(entityNew);
 
        EntityAttributePtr prevPos =
            EntityAttributePtr::dynamicCast(prevEntity->getAttribute("position"));
        armarx::VariantPtr prevPosValue = armarx::VariantPtr::dynamicCast(prevPos->getValue());
        auto prevFramedPos = prevPosValue->getClass<armarx::FramedPositionBase>();
 
        EntityAttributePtr currPos =
            EntityAttributePtr::dynamicCast(currEntity->getAttribute("position"));
        armarx::VariantPtr currPosValue = armarx::VariantPtr::dynamicCast(currPos->getValue());
        auto currFramedPos = currPosValue->getClass<armarx::FramedPositionBase>();
 
        Eigen::Vector3f prevPosVector(prevFramedPos->x, prevFramedPos->y, prevFramedPos->z);
        Eigen::Vector3f currPosVector(currFramedPos->x, currFramedPos->y, currFramedPos->z);
 
        float distance = (currPosVector - prevPosVector).norm();
 
        if (distance > minPositionDistance /* && rotation */)
        {
            // update entity
            std::map<int, memoryx::EntityBasePtr> entitiesForSegment = entityMap[segmentName];
            entitiesForSegment[std::stoi(currEntity->getId())] = currEntity;
 
            memoryx::ObjectPoseEvent o;
            o.objectName = currEntity->getName();
            o.x = currPosVector(0);
            o.y = currPosVector(1);
            o.z = currPosVector(2);
            o.type = memoryx::ObjectPoseEventType::OBJECT_POSE_UPDATE;
            o.receivedInMs = IceUtil::Time::now().toMilliSecondsDouble();
            m_simple_episodic_memory->registerObjectPoseEvent(o);
        }
    }
 
    void
    SimpleEpisodicMemoryWorkingMemoryConnector::reportEntityRemoved(
        const std::string& segmentName,
        const memoryx::EntityBasePtr& entity,
        const Ice::Current&)
    {
        ARMARX_DEBUG << "Entity removed: id = " << entity->getId();
    }
 
    void
    SimpleEpisodicMemoryWorkingMemoryConnector::reportSnapshotLoaded(const std::string& segmentName,
                                                                     const Ice::Current&)
    {
        ARMARX_DEBUG << "Snapshot loaded!";
    }
 
    void
    SimpleEpisodicMemoryWorkingMemoryConnector::reportSnapshotCompletelyLoaded(
        const Ice::Current& c)
    {
        ARMARX_DEBUG << "Snapshot completely loaded!";
    }
 
    void
    SimpleEpisodicMemoryWorkingMemoryConnector::reportMemoryCleared(const std::string& segmentName,
                                                                    const Ice::Current&)
    {
        ARMARX_DEBUG << "Scene cleared!";
    }
 
} // namespace memoryx