WorkingMemoryExample.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::WorkingMemoryExample
17 * @author (kozlov at kit dot edu)
18 * @date 2012
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 #include "WorkingMemoryExample.h"
24 
26 
31 
32 #include <MemoryX/interface/memorytypes/MemoryEntities.h>
33 
37 
38 #include <fstream>
39 #include <thread>
40 
41 
42 namespace memoryx
43 {
45  {
46  std::cout << "onInitComponent" << std::endl;
47 
48  usingProxy("WorkingMemory");
49  usingProxy("LongtermMemory");
50  usingProxy("RobotStateComponent");
51 
52 
53  // don't request topic here, see below why
54  // usingTopic("WorkingMemoryUpdates");
55 
56  testToRun = getProperty<std::string>("TestToRun").getValue();
57  }
58 
59 
61  {
62  std::cout << "onStartComponent" << std::endl;
63 
64  std::cout << "Starting WorkingMemoryExample" << std::endl;
65 
66  const Ice::CommunicatorPtr ic = getIceManager()->getCommunicator();
67 
68  memoryPrx = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
69  longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>("LongtermMemory");
70  robotStateComponentPrx = getProxy<armarx::RobotStateComponentInterfacePrx>("RobotStateComponent");
71 
72  // load snapshot, if specified
73  const std::string snapshotName = getProperty<std::string>("SnapshotName").getValue();
74  ARMARX_INFO << "Snapshot name: " << snapshotName;
75 
76  if (!snapshotName.empty())
77  {
78  longtermMemoryPrx->loadWorkingMemorySnapshot(snapshotName, memoryPrx);
79  ARMARX_INFO << "Snapshot succcessfully loaded: " << snapshotName;
80  }
81  else
82  {
83  ARMARX_ERROR << "SnapshotName parameter must be specified!";
84  }
85 
86  if (testToRun == "UpdateNotifications")
87  {
88  // Note, that all topics that have been spcified in onInitComponent are registered *after* the onConnectComponent method.
89  // So for this special test case where the topic is neede within the onConnectComponent method we must subscribe "by hand" in this method
90  // to be sure that the topic is available for our testcase.
91  usingTopic("WorkingMemoryUpdates");
92  testUpdateObserver();
93  }
94  else if (testToRun == "FileTransfer")
95  {
96  testFileTransfer();
97  }
98  else if (testToRun == "ObjectMove")
99  {
100  testObjectMovement();
101  }
102  else if (testToRun == "SnapshotLoad")
103  {
104  testSnapshots();
105  }
106  else if (testToRun == "CommonPlaces")
107  {
108  testCommonPlaces();
109  }
110  else if (testToRun == "Agent")
111  {
112  testAgent();
113  }
114  else
115  {
116  std::cout << "Unknown test name: " << testToRun << std::endl;
117  }
118  }
119 
120  void WorkingMemoryExample::reportEntityCreated(const std::string& segmentName, const memoryx::EntityBasePtr& entity,
121  const Ice::Current&)
122  {
123  std::cout << "Entity created: " << EntityPtr::dynamicCast(entity) << std::endl << std::endl;
124  }
125 
126  void WorkingMemoryExample::reportEntityUpdated(const std::string& segmentName,
127  const memoryx::EntityBasePtr& entityOld,
128  const memoryx::EntityBasePtr& entityNew, const Ice::Current&)
129  {
130  std::cout << "Entity changed: " << std::endl;
131  std::cout << "Old: " << EntityPtr::dynamicCast(entityOld) << std::endl;
132  std::cout << "New: " << EntityPtr::dynamicCast(entityNew) << std::endl << std::endl;
133  }
134 
135  void WorkingMemoryExample::reportEntityRemoved(const std::string& segmentName, const memoryx::EntityBasePtr& entity,
136  const Ice::Current&)
137  {
138  std::cout << "Entity removed: id = " << entity->getId() << std::endl << std::endl;
139  }
140 
141  void WorkingMemoryExample::reportSnapshotLoaded(const std::string& segmentName, const Ice::Current&)
142  {
143  std::cout << "Snapshot loaded!" << std::endl << std::endl;
144  }
145 
146  void WorkingMemoryExample::reportMemoryCleared(const std::string& segmentName, const Ice::Current&)
147  {
148  std::cout << "Scene cleared!" << std::endl << std::endl;
149  }
150 
151  void WorkingMemoryExample::testFileTransfer()
152  {
153  ObjectInstanceMemorySegmentBasePrx objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
154  GridFileManagerPtr fileManager(new GridFileManager(memoryPrx->getCommonStorage()));
155 
156  int ivCount = 0;
157  size_t totalBytes = 0;
158  EntityIdList ids = objectInstancesMemoryPrx->getAllEntityIds();
159 
160  for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
161  {
162  const EntityBasePtr obj = objectInstancesMemoryPrx->getEntityById(*it);
163 
164  if (obj->hasAttribute("IvFile"))
165  {
166 
167  std::ifstream fs;
168  fileManager->getFileStream(obj->getAttribute("IvFile"), fs);
169 
170  char ivFile[128];
171  fs.getline(ivFile, 128);
172 
173  std::cout << ivFile << std::endl;
174 
175  ivCount++;
176  // totalBytes += datalen;
177  }
178  }
179 
180  std::cout << "# of fetched iv-files: " << ivCount << std::endl;
181  std::cout << "Bytes transfered: " << totalBytes << std::endl;
182  }
183 
184  void WorkingMemoryExample::testUpdateObserver()
185  {
186  const Ice::CommunicatorPtr ic = getIceManager()->getCommunicator();
187  ObjectInstanceMemorySegmentBasePrx objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
188  RelationMemorySegmentBasePrx relationsMemoryPrx = memoryPrx->getRelationsSegment();
189 
190  ObjectInstancePtr cup = new ObjectInstance("cup1");
191  cup->putAttribute("Weight", 0.3f, new UnivariateNormalDistribution(0.3f, 0.5f));
192  cup->putAttribute("Description", "my favourite cup");
193 
194  cup->addClass("Cup", 0.6f);
195  cup->addClass("Mug", 0.4f);
196  std::string cupId = objectInstancesMemoryPrx->addEntity(cup);
197 
198  cup->setId(cupId);
199  std::cout << "Stored object " << cup->getName() << " in WorkingMemory with id = " << cupId << std::endl;
200  std::cout << "Most probable class is <" << cup->getMostProbableClass() <<
201  "> with prob = " << cup->getClassProbability(cup->getMostProbableClass()) << std::endl;
202 
203  Eigen::Vector3f posVec(1., 2., 3.);
205  cup->setPosition(curPos);
206 
207  FloatVector posStdVec(posVec.data(), posVec.data() + 3);
209  posDist->setMean(posStdVec);
210  posDist->setCovariance(0, 0, 0.5);
211  posDist->setCovariance(1, 1, 0.5);
212  posDist->setCovariance(2, 2, 0.5);
213  cup->setPositionUncertainty(posDist);
214 
215  objectInstancesMemoryPrx->updateEntity(cupId, cup);
216  std::cout << "Updated object " << cup->getName() << " in WorkingMemory with id = " << cupId << std::endl;
217 
218  ObjectInstancePtr cupClone = ObjectInstancePtr::dynamicCast(cup->ice_clone());
219  std::cout << "Cloned object: " << cupClone << std::endl;
220 
221  ObjectInstancePtr fridge = new ObjectInstance("Fridge");
222  fridge->setClass("Fridge", 1.f);
223  const std::string fridgeId = objectInstancesMemoryPrx->addEntity(fridge);
224 
225  std::cout << "Stored object " << fridge->getName() << " in WorkingMemory with id = " << fridgeId << std::endl;
226 
227  EntityPtr juice = new ObjectInstance("Juice");
228  const std::string juiceId = objectInstancesMemoryPrx->addEntity(juice);
229 
230  std::cout << "Stored object " << juice->getName() << " in WorkingMemory with id = " << juiceId << std::endl;
231 
232  EntityRefBasePtr cupRef = objectInstancesMemoryPrx->getEntityRefById(cupId);
233  ARMARX_CHECK_EXPRESSION(cupRef->entityId == cupId);
234  EntityRefBasePtr fridgeRef = objectInstancesMemoryPrx->getEntityRefById(fridgeId);
235  EntityRefBasePtr juiceRef = objectInstancesMemoryPrx->getEntityRefById(juiceId);
236 
237  RelationBasePtr rel1 = new Relation("isIn", {cupRef, fridgeRef});
238  relationsMemoryPrx->addEntity(rel1);
239 
240  RelationBasePtr rel2 = new Relation("isIn", {juiceRef, fridgeRef}, true, 0.7);
241  relationsMemoryPrx->addEntity(rel2);
242 
243  RelationList rels = relationsMemoryPrx->getRelationsByEntityId(cupRef->entityId);
244  std::cout << "Found relations: " << rels.size() << std::endl;
245 
246  for (RelationList::const_iterator it = rels.begin(); it != rels.end(); ++it)
247  {
248  RelationBasePtr relation = *it;
249  std::cout << relation->getName() << ": " << std::endl;
250 
251  for (EntityRefBasePtr entityRef : relation->getEntities())
252  {
253  std::cout << " * " << entityRef->entityName << std::endl;
254  }
255  }
256 
257 
258  memoryPrx->print();
259 
260  // memoryPrx->saveSnapshot("snapshot1");
261 
262  // memoryPrx->removeObject(fridgeId);
263 
264  /* EntityBasePtr cup2 = memoryPrx->getObjectById(cupId);
265  // SceneEntityPtr cup22 = SceneEntityPtr::dynamicCast(cup2);
266 
267  armarx::JSONObjectPtr jsonSerializer = new armarx::JSONObject(ic);
268  cup2->serialize(jsonSerializer);
269  const std::string jsonString = jsonSerializer->asString(true);
270  // std::cout << "JSON for cup: " << jsonString << std::endl << std::endl;
271 
272  armarx::JSONObjectPtr jsonDeserializer = new armarx::JSONObject(ic);
273  jsonDeserializer->fromString(jsonString);
274  EntityBasePtr newCup = new ObjectInstance();
275  newCup->deserialize(jsonDeserializer);
276 
277  jsonSerializer = new armarx::JSONObject(ic);
278  newCup->serialize(jsonSerializer);*/
279  // std::cout << "cup deserialized from JSON: " << jsonSerializer->toString() << std::endl << std::endl;
280 
281  // memoryPrx->removeAll();
282  // memoryPrx->saveSnapshot("db_alex.objects");
283 
284  // memoryPrx->printMemory();
285  }
286 
287  void WorkingMemoryExample::testObjectMovement()
288  {
289  ObjectInstanceMemorySegmentBasePrx objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
290 
291  ObjectInstancePtr cup = new ObjectInstance("cup");
292  cup->setClass("cup", 1.f);
293 
294  Eigen::Vector3f posVec(-5000., 2500, 1000.);
296  cup->setPosition(curPos);
297 
298  FloatVector posStdVec(posVec.data(), posVec.data() + 3);
300  posDist->setMean(posStdVec);
301  posDist->setCovariance(0, 0, 80.);
302  posDist->setCovariance(1, 1, 20.);
303  posDist->setCovariance(2, 2, 200.);
304  cup->setPositionUncertainty(posDist);
305 
306  Eigen::Quaternionf quat(0., 0., 0., 1.);
307  armarx::FramedOrientationBasePtr orient = new armarx::FramedOrientation(quat.toRotationMatrix(), armarx::GlobalFrame, "");
308  cup->setOrientation(orient);
309 
310  cup->putAttribute("Weight", 0.3f, new UnivariateNormalDistribution(0.3f, 0.01f));
311 
312  const std::string cupId = objectInstancesMemoryPrx->addEntity(cup);
313  cup->setId(cupId);
314 
315  memoryPrx->print();
316 
317  // this attribute, while not present in current observation,
318  // must be copied from old object instance by AttributeReplacementFusion
319  cup->removeAttribute("Weight");
320 
321  std::this_thread::sleep_for(std::chrono::milliseconds(3000));
322 
323  for (int i = 0; i < 500; ++i)
324  {
325  curPos->x += 10;
326  posStdVec[0] = curPos->x;
327  posDist->setMean(posStdVec);
328  cup->setPosition(curPos);
329  cup->setPositionUncertainty(posDist);
330  cup->setExistenceCertainty(i / 500.);
331  objectInstancesMemoryPrx->updateEntity(cupId, cup);
332  std::this_thread::sleep_for(std::chrono::milliseconds(100));
333  ARMARX_INFO_S << "Moving Object to " << curPos->toEigen();
334  }
335 
336  std::this_thread::sleep_for(std::chrono::milliseconds(10000));
337  objectInstancesMemoryPrx->removeEntity(cupId);
338  }
339 
340  void WorkingMemoryExample::testSnapshots()
341  {
342  ARMARX_INFO << "Content of memory after loading snapshot: ";
343  memoryPrx->print();
344  }
345 
346  void WorkingMemoryExample::testCommonPlaces()
347  {
348  ObjectInstanceMemorySegmentBasePrx objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
349 
350  const std::string snapshotName = getProperty<std::string>("CommonPlacesSnapshot").getValue();
351  WorkingMemorySnapshotInterfacePrx snapshot = longtermMemoryPrx->getWorkingMemorySnapshotListSegment()->openSnapshot(snapshotName);
352  PersistentEntitySegmentBasePrx segObjects = snapshot->getSegment("objectInstances");
353 
354  if (segObjects)
355  {
356  EntityIdList ids = segObjects->getAllEntityIds();
357 
358  for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
359  {
360  objectInstancesMemoryPrx->addEntity(segObjects->getEntityById(*it));
361  }
362  }
363  }
364 
365  void WorkingMemoryExample::testAgent()
366  {
367  AgentInstancesSegmentBasePrx agentsMemoryPrx = memoryPrx->getAgentInstancesSegment();
368 
369  Ice::CommunicatorPtr iceCommunicator = getIceManager()->getCommunicator();
370  Eigen::Vector3f positionVector(0.0, 0.0, 0.0);
371  armarx::FramedPositionPtr currentPosition = new armarx::FramedPosition(positionVector, armarx::GlobalFrame, "");
372  Eigen::Quaternionf quaternion(0.0, -1.0, 0.0, 1.0);
373  quaternion.normalize();
374  armarx::FramedOrientationBasePtr currentOrientation = new armarx::FramedOrientation(quaternion.toRotationMatrix(), armarx::GlobalFrame, "");
375 
376  auto robot = robotStateComponentPrx->getSynchronizedRobot();
377  AgentInstancePtr agent = new AgentInstance(robot->getName());
378  agent->setSharedRobot(robot);
379  agent->setStringifiedSharedRobotInterfaceProxy(iceCommunicator->proxyToString(robot));
380  agent->setAgentFilePath("Armar3/robotmodel/ArmarIII.xml");
381  agent->setOrientation(currentOrientation);
382  agent->setPosition(currentPosition);
383 
384  std::string agentId = agentsMemoryPrx->addEntity(agent);
385 
386  std::cout << "Agent Armar3 created in AgentInstancesSegment of WorkingMemory" << std::endl;
387 
388  std::this_thread::sleep_for(std::chrono::milliseconds(3000));
389 
390  for (int i = 0; i < 500; ++i)
391  {
392  currentPosition->x += 10;
393  agent->setPosition(currentPosition);
394  agentsMemoryPrx->updateEntity(agentId, agent);
395  std::this_thread::sleep_for(std::chrono::milliseconds(100));
396  }
397  }
398 
400  {
401  ARMARX_INFO << "Snapshot completely loaded!";
402  }
403 
404 }
memoryx::AgentInstancePtr
IceInternal::Handle< AgentInstance > AgentInstancePtr
Typedef of AgentEntityPtr as IceInternal::Handle<AgentEntity> for convenience.
Definition: AgentInstance.h:38
armarx::ManagedIceObject::getIceManager
IceManagerPtr getIceManager() const
Returns the IceManager.
Definition: ManagedIceObject.cpp:353
cyberglove_with_calib_22dof.ic
ic
Definition: cyberglove_with_calib_22dof.py:22
memoryx::WorkingMemoryExample::reportEntityUpdated
void reportEntityUpdated(const std::string &segmentName, const ::memoryx::EntityBasePtr &entityOld, const ::memoryx::EntityBasePtr &entityNew, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: WorkingMemoryExample.cpp:126
memoryx::WorkingMemoryExample::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: WorkingMemoryExample.cpp:60
JSONObject.h
Pose.h
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
GridFileManager.h
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
memoryx::WorkingMemoryExample::reportMemoryCleared
void reportMemoryCleared(const std::string &segmentName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: WorkingMemoryExample.cpp:146
memoryx::WorkingMemoryExample::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: WorkingMemoryExample.cpp:44
Relation.h
IceInternal::Handle< ::Ice::Communicator >
memoryx::WorkingMemoryExample::reportEntityCreated
void reportEntityCreated(const std::string &segmentName, const ::memoryx::EntityBasePtr &entity, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: WorkingMemoryExample.cpp:120
memoryx::WorkingMemoryExample::reportEntityRemoved
void reportEntityRemoved(const std::string &segmentName, const ::memoryx::EntityBasePtr &entity, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: WorkingMemoryExample.cpp:135
armarx::FloatVector
::std::vector< ::Ice::Float > FloatVector
Definition: KinematicUnitGuiPlugin.h:327
memoryx::ObjectInstancePtr
IceInternal::Handle< ObjectInstance > ObjectInstancePtr
Definition: ObjectInstance.h:42
MemoryXCoreObjectFactories.h
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
memoryx::EntityPtr
IceInternal::Handle< Entity > EntityPtr
Typedef of EntityPtr as IceInternal::Handle<Entity> for convenience.
Definition: Entity.h:46
WorkingMemoryExample.h
memoryx::MultivariateNormalDistributionPtr
IceInternal::Handle< MultivariateNormalDistribution > MultivariateNormalDistributionPtr
Definition: ProbabilityMeasures.h:231
memoryx::WorkingMemoryExample::reportSnapshotCompletelyLoaded
void reportSnapshotCompletelyLoaded(const Ice::Current &c=Ice::emptyCurrent) override
Definition: WorkingMemoryExample.cpp:399
armarx::ManagedIceObject::usingTopic
void usingTopic(const std::string &name, bool orderedPublishing=false)
Registers a proxy for subscription after initialization.
Definition: ManagedIceObject.cpp:248
ObjectInstance.h
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
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:40
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:32
armarx::VariantType::UnivariateNormalDistribution
const armarx::VariantTypeId UnivariateNormalDistribution
Definition: ProbabilityMeasures.h:34
armarx::Quaternion< float, 0 >
memoryx::WorkingMemoryExample::reportSnapshotLoaded
void reportSnapshotLoaded(const std::string &segmentName, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: WorkingMemoryExample.cpp:141
memoryx::GridFileManager
GridFileManager provides utility functions for working with files in Mongo GridFS and links to them s...
Definition: GridFileManager.h:42
MemoryXTypesObjectFactories.h
AgentInstance.h
ARMARX_INFO_S
#define ARMARX_INFO_S
Definition: Logging.h:195
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:39
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:151
armarx::VariantType::MultivariateNormalDistribution
const armarx::VariantTypeId MultivariateNormalDistribution
Definition: ProbabilityMeasures.h:36