ObjectInstanceSegmentWrapper.cpp

Go to the documentation of this file.

#include "ObjectInstanceSegmentWrapper.h"
 
// Factory shenanigans: Do not remove, these are required
// and runtime errors will be thrown if they are not included
#include <VirtualRobot/ManipulationObject.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
 
namespace memoryx
{
 
    void
    ObjectInstanceSegmentWrapper::initFromProxies(
        PriorKnowledgeInterfacePrx const& priorKnowledge,
        WorkingMemoryInterfacePrx const& workingMemory,
        armarx::RobotStateComponentInterfacePrx const& robotStateComponent)
    {
        ARMARX_CHECK_NOT_NULL(workingMemory);
        ARMARX_CHECK_NOT_NULL(priorKnowledge);
        ARMARX_CHECK_NOT_NULL(robotStateComponent);
 
        objectClassSegment.initFromProxy(priorKnowledge, {});
 
        this->robotStateComponent = robotStateComponent;
        sharedRobot = robotStateComponent->getSynchronizedRobot();
        localRobot = armarx::RemoteRobot::createLocalClone(sharedRobot);
        objectInstancesSegment = workingMemory->getObjectInstancesSegment();
    }
 
    std::vector<ObjectInstanceWrapper>
    ObjectInstanceSegmentWrapper::queryObjects(std::set<std::string> const& ids)
    {
        memoryx::EntityBaseList entities = objectInstancesSegment->getAllEntities();
        std::vector<memoryx::ObjectInstancePtr> objectInstances;
        for (auto& entity : entities)
        {
            memoryx::ObjectInstancePtr object = memoryx::ObjectInstancePtr::dynamicCast(entity);
            if (object)
            {
                if (ids.empty() || ids.count(object->getId()))
                {
                    objectInstances.push_back(object);
                }
            }
        }
 
        std::vector<ObjectInstanceWrapper> result;
        for (auto& objectInstance : objectInstances)
        {
            std::string objectName = objectInstance->getName();
 
            // TODO: Only synchronize if the timestamp is new
            if (objectInstance->hasLocalizationTimestamp())
            {
                IceUtil::Time localizationTimestamp = objectInstance->getLocalizationTimestamp();
                armarx::RemoteRobot::synchronizeLocalCloneToTimestamp(
                    localRobot, robotStateComponent, localizationTimestamp.toMicroSeconds());
            }
            else
            {
                armarx::RemoteRobot::synchronizeLocalClone(localRobot, sharedRobot);
            }
            Eigen::Matrix4f globalPose = objectInstance->getPose()->toGlobalEigen(localRobot);
 
            std::string objectClass = objectInstance->getMostProbableClass();
            auto entry = objectClassSegment.getClass(objectClass);
            if (!entry)
            {
                ARMARX_WARNING << deactivateSpam(1) << "Could not find object class '"
                               << objectClass << "'."
                               << " Skipping object '" << objectName << "'.";
                continue;
            }
 
            // This operation can be quite expensive (~ 300ms for 10 objects).
            VirtualRobot::ManipulationObjectPtr manipulationObject =
                entry->manipulationObject->clone(objectName);
            manipulationObject->setGlobalPose(globalPose);
 
            ObjectInstanceWrapper object;
            object.instanceInMemory = objectInstance;
            object.manipulationObject = manipulationObject;
            result.push_back(object);
        }
 
        return result;
    }
 
} // namespace memoryx