ObjectMemoryObserver.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    VisionX::Observers
* @author     Kai Welke (welke at kit dot edu), David Schiebener (david dot schiebener at kit dot edu)
* @copyright  2012 Humanoids Group, HIS, KIT
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "ObjectMemoryObserver.h"
 
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
#include <RobotAPI/interface/components/ViewSelectionInterface.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/EarlyVisionConverters.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/Gaussian.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/updater/ObjectLocalization/MemoryXUpdaterObjectFactories.h>
 
// checks
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
 
namespace memoryx
{
    // ********************************************************************
    // observer framework hooks
    // ********************************************************************
    void
    ObjectMemoryObserver::onInitObserver()
    {
        usingProxy(getProperty<std::string>("WorkingMemoryProxy").getValue());
        usingProxy(getProperty<std::string>("PriorKnowledgeProxy").getValue());
 
        uniqueId = 0;
 
        // register all checks
        offerConditionCheck("updated", new armarx::ConditionCheckUpdated());
        offerConditionCheck("equals", new armarx::ConditionCheckEquals());
        offerConditionCheck("inrange", new armarx::ConditionCheckInRange());
        offerConditionCheck("larger", new armarx::ConditionCheckLarger());
        offerConditionCheck("smaller", new armarx::ConditionCheckSmaller());
 
        // use working memory update topic
        usingTopic(getProperty<std::string>("WorkingMemoryListenerTopic").getValue());
    }
 
    void
    ObjectMemoryObserver::onConnectObserver()
    {
        // retrieve object memory proxy
        proxyWorkingMemory = getProxy<AbstractWorkingMemoryInterfacePrx>(
            getProperty<std::string>("WorkingMemoryProxy").getValue());
        getProxy(priorKnowledge, getProperty<std::string>("PriorKnowledgeProxy").getValue());
        priorObjectClassesSegment = priorKnowledge->getObjectClassesSegment();
        // retrieve required segments of memory
        objectInstancesSegment = ObjectInstanceMemorySegmentBasePrx::checkedCast(
            proxyWorkingMemory->getSegment("objectInstances"));
        objectClassesSegment = ObjectClassMemorySegmentBasePrx::checkedCast(
            proxyWorkingMemory->getSegment("objectClasses"));
    }
 
    // ********************************************************************
    // ObjectMemoryObserverInterface implementation
    // ********************************************************************
 
    armarx::ChannelRefBasePtr
    ObjectMemoryObserver::requestObjectClassOnce(const std::string& objectClassName,
                                                 const IceUtil::Optional<Ice::Int>& priority,
                                                 const ::Ice::Current& c)
    {
        return requestObjectClassRepeated(objectClassName, -1, priority, c);
    }
 
    armarx::ChannelRefBasePtr
    ObjectMemoryObserver::requestObjectClassRepeated(const std::string& objectClassName,
                                                     int cycleTimeMS,
                                                     const IceUtil::Optional<Ice::Int>& priority,
                                                     const ::Ice::Current& c)
    {
        ARMARX_IMPORTANT << "attending to objects of class: " << objectClassName
                         << " with cycle time " << cycleTimeMS;
 
        // assure complete ontology tree is in object classes segment
        ObjectClassList classes =
            objectClassesSegment->addPriorClassWithSubclasses(objectClassName);
 
        if (classes.size() == 0)
        {
            ARMARX_WARNING_S << "Class " << objectClassName << " not found ";
            return NULL;
        }
 
        // create name for query
        std::stringstream ss;
        ss << objectClassName << "_query_" << getUniqueId();
        std::string queryName = ss.str();
 
        // update requested entity with query
        ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(
            objectClassesSegment->getEntityByName(objectClassName)->ice_clone());
        EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapper =
            objectClass->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
 
        int localizationPriority = armarx::DEFAULT_VIEWTARGET_PRIORITY;
        if (priority)
        {
            localizationPriority = priority.get();
        }
        LocalizationQueryPtr query =
            new LocalizationQuery(queryName, objectClassName, cycleTimeMS, localizationPriority);
        recognitionWrapper->addLocalizationQuery(query);
 
        // create channel
        armarx::ChannelRefBasePtr queryChannel = createObjectLocalizationQueryChannel(query);
 
        // update class segment
        objectClassesSegment->updateEntity(objectClass->getId(), objectClass);
 
        return queryChannel;
    }
 
    void
    ObjectMemoryObserver::releaseObjectClass(const armarx::ChannelRefBasePtr& objectClassChannel,
                                             const ::Ice::Current& c)
    {
        armarx::ChannelRefPtr channelRef = armarx::ChannelRefPtr::dynamicCast(objectClassChannel);
        if (!existsChannel(objectClassChannel->channelName))
        {
            ARMARX_INFO << "Object class query channel " << objectClassChannel->channelName
                        << "does not exist - skipping removal";
            return;
        }
        std::string objectClassName = channelRef->getDataField("className")->getString();
 
        ARMARX_IMPORTANT << "releasing objects of class " << objectClassName;
 
        // remove query requested entity with query
        auto entity = objectClassesSegment->getEntityByName(objectClassName);
        ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(entity);
        if (objectClass)
        {
            EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapper =
                objectClass->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
 
            recognitionWrapper->removeLocalizationQuery(objectClassChannel->channelName);
 
            // update the entity
            objectClassesSegment->updateEntity(objectClass->getId(), objectClass);
 
            // remove reference to the class
            objectClassesSegment->removePriorClassWithSubclasses(objectClassName);
        }
 
        // remove channel
        removeChannel(objectClassChannel->channelName);
    }
 
    memoryx::ChannelRefBaseSequence
    ObjectMemoryObserver::getObjectInstancesByClass(const std::string& objectClassName,
                                                    const ::Ice::Current& c)
    {
        memoryx::ChannelRefBaseSequence channels;
 
        // retrieve all subclasses of the object class
        ObjectClassList relevantClasses =
            priorObjectClassesSegment->getClassWithSubclasses(objectClassName);
        for (ObjectClassBasePtr& objClass : relevantClasses)
        {
            ARMARX_DEBUG << "relevant class: " << objClass->getName();
        }
        // go through channels
        armarx::ChannelRegistry registry = getAvailableChannels(false);
 
        for (armarx::ChannelRegistry::iterator iter = registry.begin(); iter != registry.end();
             iter++)
        {
            // check whether channel is instances channel
            armarx::ChannelRegistryEntry channelEntry = iter->second;
            ARMARX_DEBUG << "Checking " << channelEntry.name;
 
            if (channelEntry.dataFields.find("instanceName") == channelEntry.dataFields.end())
            {
                ARMARX_DEBUG << "No instance name for " << channelEntry.name;
                continue;
            }
 
            // check whether channel belongs to requested class
            armarx::DataFieldRegistry dataFields = channelEntry.dataFields;
            armarx::DataFieldRegistry::const_iterator iterDataFields = dataFields.find("className");
 
            if (iterDataFields == dataFields.end())
            {
                ARMARX_DEBUG << "No class name for " << channelEntry.name;
                continue;
            }
 
            armarx::DataFieldRegistryEntry dataField = iterDataFields->second;
            if (!dataField.value->getInitialized())
            {
                ARMARX_DEBUG << "className not init. for " << channelEntry.name;
 
                continue;
            }
            std::string channelObjectClassName = dataField.value->getString();
 
            bool relevant = false;
 
            for (ObjectClassList::iterator iterClasses = relevantClasses.begin();
                 iterClasses != relevantClasses.end();
                 iterClasses++)
            {
                ObjectClassBasePtr objectClass = *iterClasses;
 
                if (objectClass->getName() == channelObjectClassName)
                {
                    relevant = true;
                }
            }
 
            if (relevant)
            {
                ARMARX_DEBUG << "Adding instance " << iter->first << " to result";
                channels.push_back(new armarx::ChannelRef(this, iter->first));
            }
            else
            {
                ARMARX_DEBUG << "No relevant class found";
            }
        }
 
        return channels;
    }
 
    armarx::ChannelRefBasePtr
    ObjectMemoryObserver::getFirstObjectInstance(
        const armarx::ChannelRefBasePtr& objectClassChannel,
        const Ice::Current& c)
    {
        auto instances = getObjectInstances(objectClassChannel, c);
 
        if (instances.size() > 0)
        {
            return instances.front();
        }
        else
        {
            return armarx::ChannelRefBasePtr();
        }
    }
 
    armarx::ChannelRefBasePtr
    ObjectMemoryObserver::getFirstObjectInstanceByClass(const std::string& objectClassName,
                                                        const Ice::Current& c)
    {
        auto instances = getObjectInstancesByClass(objectClassName, c);
 
        if (instances.size() > 0)
        {
            return instances.front();
        }
        else
        {
            return armarx::ChannelRefBasePtr();
        }
    }
 
    memoryx::ChannelRefBaseSequence
    ObjectMemoryObserver::getObjectInstances(const armarx::ChannelRefBasePtr& objectClassChannel,
                                             const ::Ice::Current& c)
    {
        armarx::ChannelRefPtr channelRef = armarx::ChannelRefPtr::dynamicCast(objectClassChannel);
        return getObjectInstancesByClass(channelRef->getDataField("className")->getString(), c);
    }
 
    void
    ObjectMemoryObserver::reportEntityCreated(const std::string& segmentName,
                                              const EntityBasePtr& entity,
                                              const ::Ice::Current& c)
    {
        ARMARX_VERBOSE << "reportEntityCreated(): " << entity->getName()
                       << ", segmentName: " << segmentName;
 
        ObjectInstancePtr instance = ObjectInstancePtr::dynamicCast(entity);
 
        // check whether this is the correct type of entity
        if (!instance)
        {
            return;
        }
 
        // check whether entity is in channel list -> error
        if (existsChannel(getInstanceChannelName(instance)))
        {
            ARMARX_ERROR << "Object instance " << instance->getName() << " already in channel list";
            return;
        }
 
        // create and update channel
        createObjectInstanceChannel(instance);
        updateObjectInstanceChannel(instance);
    }
 
    void
    ObjectMemoryObserver::reportEntityUpdated(const std::string& segmentName,
                                              const EntityBasePtr& entityOld,
                                              const EntityBasePtr& entityNew,
                                              const ::Ice::Current& c)
    {
        ARMARX_DEBUG << deactivateSpam(5, entityNew->getName())
                     << "ObjectMemoryObserver::reportEntityUpdated(): " << entityNew->getName()
                     << ", segmentName: " << segmentName;
        try
        {
            if (segmentName == "objectClasses")
            {
                ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(entityNew);
                EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapper =
                    objectClass->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
 
                LocalizationQueryList queries = recognitionWrapper->getLocalizationQueries();
 
                for (LocalizationQueryList::iterator iter = queries.begin(); iter != queries.end();
                     iter++)
                {
                    try
                    {
                        updateObjectLocalizationQueryChannel(
                            LocalizationQueryPtr::dynamicCast(*iter));
                    }
                    catch (armarx::InvalidChannelException& e)
                    {
                        createObjectLocalizationQueryChannel(
                            LocalizationQueryPtr::dynamicCast(*iter));
                    }
                }
 
                return;
            }
 
            if (segmentName == "objectInstances")
            {
                ObjectInstancePtr instance = ObjectInstancePtr::dynamicCast(entityNew);
 
                // check whether this is the correct type of entity
                if (!instance)
                {
                    return;
                }
 
                // check if instance has associated channel
                if (!existsChannel(getInstanceChannelName(instance)))
                {
 
                    ARMARX_INFO << "Object instance " << instance->getName()
                                << " not in channel list - creating it";
 
 
                    createObjectInstanceChannel(instance);
                }
 
                // update instance channel
                updateObjectInstanceChannel(instance);
            }
        }
        catch (...)
        {
            armarx::handleExceptions();
        }
    }
 
    void
    ObjectMemoryObserver::reportEntityRemoved(const std::string& segmentName,
                                              const EntityBasePtr& entity,
                                              const ::Ice::Current& c)
    {
        ARMARX_VERBOSE << "reportEntityRemoved(): " << entity->getName()
                       << ", segmentName: " << segmentName;
 
        ObjectInstancePtr instance = ObjectInstancePtr::dynamicCast(entity);
        ObjectClassPtr objClass = ObjectClassPtr::dynamicCast(entity);
 
        // check whether this is the correct type of entity
        if (instance)
        {
 
            // check whether entity is in channel list -> error
            if (!existsChannel(getInstanceChannelName(instance)))
            {
                ARMARX_ERROR << "Object instance " << instance->getName() << " not in channel list";
                return;
            }
 
            // remove channel
            removeChannel(getInstanceChannelName(instance));
        }
        else if (objClass)
        {
            auto channels = getObjectClassQueries(objClass->getName());
            for (auto& channel : channels)
            {
                releaseObjectClass(channel);
            }
        }
    }
 
    // Should this really be in the observer interface??? At least all other report methods should be called accordingly
    void
    ObjectMemoryObserver::reportSnapshotLoaded(const std::string& segmentName,
                                               const ::Ice::Current& c)
    {
    }
 
    void
    ObjectMemoryObserver::reportMemoryCleared(const std::string& segmentName,
                                              const ::Ice::Current& c)
    {
        std::scoped_lock lock(channelsMutex);
 
        if (segmentName == "objectClasses" || segmentName == "objectInstances")
        {
            ARMARX_IMPORTANT << "Working memory was cleared! deleting all channels";
            auto channels = getAvailableChannels(false);
 
            for (auto& channel : channels)
            {
                removeChannel(channel.first);
            }
        }
    }
 
    // ********************************************************************
    // utility methods
    // ********************************************************************
    armarx::ChannelRefPtr
    ObjectMemoryObserver::createObjectLocalizationQueryChannel(const LocalizationQueryPtr& query)
    {
        ARMARX_INFO << "Creating channel for object class: " << query->className;
        std::string channelName = query->queryName;
 
        // offer the channel
        offerChannel(channelName, "object class information for " + query->className);
 
        // set datafields
        offerDataFieldWithDefault(
            channelName, "className", query->className, "name of object class");
        offerDataFieldWithDefault(channelName,
                                  "localizationFinished",
                                  bool(false),
                                  "has the localization query finished yet?");
 
        return new armarx::ChannelRef(this, channelName);
    }
 
    void
    ObjectMemoryObserver::updateObjectLocalizationQueryChannel(const LocalizationQueryPtr& query)
    {
        ARMARX_DEBUG << "updateObjectLocalizationQueryChannel()";
 
        // create unique channel name
        std::string channelName = query->queryName;
 
        setDataField(channelName, "localizationFinished", query->getFinished());
 
        updateChannel(channelName);
    }
 
    std::vector<armarx::ChannelRefPtr>
    ObjectMemoryObserver::getObjectClassQueries(const std::string& objectClassName)
    {
        std::vector<armarx::ChannelRefPtr> result;
        armarx::ChannelRegistry registry = getAvailableChannels(false);
        for (auto& pair : registry)
        {
            armarx::ChannelRegistryEntry& entry = pair.second;
            auto it = entry.dataFields.find("className");
            if (entry.name.find("_query_") != std::string::npos && it != entry.dataFields.end() &&
                it->second.value->getType() == armarx::VariantType::String &&
                it->second.value->getString() == objectClassName)
            {
                result.push_back(new armarx::ChannelRef(this, entry.name));
            }
        }
        return result;
    }
 
    void
    ObjectMemoryObserver::createObjectInstanceChannel(const ObjectInstancePtr& instance)
    {
        // create unique channel name
        std::string channelName = getInstanceChannelName(instance);
        std::string objectInstanceName = instance->getName();
 
        ARMARX_INFO << "Creating channel " << channelName
                    << " for instance: " << objectInstanceName;
 
        // offer the channel
        offerChannel(channelName, "object instance information for " + objectInstanceName);
 
        // set datafields
        offerDataField(channelName,
                       "id",
                       armarx::VariantType::String,
                       "id of the object instance in the working memory");
        offerDataField(
            channelName, "className", armarx::VariantType::String, "name of object class");
        offerDataField(
            channelName, "instanceName", armarx::VariantType::String, "name of object instance");
        offerDataField(
            channelName, "position", armarx::VariantType::FramedPosition, "object position");
        offerDataField(channelName,
                       "orientation",
                       armarx::VariantType::FramedOrientation,
                       "quaternion for object orientation");
        offerDataField(channelName, "pose", armarx::VariantType::FramedPose, "object pose");
        offerDataField(channelName, "priority", armarx::VariantType::Int, "localization priority");
        offerDataField(channelName,
                       "existenceCertainty",
                       armarx::VariantType::Float,
                       "Existence certainty of this object");
        offerDataField(channelName,
                       "uncertaintyOfPosition",
                       armarx::VariantType::Float,
                       "Std deviation of the position uncertainty");
        offerDataField(channelName,
                       "timestamp",
                       armarx::VariantType::Timestamp,
                       "timestamp of last localization");
    }
 
    void
    ObjectMemoryObserver::updateObjectInstanceChannel(const ObjectInstancePtr& instance)
    {
        ARMARX_DEBUG << deactivateSpam(5) << "updateObjectInstanceChannel()";
 
        std::string channelName = getInstanceChannelName(instance);
 
        // set datafields
        setDataField(channelName, "id", armarx::Variant(instance->getId()));
        setDataField(channelName, "className", armarx::Variant(instance->getMostProbableClass()));
        setDataField(channelName, "instanceName", armarx::Variant(instance->getName()));
        setDataField(channelName, "position", armarx::Variant(instance->getPosition()));
        setDataField(channelName, "priority", armarx::Variant(instance->getLocalizationPriority()));
        setDataField(channelName, "orientation", armarx::Variant(instance->getOrientation()));
        setDataField(channelName, "pose", armarx::Variant(instance->getPose()));
        setDataField(
            channelName, "existenceCertainty", armarx::Variant(instance->getExistenceCertainty()));
        float stdDeviation = std::numeric_limits<float>::max();
 
        if (instance->getPositionUncertainty())
        {
            Gaussian posUncertaintyDistribution =
                EarlyVisionConverters::convertToGaussian(instance->getPositionUncertainty());
            stdDeviation = pow(posUncertaintyDistribution.getCovariance().determinant(),
                               0.5 / posUncertaintyDistribution.getDimensions());
        }
 
        setDataField(channelName, "uncertaintyOfPosition", armarx::Variant(stdDeviation));
        setDataField(
            channelName,
            "timestamp",
            armarx::Variant(instance->hasLocalizationTimestamp()
                                ? new armarx::TimestampVariant(instance->getLocalizationTimestamp())
                                : armarx::TimestampVariant::nowPtr()));
 
        // update the channel
        updateChannel(channelName);
    }
 
    std::string
    ObjectMemoryObserver::getInstanceChannelName(const EntityBasePtr& instance,
                                                 const ::Ice::Current& c) const
    {
        std::string channelName = instance->getName() + instance->getId();
        return channelName;
    }
 
    armarx::ChannelRefBasePtr
    memoryx::ObjectMemoryObserver::getObjectInstanceById(const std::string& id, const Ice::Current&)
    {
        armarx::ChannelRegistry registry = getAvailableChannels(false);
 
        for (armarx::ChannelRegistry::iterator iter = registry.begin(); iter != registry.end();
             iter++)
        {
            armarx::ChannelRegistryEntry channelEntry = iter->second;
            ARMARX_DEBUG << "Checking " << channelEntry.name;
 
            if (channelEntry.dataFields.find("instanceName") == channelEntry.dataFields.end())
            {
                //            ARMARX_DEBUG << "No instance name for " << channelEntry.name;
                continue;
            }
            auto it = channelEntry.dataFields.find("id");
            if (it == channelEntry.dataFields.end())
            {
                //            ARMARX_DEBUG << "No id for " << channelEntry.name;
                continue;
            }
            if (it->second.value->getString() == id)
            {
                return new armarx::ChannelRef(this, iter->first);
            }
        }
        return NULL;
    }
} // namespace memoryx