ObjectInstance.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::WorkingMemory
* @author     ALexey Kozlov ( kozlov at kit dot edu), Kai Welke (welke at kit got edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "ObjectInstance.h"
 
#include <ArmarXCore/observers/AbstractObjectSerializer.h>
#include <RobotAPI/components/ViewSelection/ViewSelection.h>
#include <MemoryX/interface/observers/ObjectMemoryObserverInterface.h>
#include <MemoryX/libraries/motionmodels/AbstractMotionModel.h>
 
namespace memoryx
{
 
    // ********************************************************************
    // construction / destruction
    // ********************************************************************
    ObjectInstance::ObjectInstance(const std::string& name, const std::string& id)
        : Entity()
    {
        // set entity properties (TODO: maybe move to entity constructor)
        setName(name);
        setId(id);
 
        // initialize attributes
        setExistenceCertainty(0.0f);
        setPosition(new armarx::FramedPosition());
        setOrientation(new armarx::FramedOrientation());
        putAttribute(new EntityAttribute("priority"));
        setLocalizationPriority(armarx::DEFAULT_VIEWTARGET_PRIORITY);
 
        EntityAttributePtr classNamesAttr = new EntityAttribute("classes");
        putAttribute(classNamesAttr);
 
        motionModel = NULL;
    }
 
    ObjectInstance::ObjectInstance(const ObjectInstance& source) :
        IceUtil::Shared(source),
        ::armarx::Serializable(source),
        EntityBase(),// dont copy
        ObjectInstanceBase(source),
        Entity(source)
    {
 
    }
 
    // ********************************************************************
    // attribute getters / setters
    // ********************************************************************
    float ObjectInstance::getExistenceCertainty(const ::Ice::Current&) const
    {
        return getAttributeValue("existenceCertainty")->getFloat();
    }
 
 
    void ObjectInstance::setExistenceCertainty(float existenceCertainty, const ::Ice::Current&)
    {
        putAttribute("existenceCertainty", existenceCertainty);
    }
 
 
    EntityAttributeBasePtr ObjectInstance::getPositionAttribute() const
    {
        return getAttribute("position");
    }
 
 
    armarx::FramedPositionPtr ObjectInstance::getPosition() const
    {
        return armarx::FramedPositionPtr::dynamicCast(getPositionBase());
    }
 
 
    void ObjectInstance::setPosition(const armarx::FramedPositionBasePtr& position, const ::Ice::Current&)
    {
        if (getPositionAttribute())
        {
            if (getPositionAttribute()->getUncertainty())
            {
                MultivariateNormalDistributionBasePtr uncertainty = MultivariateNormalDistributionBasePtr::dynamicCast(getPositionAttribute()->getUncertainty());
                putAttribute("position", position);
                getPositionAttribute()->setValueWithUncertainty(getPositionAttribute()->getValue(), uncertainty);
            }
            else
            {
                putAttribute("position", position);
            }
        }
        else
        {
            putAttribute("position", position);
        }
    }
 
 
    void ObjectInstance::setLocalizationTimestamp(const IceUtil::Time& timestamp)
    {
        setLocalizationTimestamp(new armarx::TimestampVariant(timestamp));
    }
 
    void ObjectInstance::setLocalizationTimestamp(const armarx::TimestampBasePtr& timestamp)
    {
        putAttribute("localizationTimestamp", timestamp);
    }
 
    bool ObjectInstance::hasLocalizationTimestamp() const
    {
        return hasAttribute("localizationTimestamp");
    }
 
    IceUtil::Time ObjectInstance::getLocalizationTimestamp() const
    {
        auto variant = getAttributeValue("localizationTimestamp");
        ARMARX_CHECK_EXPRESSION(variant) << "no timestamp was set for this object pose";
        auto timestamp = variant->get<armarx::TimestampVariant>();
        return timestamp->toTime();
    }
 
    int ObjectInstance::getLocalizationPriority(const ::Ice::Current&) const
    {
        if (!hasAttribute("priority"))
        {
            return armarx::DEFAULT_VIEWTARGET_PRIORITY;
        }
        return getAttributeValue("priority")->getInt();
    }
 
    void ObjectInstance::setLocalizationPriority(Ice::Int priority, const ::Ice::Current&)
    {
        getAttribute("priority")->setValue(new armarx::Variant(priority));
    }
 
 
    MultivariateNormalDistributionBasePtr ObjectInstance::getPositionUncertainty(const ::Ice::Current&) const
    {
        return MultivariateNormalDistributionBasePtr::dynamicCast(getPositionAttribute()->getUncertainty());
    }
 
 
    void ObjectInstance::setPositionUncertainty(const MultivariateNormalDistributionBasePtr& uncertainty, const ::Ice::Current&)
    {
        getPositionAttribute()->setValueWithUncertainty(getPositionAttribute()->getValue(), uncertainty);
        ARMARX_DEBUG << "New position uncertainty mean: " <<  getPositionUncertainty()->getMean();
    }
 
 
    EntityAttributeBasePtr ObjectInstance::getOrientationAttribute() const
    {
        return getAttribute("orientation");
    }
 
 
    armarx::FramedOrientationPtr ObjectInstance::getOrientation() const
    {
        return armarx::FramedOrientationPtr::dynamicCast(getOrientationBase());
    }
 
 
    void ObjectInstance::setOrientation(const armarx::FramedOrientationBasePtr& orientation, const ::Ice::Current&)
    {
        putAttribute("orientation", orientation);
    }
 
    armarx::FramedPosePtr ObjectInstance::getPose() const
    {
        auto pos = getPosition();
        armarx::FramedPosePtr pose = new armarx::FramedPose(pos, getOrientation(), pos->frame, pos->agent);
        return pose;
    }
 
    void ObjectInstance::setPose(const armarx::FramedPoseBasePtr& newPose)
    {
        armarx::FramedPosePtr pose = armarx::FramedPosePtr::dynamicCast(newPose);
        setPosition(pose->getPosition());
        setOrientation(pose->getOrientation());
    }
 
 
    ClassProbabilityMap ObjectInstance::getClasses(const ::Ice::Current&) const
    {
        ClassProbabilityMap classes;
 
        EntityAttributeBasePtr attr = getAttribute("classes");
        int numberValues = attr->size();
 
        std::pair<std::string, float> element;
 
        for (int v = 0 ; v < numberValues ; v++)
        {
            element.first = attr->getValueAt(v)->getString();
            element.second =  DiscreteProbabilityBasePtr::dynamicCast(attr->getUncertaintyAt(v))->getProbability();
 
            classes.insert(element);
        }
 
        return classes;
    }
 
 
    void ObjectInstance::setClass(const std::string& className, float probability,
                                  const Ice::Current&)
    {
        if (getClasses().size() == 1)
        {
            // Workaround: Don't change container size in case of container size one.
            //             Provents segfaults when other threads loop through the container
            EntityAttributeBasePtr attr = getAttribute("classes");
            attr->setValueWithUncertainty(armarx::VariantPtr(new armarx::Variant(className)),
                                          ProbabilityMeasureBasePtr(new DiscreteProbability(probability)));
        }
        else
        {
            clearClasses();
            addClass(className, probability);
        }
    }
 
 
    void ObjectInstance::addClass(const std::string& className, float probability,
                                  const Ice::Current&)
    {
        EntityAttributeBasePtr attr = getAttribute("classes");
        attr->addValueWithUncertainty(armarx::VariantPtr(new armarx::Variant(className)),
                                      ProbabilityMeasureBasePtr(new DiscreteProbability(probability)));
    }
 
 
    void ObjectInstance::clearClasses(const Ice::Current&)
    {
        EntityAttributeBasePtr attr = getAttribute("classes");
        attr->clear();
    }
 
 
    // ********************************************************************
    // special ObjectClass handling
    // ********************************************************************
    std::string ObjectInstance::getMostProbableClass(const Ice::Current&) const
    {
        ClassProbabilityMap classes = getClasses();
 
        ClassProbabilityMap::const_iterator itMax = classes.end();
 
        for (ClassProbabilityMap::const_iterator it = classes.begin(); it != classes.end(); ++it)
            if (itMax == classes.end() || it->second > itMax->second)
            {
                itMax = it;
            }
 
        return (itMax != classes.end()) ? itMax->first : "";
    }
 
 
    float ObjectInstance::getClassProbability(const std::string& className,
            const Ice::Current&) const
    {
        ClassProbabilityMap classes = getClasses();
        ClassProbabilityMap::const_iterator it = classes.find(className);
 
        if (it != classes.end())
        {
            return it->second;
        }
        else
        {
            return 0.;
        }
    }
 
    void ObjectInstance::setMotionModel(AbstractMotionModelPtr motionModel)
    {
        std::scoped_lock lock(wrappersMutex);
        ARMARX_DEBUG_S << "New motion model is being set for " << this->getName() << ": " << motionModel->getMotionModelName();
        this->motionModel = motionModel;
    }
 
    AbstractMotionModelPtr ObjectInstance::getMotionModel() const
    {
        std::scoped_lock lock(wrappersMutex);
        return motionModel;
    }
 
    Ice::ObjectPtr ObjectInstance::ice_clone() const
    {
        return this->clone();
    }
 
    ObjectInstancePtr ObjectInstance::clone(const Ice::Current& c) const
    {
        ObjectInstancePtr ret;
        {
            std::shared_lock lock1(entityMutex);
            std::scoped_lock lock2(attributesMutex);
            std::scoped_lock lock3(wrappersMutex);
            ret = new ObjectInstance(*this);
            ret->motionModel = this->motionModel;
        }
        //    ret->deepCopy(*this);
        return ret;
    }
 
 
    // ********************************************************************
    // i / o
    // ********************************************************************
    void ObjectInstance::output(std::ostream& stream) const
    {
        Entity::output(stream);
    }
 
 
    // ********************************************************************
    // protected members (Ice interface methods overwritten by c++ implementation)
    // ********************************************************************
    armarx::FramedPositionBasePtr ObjectInstance::getPositionBase(const ::Ice::Current&) const
    {
        return armarx::VariantPtr::dynamicCast(getPositionAttribute()->getValue())->getClass<armarx::FramedPositionBase>();
    }
 
    armarx::FramedOrientationBasePtr ObjectInstance::getOrientationBase(const ::Ice::Current&) const
    {
        return armarx::VariantPtr::dynamicCast(getOrientationAttribute()->getValue())->getClass<armarx::FramedOrientationBase>();
    }
 
    ObjectInstance::ObjectInstance() {}
 
}