ObjectAtPredicateProvider.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 "ObjectAtPredicateProvider.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>
 
using namespace memoryx;
 
ObjectAtPredicateProvider::ObjectAtPredicateProvider()
{
}
 
 
void ObjectAtPredicateProvider::onInitWorldStateUpdater()
{
    usingProxy("WorkingMemory");
    usingProxy("PriorKnowledge");
    usingProxy("GraphNodePoseResolver");
 
    distanceThreshold = getProperty<float>("DistanceThreshold").getValue();
    handDistanceThreshold = getProperty<float>("HandDistanceThreshold").getValue();
    sceneName = getProperty<std::string>("PlatformGraphSceneName").getValue();
}
 
void ObjectAtPredicateProvider::onConnectWorldStateUpdater()
{
    wm = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
    prior = getProxy<PriorKnowledgeInterfacePrx>("PriorKnowledge");
    psr = getProxy<GraphNodePoseResolverInterfacePrx>("GraphNodePoseResolver");
 
    agentInstances = wm->getAgentInstancesSegment();
    objectInstances = wm->getObjectInstancesSegment();
    objectClasses = prior->getObjectClassesSegment();
    graphs = prior->getGraphSegment();
 
    graphNodes = graphs->getNodesByScene(sceneName);
 
    for (const auto& classEntity : objectClasses->getAllEntities())
    {
        classNameParentsMap.insert({classEntity->getName(), objectClasses->getObjectClassByNameWithAllParents(classEntity->getName())->getParentClasses()});
    }
}
 
std::string ObjectAtPredicateProvider::getDefaultName() const
{
    return "ObjectAtPredicateProvider";
}
 
PredicateInfoList ObjectAtPredicateProvider::getPredicateInfos(const Ice::Current&)
{
    return {PredicateInfo{"objectAt", 2}};
}
 
PredicateInstanceList ObjectAtPredicateProvider::calcPredicates(const Ice::Current&)
{
    PredicateInstanceList result;
    const std::string predicateName = getPredicateInfos().at(0).name;
 
    //    float minx = -700;
    //    float maxx = 700;
    //    float miny = 400;
    //    float maxy = 1200;
 
    armarx::SharedRobotInterfacePrx robot;
 
    std::vector<std::pair<ObjectInstancePtr, Eigen::Matrix4f>> objects;
    std::vector<std::pair<ObjectInstancePtr, Eigen::Matrix4f>> hands;
 
    for (const auto& objectEntity : objectInstances->getAllEntities())
    {
        auto objectInstance = ObjectInstancePtr::dynamicCast(objectEntity);
        ARMARX_CHECK_EXPRESSION(objectInstance);
        auto classes = classNameParentsMap[objectInstance->getMostProbableClass()];
 
        armarx::FramedPosePtr objectPose = objectInstance->getPose();
        ARMARX_CHECK_EXPRESSION(objectPose);
        if (objectPose->frame != armarx::GlobalFrame && !objectPose->frame.empty())
        {
            if (!robot || robot->getName() != objectPose->agent)
            {
                auto agentInstance = agentInstances->getAgentInstanceByName(objectPose->agent);
                ARMARX_CHECK_EXPRESSION(agentInstance) << "no agent with name '" << objectPose->agent << "' present while trying to change pose of " << objectInstance->getName();
                robot = agentInstance->getSharedRobot();
            }
            if (robot)
            {
                objectPose->changeToGlobal(robot);
            }
            else
            {
                ARMARX_ERROR << "Could not change " << objectPose << " to global";
            }
        }
        if (std::find(classes.cbegin(), classes.cend(), "hand") != classes.cend())
        {
            hands.push_back({objectInstance, objectPose->toEigen()});
        }
        else
        {
            objects.push_back({objectInstance, objectPose->toEigen()});
        }
    }
 
    //    auto objClassSeg = prior->getObjectClassesSegment();
    for (const auto& objectEntry : objects)
    {
        const auto& object = objectEntry.first;
        ARMARX_CHECK_EXPRESSION(object);
        const auto& objectPosEigen = objectEntry.second;
        bool tooCloseToHand = false;
 
        for (const auto& handEntry : hands)
        {
            const auto& hand = handEntry.first;
            const auto& handPosEigen = handEntry.second;
            const float handDist = (handPosEigen.block<2, 1>(0, 3) - objectPosEigen.block<2, 1>(0, 3)).norm();
            ARMARX_INFO << "dist from object '" << object->getName() << "' to hand '" << hand->getName() << "': " << handDist;
            if (handDist < handDistanceThreshold)
            {
                ARMARX_INFO << "object '" << object->getName() << "' is too close to hand '" << hand->getName() << "'; will not check objectAt conditions";
                tooCloseToHand = true;
                break;
            }
        }
 
        if (tooCloseToHand)
        {
            continue;
        }
        ARMARX_CHECK_EXPRESSION(object);
        ARMARX_DEBUG << object->getName() << objectPosEigen;
        std::string mostProbableClass = object->getMostProbableClass();
        memoryx::EntityRefBasePtr closestNodeRef;
        float minDist = std::numeric_limits<float>::max();
 
        //@TODO just call gnpr to get closest node?
        for (const auto& node : graphNodes)
        {
            const auto& nodeInfo = getCacheEntry(node->getId());
            if (nodeInfo.node->isMetaEntity())
            {
                continue;
            }
 
            const auto& nodePose = nodeInfo.globalPose;
 
            if (nodeInfo.originalFrame == mostProbableClass)
            {
                continue;
            }
            ARMARX_CHECK_EXPRESSION(nodeInfo.node);
            ARMARX_CHECK_EXPRESSION(nodePose);
 
            ARMARX_DEBUG << nodeInfo.node->getName() << "\n" << nodePose->toEigen();
            //            if(!robot || robot->getName() != nodePos->agent)
            //            nodePos->changeFrame();
            //            Eigen::Matrix4f objPosRelativeToNode = nodePose->toEigen().inverse() * objectPosEigen;
            const float nodeDist = (nodePose->toEigen().block<2, 1>(0, 3) - objectPosEigen.block<2, 1>(0, 3)).norm();
            ARMARX_DEBUG << object->getName() << ": " << nodeInfo.node->getName() << " " << VAROUT(nodeDist);
            // this allows objects to be at robot locations
            //            if (
            //                ((objPosRelativeToNode(0, 3) >= minx && objPosRelativeToNode(0, 3) <= maxx
            //                  && objPosRelativeToNode(1, 3) >= miny && objPosRelativeToNode(1, 3) <= maxy)
            //                 || std::find(nodeInfo.parents.cbegin(), nodeInfo.parents.cend(), "robotlocation") == nodeInfo.parents.cend()
            //                )
            //                && nodeDist < minDist)
            //            {
            //                minDist = nodeDist;
            //                closestNodeRef = nodeInfo.nodeRef;
            //            }
 
            //this doesn't allow objects to be at robot locations
            if (std::find(nodeInfo.parents.cbegin(), nodeInfo.parents.cend(), "robotlocation") == nodeInfo.parents.cend()
                && nodeDist < minDist)
            {
                minDist = nodeDist;
                closestNodeRef = nodeInfo.nodeRef;
            }
        }
 
        if (closestNodeRef)
        {
            ARMARX_INFO << "closest node to '" << object->getName() << "': '" << closestNodeRef->entityName << "' with dist: " << minDist;
        }
 
        if (closestNodeRef && minDist <= distanceThreshold)
        {
            EntityRefBasePtr objectRef = objectInstances->getEntityRefById(object->getId());
            result.push_back(PredicateInstance {predicateName, {objectRef, closestNodeRef}, true});
        }
    }
 
    return result;
}
 
ObjectAtPredicateProvider::CachedNodeInfo ObjectAtPredicateProvider::getCacheEntry(const std::string& nodeId)
{
    auto it = nodeInfoMap.find(nodeId);
    if (it != nodeInfoMap.end())
    {
        return it->second;
    }
 
    GraphNodePtr n = GraphNodePtr::dynamicCast(graphs->getEntityById(nodeId));
 
    armarx::FramedPosePtr globalPose = armarx::FramedPosePtr::dynamicCast(psr->resolveToGlobalPose(n));
    if (!globalPose)
    {
        ARMARX_WARNING << "Could not resolve the following to a global pose:\n" << armarx::FramedPosePtr::dynamicCast(n->getPose())->toEigen() << "\n for node '" << n->getName() << "'";
    }
 
    CachedNodeInfo newCacheEntry {n, graphs->getEntityRefById(nodeId), n->getPose()->frame, n->getAllParentsAsStringList(), globalPose};
    nodeInfoMap.insert({n->getId(), newCacheEntry});
 
    return newCacheEntry;
}