AgentAtPredicateProvider.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    ArmarX::
* @author     Valerij Wittenbeck ( valerij.wittenbeck at kit dot edu)
* @date       2014
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "AgentAtPredicateProvider.h"
 
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <ArmarXCore/core/util/StringHelpers.h>
 
using namespace memoryx;
 
AgentAtPredicateProvider::AgentAtPredicateProvider() :
    agentAt(PredicateInfo {"agentAt", 2})
{
}
 
 
void AgentAtPredicateProvider::onInitWorldStateUpdater()
{
    usingProxy("WorkingMemory");
    usingProxy("PriorKnowledge");
    usingProxy("GraphNodePoseResolver");
    distanceThreshold = getProperty<float>("DistanceThreshold").getValue();
    humanDistanceThreshold = getProperty<float>("HumanDistanceThreshold").getValue();
    sceneName = getProperty<std::string>("PlatformGraphSceneName").getValue();
}
 
void AgentAtPredicateProvider::onConnectWorldStateUpdater()
{
    wm = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
    prior = getProxy<PriorKnowledgeInterfacePrx>("PriorKnowledge");
    psr = getProxy<GraphNodePoseResolverInterfacePrx>("GraphNodePoseResolver");
 
    agentInstances = wm->getAgentInstancesSegment();
    graphSegment = prior->getGraphSegment();
 
    graphNodes = graphSegment->getNodesByScene(sceneName);
}
 
std::string AgentAtPredicateProvider::getDefaultName() const
{
    return "AgentAtPredicateProvider";
}
 
PredicateInfoList AgentAtPredicateProvider::getPredicateInfos(const Ice::Current&)
{
    return {agentAt};
}
 
PredicateInstanceList AgentAtPredicateProvider::calcPredicates(const Ice::Current&)
{
    PredicateInstanceList result;
    const std::string predicateName = agentAt.name;
 
    for (const auto& agentEntity : agentInstances->getAllEntities())
    {
        AgentInstanceBasePtr agent = AgentInstanceBasePtr::dynamicCast(agentEntity);
        EntityRefBasePtr agentRef = wm->getAgentInstancesSegment()->getEntityRefById(agentEntity->getId());
 
        GraphNodeBasePtr closestNode = psr->getNearestNodeToPose(sceneName, "robotlocation", agent->getPoseBase());
 
        const auto agentPoseEigen = armarx::FramedPosePtr::dynamicCast(agent->getPoseBase())->toEigen();
        auto closestPoseEigen = armarx::FramedPosePtr::dynamicCast(closestNode->getPose())->toEigen();
        auto dist = (closestPoseEigen.block<2, 1>(0, 3) - agentPoseEigen.block<2, 1>(0, 3)).norm();
 
        ARMARX_INFO << "for agent '" << agent->getName() << "', closest node: '" << closestNode->getName() << "', dist: " << dist;
        float selectedDistanceThreshold = armarx::Contains(agent->getName(), "human", true) ? humanDistanceThreshold : distanceThreshold;
        if (dist <= selectedDistanceThreshold)
        {
            const CachedNodeInfo& cacheEntry = getCacheEntry(closestNode->getId());
 
            for (const auto& nodeRef : cacheEntry.agentAtNodeRefs)
            {
                ARMARX_DEBUG << "agentAt(" << agent->getName() << ", " << nodeRef->entityName << ")";
                result.push_back(PredicateInstance {predicateName, {agentRef, nodeRef}, true});
            }
        }
    }
 
    return result;
}
 
AgentAtPredicateProvider::CachedNodeInfo AgentAtPredicateProvider::getCacheEntry(const std::string& nodeId)
{
    auto it = nodeInfoCache.find(nodeId);
    if (it != nodeInfoCache.end())
    {
        return it->second;
    }
 
    CachedNodeInfo newEntry {nodeId, {}};
    std::vector<std::pair<EntityRefBasePtr, bool>> agentAtCandidates;
 
    bool allRobotLocations = true;
    for (const GraphNodeBasePtr& nodeBase : graphNodes)
    {
        GraphNodePtr node = GraphNodePtr::dynamicCast(nodeBase);
        if (node->isMetaEntity())
        {
            continue;
        }
 
        auto it = nodeParentsCache.find(node->getName());
        if (it == nodeParentsCache.end())
        {
            it = nodeParentsCache.insert({node->getName(), node->getAllParentsAsStringList()}).first;
        }
 
        auto parents = it->second;
 
        auto n = psr->getNearestRobotLocationNode(node);
        ARMARX_DEBUG << "to '" << node->getName() << "', closest robot location is '" << n->getName() << "'";
        if (n->getId() == nodeId)
        {
            //            newEntry.agentAtNodeRefs.push_back(graphSegment->getEntityRefById(node->getId()));
            bool isRobotLocation = std::find(parents.cbegin(), parents.cend(), "robotlocation") != parents.cend();
            allRobotLocations &= isRobotLocation;
            ARMARX_DEBUG << "for query node '" << graphSegment->getEntityRefById(nodeId)->entityName << "', considering node '" <<  node->getName() << "'";
            agentAtCandidates.push_back({graphSegment->getEntityRefById(node->getId()), isRobotLocation});
        }
    }
 
    if (!agentAtCandidates.empty())
    {
        if (allRobotLocations)
        {
            ARMARX_DEBUG << "for query '" << nodeId << "', result is '" << agentAtCandidates.front().first->entityName << "'";
            newEntry.agentAtNodeRefs.push_back(agentAtCandidates.front().first);
        }
        else
        {
            for (const auto& entry : agentAtCandidates)
            {
                if (!entry.second)
                {
                    ARMARX_DEBUG << "for query '" << nodeId << "', result is '" << entry.first->entityName << "'";
                    newEntry.agentAtNodeRefs.push_back(entry.first);
                    //                    break; //not sure if only one agentAt should be generated or more
                }
            }
        }
    }
 
    nodeInfoCache.insert({nodeId, newEntry});
    return newEntry;
}