32 #include <MemoryX/interface/memorytypes/MemoryEntities.h>
46 std::cout <<
"onInitComponent" << std::endl;
56 testToRun = getProperty<std::string>(
"TestToRun").getValue();
62 std::cout <<
"onStartComponent" << std::endl;
64 std::cout <<
"Starting WorkingMemoryExample" << std::endl;
68 memoryPrx = getProxy<WorkingMemoryInterfacePrx>(
"WorkingMemory");
69 longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>(
"LongtermMemory");
70 robotStateComponentPrx = getProxy<armarx::RobotStateComponentInterfacePrx>(
"RobotStateComponent");
73 const std::string snapshotName = getProperty<std::string>(
"SnapshotName").getValue();
76 if (!snapshotName.empty())
78 longtermMemoryPrx->loadWorkingMemorySnapshot(snapshotName, memoryPrx);
79 ARMARX_INFO <<
"Snapshot succcessfully loaded: " << snapshotName;
83 ARMARX_ERROR <<
"SnapshotName parameter must be specified!";
86 if (testToRun ==
"UpdateNotifications")
94 else if (testToRun ==
"FileTransfer")
98 else if (testToRun ==
"ObjectMove")
100 testObjectMovement();
102 else if (testToRun ==
"SnapshotLoad")
106 else if (testToRun ==
"CommonPlaces")
110 else if (testToRun ==
"Agent")
116 std::cout <<
"Unknown test name: " << testToRun << std::endl;
123 std::cout <<
"Entity created: " << EntityPtr::dynamicCast(entity) << std::endl << std::endl;
127 const memoryx::EntityBasePtr& entityOld,
128 const memoryx::EntityBasePtr& entityNew,
const Ice::Current&)
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;
138 std::cout <<
"Entity removed: id = " << entity->getId() << std::endl << std::endl;
143 std::cout <<
"Snapshot loaded!" << std::endl << std::endl;
148 std::cout <<
"Scene cleared!" << std::endl << std::endl;
151 void WorkingMemoryExample::testFileTransfer()
153 ObjectInstanceMemorySegmentBasePrx objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
157 size_t totalBytes = 0;
158 EntityIdList ids = objectInstancesMemoryPrx->getAllEntityIds();
160 for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
162 const EntityBasePtr obj = objectInstancesMemoryPrx->getEntityById(*it);
164 if (obj->hasAttribute(
"IvFile"))
168 fileManager->getFileStream(obj->getAttribute(
"IvFile"), fs);
171 fs.getline(ivFile, 128);
173 std::cout << ivFile << std::endl;
180 std::cout <<
"# of fetched iv-files: " << ivCount << std::endl;
181 std::cout <<
"Bytes transfered: " << totalBytes << std::endl;
184 void WorkingMemoryExample::testUpdateObserver()
187 ObjectInstanceMemorySegmentBasePrx objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
188 RelationMemorySegmentBasePrx relationsMemoryPrx = memoryPrx->getRelationsSegment();
192 cup->putAttribute(
"Description",
"my favourite cup");
194 cup->addClass(
"Cup", 0.6f);
195 cup->addClass(
"Mug", 0.4f);
196 std::string cupId = objectInstancesMemoryPrx->addEntity(cup);
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;
203 Eigen::Vector3f posVec(1., 2., 3.);
205 cup->setPosition(curPos);
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);
215 objectInstancesMemoryPrx->updateEntity(cupId, cup);
216 std::cout <<
"Updated object " << cup->getName() <<
" in WorkingMemory with id = " << cupId << std::endl;
219 std::cout <<
"Cloned object: " << cupClone << std::endl;
222 fridge->setClass(
"Fridge", 1.f);
223 const std::string fridgeId = objectInstancesMemoryPrx->addEntity(fridge);
225 std::cout <<
"Stored object " << fridge->getName() <<
" in WorkingMemory with id = " << fridgeId << std::endl;
227 EntityPtr juice =
new ObjectInstance(
"Juice");
228 const std::string juiceId = objectInstancesMemoryPrx->addEntity(juice);
230 std::cout <<
"Stored object " << juice->getName() <<
" in WorkingMemory with id = " << juiceId << std::endl;
232 EntityRefBasePtr cupRef = objectInstancesMemoryPrx->getEntityRefById(cupId);
234 EntityRefBasePtr fridgeRef = objectInstancesMemoryPrx->getEntityRefById(fridgeId);
235 EntityRefBasePtr juiceRef = objectInstancesMemoryPrx->getEntityRefById(juiceId);
237 RelationBasePtr rel1 =
new Relation(
"isIn", {cupRef, fridgeRef});
238 relationsMemoryPrx->addEntity(rel1);
240 RelationBasePtr rel2 =
new Relation(
"isIn", {juiceRef, fridgeRef},
true, 0.7);
241 relationsMemoryPrx->addEntity(rel2);
243 RelationList rels = relationsMemoryPrx->getRelationsByEntityId(cupRef->entityId);
244 std::cout <<
"Found relations: " << rels.size() << std::endl;
246 for (RelationList::const_iterator it = rels.begin(); it != rels.end(); ++it)
248 RelationBasePtr relation = *it;
249 std::cout << relation->getName() <<
": " << std::endl;
251 for (EntityRefBasePtr entityRef : relation->getEntities())
253 std::cout <<
" * " << entityRef->entityName << std::endl;
287 void WorkingMemoryExample::testObjectMovement()
289 ObjectInstanceMemorySegmentBasePrx objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
292 cup->setClass(
"cup", 1.f);
294 Eigen::Vector3f posVec(-5000., 2500, 1000.);
296 cup->setPosition(curPos);
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);
308 cup->setOrientation(orient);
312 const std::string cupId = objectInstancesMemoryPrx->addEntity(cup);
319 cup->removeAttribute(
"Weight");
321 std::this_thread::sleep_for(std::chrono::milliseconds(3000));
323 for (
int i = 0; i < 500; ++i)
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));
336 std::this_thread::sleep_for(std::chrono::milliseconds(10000));
337 objectInstancesMemoryPrx->removeEntity(cupId);
340 void WorkingMemoryExample::testSnapshots()
342 ARMARX_INFO <<
"Content of memory after loading snapshot: ";
346 void WorkingMemoryExample::testCommonPlaces()
348 ObjectInstanceMemorySegmentBasePrx objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
350 const std::string snapshotName = getProperty<std::string>(
"CommonPlacesSnapshot").getValue();
351 WorkingMemorySnapshotInterfacePrx snapshot = longtermMemoryPrx->getWorkingMemorySnapshotListSegment()->openSnapshot(snapshotName);
352 PersistentEntitySegmentBasePrx segObjects = snapshot->getSegment(
"objectInstances");
356 EntityIdList ids = segObjects->getAllEntityIds();
358 for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
360 objectInstancesMemoryPrx->addEntity(segObjects->getEntityById(*it));
365 void WorkingMemoryExample::testAgent()
367 AgentInstancesSegmentBasePrx agentsMemoryPrx = memoryPrx->getAgentInstancesSegment();
370 Eigen::Vector3f positionVector(0.0, 0.0, 0.0);
373 quaternion.normalize();
376 auto robot = robotStateComponentPrx->getSynchronizedRobot();
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);
384 std::string agentId = agentsMemoryPrx->addEntity(agent);
386 std::cout <<
"Agent Armar3 created in AgentInstancesSegment of WorkingMemory" << std::endl;
388 std::this_thread::sleep_for(std::chrono::milliseconds(3000));
390 for (
int i = 0; i < 500; ++i)
392 currentPosition->x += 10;
393 agent->setPosition(currentPosition);
394 agentsMemoryPrx->updateEntity(agentId, agent);
395 std::this_thread::sleep_for(std::chrono::milliseconds(100));