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 <vector>
 
#include <ArmarXCore/core/system/ImportExportComponent.h>
 
#include <MemoryX/core/memory/PersistentEntitySegment.h>
#include <MemoryX/interface/core/EntityBase.h>
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/motionmodels/MotionModelObjectFactories.h>
 
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>;
} // namespace memoryx