GraphNode.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::Core
* @author     Raphael Grimm <raphael dot grimm at kit dot edu>
* @date       2014
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
#include "GraphNode.h"
 
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/core/UserException.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
//init static class members
const std::string memoryx::GraphNode::GRAPH_NODE_ATTR_SCENE{"scene"};
 
//constants
#define GRAPH_NODE_ATTR_SCENE memoryx::GraphNode::GRAPH_NODE_ATTR_SCENE
#define GRAPH_NODE_ATTR_POSE "pose"
#define GRAPH_NODE_ATTR_ADJ_NODES "adjacentNodes"
 
memoryx::GraphNode::GraphNode()
{
}
 
memoryx::GraphNode::GraphNode(float x,
                              float y,
                              float alpha,
                              const std::string& nodeName,
                              const std::string& scene)
{
    Eigen::AngleAxisf aa(alpha, Eigen::Vector3f(0, 0, 1));
    Eigen::Matrix3f rot = aa.toRotationMatrix();
    setName(nodeName);
    putAttribute(GRAPH_NODE_ATTR_POSE,
                 ::armarx::FramedPoseBasePtr{new ::armarx::FramedPose(
                     rot, Eigen::Vector3f(x, y, 0), armarx::GlobalFrame, "")});
    putAttribute(GRAPH_NODE_ATTR_SCENE, scene);
    clearAdjacentNodes(); //init adjacent node attr
}
 
memoryx::GraphNode::GraphNode(armarx::FramedPoseBasePtr pose,
                              const std::string& nodeName,
                              const std::string& scene)
{
    setName(nodeName);
 
    if (pose)
    {
        putAttribute(GRAPH_NODE_ATTR_POSE, pose);
    }
 
    putAttribute(GRAPH_NODE_ATTR_SCENE, scene);
    clearAdjacentNodes(); //init adjacent node attr
}
 
memoryx::GraphNode::~GraphNode()
{
}
 
armarx::FramedPoseBasePtr
memoryx::GraphNode::getPose(const Ice::Current&) const
{
    return getAttributeValue(GRAPH_NODE_ATTR_POSE)->get<armarx::FramedPoseBase>();
}
 
void
memoryx::GraphNode::setPose(const armarx::FramedPoseBasePtr& pose, const Ice::Current&)
{
    putAttribute(GRAPH_NODE_ATTR_POSE, pose);
}
 
std::string
memoryx::GraphNode::getScene(const Ice::Current&) const
{
    return getAttributeValue(GRAPH_NODE_ATTR_SCENE)->get<std::string>();
}
 
void
memoryx::GraphNode::setScene(const std::string& scene, const Ice::Current&)
{
    putAttribute(GRAPH_NODE_ATTR_SCENE, scene);
}
 
int
memoryx::GraphNode::getOutdegree(const Ice::Current&) const
{
    return getAttribute(GRAPH_NODE_ATTR_ADJ_NODES)->size();
}
 
memoryx::EntityRefBasePtr
memoryx::GraphNode::getAdjacentNode(Ice::Int i, const Ice::Current&)
{
    auto deg = getOutdegree();
 
    if (i < 0 || i >= deg)
    {
        std::stringstream s;
        s << "Graph node: Access to adjacent node with index " << i << "! (outdegree is " << deg
          << ")";
        throw armarx::IndexOutOfBoundsException{s.str()};
    }
 
    return armarx::VariantPtr::dynamicCast(getAttribute(GRAPH_NODE_ATTR_ADJ_NODES)->getValueAt(i))
        ->getClass<memoryx::EntityRefBase>();
}
 
memoryx::EntityRefBasePtr
memoryx::GraphNode::getAdjacentNodeById(const std::string& nodeId, const Ice::Current& c)
{
    int nodes = getOutdegree();
    for (int i = 0; i < nodes; ++i)
    {
        auto node = getAdjacentNode(i);
        if (node->entityId == nodeId)
        {
            return node;
        }
    }
    return nullptr;
}
 
memoryx::GraphNodeBaseList
memoryx::GraphNode::getAdjacentNodes(const Ice::Current& c)
{
    memoryx::GraphNodeBaseList result;
    int nodes = getOutdegree();
    for (int i = 0; i < nodes; ++i)
    {
        auto nodeEntity = getAdjacentNode(i);
        auto graphNode = GraphNodeBasePtr::dynamicCast(nodeEntity->getEntity());
        ARMARX_CHECK_EXPRESSION(graphNode);
        result.push_back(graphNode);
    }
    return result;
}
 
void
memoryx::GraphNode::addAdjacentNode(const memoryx::EntityRefBasePtr& newAdjacentNode,
                                    const Ice::Current& c)
{
    ARMARX_VERBOSE_S << "node " << getName() << " (" << getId() << ") in scene " << getScene()
                     << ": adding new adjacent node...";
 
    if (!memoryx::GraphNodeBasePtr::dynamicCast(newAdjacentNode->getEntity(c)))
    {
        std::stringstream s{};
        s << "Expected " << memoryx::GraphNodeBase::ice_id(c);
        throw armarx::InvalidTypeException{s.str()};
    }
 
    getAttribute(GRAPH_NODE_ATTR_ADJ_NODES)
        ->addValue(armarx::VariantBasePtr{new armarx::Variant{newAdjacentNode}});
    ARMARX_VERBOSE_S << "adding new adjacent node: done! (new outdegree: " << getOutdegree() << ")";
}
 
void
memoryx::GraphNode::clearAdjacentNodes(const Ice::Current&)
{
    putAttribute(
        memoryx::EntityAttributeBasePtr{new memoryx::EntityAttribute{GRAPH_NODE_ATTR_ADJ_NODES}});
}
 
bool
memoryx::GraphNode::removeAdjacentNode(const std::string& nodeId, const Ice::Current&)
{
    int nodes = getOutdegree();
    for (int i = 0; i < nodes; ++i)
    {
        auto node = getAdjacentNode(i);
        if (node->entityId == nodeId)
        {
            getAttribute(GRAPH_NODE_ATTR_ADJ_NODES)->removeValueAt(i);
            return true;
        }
    }
    return false;
}
 
float
memoryx::GraphNode::getYawAngle() const
{
    Eigen::Matrix3f mat = armarx::QuaternionPtr::dynamicCast(getPose()->orientation)->toEigen();
    Eigen::Vector3f rpy = mat.eulerAngles(0, 1, 2);
    return rpy[2];
}
 
Ice::ObjectPtr
memoryx::GraphNode::ice_clone() const
{
    return this->clone();
}
 
memoryx::GraphNodePtr
memoryx::GraphNode::clone(const Ice::Current&) const
{
    GraphNodePtr ret = new GraphNode(*this);
    //    ret->deepCopy(*this);
    return ret;
}