GraphMemorySegment.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "GraphMemorySegment.h"
 
#include <limits>
#include <unordered_map>
#include <unordered_set>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <MemoryX/libraries/memorytypes/variants/GraphNode/GraphNode.h>
 
memoryx::GraphMemorySegment::GraphMemorySegment(CollectionInterfacePrx entityCollection,
                                                Ice::CommunicatorPtr ic,
                                                bool useMongoIds) :
    PersistentEntitySegment(entityCollection, ic, useMongoIds), GraphMemorySegmentBase()
{
}
 
memoryx::GraphMemorySegment::~GraphMemorySegment()
{
}
 
Ice::StringSeq
memoryx::GraphMemorySegment::getScenes(const Ice::Current& c) const
{
    std::unordered_set<std::string> names{};
    const auto ids = getAllEntityIds(c);
 
    for (const auto& id : ids)
    {
        auto elem = getNodeById(id, c);
 
        if (elem)
        {
            names.insert(elem->getScene());
        }
    }
 
    return Ice::StringSeq{names.begin(), names.end()};
}
 
memoryx::GraphNodeBaseList
memoryx::GraphMemorySegment::getNodesByScene(const std::string& sceneName, const Ice::Current& c)
{
    memoryx::GraphNodeBaseList result{};
    auto filtered = getEntitiesByAttrValue(memoryx::GraphNode::GRAPH_NODE_ATTR_SCENE, sceneName, c);
 
    for (auto& elem : filtered)
    {
        auto ptr = memoryx::GraphNodeBasePtr::dynamicCast(elem);
 
        if (ptr)
        {
            result.push_back(ptr);
        }
    }
 
    return result;
}
 
memoryx::GraphNodeBasePtr
memoryx::GraphMemorySegment::getNodeById(const std::string& entityId, const Ice::Current& c)
{
    return memoryx::GraphNodeBasePtr::dynamicCast(getEntityById(entityId, c));
}
 
memoryx::GraphNodeBasePtr
memoryx::GraphMemorySegment::getNodeById(const std::string& entityId, const Ice::Current& c) const
{
    return memoryx::GraphNodeBasePtr::dynamicCast(getEntityById(entityId, c));
}
 
memoryx::GraphNodeBasePtr
memoryx::GraphMemorySegment::getNodeByName(const std::string& entityName, const Ice::Current& c)
{
    return memoryx::GraphNodeBasePtr::dynamicCast(getEntityByName(entityName, c));
}
 
memoryx::GraphNodeBaseList
memoryx::GraphMemorySegment::getAllNodes(const Ice::Current& c)
{
    memoryx::GraphNodeBaseList nodes{};
    const auto ids = getAllEntityIds(c);
 
    for (const auto& id : ids)
    {
        auto elem = getNodeById(id, c);
 
        if (elem)
        {
            nodes.push_back(elem);
        }
    }
 
    return nodes;
}
 
void
memoryx::GraphMemorySegment::clearScene(const std::string& sceneName, const Ice::Current& c)
{
    ARMARX_INFO_S << "GraphMemorySegment: clearing scene " << sceneName << "...";
    auto filtered = getEntitiesByAttrValue(memoryx::GraphNode::GRAPH_NODE_ATTR_SCENE, sceneName, c);
 
    for (auto& elem : filtered)
    {
        removeEntity(elem->getId(c));
    }
 
    ARMARX_INFO_S << "GraphMemorySegment: clearing scene " << sceneName << ". Removed "
                  << filtered.size() << " elements";
}
 
bool
memoryx::GraphMemorySegment::removeNode(const std::string& nodeId, const Ice::Current&)
{
    GraphNodeBasePtr node = getNodeById(nodeId);
    auto allnodes = getAllNodes();
    for (auto& curNode : allnodes)
    {
        if (curNode->removeAdjacentNode(nodeId))
        {
            updateEntity(curNode->getId(), curNode);
        }
    }
    if (node)
    {
        removeEntity(nodeId);
        return true;
    }
    return false;
}
 
bool
memoryx::GraphMemorySegment::removeEdge(const std::string& startNodeId,
                                        const std::string& endNodeId,
                                        const Ice::Current&)
{
    GraphNodeBasePtr startNode = getNodeById(startNodeId);
    if (startNode)
    {
        if (startNode->removeAdjacentNode(endNodeId))
        {
            updateEntity(startNodeId, startNode);
            return true;
        }
    }
    return false;
}
 
std::string
memoryx::GraphMemorySegment::addNode(const memoryx::GraphNodeBasePtr& node, const Ice::Current& c)
{
    ARMARX_INFO_S << "GraphMemorySegment adding node " << (node ? node->getName() : "NULL node");
    return addEntity(node, c);
}
 
bool
memoryx::GraphMemorySegment::addEdge(const std::string& fromId,
                                     const std::string& toId,
                                     const Ice::Current& c)
{
    ARMARX_INFO_S << "GraphMemorySegment adding edge " << fromId << " -> " << toId << "...";
    auto fromNode = getNodeById(fromId, c);
 
    if (!fromNode)
    {
        ARMARX_ERROR_S << "GraphMemorySegment addEdge: Id " << fromId
                       << " does not reference a node";
        return false;
    }
 
    auto toNode = getNodeById(toId, c);
 
    if (!toNode)
    {
        ARMARX_ERROR_S << "GraphMemorySegment addEdge: Id " << toId << " does not reference a node";
        return false;
    }
 
    //this does not change the data stored in the segment
    fromNode->addAdjacentNode(getEntityRefById(toId, c), c);
    //update memory
    updateEntity(fromId, fromNode, c);
    ARMARX_INFO_S << "GraphMemorySegment adding edge " << fromId << " -> " << toId << "done!";
    return true;
}
 
bool
memoryx::GraphMemorySegment::hasScene(const ::std::string& sceneName, const ::Ice::Current& c)
{
    return getEntitiesByAttrValue(memoryx::GraphNode::GRAPH_NODE_ATTR_SCENE, sceneName, c).size() >
           0;
}
 
bool
memoryx::GraphMemorySegment::hasNodeWithName(const ::std::string& sceneName,
                                             const ::std::string& nodeName,
                                             const ::Ice::Current&)
{
    auto nodes = getNodesByScene(sceneName);
 
    for (const auto& node : nodes)
    {
        if (node->getName() == nodeName)
        {
            return true;
        }
    }
 
    return false;
}
 
memoryx::GraphNodeBasePtr
memoryx::GraphMemorySegment::getNodeFromSceneByName(const ::std::string& sceneName,
                                                    const ::std::string& nodeName,
                                                    const ::Ice::Current&)
{
    auto nodes = getNodesByScene(sceneName);
 
    for (const auto& node : nodes)
    {
        if (node->getName() == nodeName)
        {
            return node;
        }
    }
 
    return memoryx::GraphNodeBasePtr{};
}
 
//std::string memoryx::GraphMemorySegment::getIdOfNearestNode(const ::std::string& sceneName, ::Ice::Float x, ::Ice::Float y, const ::Ice::Current&)
//{
//    auto distQuad = [x, y](armarx::FramedPoseBasePtr && node)
//    {
//        float deltaX = x - node->position->x;
//        float deltaY = y - node->position->y;
//        return deltaX * deltaX + deltaY * deltaY;
//    };
 
//    auto nodes = getNodesByScene(sceneName);
//    std::string nearestId {};
//    float distance = std::numeric_limits<float>::max();
 
//    for (const auto& nodeBase : nodes)
//    {
//        GraphNodePtr node = GraphNodePtr::dynamicCast(nodeBase);
//        if (node->isMetaEntity())
//        {
//            continue;
//        }
 
//        if (node->getPose()->frame == armarx::GlobalFrame || node->getPose()->frame.empty())
//        {
//            float currentDistance = distQuad(node->getPose());
 
//            if (currentDistance < distance)
//            {
//                nearestId = node->getId();
//                distance = currentDistance;
//            }
//        }
//    }
 
//    if (nearestId.empty())
//    {
//        ARMARX_WARNING_S << "Could not find nearest node for " << VAROUT(x) << VAROUT(y) << VAROUT(type) << VAROUT(sceneName);
//    }
 
//    return nearestId;
//}
 
memoryx::GraphNodeBaseList
memoryx::GraphMemorySegment::aStar(const ::std::string& idFrom,
                                   const ::std::string& idTo,
                                   const ::Ice::Current&)
{
    //check everything
    auto start = getNodeById(idFrom);
    auto goal = getNodeById(idTo);
 
    if (!start)
    {
        std::stringstream s{};
        s << "aStar: Node from: No node with ID: " << idFrom;
        ARMARX_ERROR_S << s.str();
        throw memoryx::EntityNotFoundException{s.str()};
    }
 
    if (!goal)
    {
        std::stringstream s{};
        ARMARX_ERROR_S << s.str();
        s << "aStar: Node to: No node with ID: " << idTo;
        throw memoryx::EntityNotFoundException{s.str()};
    }
 
    ARMARX_VERBOSE_S << "memoryx::GraphMemorySegment::aStar from " << start->getPose()->frame
                     << " to " << goal->getPose()->frame;
 
 
    if (start->getScene() != goal->getScene())
    {
        std::stringstream s{};
        ARMARX_ERROR_S << s.str();
        s << "aStar: Nodes from and to belong to different scenes: " << start->getScene() << " | "
          << goal->getScene();
        throw armarx::InvalidArgumentException{s.str()};
    }
 
    /// TODO add some mutex
    //copy state (in case the state gets altered during search) + memoryx::GraphNodeBasePtr now can be compared (ptr addresses are unique)
    std::unordered_map<std::string, memoryx::GraphNodeBasePtr> nodes{};
 
    for (const auto& node : getNodesByScene(start->getScene()))
    {
        nodes[node->getId()] = node;
    }
 
    //override start and goal to keep uniqueness
    nodes[start->getId()] = start;
    nodes[goal->getId()] = goal;
 
 
    //init
    auto dist = [](memoryx::GraphNodeBasePtr n1, memoryx::GraphNodeBasePtr n2)
    {
        auto p1 = n1->getPose();
        auto p2 = n2->getPose();
        const float dX = p1->position->x - p2->position->x;
        const float dY = p1->position->y - p2->position->y;
        return std::sqrt(dX * dX + dY * dY);
    };
 
    memoryx::GraphNodeBaseList path{};
    memoryx::GraphNodeBaseList closedSet{};
    memoryx::GraphNodeBaseList openSet{};
    openSet.push_back(start);
 
    std::unordered_map<std::string, float> gScore;
    gScore[start->getId()] = 0.f;
    std::unordered_map<std::string, float> fScore;
    fScore[start->getId()] = gScore.at(start->getId()) + dist(start, goal);
    std::unordered_map<std::string, memoryx::GraphNodeBasePtr> cameFrom;
    cameFrom[goal->getId()] = start; //in case start==goal
 
    ARMARX_VERBOSE_S << "memoryx::GraphMemorySegment::aStar from " << idFrom << " to " << idTo
                     << ". Start path search.";
 
    while (!openSet.empty())
    {
        //find best neighbour
        auto currentIT = std::min_element(
            openSet.begin(),
            openSet.end(),
            [&fScore](const memoryx::GraphNodeBasePtr& a, const memoryx::GraphNodeBasePtr& b)
            { return fScore.at(a->getId()) < fScore.at(b->getId()); });
        assert(currentIT != openSet.end());
        memoryx::GraphNodeBasePtr current = *currentIT;
 
 
        //check if done
        if (current->getId() == goal->getId())
        {
            auto cameFromNode = goal;
 
            while (cameFromNode->getId() != start->getId())
            {
                path.insert(path.begin(), cameFromNode);
                cameFromNode = cameFrom.at(cameFromNode->getId());
            }
 
            path.insert(path.begin(), start);
            break;
        }
 
        openSet.erase(currentIT);
        closedSet.push_back(current);
 
        //update
        for (int i = 0; i < current->getOutdegree(); i++)
        {
            auto neighbor = nodes.at(current->getAdjacentNode(i)->getEntity()->getId());
 
            assert(neighbor);
 
            if (std::find(closedSet.begin(), closedSet.end(), neighbor) != closedSet.end())
            {
                continue;
            }
 
            float tentativeGScore = gScore.at(current->getId()) + dist(current, neighbor);
            bool notInOS = std::find(openSet.begin(), openSet.end(), neighbor) == openSet.end();
 
            if (notInOS || tentativeGScore < gScore.at(neighbor->getId()))
            {
                cameFrom[neighbor->getId()] = current;
                gScore[neighbor->getId()] = tentativeGScore;
                fScore[neighbor->getId()] = tentativeGScore + dist(neighbor, goal);
 
                if (notInOS)
                {
                    openSet.push_back(neighbor);
                }
            }
        }
    }
 
    ARMARX_VERBOSE_S << "memoryx::GraphMemorySegment::aStar from " << idFrom << " to " << idTo
                     << ". Done!";
    return path;
}