PersistentObjectInstanceSegment.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     Kai Welke ( welke 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/system/ImportExportComponent.h>
 
#include <MemoryX/core/memory/PersistentEntitySegment.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/motionmodels/MotionModelObjectFactories.h>
 
#include <MemoryX/interface/core/EntityBase.h>
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
 
#include <vector>
 
namespace memoryx
{
    class ARMARXCOMPONENT_IMPORT_EXPORT PersistentObjectInstanceSegment :
        virtual public PersistentEntitySegment,
        virtual public PersistentObjectInstanceSegmentBase
    {
    public:
        PersistentObjectInstanceSegment(CollectionInterfacePrx entityCollection, Ice::CommunicatorPtr ic, bool useMongoIds = true) :
            PersistentEntitySegment(entityCollection, ic, useMongoIds), PersistentObjectInstanceSegmentBase()
        {
        }
 
        ObjectInstanceBasePtr getObjectInstanceById(const ::std::string& id, const ::Ice::Current& = Ice::emptyCurrent) const override
        {
            return ObjectInstanceBasePtr::dynamicCast(getEntityById(id));
        }
 
        ObjectInstanceBasePtr getObjectInstanceByName(const ::std::string& name, const ::Ice::Current& = Ice::emptyCurrent) const override
        {
            return ObjectInstanceBasePtr::dynamicCast(getEntityByName(name));
        }
 
        ObjectInstanceList getObjectInstancesByClass(const ::std::string& className, const ::Ice::Current& c = Ice::emptyCurrent) const override
        {
            ObjectInstanceList result;
            NameList classList;
            classList.push_back(className);
            return getObjectInstancesByClassList(classList, c);
        }
 
        ObjectInstanceList getObjectInstancesByClassList(const NameList& classList, const ::Ice::Current& = Ice::emptyCurrent) const override
        {
            ObjectInstanceList result;
            const EntityBaseList entityList = getEntitiesByAttrValueList("classes", classList);
 
            for (EntityBaseList::const_iterator it = entityList.begin(); it != entityList.end(); ++it)
            {
                const ObjectInstanceBasePtr instance = ObjectInstanceBasePtr::dynamicCast(*it);
 
                if (instance)
                {
                    result.push_back(instance);
                }
            }
 
            return result;
        }
 
        void setNewMotionModel(const ::std::string& entityId, const ::memoryx::MotionModelInterfacePtr& newMotionModel, const ::Ice::Current& = Ice::emptyCurrent) override
        {
            throw armarx::LocalException() << "This function does not work on the persistent instance segment since the motion model is only temporary!";
 
 
 
            ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(getObjectInstanceById(entityId));
            MotionModelInterfacePtr oldMotionModel = object->getMotionModel();
 
            if (oldMotionModel)
            {
                armarx::LinkedPoseBasePtr oldPose = oldMotionModel->getPoseAtLastLocalisation();
 
                if (oldPose)
                {
                    newMotionModel->setPoseAtLastLocalisation(oldPose, NULL, NULL);
                }
                else
                {
                    ARMARX_ERROR_S << "Object has an old motion model, but that motion model has no pose.";
                }
            }
            else
            {
                ARMARX_WARNING_S << "Object didn't have a motion model before, this may cause problems";
            }
 
            object->setMotionModel(AbstractMotionModelPtr::dynamicCast(newMotionModel));
        }
 
 
        void setObjectPose(const std::string& entityId, const armarx::LinkedPoseBasePtr& objectPose, const ::Ice::Current& = Ice::emptyCurrent) override
        {
            ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(getObjectInstanceById(entityId));
 
            armarx::FramedPositionPtr position = new armarx::FramedPosition(armarx::Vector3Ptr::dynamicCast(objectPose->position)->toEigen(), objectPose->frame, objectPose->agent);
            object->setPosition(position);
            armarx::FramedOrientationPtr orientation = new armarx::FramedOrientation(armarx::QuaternionPtr::dynamicCast(objectPose->orientation)->toEigen(), objectPose->frame, objectPose->agent);
            object->setOrientation(orientation);
 
            //armarx::LinkedPosePtr newPose = armarx::LinkedPosePtr::dynamicCast(object->getMotionModel()->getPoseAtLastLocalisation());
            //newPose->position = objectPose->position;
            //newPose->orientation = objectPose->orientation;
            //newPose->frame = objectPose->frame;
            object->getMotionModel()->setPoseAtLastLocalisation(objectPose, NULL, NULL);
        }
 
        void setObjectPoseWithoutMotionModel(const std::string& entityId, const armarx::FramedPoseBasePtr& objectPose, const Ice::Current&) override
        {
            ObjectInstancePtr object = ObjectInstancePtr::dynamicCast(getObjectInstanceById(entityId));
 
            armarx::FramedPositionPtr position = new armarx::FramedPosition(armarx::Vector3Ptr::dynamicCast(objectPose->position)->toEigen(), objectPose->frame, objectPose->agent);
            object->setPosition(position);
            object->setPositionUncertainty(MultivariateNormalDistribution::CreateDefaultDistribution());
            armarx::FramedOrientationPtr orientation = new armarx::FramedOrientation(armarx::QuaternionPtr::dynamicCast(objectPose->orientation)->toEigen(), objectPose->frame, objectPose->agent);
            object->setOrientation(orientation);
 
 
            if (object->getMotionModel())
            {
                ARMARX_WARNING_S << object->getName() << " has a motion model - You should not call setObjectPoseWithoutMotionModel() when a motion model is set. Use set setObjectPose() instead!";
            }
        }
 
 
        std::string addObjectInstance(const std::string& instanceName, const std::string& className, const armarx::LinkedPoseBasePtr& objectPose,
                                      const ::memoryx::MotionModelInterfacePtr& motionModel, const ::Ice::Current& = Ice::emptyCurrent) override
        {
            ObjectInstancePtr object = new ObjectInstance(instanceName);
            armarx::FramedPositionPtr position = new armarx::FramedPosition(armarx::Vector3Ptr::dynamicCast(objectPose->position)->toEigen(), objectPose->frame, objectPose->agent);
            object->setPosition(position);
            armarx::FramedOrientationPtr orientation = new armarx::FramedOrientation(armarx::QuaternionPtr::dynamicCast(objectPose->orientation)->toEigen(), objectPose->frame, objectPose->agent);
            object->setOrientation(orientation);
            FloatVector mean = {objectPose->position->x, objectPose->position->y, objectPose->position->z};
            FloatVector variances = {10, 10, 10};
            MultivariateNormalDistributionPtr uncertainty = new MultivariateNormalDistribution(mean, variances);
            object->setPositionUncertainty(uncertainty);
            object->addClass(className, 1.0);
            object->setExistenceCertainty(1.0);
 
            motionModel->setPoseAtLastLocalisation(objectPose, NULL, NULL);
            object->setMotionModel(AbstractMotionModelPtr::dynamicCast(motionModel));
 
            return addEntity(object);
        }
 
    };
 
    using PersistentObjectInstanceSegmentPtr = IceInternal::Handle<PersistentObjectInstanceSegment>;
}