ObjectLocalizationMemoryUpdater.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 Kai Welke <welke@kit.edu>
18 * @copyright 2012 Kai Welke
19 * @license http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 #include <VirtualRobot/LinkedCoordinate.h>
24 #include <VirtualRobot/math/Helpers.h>
25 
26 #include <ArmarXCore/core/exceptions/local/FileIOException.h>
27 #include <ArmarXCore/core/system/ArmarXDataPath.h>
28 #include <ArmarXCore/core/util/OnScopeExit.h>
29 #include <ArmarXCore/core/util/StringHelpers.h>
30 
31 #include <RobotAPI/libraries/core/LinkedPose.h>
32 
33 #include <MemoryX/libraries/helpers/EarlyVisionHelpers/EarlyVisionConverters.h>
34 #include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
35 #include <MemoryX/libraries/updater/ObjectLocalization/ObjectLocalizationMemoryUpdater.h>
36 
37 // localization strategies
38 #include <MemoryX/interface/components/WorkingMemoryInterface.h>
39 #include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/LocalizationStrategyOnce.h>
40 #include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/LocalizationStrategyRepeated.h>
41 #include <MemoryX/libraries/motionmodels/AbstractMotionModel.h>
42 #include <MemoryX/libraries/motionmodels/MotionModelRobotHand.h>
43 #include <MemoryX/libraries/motionmodels/MotionModelStaticObject.h>
44 
45 namespace memoryx
46 {
47  // *******************************************************
48  // public members
49  // *******************************************************
50  ObjectLocalizationMemoryUpdater::ObjectLocalizationMemoryUpdater() :
51  classSegmentName("objectClasses"),
52  instanceSegmentName("objectInstances"),
53  agentInstancesSegmentName("agentInstances"),
54  referenceFrameName("Platform")
55  {
56  }
57 
58  void
59  ObjectLocalizationMemoryUpdater::setSegmentNames(std::string classSegmentName,
60  std::string instanceSegmentName)
61  {
62  // retrieve segments
63  this->classSegmentName = classSegmentName;
64  this->instanceSegmentName = instanceSegmentName;
65  }
66 
67  void
68  ObjectLocalizationMemoryUpdater::setReferenceFrame(const std::string& referenceFrameName)
69  {
70  this->referenceFrameName = referenceFrameName;
71  }
72 
73  // *******************************************************
74  // implementation of ManagedIceObject callbacks
75  // *******************************************************
76  void
77  ObjectLocalizationMemoryUpdater::onInitComponent()
78  {
79  // retrieve relevant segments
80  objectClassSegment = getSegment<ObjectClassMemorySegmentBase>(classSegmentName);
81  objectInstanceSegment = getSegment<ObjectInstanceMemorySegment>(instanceSegmentName);
82  agentInstancesSegment = getSegment<AgentInstancesSegment>(agentInstancesSegmentName);
83 
84  // register for WM updates
85  usingTopic(getWorkingMemory()->getListenerTopicName());
86 
87  // we need a remote robot proxy
88  usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
89  usingProxy(getProperty<std::string>("LongtermMemoryName").getValue());
90 
91  // this will be used to measure the head motion speed, to decide whether objects can be localized
92  if (!getProperty<std::string>("RobotStateObserverName").getValue().empty())
93  {
94  usingProxy(getProperty<std::string>("RobotStateObserverName").getValue());
95  }
96 
97  if (!getProperty<std::string>("CommonPlacesLearnerName").getValue().empty())
98  {
99  usingProxy(getProperty<std::string>("CommonPlacesLearnerName").getValue());
100  }
101 
102  workingMemoryName = getProperty<std::string>("WorkingMemoryName").getValue();
103  checkFieldOfView = getProperty<bool>("CheckFieldOfView").getValue();
104  cameraOpeningAngle = getProperty<float>("CameraOpeningAngle").getValue();
105  robotNodeNameLeftCamera = getProperty<std::string>("RobotNodeNameLeftCamera").getValue();
106  robotNodeNameRightCamera = getProperty<std::string>("RobotNodeNameRightCamera").getValue();
107  handNodeNameLeft = getProperty<std::string>("HandNodeNameLeft").getValue();
108  handNodeNameRight = getProperty<std::string>("HandNodeNameRight").getValue();
109  headMotionVelocityLimit = getProperty<float>("HeadMotionVelocityLimit").getValue();
110 
111  if (checkFieldOfView)
112  {
113  ARMARX_IMPORTANT << "Objects will only be localized when they are assumed to be in the "
114  "field of view";
115  }
116  else
117  {
118  ARMARX_IMPORTANT
119  << "Objects will always be localized, even when they are not in the field of view";
120  }
121  }
122 
123  void
124  ObjectLocalizationMemoryUpdater::onConnectComponent()
125  {
126  robotStateInterfacePrx = getProxy<armarx::RobotStateComponentInterfacePrx>(
127  getProperty<std::string>("RobotStateComponentName").getValue());
128  longtermMemoryPrx = getProxy<memoryx::LongtermMemoryInterfacePrx>(
129  getProperty<std::string>("LongtermMemoryName").getValue());
130  auto robot = robotStateInterfacePrx->getSynchronizedRobot();
131  referenceFrameName = robot->getRootNode()->getName();
132  agentName = robot->getName();
133 
134  if (!getProperty<std::string>("RobotStateObserverName").getValue().empty())
135  {
136  robotStateObserverProxy = getProxy<armarx::ObserverInterfacePrx>(
137  getProperty<std::string>("RobotStateObserverName").getValue());
138  headTCPVelocityDatafieldID = new armarx::DataFieldIdentifier(
139  getProperty<std::string>("RobotStateObserverName").getValue(),
140  "TCPVelocities",
141  getProperty<std::string>("GazeTCPName").getValue());
142  }
143 
144  if (!getProperty<std::string>("CommonPlacesLearnerName").getValue().empty())
145  {
146  commonPlacesLearnerProxy = getProxy<memoryx::CommonPlacesLearnerInterfacePrx>(
147  getProperty<std::string>("CommonPlacesLearnerName").getValue());
148  }
149 
150  // start scheduler
151  ARMARX_INFO << "Starting localization jobs";
152  localizationTask = new armarx::PeriodicTask<ObjectLocalizationMemoryUpdater>(
153  this, &ObjectLocalizationMemoryUpdater::runJobs, 10, false, "", false);
154  localizationTask->start();
155  }
156 
157  void
158  ObjectLocalizationMemoryUpdater::onDisconnectComponent()
159  {
160  if (localizationTask)
161  {
162  localizationTask->stop();
163  }
164  }
165 
166  // *******************************************************
167  // implementation of WorkingMemoryListenerInterface
168  // *******************************************************
169  void
170  ObjectLocalizationMemoryUpdater::reportEntityUpdated(const std::string& segmentName,
171  const EntityBasePtr& entityOld,
172  const EntityBasePtr& entityNew,
173  const Ice::Current&)
174  {
175  ARMARX_DEBUG << deactivateSpam(4)
176  << "ObjectLocalizationMemoryUpdater::reportEntityUpdated()";
177 
178  // only updates from class segment
179  if (segmentName != classSegmentName)
180  {
181  return;
182  }
183 
184  // retrieve queries
185  ObjectClassPtr objectClassOld = ObjectClassPtr::dynamicCast(entityOld);
186  ObjectClassPtr objectClassNew = ObjectClassPtr::dynamicCast(entityNew);
187 
188  if (!objectClassOld || !objectClassNew)
189  {
190  return;
191  }
192 
193  EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapperOld =
194  objectClassOld->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
195  EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrappeNew =
196  objectClassNew->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
197 
198  LocalizationQueryList queriesOld = recognitionWrapperOld->getLocalizationQueries();
199  LocalizationQueryList queriesNew = recognitionWrappeNew->getLocalizationQueries();
200 
201  std::unique_lock lock(jobsMutex);
202 
203  // start new queries
204  LocalizationQueryList queriesCreated = getMissingQueries(queriesOld, queriesNew);
205 
206  for (LocalizationQueryList::iterator iter = queriesCreated.begin();
207  iter != queriesCreated.end();
208  iter++)
209  {
210  LocalizationQueryPtr query = LocalizationQueryPtr::dynamicCast(*iter);
211  ARMARX_INFO << "Adding new query " << query->queryName;
212 
213  // add and create jobs
214  std::vector<LocalizationJobPtr> createdjobs = query->createJobs(objectClassSegment);
215 
216  for (LocalizationJobPtr job : createdjobs)
217  {
218  ARMARX_DEBUG << deactivateSpam(1) << "Created job for "
219  << job->getRecognitionMethod();
220  }
221 
222  std::move(createdjobs.begin(), createdjobs.end(), std::back_inserter(jobs));
223  }
224 
225  // stop removed queries
226  LocalizationQueryList queriesRemoved = getMissingQueries(queriesNew, queriesOld);
227 
228  for (LocalizationQueryList::iterator iter = queriesRemoved.begin();
229  iter != queriesRemoved.end();
230  iter++)
231  {
232  LocalizationQueryPtr query = LocalizationQueryPtr::dynamicCast(*iter);
233 
234  // remove jobs for this query
235  std::vector<LocalizationJobPtr>::iterator iterJobs = jobs.begin();
236 
237  while (iterJobs != jobs.end())
238  {
239  if ((*iterJobs)->getQueryName() == query->queryName)
240  {
241  iterJobs = jobs.erase(iterJobs);
242  }
243  else
244  {
245  iterJobs++;
246  }
247  }
248 
249  ARMARX_INFO << "removed query " << query->queryName;
250  }
251  }
252 
253  // *******************************************************
254  // private methods
255  // *******************************************************
256  void
257  ObjectLocalizationMemoryUpdater::predictPoses()
258  {
259  std::unique_lock lock(predictionMutex);
260 
261  EntityIdList allEntityIDs = objectInstanceSegment->getAllEntityIds();
262 
263  for (EntityIdList::iterator it = allEntityIDs.begin(); it != allEntityIDs.end(); it++)
264  {
265  auto entity = objectInstanceSegment->getEntityById(*it);
266  if (!entity)
267  {
268  continue;
269  }
270  try
271  {
272  ObjectInstancePtr objectInstance = ObjectInstancePtr::dynamicCast(entity);
273 
274  AbstractMotionModelPtr motionModel = objectInstance->getMotionModel();
275  if (!motionModel)
276  {
277  continue;
278  }
279 
280  ARMARX_DEBUG << deactivateSpam(5, objectInstance->getName()) << "prediction for "
281  << objectInstance->getName()
282  << " motion model: " << objectInstance->getMotionModel()->ice_id();
283 
284  armarx::LinkedPosePtr predictedPose =
285  armarx::LinkedPosePtr::dynamicCast(motionModel->getPredictedPose());
286  MultivariateNormalDistributionBasePtr positionUncertainty =
287  motionModel->getUncertainty();
288 
289  ARMARX_DEBUG << deactivateSpam(1, objectInstance->getName())
290  << "predicted pose: " << predictedPose->toEigen();
291  ARMARX_DEBUG
292  << deactivateSpam(1, objectInstance->getName()) << "last localization: "
293  << objectInstance->getMotionModel()->getPoseAtLastLocalisation()->output();
294 
295  if (positionUncertainty)
296  {
297  ARMARX_DEBUG << deactivateSpam(1, objectInstance->getName())
298  << "positionUncertainty->getDimensions(): "
299  << positionUncertainty->getDimensions();
300  Gaussian uncertaintyGaussian =
301  EarlyVisionConverters::convertToGaussian(positionUncertainty);
302  ARMARX_DEBUG << deactivateSpam(1, objectInstance->getName())
303  << "uncertaintyGaussian: " << uncertaintyGaussian.getCovariance();
304  }
305 
306  if (predictedPose->frame.empty())
307  {
308  ARMARX_VERBOSE << "predicting pose of " << objectInstance->getName()
309  << " position: " << predictedPose->position->x << " "
310  << predictedPose->position->y << " "
311  << predictedPose->position->z
312  << " frame: " << predictedPose->frame;
313  }
314 
315  // ObjectInstancePtr update = new ObjectInstance(objectInstance->getName());
316  objectInstance->setPosition(new armarx::FramedPosition(
317  predictedPose->toEigen(), predictedPose->frame, predictedPose->agent));
318  objectInstance->setOrientation(new armarx::FramedOrientation(
319  predictedPose->toEigen(), predictedPose->frame, predictedPose->agent));
320 
321  if (positionUncertainty)
322  {
323  objectInstance->setPositionUncertainty(positionUncertainty);
324  }
325 
326  objectInstance->setClass(
327  objectInstance->getMostProbableClass(),
328  objectInstance->getClassProbability(objectInstance->getMostProbableClass()));
329  objectInstance->setExistenceCertainty(objectInstance->getExistenceCertainty());
330  // update->setMotionModel(objectInstance->getMotionModel());
331  if (objectInstanceSegment->getListenerProxy() &&
332  objectInstanceSegment->hasEntityById(objectInstance->getId(),
333  Ice::emptyCurrent))
334  {
335  auto reportClone = objectInstance->clone();
336  objectInstanceSegment->getListenerProxy()->reportEntityUpdated(
337  objectInstanceSegment->getSegmentName(), reportClone, reportClone);
338  }
339  // objectInstanceSegment->updateEntity(*it, update, Ice::StringSeq{"KalmanFilterFusion"});
340 
341  //objectInstance = ObjectInstancePtr::dynamicCast(objectInstanceSegment->getEntityById(*it));
342  //FramedPositionPtr pos = objectInstance->getPosition();
343  //ARMARX_DEBUG << "new position of " << objectInstance->getName() << ": " << pos->x << " " << pos->y << " " << pos->z << " " << pos->frame;
344  //FramedOrientationPtr ori = objectInstance->getOrientation();
345  //ARMARX_DEBUG << "new orientation of " << objectInstance->getName() << ": " << ori->toEigen();
346  }
347  catch (...)
348  {
349  ARMARX_INFO << deactivateSpam(2, entity->getName())
350  << "Predicting poses for object '" << entity->getName()
351  << "' failed: " << armarx::GetHandledExceptionString();
352  }
353  }
354  }
355 
356  void
357  ObjectLocalizationMemoryUpdater::scheduleJobs()
358  {
359  //use a lambda so we can quickly leave this block by calling return
360  [&]
361  {
362  std::unique_lock lock(jobsMutex);
363 
364  // collect waiting jobs
365  std::multimap<std::string, LocalizationJobPtr> recognizerJobs;
366  std::set<std::string> recognizerNames;
367 
368  bool aJobIsWaiting = false;
369 
370  for (const LocalizationJobPtr& job : jobs)
371  {
372  if (job->waiting())
373  {
374  recognizerJobs.insert(std::make_pair(job->getRecognitionMethod(), job));
375  recognizerNames.insert(job->getRecognitionMethod());
376  //ARMARX_DEBUG << job->getQueryName() << " is waiting";
377  aJobIsWaiting = true;
378  }
379  else
380  {
381  ARMARX_DEBUG << deactivateSpam(1) << job->getQueryName() << " is not waiting";
382  }
383  }
384 
385  ARMARX_VERBOSE << deactivateSpam(60, armarx::ValueToString(recognizerNames.size()))
386  << "localizer jobs that should be started: " << recognizerNames
387  << " (number of jobs = " << jobs.size() << ")";
388 
389  if (!aJobIsWaiting)
390  {
391  ARMARX_DEBUG << deactivateSpam(5) << "No object needs to be localized";
392  return;
393  }
394 
395  Eigen::Vector3f tcpVelocity;
396  tcpVelocity.setZero();
397 
398  if (robotStateObserverProxy)
399  {
400  if (robotStateObserverProxy->existsDataField(
401  headTCPVelocityDatafieldID->getChannelName(),
402  headTCPVelocityDatafieldID->getDataFieldName()))
403  {
404  tcpVelocity =
405  armarx::VariantPtr::dynamicCast(
406  robotStateObserverProxy->getDataField(headTCPVelocityDatafieldID))
407  ->get<armarx::FramedDirection>()
408  ->toEigen();
409  }
410  }
411 
412  ARMARX_DEBUG << deactivateSpam(1) << "Head velocity: " << tcpVelocity.norm();
413 
414  // if head speed is too high, don't localize
415  if (tcpVelocity.norm() > headMotionVelocityLimit)
416  {
417  ARMARX_INFO << deactivateSpam(2)
418  << "Not localizing anything because head velocity is too high: "
419  << tcpVelocity.norm();
420  return;
421  }
422 
423  // start jobs, if recognition method is available
424  for (const auto& recognitionMethod : recognizerNames)
425  {
426 
427  // retrieve jobs for recognition method
428  if (isRecognitionMethodRunning(recognitionMethod))
429  {
430  ARMARX_DEBUG << deactivateSpam(0.5) << "Recognition method "
431  << recognitionMethod << " is busy";
432  continue;
433  }
434 
435 
436  // elements contains two iterators, the first one points to the first and the second one to the last element of
437  // the list containing all jobs with the given recognition method
438  std::pair<std::map<std::string, LocalizationJobPtr>::iterator,
439  std::map<std::string, LocalizationJobPtr>::iterator>
440  elements = recognizerJobs.equal_range(recognitionMethod);
441  std::map<std::string, LocalizationJobPtr>::iterator iterElements = elements.first;
442 
443  LocalizationJobContainerPtr container =
444  new LocalizationJobContainer(recognitionMethod);
445  bool hasJob = false;
446  std::unique_lock lockPrediction(predictionMutex, std::defer_lock);
447 
448  if (lockPrediction.try_lock())
449  {
450  while (iterElements != elements.second)
451  {
452  LocalizationJobPtr job = iterElements->second;
453 
454  // filter the objects in the job: only try to localize those that should currently be visible, have not been found yet or were lost again
455  std::vector<std::string> classNames = job->getAllClassNames();
456  std::vector<std::string> classNamesToBeLocalized;
457 
458  for (size_t i = 0; i < classNames.size(); i++)
459  {
460  ObjectInstanceList instances =
461  objectInstanceSegment->getObjectInstancesByClass(classNames.at(i));
462 
463  if (instances.size() != 0)
464  {
465  bool atLeastOneInstanceIsVisibleOrLost = false;
466 
467  for (size_t j = 0; j < instances.size(); j++)
468  {
469  ObjectInstancePtr instance =
470  ObjectInstancePtr::dynamicCast(instances.at(j));
471  float existenceCertainty = instance->getExistenceCertainty();
472  armarx::FramedPositionPtr pos = instance->getPosition();
473 
474  if (checkObjectVisibility(pos) || existenceCertainty < 0.5f)
475  {
476  atLeastOneInstanceIsVisibleOrLost = true;
477  }
478  }
479 
480  if (atLeastOneInstanceIsVisibleOrLost)
481  {
482  for (const auto& instance : instances)
483  {
484  if (ObjectInstancePtr::dynamicCast(instance)
485  ->getMotionModel())
486  {
487  ObjectInstancePtr::dynamicCast(instance)
488  ->getMotionModel()
489  ->savePredictedPoseAtStartOfCurrentLocalization();
490  }
491  }
492 
493  classNamesToBeLocalized.push_back(classNames.at(i));
494  }
495  else
496  {
497  ARMARX_INFO
498  << deactivateSpam(5, classNames.at(i))
499  << "Not trying to localize " << classNames.at(i)
500  << " because it is currently not in the field of view";
501  }
502  }
503  else
504  {
505  // object has not been found yet, so we will try to localize it
506  classNamesToBeLocalized.push_back(classNames.at(i));
507  }
508  }
509 
510  // if at least one object in the job should be visible, we start the localization
511  if (classNamesToBeLocalized.size() > 0)
512  {
513  job->setClassNamesToBeLocalized(classNamesToBeLocalized);
514  container->addJob(job);
515  hasJob = true;
516  }
517 
518  iterElements++;
519  }
520  }
521 
522  // start recognition method
523  if (hasJob)
524  {
525  startLocalization(container);
526  }
527  }
528  }();
529 
530  try
531  {
532  predictPoses();
533  }
534  catch (...)
535  {
536  ARMARX_ERROR << "predicting poses caused an exception!";
537  armarx::handleExceptions();
538  }
539  }
540 
541  void
542  ObjectLocalizationMemoryUpdater::runJobs()
543  {
544  try
545  {
546  scheduleJobs();
547  }
548  catch (std::exception&)
549  {
550  ARMARX_ERROR << "Scheduling jobs threw an exception!";
551  armarx::handleExceptions();
552  }
553  }
554 
555  void
556  ObjectLocalizationMemoryUpdater::startLocalization(
557  const LocalizationJobContainerPtr& jobContainer)
558  {
559  ObjectClassNameList objectClassNames = jobContainer->getClassNamesUnique();
560  std::string recognitionMethod = jobContainer->getRecognitionMethod();
561 
562  ObjectLocalizerInterfacePrx proxy = getProxyCached(recognitionMethod);
563 
564  if (!proxy)
565  {
566  ARMARX_ERROR << deactivateSpam(5)
567  << "No running proxy found for recognition method: " << recognitionMethod;
568  // update queries so that statcharts waiting on queries can continue
569  finishJobContainer(jobContainer);
570  return;
571  }
572 
573 
574  // ARMARX_DEBUG_S << "Got shared robot proxy " << sharedRobotProxy;
575  // make snapshot of robot state at latest possible point
576  auto robotName = robotStateInterfacePrx->getSynchronizedRobot()->getName();
577  armarx::SharedRobotInterfacePrx sharedRobotProxy =
578  robotStateInterfacePrx->getRobotSnapshot(robotName);
579  jobContainer->setRobotState(sharedRobotProxy);
580 
581  // get global pose of robot in world CS, if available
582  armarx::PosePtr robotPose = new armarx::Pose(Eigen::Matrix4f::Identity());
583  robotPose = armarx::PosePtr::dynamicCast(sharedRobotProxy->getGlobalPose());
584 
585 
586  jobContainer->setRobotPose(robotPose);
587 
588 
589  // start jobs
590  setRecognitionMethodRunningState(recognitionMethod, true);
591 
592  jobContainer->startJobs();
593  ARMARX_VERBOSE << deactivateSpam(5, jobContainer->getRecognitionMethod())
594  << "starting localization of " << objectClassNames << " with "
595  << jobContainer->getRecognitionMethod();
596  proxy->begin_localizeObjectClasses(
597  objectClassNames,
598  Ice::newCallback(this, &ObjectLocalizationMemoryUpdater::localizationFinished),
599  jobContainer);
600  }
601 
602  bool
603  ObjectLocalizationMemoryUpdater::isRecognitionMethodRunning(
604  const std::string& recognitionMethod)
605  {
606  std::unique_lock lock(recognitionMethodStateMutex);
607  std::map<std::string, bool>::iterator iter =
608  recognitionMethodRunning.find(recognitionMethod);
609 
610  if (iter == recognitionMethodRunning.end())
611  {
612  return false;
613  }
614 
615  bool result = recognitionMethodRunning[recognitionMethod];
616  return result;
617  }
618 
619  void
620  ObjectLocalizationMemoryUpdater::setRecognitionMethodRunningState(
621  const std::string& recognitionMethod,
622  bool running)
623  {
624  std::unique_lock lock(recognitionMethodStateMutex);
625  ARMARX_DEBUG << deactivateSpam(1) << "Setting running state of recognition method "
626  << recognitionMethod << " to " << running;
627  recognitionMethodRunning[recognitionMethod] = running;
628  }
629 
630  void
631  ObjectLocalizationMemoryUpdater::finishJobContainer(LocalizationJobContainerPtr jobContainer)
632  {
633  // set all jobs in the container to finished
634  jobContainer->finishJobs();
635  setRecognitionMethodRunningState(jobContainer->getRecognitionMethod(), false);
636  // update queries in object class segment
637  updateQueries(jobContainer);
638  }
639 
640  void
641  ObjectLocalizationMemoryUpdater::localizationFinished(const Ice::AsyncResultPtr& r)
642  {
643 
644  ObjectLocalizerInterfacePrx objectLocalizer =
645  ObjectLocalizerInterfacePrx::uncheckedCast(r->getProxy());
646  LocalizationJobContainerPtr jobContainer =
647  LocalizationJobContainerPtr::dynamicCast(r->getCookie());
648  ObjectLocalizationResultList resultList;
649 
650  try
651  {
652  resultList = objectLocalizer->end_localizeObjectClasses(r);
653 
654 
655  if (jobContainer->getClassNamesUnique().empty())
656  {
657  ARMARX_WARNING << "There is a bug: Returned from localization, but list of class "
658  "names is empty";
659  finishJobContainer(jobContainer);
660  return;
661  }
662  std::string allObjectNames;
663 
664  for (size_t i = 0; i < jobContainer->getClassNamesUnique().size(); i++)
665  {
666  allObjectNames.append(jobContainer->getClassNamesUnique().at(i));
667  allObjectNames.append(" ");
668  }
669 
670  ARMARX_DEBUG << deactivateSpam(1) << "Localization of " << allObjectNames
671  << " returned - number of detected instances: " << resultList.size();
672 
673 
674  if (resultList.empty())
675  {
676  // if the object was not found although it should have been visible, reduce the existence certainty
677  if (checkFieldOfView)
678  {
679  ObjectInstanceList instances = objectInstanceSegment->getObjectInstancesByClass(
680  jobContainer->getClassNamesUnique().at(0));
681 
682  if (instances.size() == 1)
683  {
684  ObjectInstancePtr instance =
685  ObjectInstancePtr::dynamicCast(instances.at(0));
686  float oldExistenceCertainty = instance->getExistenceCertainty();
687  instance->setExistenceCertainty(
688  getProperty<float>(
689  "ExistenceCertaintyReductionFactorWhenLocalizationFailed")
690  .getValue() *
691  oldExistenceCertainty);
692  // if(instance->getExistenceCertainty() < 0.1)
693  // instance->setExistenceCertainty(0.1);
694  ARMARX_INFO << deactivateSpam(3, instance->getName()) << instance->getName()
695  << " not found in view altough it should be there - decreasing "
696  "existence certainty to: "
697  << instance->getExistenceCertainty();
698  if (objectInstanceSegment->getListenerProxy() &&
699  objectInstanceSegment->hasEntityById(instance->getId(),
700  Ice::emptyCurrent))
701  {
702  auto reportClone = instance->clone();
703  objectInstanceSegment->getListenerProxy()->reportEntityUpdated(
704  objectInstanceSegment->getSegmentName(), reportClone, reportClone);
705  }
706  }
707  else if (instances.size() > 1)
708  {
709  ARMARX_IMPORTANT << "Dealing with failed localizations is not implemented "
710  "for the case that multiple instances exist";
711  }
712  }
713  }
714  else
715  {
716  // generate / fuse instances
717  const std::string rootNodeName =
718  jobContainer->getRobotState()->getRootNode()->getName();
719  for (const ObjectLocalizationResult& result : resultList)
720  {
721 
722  if (result.objectClassName.empty())
723  {
724  ARMARX_ERROR << "Object name in localization result is empty";
725  }
726 
727  armarx::SharedRobotInterfacePrx robotState = jobContainer->getRobotState();
728  armarx::PosePtr robotPose = jobContainer->getRobotPose();
729  if (result.timeStamp)
730  {
731  armarx::TimestampVariantPtr timestamp =
732  armarx::TimestampVariantPtr::dynamicCast(result.timeStamp);
733  IceUtil::Time now = armarx::TimeUtil::GetTime();
734  ARMARX_INFO << deactivateSpam(3) << "object pose is already "
735  << (now - timestamp->toTime()).toMilliSecondsDouble()
736  << "ms old";
737  robotState = robotStateInterfacePrx->getRobotSnapshotAtTimestamp(
738  timestamp->getTimestamp());
739  if (!robotState)
740  {
741  ARMARX_WARNING << deactivateSpam(2)
742  << "getRobotSnapshotAtTimestamp returned NULL, using "
743  "current robot state "
744  << "(possibly a bug in the simulation, time server, or "
745  "something else)";
746  robotState = robotStateInterfacePrx->getRobotSnapshot("");
747  }
748  robotPose = armarx::PosePtr::dynamicCast(robotState->getGlobalPose());
749  jobContainer->setRobotState(robotState);
750  jobContainer->setRobotPose(robotPose);
751  }
752  else
753  {
754  ARMARX_WARNING << deactivateSpam(1, jobContainer->getRecognitionMethod())
755  << "Recognition method "
756  << jobContainer->getRecognitionMethod()
757  << " did not provide a timestamp - using robotstate from "
758  "before starting recognition";
759  }
760  ARMARX_DEBUG << deactivateSpam(1, result.objectClassName)
761  << "Object: " << result.objectClassName;
762 
763  // new instance for updating
764  ObjectInstancePtr update = new ObjectInstance(
765  result.instanceName.empty() ? result.objectClassName : result.instanceName);
766  update->setLocalizationTimestamp(result.timeStamp);
767  // position and orientation
768  armarx::FramedPositionPtr measuredPosition =
769  armarx::FramedPositionPtr::dynamicCast(result.position);
770  armarx::FramedOrientationPtr measuredOrientation =
771  armarx::FramedOrientationPtr::dynamicCast(result.orientation);
772 
773  Eigen::Vector3f positionBeforeFrameChange = measuredPosition->toEigen();
774  measuredPosition->changeFrame(robotState, rootNodeName);
775  measuredOrientation->changeFrame(robotState, rootNodeName);
776  Eigen::Vector3f positionTranslation =
777  measuredPosition->toEigen() - positionBeforeFrameChange;
778  Eigen::Matrix4f measuredPose = math::Helpers::Pose(
779  measuredPosition->toEigen(), measuredOrientation->toEigen());
780 
781  ARMARX_DEBUG << deactivateSpam(1, result.objectClassName)
782  << result.objectClassName
783  << "New pose from localizer: " << measuredPose
784  << " (in reference frame " << measuredPosition->frame << ")";
785 
786  // now we have new position and orientation in reference frame
787  armarx::FramedPositionPtr posInRef = new armarx::FramedPosition(
788  measuredPose, rootNodeName, measuredPosition->agent);
789  armarx::FramedOrientationPtr orientationInRef = new armarx::FramedOrientation(
790  measuredPose, rootNodeName, measuredPosition->agent);
791  armarx::LinkedPosePtr linkedPose =
792  new armarx::LinkedPose(measuredPose, rootNodeName, robotState);
793 
794  update->setPosition(posInRef);
795  update->setOrientation(orientationInRef);
796 
797  int priority = 0.0;
798  for (LocalizationQueryPtr& q : jobContainer->getQueries())
799  {
800  if (q->className == result.objectClassName)
801  {
802  priority = q->priority;
803  }
804  }
805  update->setLocalizationPriority(priority);
806 
807  // it is not obvious how to rotate the covariance matrix. it seems that it is not as simple as this:
808  // MultivariateNormalDistributionPtr uncertainty = MultivariateNormalDistributionPtr::dynamicCast(result.positionNoise);
809  // Eigen::Matrix3f covariance = uncertainty->toEigenCovariance().block<3,3>(0,0);
810  // Eigen::Matrix3f covarianceInRef = transformationToReferenceFrame.block<3,3>(0,0) * covariance;
811  // MultivariateNormalDistributionPtr uncertaintyInRef = new MultivariateNormalDistribution(posInRef->toEigen(), covarianceInRef);
812  MultivariateNormalDistributionBasePtr uncertaintyInRef = result.positionNoise;
813  auto mean = uncertaintyInRef->getMean();
814  if (mean.size() == 3)
815  {
816  Eigen::Vector3f meanEigen(mean.at(0), mean.at(1), mean.at(2));
817  meanEigen += positionTranslation;
818  uncertaintyInRef->setMean(
819  memoryx::FloatVector{meanEigen(0), meanEigen(1), meanEigen(2)});
820  }
821  update->setPositionUncertainty(uncertaintyInRef);
822 
823  // classes
824  update->addClass(result.objectClassName, result.recognitionCertainty);
825 
826  // existence certainty
827  ARMARX_INFO << deactivateSpam(5, result.objectClassName)
828  << result.objectClassName
829  << ": existence certainty: " << result.recognitionCertainty;
830  update->setExistenceCertainty(result.recognitionCertainty);
831 
832  // find correspondence in ObjectInstanceLayer (TODO: Method not using motion models and correct reference frame (LinkedCoordinate))
833  ObjectInstancePtr correspondingEntity = ObjectInstancePtr::dynamicCast(
834  objectInstanceSegment->getCorrespondingObjectInstance(update));
835 
836  // add entity to or update memory segment
837  if (!correspondingEntity)
838  {
839  ARMARX_INFO << "Adding new entity " << update->getName();
840 
841  if (!update->getMotionModel())
842  {
843  setMotionModel(update);
844  }
845 
846  update->getMotionModel()->setPoseAtLastLocalisation(
847  linkedPose, robotPose, uncertaintyInRef);
848  objectInstanceSegment->addEntity(update);
849  }
850  else
851  {
852  ARMARX_DEBUG << "updating entity: " << update->getName();
853 
854  std::unique_lock lock(predictionMutex);
855 
856  if (!correspondingEntity->getMotionModel())
857  {
858  ARMARX_IMPORTANT
859  << "Corresponding entity has no motion model, creating a new one";
860  setMotionModel(correspondingEntity);
861  correspondingEntity->getMotionModel()->setPoseAtLastLocalisation(
862  linkedPose, robotPose, uncertaintyInRef);
863  }
864 
865  AbstractMotionModelPtr motionModel =
866  ObjectInstancePtr::dynamicCast(correspondingEntity)->getMotionModel();
867 
868  if (motionModel)
869  {
870  // update->setMotionModel(motionModel);
871  // insert into memory to give the fusion methods a chance
872  objectInstanceSegment->updateEntityInternal(
873  correspondingEntity->getId(), update);
874  ARMARX_DEBUG
875  << /*deactivateSpam(1, update->getName())<< */ "Ori of update: "
876  << *update->getOrientation();
877 
878  // update motion model and predict current pose
879  ObjectInstancePtr objectInstance = ObjectInstancePtr::dynamicCast(
880  objectInstanceSegment->getEntityById(correspondingEntity->getId()));
881  // ARMARX_INFO << deactivateSpam(1) << "Object Instance localization timestamp:" << (objectInstance->hasLocalizationTimestamp() ? objectInstance->getLocalizationTimestamp().toDateTime() : "none") << " timestamp of update: " <<
882  // (update->hasLocalizationTimestamp() ? update->getLocalizationTimestamp().toDateTime() : "none") ;
883  ARMARX_DEBUG /*<< deactivateSpam(1, update->getName()) */
884  << update->getName()
885  << " obj Pose from memory: " << objectInstance->getPose()->output()
886  << " robot global pose: "
887  << jobContainer->getRobotState()->getGlobalPose()->output();
888  objectInstance->setLocalizationTimestamp(result.timeStamp);
889  armarx::LinkedPosePtr linkedObjectPose =
890  new armarx::LinkedPose(*objectInstance->getPose(), robotState);
891  ARMARX_DEBUG
892  << /*deactivateSpam(1, update->getName()) <<*/ "Ori after fusion: "
893  << *objectInstance->getOrientation();
894 
895  motionModel->setPoseAtLastLocalisation(
896  linkedObjectPose,
897  robotPose,
898  objectInstance->getPositionUncertainty());
899  armarx::LinkedPosePtr predictedPose =
900  armarx::LinkedPosePtr::dynamicCast(motionModel->getPredictedPose());
901  ARMARX_DEBUG << /*deactivateSpam(1, update->getName()) <<*/ VAROUT(
902  predictedPose->output());
903  objectInstance->setPose(predictedPose);
904  }
905  else
906  {
907  ARMARX_WARNING << "no motion model for obj instance "
908  << update->getName() << " set";
909  }
910  }
911 
912  if (commonPlacesLearnerProxy)
913  {
914  ARMARX_VERBOSE << "Using object instance " << VAROUT(update)
915  << " to train common places learner";
916  try
917  {
918  commonPlacesLearnerProxy->learnFromObject(update);
919  }
920  catch (...)
921  {
922  ARMARX_INFO << "Learning failed: "
923  << armarx::GetHandledExceptionString();
924  }
925  }
926  }
927  }
928  }
929  catch (std::exception& e)
930  {
931  ARMARX_ERROR << deactivateSpam(2, jobContainer->getRecognitionMethod())
932  << "Localization failed of type " << jobContainer->getRecognitionMethod()
933  << "\nReason: " << e.what();
934  //handleExceptions();
935  }
936 
937 
938  finishJobContainer(jobContainer);
939  }
940 
941  void
942  ObjectLocalizationMemoryUpdater::updateQueries(const LocalizationJobContainerPtr& jobContainer)
943  {
944  std::vector<LocalizationQueryPtr> queries = jobContainer->getQueries();
945 
946  for (std::vector<LocalizationQueryPtr>::iterator iter = queries.begin();
947  iter != queries.end();
948  iter++)
949  {
950  LocalizationQueryPtr query = *iter;
951 
952  if (query->getFinished())
953  {
954  // update working memory segment
955  EntityBasePtr entity = objectClassSegment->getEntityByName(
956  query->className, Ice::emptyCurrent); // pass Ice::Current to get a safe clone
957 
958  if (entity)
959  {
960  ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(entity);
961 
962  if (objectClass)
963  {
964  EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapper =
965  objectClass->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
966  recognitionWrapper->updateLocalizationQuery(query->queryName, query);
967  }
968 
969  ARMARX_DEBUG << deactivateSpam(1) << "Updating query " << query->queryName
970  << " with state " << query->getFinished();
971 
972  // update entity in segment with current state
973  objectClassSegment->updateClass(query->className, objectClass);
974  }
975  }
976  }
977  }
978 
979  ObjectLocalizerInterfacePrx
980  ObjectLocalizationMemoryUpdater::getProxyCached(const std::string& recognitionMethod)
981  {
982  // find proxy in list of already obtained proxies
983  std::map<std::string, ObjectLocalizerInterfacePrx>::iterator iter;
984  iter = knownObjectProxies.find(recognitionMethod);
985 
986  if (iter != knownObjectProxies.end())
987  {
988  // TODO: check if proxy is still valid
989  return iter->second;
990  }
991 
992  // create new proxy if not found
993  ObjectLocalizerInterfacePrx objectLocalizerProxy;
994 
995  try
996  {
997  objectLocalizerProxy =
998  getWorkingMemory()->getProxy<ObjectLocalizerInterfacePrx>(recognitionMethod);
999  }
1000  catch (...)
1001  {
1002  // exceptions only indicate an issue with the proxy
1003  // this is handled below when the Prx is checked for NULL
1004  }
1005 
1006  if (objectLocalizerProxy)
1007  {
1008  std::pair<std::string, ObjectLocalizerInterfacePrx> entry;
1009  entry.first = recognitionMethod;
1010  entry.second =
1011  getWorkingMemory()->getProxy<ObjectLocalizerInterfacePrx>(recognitionMethod);
1012 
1013  knownObjectProxies.insert(entry);
1014  }
1015 
1016  return objectLocalizerProxy;
1017  }
1018 
1019  LocalizationQueryList
1020  ObjectLocalizationMemoryUpdater::getMissingQueries(const LocalizationQueryList& queriesBase,
1021  const LocalizationQueryList& queriesCompare)
1022  {
1023  LocalizationQueryList missing;
1024 
1025  for (LocalizationQueryList::const_iterator iter = queriesCompare.begin();
1026  iter != queriesCompare.end();
1027  iter++)
1028  {
1029  bool found = false;
1030 
1031  for (LocalizationQueryList::const_iterator iterBase = queriesBase.begin();
1032  iterBase != queriesBase.end();
1033  iterBase++)
1034  {
1035  if ((*iterBase)->queryName == (*iter)->queryName)
1036  {
1037  found = true;
1038  }
1039  }
1040 
1041  if (!found)
1042  {
1043  missing.push_back(*iter);
1044  }
1045  }
1046 
1047  return missing;
1048  }
1049 
1050  bool
1051  ObjectLocalizationMemoryUpdater::checkObjectVisibility(armarx::FramedPositionPtr pos)
1052  {
1053  if (!checkFieldOfView)
1054  {
1055  return true;
1056  }
1057 
1058  auto robot = robotStateInterfacePrx->getSynchronizedRobot();
1059  armarx::FramedPositionPtr posInLeftCamCS =
1060  armarx::FramedPositionPtr::dynamicCast(pos->clone());
1061  posInLeftCamCS->changeFrame(robot, robotNodeNameLeftCamera);
1062  Eigen::Vector3f positionInLeftCamCS = posInLeftCamCS->toEigen();
1063  armarx::FramedPositionPtr posInRightCamCS =
1064  armarx::FramedPositionPtr::dynamicCast(pos->clone());
1065  posInRightCamCS->changeFrame(robot, robotNodeNameRightCamera);
1066  Eigen::Vector3f positionInRightCamCS = posInRightCamCS->toEigen();
1067 
1068  // if object is too far away, don't localize
1069  if (positionInLeftCamCS(2) > getProperty<float>("MaximalObjectDistance").getValue())
1070  {
1071  return false;
1072  }
1073 
1074  Eigen::Vector3f viewDirection = Eigen::Vector3f::UnitZ();
1075  positionInLeftCamCS.normalize();
1076  positionInRightCamCS.normalize();
1077  float angleLeft = acos(viewDirection.dot(positionInLeftCamCS));
1078  float angleRight = acos(viewDirection.dot(positionInRightCamCS));
1079 
1080  if (2 * angleLeft < cameraOpeningAngle && 2 * angleRight < cameraOpeningAngle)
1081  {
1082  return true;
1083  }
1084  else
1085  {
1086  return false;
1087  }
1088  }
1089 
1090  void
1091  ObjectLocalizationMemoryUpdater::setMotionModel(ObjectInstancePtr& objectInstance)
1092  {
1093  std::string objectClassName = objectInstance->getMostProbableClass();
1094  // create a motion model
1095  ObjectClassPtr objectClass =
1096  ObjectClassPtr::dynamicCast(objectClassSegment->getEntityByName(objectClassName));
1097  EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapper =
1098  objectClass->getWrapper<EntityWrappers::ObjectRecognitionWrapper>();
1099 
1100  if (!recognitionWrapper)
1101  {
1102  recognitionWrapper =
1103  objectClass->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
1104  }
1105 
1106  std::string motionModelName = recognitionWrapper->getDefaultMotionModel();
1107  ARMARX_INFO << "Motion model is " << motionModelName;
1108  AbstractMotionModel::EMotionModelType motionModelType =
1109  AbstractMotionModel::getMotionModelTypeByName(motionModelName);
1110  AbstractMotionModelPtr motionModel;
1111 
1112  switch (motionModelType)
1113  {
1114  case AbstractMotionModel::eMotionModelStaticObject:
1115  {
1116  motionModel = new MotionModelStaticObject(robotStateInterfacePrx);
1117  break;
1118  }
1119 
1120  case AbstractMotionModel::eMotionModelRobotHand:
1121  {
1122  std::string handNodeName;
1123 
1124  if (objectClassName.find("left") != std::string::npos ||
1125  objectClassName.find("Left") != std::string::npos)
1126  {
1127  handNodeName = handNodeNameLeft;
1128  }
1129  else
1130  {
1131  handNodeName = handNodeNameRight;
1132  }
1133 
1134  ARMARX_INFO << "The hand node name in the kinematic model is assumed to be "
1135  << handNodeName;
1136  motionModel = new MotionModelRobotHand(robotStateInterfacePrx, handNodeName);
1137  break;
1138  }
1139 
1140  case AbstractMotionModel::eMotionModelKBM:
1141  {
1142  if (armarx::Contains(objectClassName, "left", true))
1143  {
1144  motionModel = new MotionModelKBM(
1145  getProperty<std::string>("KBMReferenceNodeName").getValue(),
1146  getProperty<std::string>("KBMLeftArmNodeSetName").getValue(),
1147  robotStateInterfacePrx,
1148  longtermMemoryPrx);
1149  }
1150  else
1151  {
1152  motionModel = new MotionModelKBM(
1153  getProperty<std::string>("KBMReferenceNodeName").getValue(),
1154  getProperty<std::string>("KBMRightArmNodeSetName").getValue(),
1155  robotStateInterfacePrx,
1156  longtermMemoryPrx);
1157  }
1158 
1159  break;
1160  }
1161 
1162  default:
1163  ARMARX_WARNING << "Motion model type " << motionModelName
1164  << " not yet implemented here!";
1165  motionModel = new MotionModelStaticObject(robotStateInterfacePrx);
1166  break;
1167  }
1168 
1169  objectInstance->setMotionModel(motionModel);
1170  }
1171 } // namespace memoryx
MotionModelRobotHand.h
Gaussian::getCovariance
const covariance_type & getCovariance() const
Definition: Gaussian.h:82
memoryx::AbstractMotionModelPtr
IceInternal::Handle< AbstractMotionModel > AbstractMotionModelPtr
Definition: ObjectInstance.h:46
LinkedPose.h
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
memoryx::LocalizationJobContainerPtr
IceUtil::Handle< LocalizationJobContainer > LocalizationJobContainerPtr
Definition: LocalizationJobContainer.h:181
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::viz::toEigen
Eigen::Matrix4f toEigen(data::GlobalPose const &pose)
Definition: Interaction.h:48
OnScopeExit.h
memoryx::WorkingMemoryUpdater::getWorkingMemory
AbstractWorkingMemoryPtr getWorkingMemory()
Definition: WorkingMemoryUpdater.h:113
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
ObjectLocalizationMemoryUpdater.h
FileIOException.h
memoryx::LocalizationJobPtr
IceUtil::Handle< LocalizationJob > LocalizationJobPtr
Definition: LocalizationJob.h:74
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::Contains
bool Contains(const ContainerType &container, const ElementType &searchElement)
Definition: algorithm.h:330
memoryx::ObjectLocalizationMemoryUpdater::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: ObjectLocalizationMemoryUpdater.cpp:124
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
ObjectRecognitionWrapper.h
memoryx::LocalizationQueryPtr
IceInternal::Handle< LocalizationQuery > LocalizationQueryPtr
Definition: LocalizationJobContainer.h:39
StringHelpers.h
memoryx::EntityWrappers::ObjectRecognitionWrapperPtr
IceInternal::Handle< ObjectRecognitionWrapper > ObjectRecognitionWrapperPtr
Definition: ObjectRecognitionWrapper.h:57
cxxopts::empty
bool empty(const std::string &s)
Definition: cxxopts.hpp:234
memoryx::AbstractMotionModel::getMotionModelTypeByName
static EMotionModelType getMotionModelTypeByName(std::string motionModelName)
Definition: AbstractMotionModel.cpp:91
IceInternal::Handle< ObjectClass >
armarx::FloatVector
::std::vector<::Ice::Float > FloatVector
Definition: KinematicUnitGuiPlugin.h:325
armarx::GetHandledExceptionString
std::string GetHandledExceptionString()
Definition: Exception.cpp:165
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
memoryx::EarlyVisionConverters::convertToGaussian
Gaussian convertToGaussian(const NormalDistributionBasePtr &normalDistribution)
Definition: EarlyVisionConverters.cpp:30
armarx::VariantType::LinkedPose
const VariantTypeId LinkedPose
Definition: LinkedPose.h:43
armarx::mean
std::optional< float > mean(const boost::circular_buffer< NameValueMap > &buffer, const std::string &key)
Definition: KinematicUnitGuiPlugin.cpp:1620
armarx::ValueToString
std::string ValueToString(const T &value)
Definition: StringHelpers.h:61
MotionModelStaticObject.h
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
armarx::FramedPosition
The FramedPosition class.
Definition: FramedPose.h:157
memoryx::ObjectLocalizationMemoryUpdater::ObjectLocalizationMemoryUpdater
ObjectLocalizationMemoryUpdater()
Definition: ObjectLocalizationMemoryUpdater.cpp:50
memoryx::ObjectInstancePtr
IceInternal::Handle< ObjectInstance > ObjectInstancePtr
Definition: ObjectInstance.h:43
memoryx::AbstractMotionModel::eMotionModelRobotHand
@ eMotionModelRobotHand
Definition: AbstractMotionModel.h:45
memoryx::ObjectLocalizationMemoryUpdater::reportEntityUpdated
void reportEntityUpdated(const std::string &segmentName, const EntityBasePtr &entityOld, const EntityBasePtr &entityNew, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ObjectLocalizationMemoryUpdater.cpp:170
memoryx::AbstractMotionModel::eMotionModelKBM
@ eMotionModelKBM
Definition: AbstractMotionModel.h:47
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::FramedOrientation
The FramedOrientation class.
Definition: FramedPose.h:215
q
#define q
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:68
LocalizationStrategyOnce.h
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
armarx::control::common::getValue
T getValue(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName)
Definition: utils.h:71
armarx::ManagedIceObject::usingTopic
void usingTopic(const std::string &name, bool orderedPublishing=false)
Registers a proxy for subscription after initialization.
Definition: ManagedIceObject.cpp:254
AbstractMotionModel.h
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:39
armarx::FramedDirection
FramedDirection is a 3 dimensional direction vector with a reference frame. The reference frame can b...
Definition: FramedPose.h:86
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
IceUtil::Handle
Definition: forward_declarations.h:30
EarlyVisionConverters.h
IceInternal::ProxyHandle<::IceProxy::armarx::SharedRobotInterface >
memoryx::ObjectLocalizationMemoryUpdater::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: ObjectLocalizationMemoryUpdater.cpp:158
memoryx::EntityWrappers::ObjectRecognitionWrapper
Definition: ObjectRecognitionWrapper.h:40
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:99
memoryx::AbstractMotionModel::eMotionModelStaticObject
@ eMotionModelStaticObject
Definition: AbstractMotionModel.h:44
memoryx::ObjectLocalizationMemoryUpdater::setSegmentNames
void setSegmentNames(std::string classSegmentName, std::string instanceSegmentName)
Definition: ObjectLocalizationMemoryUpdater.cpp:59
memoryx::ObjectClassPtr
IceInternal::Handle< ObjectClass > ObjectClassPtr
Definition: ObjectClass.h:35
armarx::PeriodicTask
Definition: ArmarXManager.h:70
memoryx::ObjectLocalizationMemoryUpdater::setReferenceFrame
void setReferenceFrame(const std::string &referenceFrameName)
Definition: ObjectLocalizationMemoryUpdater.cpp:68
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:38
Gaussian
Definition: Gaussian.h:50
armarx::handleExceptions
void handleExceptions()
Definition: Exception.cpp:157
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
LocalizationStrategyRepeated.h
memoryx::ObjectLocalizationMemoryUpdater::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: ObjectLocalizationMemoryUpdater.cpp:77
ArmarXDataPath.h
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:154
armarx::DataFieldIdentifier
DataFieldIdentifier provide the basis to identify data field within a distributed ArmarX scenario.
Definition: DataFieldIdentifier.h:48
armarx::control::common::MPStatus::running
@ running
memoryx::AbstractMotionModel::EMotionModelType
EMotionModelType
Definition: AbstractMotionModel.h:42