GraphNodePoseResolver.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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::ArmarXObjects::GraphNodePoseResolver
 * @author     Valerij Wittenbeck ( valerij.wittenbeck at student dot kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include <MemoryX/components/GraphNodePoseResolver/GraphNodePoseResolver.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <VirtualRobot/MathTools.h>
 
namespace memoryx
{
 
    GraphNodePoseResolver::GraphNodePoseResolver()
    {
    }
 
    void GraphNodePoseResolver::onInitComponent()
    {
        usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
        //        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
        usingProxy("PriorKnowledge");
    }
 
    void GraphNodePoseResolver::onConnectComponent()
    {
        wm =  getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
        prior = getProxy<PriorKnowledgeInterfacePrx>("PriorKnowledge");
 
        try
        {
            sleep(1); //let's wait for it a bit
            rsc = getProxy<armarx::RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
        }
        catch (...)
        {
            ARMARX_IMPORTANT << "RobotStateComponent proxy not found, continuing without...";
        }
 
        graphSegment = prior->getGraphSegment();
        objectInstances = wm->getObjectInstancesSegment();
 
        relativePlacesettingPositions["placesetting"]["cup"] = new armarx::FramedPosition(Eigen::Vector3f {180.f, 180.f, 0.f}, "roundtable", "");
    }
 
    void GraphNodePoseResolver::onDisconnectComponent()
    {
    }
 
    void GraphNodePoseResolver::onExitComponent()
    {
    }
 
    armarx::FramedPosePtr GraphNodePoseResolver::getRelativeNodePositionForObject(const std::string& objectClassName, const GraphNodeBasePtr& node)
    {
        auto objSeg = prior->getObjectClassesSegment();
 
        for (const auto& placesetting : relativePlacesettingPositions)
        {
            std::string settingName = placesetting.first;
 
            if (node->getName().find(settingName) != std::string::npos)
            {
                for (const auto& relativePosition : placesetting.second)
                {
                    if (objectClassName != relativePosition.first && !objSeg->isParentClass(objectClassName, relativePosition.first))
                    {
                        continue;
                    }
 
                    armarx::FramedPositionPtr pos = relativePosition.second;
                    auto poseMat = armarx::FramedPosePtr::dynamicCast(node->getPose())->toEigen();
                    auto offset = poseMat.block<3, 3>(0, 0) * pos->toEigen();
                    poseMat.block<3, 1>(0, 3) += offset;
                    return new armarx::FramedPose(poseMat, pos->frame, "");
                    break;
                }
            }
        }
 
        return armarx::FramedPosePtr();
    }
 
    GraphNodePoseResolver::CachedNodeInfo GraphNodePoseResolver::getCacheEntry(const std::string& nodeId)
    {
        std::unique_lock lock(cacheMutex);
        auto it = nodeInfoCache.find(nodeId);
        if (it != nodeInfoCache.end())
        {
            return it->second;
        }
 
        GraphNodePtr node = GraphNodePtr::dynamicCast(graphSegment->getEntityById(nodeId));
        const auto nodePose = armarx::FramedPosePtr::dynamicCast(node->getPose());
 
        armarx::FramedPosePtr globalPose = nodePose;
 
        if (nodePose->frame != armarx::GlobalFrame && !nodePose->frame.empty())
        {
            auto instances = objectInstances->getObjectInstancesByClass(nodePose->frame);
 
            if (instances.empty())
            {
                ARMARX_WARNING_S << "No object instances of object class " + nodePose->frame + " found";
                throw NodeNotResolveableException("No object instances of object class " + nodePose->frame + " found");
            }
 
            ObjectInstancePtr objInstance = ObjectInstancePtr::dynamicCast(instances.front());
 
            armarx::FramedPosePtr instancePose = objInstance->getPose();
            bool instanceFrameGlobal = instancePose->frame == armarx::GlobalFrame || instancePose->frame.empty();
            bool needToConvertToGlobal = !instanceFrameGlobal && rsc && rsc->getSynchronizedRobot() && rsc->getSynchronizedRobot()->getGlobalPose();
            ARMARX_CHECK_EXPRESSION(!needToConvertToGlobal) <<
                    "RobotStateComponent cannot be null when an object's frame is not global (object '" << objInstance->getName() << "', frame: '" << instancePose->frame << "')";
 
            Eigen::Matrix4f globalObjPose;
            if (needToConvertToGlobal)
            {
                globalObjPose = instancePose->toGlobal(rsc->getSynchronizedRobot())->toEigen();
            }
            else
            {
                globalObjPose = instancePose->toEigen();
            }
 
            Eigen::Matrix4f localNodePose = nodePose->toEigen();
            Eigen::Matrix4f globalNodePose = globalObjPose * localNodePose;
            globalPose = new armarx::FramedPose(globalNodePose, armarx::GlobalFrame, "");
        }
 
        CachedNodeInfo newInfo {node, globalPose, node->getAllParentsAsStringList()};
        nodeInfoCache.insert({node->getId(), newInfo});
        return newInfo;
    }
 
    GraphNodeBasePtr GraphNodePoseResolver::getNearestNodeToPoseImpl(const std::string& sceneName, const std::string& nodeParentName, const Eigen::Matrix4f& pose, bool ignoreOrientation, bool ignoreParent)
    {
        bool graphHasParent = graphSegment->getNodeFromSceneByName(sceneName, nodeParentName);
        float distDeltaThreshold = 200.f;
        float minDist = std::numeric_limits<float>::max();
        float minDegDist = std::numeric_limits<float>::max();
        float degDist = minDist;
        GraphNodePtr closestNode;
        Eigen::Matrix4f closestNodeEigenPose = Eigen::Matrix4f::Identity();
 
        std::unique_lock lock(cacheMutex);
        auto it = sceneNodesCache.find(sceneName);
        if (it == sceneNodesCache.end())
        {
            it = sceneNodesCache.insert({sceneName, graphSegment->getNodesByScene(sceneName)}).first;
        }
 
        for (const auto& node : it->second)
        {
            CachedNodeInfo nodeInfo;
            try
            {
                nodeInfo = getCacheEntry(node->getId());
            }
            catch (armarx::LocalException& e)
            {
                ARMARX_ERROR << e.what();
                continue;
            }
 
            //last term is for backwards-compability; where nodes don't have parents yet
            bool validParentship = ignoreParent ||
                                   std::find(nodeInfo.parents.cbegin(), nodeInfo.parents.cend(), nodeParentName) != nodeInfo.parents.cend() ||
                                   (!graphHasParent && nodeInfo.parents.empty());
            auto nodePoseEigen = nodeInfo.globalPose->toEigen();
 
            if (!validParentship || nodeInfo.node->getScene() != sceneName || nodeInfo.node->isMetaEntity())
            {
                continue;
            }
 
            if (!ignoreOrientation)
            {
                Eigen::Matrix3f diffRot = pose.block<3, 3>(0, 0) * nodePoseEigen.block<3, 3>(0, 0).inverse();
 
                Eigen::Matrix4f  diffPose = Eigen::Matrix4f::Identity();
                diffPose.block<3, 3>(0, 0) = diffRot;
                Eigen::Vector3f axis;
                VirtualRobot::MathTools::eigen4f2axisangle(diffPose, axis, degDist);
                degDist *= 180 / M_PI;
            }
 
            const float dist = (pose.block<2, 1>(0, 3) - nodePoseEigen.block<2, 1>(0, 3)).norm();
            const float candidateDist = (closestNodeEigenPose.block<2, 1>(0, 3) - nodePoseEigen.block<2, 1>(0, 3)).norm();
 
            ARMARX_DEBUG << "for query (" << pose(0, 3) << ", " << pose(1, 3) << "), considering node: '" << nodeInfo.node->getName() << "', scene '" << nodeInfo.node->getScene() << "'"
                         << ", distances: " << dist <<  ", " << candidateDist << ", " << degDist;
 
            const bool inAmbiguousZone = candidateDist < distDeltaThreshold;
 
            if (!closestNode || (inAmbiguousZone && fabs(degDist) < minDegDist) || (!inAmbiguousZone && fabs(dist) <= minDist))
            {
                minDist = dist;
                minDegDist = degDist;
                closestNode = nodeInfo.node;
                closestNodeEigenPose = nodePoseEigen;
            }
        }
 
        ARMARX_CHECK_EXPRESSION(closestNode) << "Could not find closest node to query: '" << sceneName << "', '" << nodeParentName << "',\n" << pose;
        return closestNode;
    }
 
    armarx::FramedPoseBasePtr GraphNodePoseResolver::resolveToGlobalPose(const GraphNodeBasePtr& node, const Ice::Current&)
    {
        ARMARX_CHECK_EXPRESSION(node) << "input node cannot be null";
 
        return getCacheEntry(node->getId()).globalPose;
    }
 
    std::string GraphNodePoseResolver::getNearestNodeIdToPosition(const std::string& sceneName, const std::string& nodeParentName, Ice::Float x, Ice::Float y, const Ice::Current&)
    {
        return getNearestNodeToPosition(sceneName, nodeParentName, x, y)->getId();
    }
 
    GraphNodeBasePtr GraphNodePoseResolver::getNearestNodeToPosition(const std::string& sceneName, const std::string& nodeParentName, Ice::Float x, Ice::Float y, const Ice::Current&)
    {
        Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
        pose(0, 3) = x;
        pose(1, 3) = y;
 
        return getNearestNodeToPoseImpl(sceneName, nodeParentName, pose, true, nodeParentName.empty());
    }
 
    std::string GraphNodePoseResolver::getNearestNodeIdToPose(const std::string& sceneName, const std::string& nodeParentName, const armarx::FramedPoseBasePtr& pose, const Ice::Current&)
    {
        return getNearestNodeToPose(sceneName, nodeParentName, pose)->getId();
    }
 
    GraphNodeBasePtr GraphNodePoseResolver::getNearestNodeToPose(const std::string& sceneName, const std::string& nodeParentName, const armarx::FramedPoseBasePtr& pose, const Ice::Current&)
    {
        auto poseEigen = armarx::FramedPosePtr::dynamicCast(pose)->toEigen();
        return getNearestNodeToPoseImpl(sceneName, nodeParentName, poseEigen, false, nodeParentName.empty());
    }
 
    std::string GraphNodePoseResolver::getNearestRobotLocationNodeId(const GraphNodeBasePtr& node, const Ice::Current&)
    {
        return getNearestRobotLocationNode(node)->getId();
    }
 
    GraphNodeBasePtr GraphNodePoseResolver::getNearestRobotLocationNode(const GraphNodeBasePtr& node, const Ice::Current&)
    {
        ARMARX_CHECK_EXPRESSION(node) << "node argument cannot be null";
 
        const CachedNodeInfo& inputNodeInfo = getCacheEntry(node->getId());
        return getNearestNodeToPoseImpl(inputNodeInfo.node->getScene(), "robotlocation", inputNodeInfo.globalPose->toEigen());
    }
 
    void GraphNodePoseResolver::forceRefetch(const std::string& nodeId, const Ice::Current&)
    {
        std::unique_lock lock(cacheMutex);
        auto it = nodeInfoCache.find(nodeId);
        if (it != nodeInfoCache.end())
        {
            nodeInfoCache.erase(it);
        }
        sceneNodesCache.clear();
    }
 
    void GraphNodePoseResolver::forceRefetchForAll(const Ice::Current&)
    {
        std::unique_lock lock(cacheMutex);
        nodeInfoCache.clear();
        sceneNodesCache.clear();
    }
}