WorkingMemoryExample.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::WorkingMemoryExample
* @author     (kozlov at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "WorkingMemoryExample.h"
 
#include <RobotAPI/libraries/core/Pose.h>
 
#include <ArmarXCore/util/json/JSONObject.h>
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
 
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
 
#include <MemoryX/libraries/memorytypes/entity/Relation.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
 
#include <fstream>
#include <thread>
 
 
namespace memoryx
{
    void WorkingMemoryExample::onInitComponent()
    {
        std::cout << "onInitComponent" << std::endl;
 
        usingProxy("WorkingMemory");
        usingProxy("LongtermMemory");
        usingProxy("RobotStateComponent");
 
 
        // don't request topic here, see below why
        // usingTopic("WorkingMemoryUpdates");
 
        testToRun = getProperty<std::string>("TestToRun").getValue();
    }
 
 
    void WorkingMemoryExample::onConnectComponent()
    {
        std::cout << "onStartComponent" << std::endl;
 
        std::cout << "Starting WorkingMemoryExample" << std::endl;
 
        const Ice::CommunicatorPtr ic = getIceManager()->getCommunicator();
 
        memoryPrx = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
        longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>("LongtermMemory");
        robotStateComponentPrx = getProxy<armarx::RobotStateComponentInterfacePrx>("RobotStateComponent");
 
        // load snapshot, if specified
        const std::string snapshotName = getProperty<std::string>("SnapshotName").getValue();
        ARMARX_INFO << "Snapshot name: " << snapshotName;
 
        if (!snapshotName.empty())
        {
            longtermMemoryPrx->loadWorkingMemorySnapshot(snapshotName, memoryPrx);
            ARMARX_INFO << "Snapshot succcessfully loaded: " << snapshotName;
        }
        else
        {
            ARMARX_ERROR << "SnapshotName parameter must be specified!";
        }
 
        if (testToRun == "UpdateNotifications")
        {
            // Note, that all topics that have been spcified in onInitComponent are registered *after* the onConnectComponent method.
            // So for this special test case where the topic is neede within the onConnectComponent method we must subscribe "by hand" in this method
            // to be sure that the topic is available for our testcase.
            usingTopic("WorkingMemoryUpdates");
            testUpdateObserver();
        }
        else if (testToRun == "FileTransfer")
        {
            testFileTransfer();
        }
        else if (testToRun == "ObjectMove")
        {
            testObjectMovement();
        }
        else if (testToRun == "SnapshotLoad")
        {
            testSnapshots();
        }
        else if (testToRun == "CommonPlaces")
        {
            testCommonPlaces();
        }
        else if (testToRun == "Agent")
        {
            testAgent();
        }
        else
        {
            std::cout << "Unknown test name: " << testToRun << std::endl;
        }
    }
 
    void WorkingMemoryExample::reportEntityCreated(const std::string& segmentName, const memoryx::EntityBasePtr& entity,
            const Ice::Current&)
    {
        std::cout << "Entity created: " << EntityPtr::dynamicCast(entity) << std::endl << std::endl;
    }
 
    void WorkingMemoryExample::reportEntityUpdated(const std::string& segmentName,
            const memoryx::EntityBasePtr& entityOld,
            const memoryx::EntityBasePtr& entityNew, const Ice::Current&)
    {
        std::cout << "Entity changed: " << std::endl;
        std::cout << "Old: " << EntityPtr::dynamicCast(entityOld) << std::endl;
        std::cout << "New: " << EntityPtr::dynamicCast(entityNew) << std::endl << std::endl;
    }
 
    void WorkingMemoryExample::reportEntityRemoved(const std::string& segmentName, const memoryx::EntityBasePtr& entity,
            const Ice::Current&)
    {
        std::cout << "Entity removed: id = " << entity->getId() << std::endl << std::endl;
    }
 
    void WorkingMemoryExample::reportSnapshotLoaded(const std::string& segmentName, const Ice::Current&)
    {
        std::cout << "Snapshot loaded!" << std::endl << std::endl;
    }
 
    void WorkingMemoryExample::reportMemoryCleared(const std::string& segmentName, const Ice::Current&)
    {
        std::cout << "Scene cleared!" << std::endl << std::endl;
    }
 
    void WorkingMemoryExample::testFileTransfer()
    {
        ObjectInstanceMemorySegmentBasePrx objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
        GridFileManagerPtr fileManager(new GridFileManager(memoryPrx->getCommonStorage()));
 
        int ivCount = 0;
        size_t totalBytes = 0;
        EntityIdList ids = objectInstancesMemoryPrx->getAllEntityIds();
 
        for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
        {
            const EntityBasePtr obj = objectInstancesMemoryPrx->getEntityById(*it);
 
            if (obj->hasAttribute("IvFile"))
            {
 
                std::ifstream fs;
                fileManager->getFileStream(obj->getAttribute("IvFile"), fs);
 
                char ivFile[128];
                fs.getline(ivFile, 128);
 
                std::cout << ivFile << std::endl;
 
                ivCount++;
                //            totalBytes += datalen;
            }
        }
 
        std::cout << "# of fetched iv-files: " << ivCount << std::endl;
        std::cout << "Bytes transfered: " << totalBytes << std::endl;
    }
 
    void WorkingMemoryExample::testUpdateObserver()
    {
        const Ice::CommunicatorPtr ic = getIceManager()->getCommunicator();
        ObjectInstanceMemorySegmentBasePrx objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
        RelationMemorySegmentBasePrx relationsMemoryPrx = memoryPrx->getRelationsSegment();
 
        ObjectInstancePtr cup = new ObjectInstance("cup1");
        cup->putAttribute("Weight", 0.3f, new UnivariateNormalDistribution(0.3f, 0.5f));
        cup->putAttribute("Description", "my favourite cup");
 
        cup->addClass("Cup", 0.6f);
        cup->addClass("Mug", 0.4f);
        std::string cupId = objectInstancesMemoryPrx->addEntity(cup);
 
        cup->setId(cupId);
        std::cout << "Stored object " << cup->getName() << " in WorkingMemory with id = " << cupId << std::endl;
        std::cout << "Most probable class is <" << cup->getMostProbableClass() <<
                  "> with prob = " << cup->getClassProbability(cup->getMostProbableClass()) << std::endl;
 
        Eigen::Vector3f posVec(1., 2., 3.);
        armarx::FramedPositionPtr curPos = new armarx::FramedPosition(posVec, armarx::GlobalFrame, "");
        cup->setPosition(curPos);
 
        FloatVector posStdVec(posVec.data(), posVec.data() + 3);
        MultivariateNormalDistributionPtr posDist = new MultivariateNormalDistribution(3);
        posDist->setMean(posStdVec);
        posDist->setCovariance(0, 0, 0.5);
        posDist->setCovariance(1, 1, 0.5);
        posDist->setCovariance(2, 2, 0.5);
        cup->setPositionUncertainty(posDist);
 
        objectInstancesMemoryPrx->updateEntity(cupId, cup);
        std::cout << "Updated object " << cup->getName() << " in WorkingMemory with id = " << cupId << std::endl;
 
        ObjectInstancePtr cupClone = ObjectInstancePtr::dynamicCast(cup->ice_clone());
        std::cout << "Cloned object: " << cupClone <<  std::endl;
 
        ObjectInstancePtr fridge = new ObjectInstance("Fridge");
        fridge->setClass("Fridge", 1.f);
        const std::string fridgeId = objectInstancesMemoryPrx->addEntity(fridge);
 
        std::cout << "Stored object " << fridge->getName() << " in WorkingMemory with id = " << fridgeId << std::endl;
 
        EntityPtr juice = new ObjectInstance("Juice");
        const std::string juiceId = objectInstancesMemoryPrx->addEntity(juice);
 
        std::cout << "Stored object " << juice->getName() << " in WorkingMemory with id = " << juiceId << std::endl;
 
        EntityRefBasePtr cupRef = objectInstancesMemoryPrx->getEntityRefById(cupId);
        ARMARX_CHECK_EXPRESSION(cupRef->entityId == cupId);
        EntityRefBasePtr fridgeRef = objectInstancesMemoryPrx->getEntityRefById(fridgeId);
        EntityRefBasePtr juiceRef = objectInstancesMemoryPrx->getEntityRefById(juiceId);
 
        RelationBasePtr rel1 = new Relation("isIn", {cupRef, fridgeRef});
        relationsMemoryPrx->addEntity(rel1);
 
        RelationBasePtr rel2 = new Relation("isIn", {juiceRef, fridgeRef}, true, 0.7);
        relationsMemoryPrx->addEntity(rel2);
 
        RelationList rels = relationsMemoryPrx->getRelationsByEntityId(cupRef->entityId);
        std::cout << "Found relations: " << rels.size() << std::endl;
 
        for (RelationList::const_iterator it = rels.begin(); it != rels.end(); ++it)
        {
            RelationBasePtr relation = *it;
            std::cout << relation->getName() << ": " << std::endl;
 
            for (EntityRefBasePtr entityRef : relation->getEntities())
            {
                std::cout << " * " << entityRef->entityName << std::endl;
            }
        }
 
 
        memoryPrx->print();
 
        //            memoryPrx->saveSnapshot("snapshot1");
 
        //            memoryPrx->removeObject(fridgeId);
 
        /*            EntityBasePtr cup2 = memoryPrx->getObjectById(cupId);
        //            SceneEntityPtr cup22 = SceneEntityPtr::dynamicCast(cup2);
 
                    armarx::JSONObjectPtr jsonSerializer = new armarx::JSONObject(ic);
                    cup2->serialize(jsonSerializer);
                    const std::string jsonString = jsonSerializer->asString(true);
        //            std::cout << "JSON for cup: " << jsonString << std::endl << std::endl;
 
                    armarx::JSONObjectPtr jsonDeserializer = new armarx::JSONObject(ic);
                    jsonDeserializer->fromString(jsonString);
                    EntityBasePtr newCup = new ObjectInstance();
                    newCup->deserialize(jsonDeserializer);
 
                    jsonSerializer = new armarx::JSONObject(ic);
                    newCup->serialize(jsonSerializer);*/
        //            std::cout << "cup deserialized from JSON: " << jsonSerializer->toString() << std::endl << std::endl;
 
        //            memoryPrx->removeAll();
        //            memoryPrx->saveSnapshot("db_alex.objects");
 
        //        memoryPrx->printMemory();
    }
 
    void WorkingMemoryExample::testObjectMovement()
    {
        ObjectInstanceMemorySegmentBasePrx objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
 
        ObjectInstancePtr cup = new ObjectInstance("cup");
        cup->setClass("cup", 1.f);
 
        Eigen::Vector3f posVec(-5000., 2500, 1000.);
        armarx::FramedPositionPtr curPos = new armarx::FramedPosition(posVec, armarx::GlobalFrame, "");
        cup->setPosition(curPos);
 
        FloatVector posStdVec(posVec.data(), posVec.data() + 3);
        MultivariateNormalDistributionPtr posDist = new MultivariateNormalDistribution(3);
        posDist->setMean(posStdVec);
        posDist->setCovariance(0, 0, 80.);
        posDist->setCovariance(1, 1, 20.);
        posDist->setCovariance(2, 2, 200.);
        cup->setPositionUncertainty(posDist);
 
        Eigen::Quaternionf quat(0., 0., 0., 1.);
        armarx::FramedOrientationBasePtr orient = new armarx::FramedOrientation(quat.toRotationMatrix(), armarx::GlobalFrame, "");
        cup->setOrientation(orient);
 
        cup->putAttribute("Weight", 0.3f, new UnivariateNormalDistribution(0.3f, 0.01f));
 
        const std::string cupId = objectInstancesMemoryPrx->addEntity(cup);
        cup->setId(cupId);
 
        memoryPrx->print();
 
        // this attribute, while not present in current observation,
        // must be copied from old object instance by AttributeReplacementFusion
        cup->removeAttribute("Weight");
 
        std::this_thread::sleep_for(std::chrono::milliseconds(3000));
 
        for (int i = 0; i < 500; ++i)
        {
            curPos->x += 10;
            posStdVec[0] = curPos->x;
            posDist->setMean(posStdVec);
            cup->setPosition(curPos);
            cup->setPositionUncertainty(posDist);
            cup->setExistenceCertainty(i / 500.);
            objectInstancesMemoryPrx->updateEntity(cupId, cup);
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            ARMARX_INFO_S << "Moving Object to " << curPos->toEigen();
        }
 
        std::this_thread::sleep_for(std::chrono::milliseconds(10000));
        objectInstancesMemoryPrx->removeEntity(cupId);
    }
 
    void WorkingMemoryExample::testSnapshots()
    {
        ARMARX_INFO << "Content of memory after loading snapshot: ";
        memoryPrx->print();
    }
 
    void WorkingMemoryExample::testCommonPlaces()
    {
        ObjectInstanceMemorySegmentBasePrx objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
 
        const std::string snapshotName = getProperty<std::string>("CommonPlacesSnapshot").getValue();
        WorkingMemorySnapshotInterfacePrx snapshot = longtermMemoryPrx->getWorkingMemorySnapshotListSegment()->openSnapshot(snapshotName);
        PersistentEntitySegmentBasePrx segObjects = snapshot->getSegment("objectInstances");
 
        if (segObjects)
        {
            EntityIdList ids = segObjects->getAllEntityIds();
 
            for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
            {
                objectInstancesMemoryPrx->addEntity(segObjects->getEntityById(*it));
            }
        }
    }
 
    void WorkingMemoryExample::testAgent()
    {
        AgentInstancesSegmentBasePrx agentsMemoryPrx = memoryPrx->getAgentInstancesSegment();
 
        Ice::CommunicatorPtr iceCommunicator = getIceManager()->getCommunicator();
        Eigen::Vector3f positionVector(0.0, 0.0, 0.0);
        armarx::FramedPositionPtr currentPosition = new armarx::FramedPosition(positionVector, armarx::GlobalFrame, "");
        Eigen::Quaternionf quaternion(0.0, -1.0, 0.0, 1.0);
        quaternion.normalize();
        armarx::FramedOrientationBasePtr currentOrientation = new armarx::FramedOrientation(quaternion.toRotationMatrix(), armarx::GlobalFrame, "");
 
        auto robot = robotStateComponentPrx->getSynchronizedRobot();
        AgentInstancePtr agent = new AgentInstance(robot->getName());
        agent->setSharedRobot(robot);
        agent->setStringifiedSharedRobotInterfaceProxy(iceCommunicator->proxyToString(robot));
        agent->setAgentFilePath("Armar3/robotmodel/ArmarIII.xml");
        agent->setOrientation(currentOrientation);
        agent->setPosition(currentPosition);
 
        std::string agentId = agentsMemoryPrx->addEntity(agent);
 
        std::cout << "Agent Armar3 created in AgentInstancesSegment of WorkingMemory" << std::endl;
 
        std::this_thread::sleep_for(std::chrono::milliseconds(3000));
 
        for (int i = 0; i < 500; ++i)
        {
            currentPosition->x += 10;
            agent->setPosition(currentPosition);
            agentsMemoryPrx->updateEntity(agentId, agent);
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }
 
    void memoryx::WorkingMemoryExample::reportSnapshotCompletelyLoaded(const Ice::Current& c)
    {
        ARMARX_INFO << "Snapshot completely loaded!";
    }
 
}