GraphNode.cpp
Go to the documentation of this file.
1 /*
2 * This file is part of ArmarX.
3 *
4 * ArmarX is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
6 * published by the Free Software Foundation.
7 *
8 * ArmarX is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 *
16 * @package MemoryX::Core
17 * @author Raphael Grimm <raphael dot grimm at kit dot edu>
18 * @date 2014
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 #include "GraphNode.h"
23 
24 #include <ArmarXCore/interface/core/UserException.h>
27 #include <Eigen/Geometry>
29 
30 //init static class members
32 {
33  "scene"
34 };
35 
36 //constants
37 #define GRAPH_NODE_ATTR_SCENE memoryx::GraphNode::GRAPH_NODE_ATTR_SCENE
38 #define GRAPH_NODE_ATTR_POSE "pose"
39 #define GRAPH_NODE_ATTR_ADJ_NODES "adjacentNodes"
40 
41 memoryx::GraphNode::GraphNode()
42 {
43 
44 }
45 
46 memoryx::GraphNode::GraphNode(float x, float y, float alpha, const std::string& nodeName, const std::string& scene)
47 {
48  Eigen::AngleAxisf aa(alpha, Eigen::Vector3f(0, 0, 1));
49  Eigen::Matrix3f rot = aa.toRotationMatrix();
50  setName(nodeName);
51  putAttribute(GRAPH_NODE_ATTR_POSE, ::armarx::FramedPoseBasePtr {new ::armarx::FramedPose(
52  rot,
53  Eigen::Vector3f(x, y, 0),
55  "")
56  });
57  putAttribute(GRAPH_NODE_ATTR_SCENE, scene);
58  clearAdjacentNodes();//init adjacent node attr
59 }
60 
61 memoryx::GraphNode::GraphNode(armarx::FramedPoseBasePtr pose, const std::string& nodeName, const std::string& scene)
62 {
63  setName(nodeName);
64 
65  if (pose)
66  {
67  putAttribute(GRAPH_NODE_ATTR_POSE, pose);
68  }
69 
70  putAttribute(GRAPH_NODE_ATTR_SCENE, scene);
71  clearAdjacentNodes();//init adjacent node attr
72 }
73 
75 {
76 }
77 
78 armarx::FramedPoseBasePtr memoryx::GraphNode::getPose(const Ice::Current&) const
79 {
80  return getAttributeValue(GRAPH_NODE_ATTR_POSE)->get<armarx::FramedPoseBase>();
81 }
82 
83 void memoryx::GraphNode::setPose(const armarx::FramedPoseBasePtr& pose, const Ice::Current&)
84 {
85  putAttribute(GRAPH_NODE_ATTR_POSE, pose);
86 }
87 
88 std::string memoryx::GraphNode::getScene(const Ice::Current&) const
89 {
90  return getAttributeValue(GRAPH_NODE_ATTR_SCENE)->get<std::string>();
91 }
92 
93 void memoryx::GraphNode::setScene(const std::string& scene, const Ice::Current&)
94 {
95  putAttribute(GRAPH_NODE_ATTR_SCENE, scene);
96 }
97 
98 int memoryx::GraphNode::getOutdegree(const Ice::Current&) const
99 {
100  return getAttribute(GRAPH_NODE_ATTR_ADJ_NODES)->size();
101 }
102 
103 memoryx::EntityRefBasePtr memoryx::GraphNode::getAdjacentNode(Ice::Int i, const Ice::Current&)
104 {
105  auto deg = getOutdegree();
106 
107  if (i < 0 || i >= deg)
108  {
109  std::stringstream s;
110  s << "Graph node: Access to adjacent node with index " << i << "! (outdegree is " << deg << ")";
111  throw armarx::IndexOutOfBoundsException {s.str()};
112  }
113 
114  return armarx::VariantPtr::dynamicCast(getAttribute(GRAPH_NODE_ATTR_ADJ_NODES)->getValueAt(i))->getClass< memoryx::EntityRefBase>();
115 }
116 
117 memoryx::EntityRefBasePtr memoryx::GraphNode::getAdjacentNodeById(const std::string& nodeId, const Ice::Current& c)
118 {
119  int nodes = getOutdegree();
120  for (int i = 0; i < nodes; ++i)
121  {
122  auto node = getAdjacentNode(i);
123  if (node->entityId == nodeId)
124  {
125  return node;
126  }
127  }
128  return nullptr;
129 }
130 
131 memoryx::GraphNodeBaseList memoryx::GraphNode::getAdjacentNodes(const Ice::Current& c)
132 {
133  memoryx::GraphNodeBaseList result;
134  int nodes = getOutdegree();
135  for (int i = 0; i < nodes; ++i)
136  {
137  auto nodeEntity = getAdjacentNode(i);
138  auto graphNode = GraphNodeBasePtr::dynamicCast(nodeEntity->getEntity());
139  ARMARX_CHECK_EXPRESSION(graphNode);
140  result.push_back(graphNode);
141  }
142  return result;
143 }
144 
145 void memoryx::GraphNode::addAdjacentNode(const memoryx::EntityRefBasePtr& newAdjacentNode, const Ice::Current& c)
146 {
147  ARMARX_VERBOSE_S << "node " << getName() << " (" << getId() << ") in scene " << getScene()
148  << ": adding new adjacent node...";
149 
150  if (!memoryx::GraphNodeBasePtr::dynamicCast(newAdjacentNode->getEntity(c)))
151  {
152  std::stringstream s {};
153  s << "Expected " << memoryx::GraphNodeBase::ice_id(c);
154  throw armarx::InvalidTypeException {s.str()};
155  }
156 
157  getAttribute(GRAPH_NODE_ATTR_ADJ_NODES)->addValue(armarx::VariantBasePtr {new armarx::Variant{newAdjacentNode}});
158  ARMARX_VERBOSE_S << "adding new adjacent node: done! (new outdegree: " << getOutdegree() << ")";
159 }
160 
161 void memoryx::GraphNode::clearAdjacentNodes(const Ice::Current&)
162 {
163  putAttribute(memoryx::EntityAttributeBasePtr {new memoryx::EntityAttribute{GRAPH_NODE_ATTR_ADJ_NODES}});
164 }
165 
166 bool memoryx::GraphNode::removeAdjacentNode(const std::string& nodeId, const Ice::Current&)
167 {
168  int nodes = getOutdegree();
169  for (int i = 0; i < nodes; ++i)
170  {
171  auto node = getAdjacentNode(i);
172  if (node->entityId == nodeId)
173  {
174  getAttribute(GRAPH_NODE_ATTR_ADJ_NODES)->removeValueAt(i);
175  return true;
176  }
177  }
178  return false;
179 }
180 
182 {
183  Eigen::Matrix3f mat = armarx::QuaternionPtr::dynamicCast(getPose()->orientation)->toEigen();
184  Eigen::Vector3f rpy = mat.eulerAngles(0, 1, 2);
185  return rpy[2];
186 }
187 
189 {
190  return this->clone();
191 }
192 
194 {
195  GraphNodePtr ret = new GraphNode(*this);
196  // ret->deepCopy(*this);
197  return ret;
198 
199 }
200 
201 
memoryx::GraphNode::addAdjacentNode
void addAdjacentNode(const ::memoryx::EntityRefBasePtr &newAdjacentNode, const ::Ice::Current &c=Ice::emptyCurrent) override
Definition: GraphNode.cpp:145
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
armarx::aron::ret
ReaderT::InputType T & ret
Definition: rw.h:21
memoryx::GraphNode::clone
GraphNodePtr clone(const Ice::Current &=Ice::emptyCurrent) const
Definition: GraphNode.cpp:193
memoryx::GraphNode::getAdjacentNodeById
::memoryx::EntityRefBasePtr getAdjacentNodeById(const std::string &nodeId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: GraphNode.cpp:117
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:37
memoryx::GraphNode::getOutdegree
int getOutdegree(const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: GraphNode.cpp:98
memoryx::GraphNode::getPose
::armarx::FramedPoseBasePtr getPose(const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: GraphNode.cpp:78
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
memoryx::GraphNode::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: GraphNode.cpp:188
GRAPH_NODE_ATTR_ADJ_NODES
#define GRAPH_NODE_ATTR_ADJ_NODES
Definition: GraphNode.cpp:39
memoryx::GraphNode::~GraphNode
~GraphNode() override
Definition: GraphNode.cpp:74
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
memoryx::GraphNode::getAdjacentNode
::memoryx::EntityRefBasePtr getAdjacentNode(::Ice::Int i, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: GraphNode.cpp:103
IceInternal::Handle<::armarx::VariantBase >
memoryx::VariantType::GraphNode
const armarx::VariantTypeId GraphNode
Definition: GraphNode.h:39
memoryx::GraphNode::setScene
void setScene(const std::string &scene, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: GraphNode.cpp:93
FramedPose.h
GRAPH_NODE_ATTR_SCENE
#define GRAPH_NODE_ATTR_SCENE
Definition: GraphNode.cpp:37
GraphNode.h
GRAPH_NODE_ATTR_POSE
#define GRAPH_NODE_ATTR_POSE
Definition: GraphNode.cpp:38
memoryx::GraphNode::clearAdjacentNodes
void clearAdjacentNodes(const ::Ice::Current &=Ice::emptyCurrent) override
Definition: GraphNode.cpp:161
memoryx::GraphNode::GRAPH_NODE_ATTR_SCENE
static const std::string GRAPH_NODE_ATTR_SCENE
Definition: GraphNode.h:59
memoryx::GraphNode::getAdjacentNodes
memoryx::GraphNodeBaseList getAdjacentNodes(const Ice::Current &c=Ice::emptyCurrent) override
Definition: GraphNode.cpp:131
ExpressionException.h
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
memoryx::GraphNode::getYawAngle
float getYawAngle() const
Definition: GraphNode.cpp:181
memoryx::EntityAttribute
Attribute of MemoryX entities.
Definition: EntityAttribute.h:48
armarx::VariantType::Int
const VariantTypeId Int
Definition: Variant.h:916
ARMARX_VERBOSE_S
#define ARMARX_VERBOSE_S
Definition: Logging.h:200
memoryx::GraphNode::setPose
void setPose(const ::armarx::FramedPoseBasePtr &pose, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: GraphNode.cpp:83
memoryx::GraphNode::removeAdjacentNode
bool removeAdjacentNode(const std::string &nodeId, const Ice::Current &) override
Definition: GraphNode.cpp:166
armarx::aron::type::ObjectPtr
std::shared_ptr< Object > ObjectPtr
Definition: Object.h:36
Logging.h
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
memoryx::GraphNode::getScene
::std::string getScene(const ::Ice::Current &=Ice::emptyCurrent) const override
Definition: GraphNode.cpp:88