WorldStateObserver.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::WorldStateObserver
* @author     David Schiebener ( schiebener at kit dot edu)
* @date       2014
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "WorldStateObserver.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/core/entity/EntityRef.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/memorytypes/entity/Relation.h>
 
namespace memoryx
{
    void
    WorldStateObserver::onInitComponent()
    {
        usingProxy("WorkingMemory");
        usingProxy("PriorKnowledge");
        //    usingProxy("HandUnitObserver");
    }
 
    void
    WorldStateObserver::onConnectComponent()
    {
        getProxy(wm, "WorkingMemory");
        getProxy(prior, "PriorKnowledge");
 
        objectInstances = wm->getObjectInstancesSegment();
        objectClasses = prior->getObjectClassesSegment();
        objectRelations = wm->getRelationsSegment();
    }
 
    memoryx::PredicateInstanceList
    WorldStateObserver::getWorldState(const Ice::Current&)
    {
 
        PredicateInstanceList worldState;
        observablePredicateInstances.clear();
        {
            std::unique_lock lock(updaterMutex);
            //std::vector<Ice::AsyncResultPtr> results;
            auto start = IceUtil::Time::now().toMilliSeconds();
#define TM(msg)                                                                                    \
    ARMARX_VERBOSE << msg << ": " << (IceUtil::Time::now().toMilliSeconds() - start) << "ms";      \
    start = IceUtil::Time::now().toMilliSeconds();
 
            for (const auto& pair : updaters)
            {
                PredicateInstanceList allowedPredicates;
 
                ARMARX_INFO << "calculating predicates of '" << pair.first << "'";
                try
                {
                    for (const PredicateInstance& predicate : pair.second->calcPredicates())
                    {
                        if (areAllowed(predicate.argValues))
                        {
                            allowedPredicates.push_back(predicate);
                        }
                    }
                }
                catch (...)
                {
                    armarx::handleExceptions();
                }
 
                addListToList(worldState, allowedPredicates);
                TM(pair.first)
            }
        }
 
        addListToList(worldState, getNonobservableRelationsAndPredicates());
 
        return worldState;
    }
 
    bool
    WorldStateObserver::isObservable(const std::string& predicateName, const Ice::Current&)
    {
        auto it = std::find_if(updaters.cbegin(),
                               updaters.cend(),
                               [&](const std::pair<std::string, WorldStateUpdaterInterfacePrx>& arg)
                               {
                                   for (const auto& predicateInfo : arg.second->getPredicateInfos())
                                   {
                                       if (predicateInfo.name == predicateName)
                                       {
                                           return true;
                                       }
                                   }
 
                                   return false;
                               });
        return it != updaters.end();
    }
 
    bool
    WorldStateObserver::updatePredicateValue(const PredicateInstance& pi,
                                             bool removePredicate,
                                             const Ice::Current&)
    {
        memoryx::RelationBasePtr relation =
            objectRelations->getRelationByAttrValues(pi.name, pi.argValues, pi.sign);
 
        if (removePredicate)
        {
            if (relation)
            {
                ARMARX_VERBOSE << "Removing relation " << relation->getName();
                objectRelations->removeEntity(relation->getId());
            }
            else
            {
                ARMARX_INFO << "Trying to remove the relation " << pi.name
                            << ", but it doesn't exist";
                return false;
            }
        }
        else
        {
            if (!relation)
            {
                ARMARX_VERBOSE << "Adding relation " << pi.name;
                memoryx::RelationBasePtr relation = new Relation(pi.name, pi.argValues, pi.sign);
                objectRelations->addEntity(relation);
            }
            else
            {
                ARMARX_VERBOSE << "Trying to add the relation " << relation->getName()
                               << ", but it is already there";
                return false;
            }
        }
 
        return true;
    }
 
    void
    WorldStateObserver::setPredicateArgumentWhitelist(const EntityBaseList& argumentWhitelist,
                                                      const Ice::Current&)
    {
        this->argumentWhitelist = argumentWhitelist;
    }
 
    void
    WorldStateObserver::resetPredicateArgumentWhitelist(const Ice::Current&)
    {
        argumentWhitelist.clear();
    }
 
    bool
    WorldStateObserver::areAllowed(const std::vector<EntityRefBasePtr>& entityRefs)
    {
        if (argumentWhitelist.empty())
        {
            return true;
        }
 
        for (const auto& ref : entityRefs)
        {
            auto id = ref->entityId;
 
            if (auto instance = ObjectInstancePtr::dynamicCast(ref->getEntity()))
            {
                id = objectClasses->getEntityByName(instance->getMostProbableClass())->getId();
            }
 
            bool whitelisted = std::find_if(argumentWhitelist.cbegin(),
                                            argumentWhitelist.cend(),
                                            [&](const EntityBasePtr& entity) {
                                                return entity->getId() == id;
                                            }) != argumentWhitelist.cend();
 
            if (!whitelisted)
            {
                return false;
            }
        }
 
        return true;
    }
 
    void
    WorldStateObserver::addListToList(PredicateInstanceList& target,
                                      const PredicateInstanceList& source)
    {
        //    ARMARX_VERBOSE << "target list: " << target;
        for (size_t i = 0; i < source.size(); i++)
        {
            std::string entityNames;
 
            for (const auto& entity : source.at(i).argValues)
            {
                entityNames += entity->entityName + ", ";
            }
 
            ARMARX_DEBUG << "adding to target: " << source.at(i).name << " args:  " << entityNames;
            target.push_back(source.at(i));
        }
    }
 
    PredicateInstanceList
    WorldStateObserver::getNonobservableRelationsAndPredicates()
    {
        PredicateInstanceList returnPIList;
 
        EntityIdList allEntityIDs = objectRelations->getAllEntityIds();
        ARMARX_VERBOSE << "Adding " << allEntityIDs.size()
                       << " relations from the relations segment to the world state";
 
        for (const std::string& id : allEntityIDs)
        {
            const RelationBasePtr relation = objectRelations->getRelationById(id);
 
            returnPIList.push_back(PredicateInstance{
                relation->getName(), relation->getEntities(), relation->getSign()});
        }
 
        return returnPIList;
    }
 
    void
    memoryx::WorldStateObserver::addObservablePredicateInstances(
        const PredicateInstanceList& predicates,
        const Ice::Current&)
    {
        observablePredicateInstances.insert(
            observablePredicateInstances.end(), predicates.begin(), predicates.end());
    }
 
    void
    memoryx::WorldStateObserver::registerAsUpdater(const std::string& name,
                                                   const WorldStateUpdaterInterfacePrx& updater,
                                                   const Ice::Current&)
    {
        std::unique_lock lock(updaterMutex);
 
        if (updaters.find(name) != updaters.end())
        {
            throw armarx::LocalException()
                << "Updater with name " << name << " already registered.";
        }
 
        updaters.insert(std::make_pair(name, updater));
    }
 
    WorldStateUpdaterInterfaceList
    memoryx::WorldStateObserver::getRegisteredUpdaters(const Ice::Current&)
    {
        WorldStateUpdaterInterfaceList result;
 
        for (const auto& updater : updaters)
        {
            result.push_back(updater.second);
        }
 
        return result;
    }
 
    std::string
    WorldStateObserver::GetDefaultName()
    {
        return "WorldStateObserver";
    }
} // namespace memoryx
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(::memoryx::WorldStateObserver,
                                     ::memoryx::WorldStateObserver::GetDefaultName());