GraspMemory.cpp
Go to the documentation of this file.
1 
2 #include "GraspMemory.h"
3 
4 #include <string>
5 
6 #include <Eigen/Core>
7 #include <Eigen/Geometry>
8 
9 #include <SimoxUtility/algorithm/get_map_keys_values.h>
10 #include <SimoxUtility/algorithm/string.h>
11 #include <VirtualRobot/XML/BaseIO.h>
12 
13 #include <ArmarXCore/core/PackagePath.h>
14 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
15 #include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
16 #include <ArmarXCore/core/logging/Logging.h>
17 #include <ArmarXCore/core/time/Clock.h>
18 #include <ArmarXCore/core/time/Metronome.h>
19 #include <ArmarXCore/core/time/forward_declarations.h>
20 
21 #include <RobotAPI/components/ArViz/Client/elements/Color.h>
22 #include <RobotAPI/components/ArViz/Client/elements/Robot.h>
23 #include <RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h>
24 #include <RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h>
25 #include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
26 #include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
27 #include <RobotAPI/libraries/armem/core/MemoryID.h>
28 #include <RobotAPI/libraries/armem/core/error.h>
29 #include <RobotAPI/libraries/armem/core/ice_conversions.h>
30 #include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
31 #include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
32 
33 namespace armarx::armem::server::grasp
34 {
35  armarx::PropertyDefinitionsPtr
36  GraspMemory::createPropertyDefinitions()
37  {
38  setMemoryName("Grasp");
39 
40  armarx::PropertyDefinitionsPtr defs =
41  new ComponentPropertyDefinitions(getConfigIdentifier());
42 
43  defs->topic(debugObserver);
44 
45  defs->optional(enableRemoteGui, "remoteGui.enable", "Enable/Disable Remote GUI");
46  defs->optional(gui.trackNewEntities,
47  "EnableTrackingOfNewEntities",
48  "Enable/Disable the automatic visual tracking of newly commited Entities");
49 
50  defs->optional(enableVisualizeKnownGraspCandidates,
51  "p.enableVisualizeKnownGraspCandidates");
52  defs->optional(enableVisualizeKnownGraspCandidatesPreposes,
53  "p.enableVisualizeKnownGraspCandidatesPreposes");
54  defs->optional(enableVisualizeKnownGraspCandidatesApproach,
55  "p.enableVisualizeKnownGraspCandidatesApproach");
56 
57  defs->optional(frequencyHzVisualizeKnownGraspCandidates,
58  "p.frequencyHzVisualizeKnownGraspCandidates");
59 
60 
61  return defs;
62  }
63 
64  std::string
65  GraspMemory::getDefaultName() const
66  {
67  return "GraspMemory";
68  }
69 
70  GraspMemory::GraspMemory() : knownGraspProviderSegment(iceAdapter())
71  {
72  addPlugin(virtualRobotReaderPlugin);
73  }
74 
75  void
76  GraspMemory::onInitComponent()
77  {
78  workingMemory().addCoreSegment("GraspCandidate",
79  armarx::grasping::arondto::GraspCandidate::ToAronType());
80  workingMemory().addCoreSegment(
81  "BimanualGraspCandidate",
82  armarx::grasping::arondto::BimanualGraspCandidate::ToAronType());
83  workingMemory().addCoreSegment(
84  "KnownGraspCandidate", armarx::armem::grasping::arondto::KnownGraspInfo::ToAronType());
85 
86  knownGraspProviderSegment.init();
87  }
88 
89  void
90  GraspMemory::onConnectComponent()
91  {
92  if (enableRemoteGui)
93  {
94  createRemoteGuiTab();
95  RemoteGui_startRunningTask();
96  }
97 
98  ARMARX_CHECK_NULL(taskVisuKnownGrasps);
99 
100  taskVisuKnownGrasps =
101  new RunningTask<GraspMemory>(this, &GraspMemory::visualizeKnownGraspCandidates);
102  taskVisuKnownGrasps->start();
103  }
104 
105  void
106  GraspMemory::onDisconnectComponent()
107  {
108  if (taskVisuKnownGrasps)
109  {
110  taskVisuKnownGrasps->stop();
111  taskVisuKnownGrasps = nullptr;
112  }
113 
114  // try to clear all layers
115  {
116  for (auto& [_, layer] : graspVisuPerObject)
117  {
118  layer.clear();
119  layer.markForDeletion();
120  }
121 
122  arviz.commit(simox::alg::get_values(graspVisuPerObject));
123  graspVisuPerObject.clear();
124  }
125  }
126 
127  void
128  GraspMemory::onExitComponent()
129  {
130  }
131 
132  // WRITING
133 
134  armem::data::CommitResult
135  GraspMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
136  {
137  std::vector<armem::MemoryID> trackedEntityIds;
138  {
139  std::unique_lock lock(gui.visualizationMutex);
140  trackedEntityIds = gui.trackedEntityIds;
141  }
142 
143  if (!trackedEntityIds.empty())
144  {
145  for (auto& update : commit.updates)
146  {
147  armem::MemoryID entityID = armarx::fromIce<armem::MemoryID>(update.entityID);
148  entityID.clearTimestamp();
149  if (std::find(trackedEntityIds.begin(), trackedEntityIds.end(), entityID) !=
150  trackedEntityIds.end())
151  {
152  workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance(
153  [this](const auto& instance) { removeInstanceFromVisu(instance.id()); });
154  }
155  }
156  }
157 
158  if (gui.trackNewEntities)
159  {
160  for (auto& update : commit.updates)
161  {
162  armem::MemoryID entityID = armarx::fromIce<armem::MemoryID>(update.entityID);
163  entityID.clearTimestamp();
164  if (!workingMemory().findEntity(entityID))
165  {
166  std::unique_lock lock(gui.visualizationMutex);
167  gui.trackedEntityIds.emplace_back(entityID);
168  trackedEntityIds.emplace_back(entityID);
169  }
170  }
171  }
172 
173  // This function is overloaded to trigger the remote gui rebuild.
174  armem::data::CommitResult result = ReadWritePluginUser::commit(commit);
175  gui.tab.rebuild = true;
176 
177 
178  if (!trackedEntityIds.empty())
179  {
180  for (auto& update : commit.updates)
181  {
182  armem::MemoryID entityID = armarx::fromIce<armem::MemoryID>(update.entityID);
183  entityID.clearTimestamp();
184  if (std::find(trackedEntityIds.begin(), trackedEntityIds.end(), entityID) !=
185  trackedEntityIds.end())
186  {
187  workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance(
188  [this](const auto& instance) { addInstanceToVisu(instance.id()); });
189  }
190  }
191  }
192  return result;
193  }
194 
195  // READING
196 
197  // Inherited from Plugin
198 
199  armem::actions::data::GetActionsOutputSeq
200  GraspMemory::getActions(const armem::actions::data::GetActionsInputSeq& inputs)
201  {
202  using namespace armem::actions;
203 
204  GetActionsOutputSeq outputs;
205  for (const auto& input : inputs)
206  {
207  auto memoryID = armarx::fromIce<armem::MemoryID>(input.id);
208  if (armem::contains(armem::MemoryID("Grasp"), memoryID) and not memoryID.hasGap())
209  {
210  if (armem::contains(armem::MemoryID("Grasp", "BimanualGraspCandidate"), memoryID) ||
211  armem::contains(armem::MemoryID("Grasp", "KnownGraspCandidate"), memoryID))
212  {
213  continue; // Not supported, ignore.
214  }
215 
216  std::vector<MenuEntry> actions;
217 
218  if (!memoryID.hasTimestamp() && !memoryID.hasInstanceIndex())
219  {
220  if (memoryID.hasEntityName())
221  {
222  if (std::find(gui.trackedEntityIds.begin(),
223  gui.trackedEntityIds.end(),
224  memoryID) == gui.trackedEntityIds.end())
225  {
226  actions.push_back(Action{"track", "Track this entity in arviz"});
227  }
228  else
229  {
230  actions.push_back(
231  Action{"untrack", "Stop tracking this entity in arviz"});
232  }
233  }
234  else
235  {
236  actions.push_back(
237  Action{"track", "Track all underlying entities in arviz"});
238  actions.push_back(
239  Action{"untrack", "Stop tracking all underlying entities in arviz"});
240  }
241  }
242 
243  actions.push_back(Action{"vis", "Visualize all contained grasp candidates"});
244  actions.push_back(
245  Action{"rem", "Remove all contained grasp candidates from visualization"});
246  actions.push_back(SubMenu{"high",
247  "Highlight all contain grasp candidates",
248  {Action{"pink", "in pink"},
249  Action{"red", "in red"},
250  Action{"blue", "in blue"},
251  Action{"yellow", "in yellow"},
252  Action{"purple", "in purple"}}});
253  actions.push_back(Action{"reset", "Reset highlight layer"});
254 
255  Menu menu{actions};
256  outputs.push_back({menu.toIce()});
257  }
258  }
259 
260  return outputs;
261  }
262 
263  armem::actions::data::ExecuteActionOutputSeq
264  GraspMemory::executeActions(const armem::actions::data::ExecuteActionInputSeq& inputs)
265  {
266  using namespace armem::actions;
267  ExecuteActionOutputSeq outputs;
268 
269  for (const auto& input : inputs)
270  {
271  auto memoryID = armarx::fromIce<armem::MemoryID>(input.id);
272  if (input.actionPath == ActionPath{"vis"} || input.actionPath == ActionPath{"rem"})
273  {
274  if (armem::contains(armem::MemoryID("Grasp"), memoryID) and not memoryID.hasGap())
275  {
276  {
277  if (armem::contains(armem::MemoryID("Grasp", "BimanualGraspCandidate"),
278  memoryID) ||
279  armem::contains(armem::MemoryID("Grasp", "KnownGraspCandidate"),
280  memoryID))
281  {
282  std::stringstream sstream;
283  sstream << "Currently visualization for CoreSegment "
284  << memoryID.coreSegmentName << " is not yet supported";
285  outputs.emplace_back(false, sstream.str());
286  }
287  else
288  {
289  if (memoryID.hasInstanceIndex())
290  {
291  if (input.actionPath == ActionPath{"vis"})
292  {
293  addInstanceToVisu(memoryID);
294  }
295  else if (input.actionPath == ActionPath{"rem"})
296  {
297  removeInstanceFromVisu(memoryID);
298  }
299  }
300  else if (memoryID.hasTimestamp())
301  {
302  workingMemory().getSnapshot(memoryID).forEachInstance(
303  [this, &input](const auto& instance)
304  {
305  if (input.actionPath == ActionPath{"vis"})
306  {
307  addInstanceToVisu(instance.id());
308  }
309  else if (input.actionPath == ActionPath{"rem"})
310  {
311  removeInstanceFromVisu(instance.id());
312  }
313  });
314  }
315  else if (memoryID.hasEntityName())
316  {
317  workingMemory().getEntity(memoryID).forEachInstance(
318  [this, &input](const auto& instance)
319  {
320  if (input.actionPath == ActionPath{"vis"})
321  {
322  addInstanceToVisu(instance.id());
323  }
324  else if (input.actionPath == ActionPath{"rem"})
325  {
326  removeInstanceFromVisu(instance.id());
327  }
328  });
329  }
330  else if (memoryID.hasProviderSegmentName())
331  {
332  workingMemory().getProviderSegment(memoryID).forEachInstance(
333  [this, &input](const auto& instance)
334  {
335  if (input.actionPath == ActionPath{"vis"})
336  {
337  addInstanceToVisu(instance.id());
338  }
339  else if (input.actionPath == ActionPath{"rem"})
340  {
341  removeInstanceFromVisu(instance.id());
342  }
343  });
344  }
345  else
346  {
347  if (input.actionPath == ActionPath{"rem"})
348  {
349  std::unique_lock lock(gui.visualizationMutex);
350  gui.visibleInstanceIds.clear();
351  // mark all layers for deletion
352  for (std::string& layer : gui.activeLayers)
353  {
354  arviz.commitDeleteLayer(layer + "_reachable");
355  arviz.commitDeleteLayer(layer + "_unreachable");
356  }
357  gui.activeLayers.clear();
358  }
359  else if (input.actionPath == ActionPath{"vis"})
360  {
361  //currently only visualization for CoreSegment GraspCandidate available
362  workingMemory()
363  .getCoreSegment("GraspCandidate")
364  .forEachInstance([this](const auto& instance)
365  { addInstanceToVisu(instance.id()); });
366  }
367  }
368 
369  visualizeGraspCandidates();
370  outputs.emplace_back(true, "");
371  }
372  }
373  }
374  else
375  {
376  std::stringstream sstream;
377  sstream << "MemoryID " << memoryID
378  << " does not refer to a valid Grasp Memory Part.";
379  outputs.emplace_back(false, sstream.str());
380  }
381  }
382  else if (input.actionPath == ActionPath{"track"} ||
383  input.actionPath == ActionPath{"untrack"})
384  {
385  std::vector<armem::MemoryID> entityIDs;
386  if (memoryID.hasEntityName())
387  {
388  entityIDs.emplace_back(memoryID);
389  }
390  else if (memoryID.hasProviderSegmentName())
391  {
392  workingMemory().getProviderSegment(memoryID).forEachEntity(
393  [&entityIDs](const auto& entity) { entityIDs.emplace_back(entity.id()); });
394  }
395  else
396  {
397  //currently only visualization for CoreSegment GraspCandidate available
398  workingMemory()
399  .getCoreSegment("GraspCandidate")
400  .forEachEntity([&entityIDs](const auto& entity)
401  { entityIDs.emplace_back(entity.id()); });
402  }
403  for (auto& entityID : entityIDs)
404  {
405  if (input.actionPath == ActionPath{"track"})
406  {
407  //visualize latest snapshot of entity and refresh if necessary
408  workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance(
409  [this](const auto& instance) { addInstanceToVisu(instance.id()); });
410  gui.trackedEntityIds.push_back(entityID);
411  ARMARX_INFO << "starting to track " << entityID;
412  outputs.emplace_back(true, "");
413  }
414  else if (input.actionPath == ActionPath{"untrack"})
415  {
416  auto pos = std::find(
417  gui.trackedEntityIds.begin(), gui.trackedEntityIds.end(), entityID);
418  if (pos != gui.trackedEntityIds.end())
419  {
420  gui.trackedEntityIds.erase(pos);
421  ARMARX_INFO << "Stop tracking of " << entityID;
422  }
423  outputs.emplace_back(true, "");
424  }
425  }
426  }
427  else if (input.actionPath.front() == "high")
428  {
429  addToHighlightLayer(memoryID, input.actionPath.back());
430  }
431  else if (input.actionPath == ActionPath{"reset"})
432  {
433  arviz.commit(arviz.layer("HighlightedGrasps"));
434  }
435  else
436  {
437  std::stringstream sstream;
438  sstream << "Action path " << input.actionPath << " is not defined.";
439  outputs.emplace_back(false, sstream.str());
440  }
441  }
442  return outputs;
443  }
444 
445  // REMOTE GUI
446 
447  void
448  GraspMemory::createRemoteGuiTab()
449  {
450  using namespace armarx::RemoteGui::Client;
451  GridLayout root;
452  {
453  gui.tab.selectAll.setLabel("Select All");
454  gui.tab.deselectAll.setLabel("Deselect All");
455  gui.tab.showUnlimitedInstances.setValue(gui.unlimitedInstances);
456  gui.tab.maxInstances.setRange(0, 1000);
457  gui.tab.maxInstances.setValue(gui.maxInstances);
458  int row = 0;
459 
460  // bimanual candidates are not available for visualization for now
461  // {
462  // root.add(Label("CoreSegment"), Pos{row, 0});
463  // row++;
464  // std::vector<std::string> options = workingMemory().getCoreSegmentNames();
465  // if (options.empty())
466  // {
467  // options.push_back("<None>");
468  // }
469  // gui.tab.selectCoreSegment.setOptions(options);
470  // if (gui.coreSegIndex >= 0 && gui.coreSegIndex < static_cast<int>(options.size()))
471  // {
472  // gui.tab.selectCoreSegment.setIndex(gui.coreSegIndex);
473  // gui.coreSegment = options[gui.coreSegIndex];
474  // }
475  // root.add(gui.tab.selectCoreSegment, Pos{row,0});
476  // row++;
477  // }
478  if (workingMemory().hasCoreSegment(gui.coreSegment))
479  {
480  {
481  root.add(Label("Provider"), Pos{row, 0});
482  row++;
483  std::vector<std::string> options =
484  workingMemory().getCoreSegment(gui.coreSegment).getProviderSegmentNames();
485  if (options.empty())
486  {
487  options.push_back("<None>");
488  }
489  else
490  {
491  options.insert(options.begin(), "<All>");
492  }
493 
494  gui.tab.selectProvider.setOptions(options);
495 
496  if (gui.providerIndex >= 0 &&
497  gui.providerIndex < static_cast<int>(options.size()))
498  {
499  gui.tab.selectProvider.setIndex(gui.providerIndex);
500  gui.provider = options[gui.providerIndex];
501  }
502  root.add(gui.tab.selectProvider, Pos{row, 0});
503  row++;
504  }
505 
506  if (gui.provider == "<All>" || workingMemory()
507  .getCoreSegment(gui.coreSegment)
508  .hasProviderSegment(gui.provider))
509  {
510  std::vector<MemoryID> providers;
511  {
512  root.add(Label("Entity"), Pos{row, 0});
513  row++;
514  std::vector<std::string> options;
515 
516  if (gui.provider == "<All>")
517  {
518  workingMemory()
519  .getCoreSegment(gui.coreSegment)
520  .forEachProviderSegment([&providers](const auto& provider)
521  { providers.push_back(provider.id()); });
522  }
523  else
524  {
525  providers.push_back(
526  MemoryID(workingMemory().name(), gui.coreSegment, gui.provider));
527  }
528  for (MemoryID& provider : providers)
529  {
530  for (std::string& entity :
531  workingMemory().getProviderSegment(provider).getEntityNames())
532  {
533  options.push_back(entity);
534  }
535  }
536  if (options.empty())
537  {
538  options.push_back("<None>");
539  }
540  else
541  {
542  options.insert(options.begin(), "<All>");
543  }
544  gui.tab.selectEntity.setOptions(options);
545  if (gui.entityIndex >= 0 &&
546  gui.entityIndex < static_cast<int>(options.size()))
547  {
548  gui.tab.selectEntity.setIndex(gui.entityIndex);
549  gui.entity = options[gui.entityIndex];
550  }
551  root.add(gui.tab.selectEntity, Pos{row, 0});
552  row++;
553  }
554 
555  if (gui.entity == "<All>" ||
556  (gui.provider == "<All>" &&
557  workingMemory().getCoreSegment(gui.coreSegment).hasEntity(gui.entity)) ||
558  workingMemory()
559  .getCoreSegment(gui.coreSegment)
560  .getProviderSegment(gui.provider)
561  .hasEntity(gui.entity))
562  {
563  std::vector<MemoryID> entities;
564  {
565  root.add(Label("Snapshot"), Pos{row, 0});
566  row++;
567  std::vector<std::string> options;
568  for (MemoryID& provider : providers)
569  {
570  if (gui.entity == "<All>")
571  {
572  workingMemory().getProviderSegment(provider).forEachEntity(
573  [&entities](auto& entity)
574  { entities.push_back(entity.id()); });
575  }
576  else
577  {
578  if (workingMemory().getProviderSegment(provider).hasEntity(
579  gui.entity))
580  {
581  entities.push_back(MemoryID(workingMemory().name(),
582  gui.coreSegment,
583  provider.providerSegmentName,
584  gui.entity));
585  }
586  }
587  }
588 
589  for (MemoryID& entity : entities)
590  {
591  workingMemory().getEntity(entity).forEachSnapshot(
592  [this, &options](const auto& snapshot)
593  {
594  std::string time =
595  armem::toDateTimeMilliSeconds(snapshot.time());
596  gui.timeMap[time] = snapshot.time();
597  options.push_back(time);
598  });
599  }
600 
601  if (options.empty())
602  {
603  options.push_back("<None>");
604  }
605  else
606  {
607  options.insert(options.begin(), "<All>");
608  }
609  gui.tab.selectSnapshot.setOptions(options);
610  if (gui.snapshotIndex >= 0 &&
611  gui.snapshotIndex < static_cast<int>(options.size()))
612  {
613  gui.tab.selectSnapshot.setIndex(gui.snapshotIndex);
614  if (options[gui.snapshotIndex] != "<None>")
615  {
616  gui.snapshot = options[gui.snapshotIndex];
617  }
618  }
619  root.add(gui.tab.selectSnapshot, Pos{row, 0});
620  row++;
621  }
622  bool comboExists;
623  if (gui.snapshot == "<All>")
624  {
625  comboExists = true;
626  }
627  else
628  {
629  for (MemoryID& entity : entities)
630  {
631  if (workingMemory().getEntity(entity).hasSnapshot(
632  gui.timeMap.at(gui.snapshot)))
633  {
634  comboExists = true;
635  }
636  }
637  }
638  if (comboExists)
639  {
640  root.add(Label("Instances"), Pos{row, 0});
641  row++;
642  root.add(Label("Show all Instances"), Pos{row, 0});
643  root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
644  row++;
645  root.add(Label("Limit for shown Instances"), Pos{row, 0});
646  root.add(gui.tab.maxInstances, Pos{row, 1});
647  row++;
648  root.add(gui.tab.selectAll, Pos{row, 0});
649  row++;
650  root.add(gui.tab.deselectAll, Pos{row, 0});
651  row++;
652  gui.tab.checkInstances.clear();
653  std::vector<MemoryID> snapshots;
654  for (MemoryID& entity : entities)
655  {
656  if (gui.snapshot == "<All>")
657  {
658  workingMemory().getEntity(entity).forEachSnapshot(
659  [&snapshots](auto& snapshot)
660  { snapshots.push_back(snapshot.id()); });
661  }
662  else
663  {
664  if (workingMemory().getEntity(entity).hasSnapshot(
665  gui.timeMap.at(gui.snapshot)))
666  {
667  snapshots.push_back(
668  workingMemory()
669  .getEntity(entity)
670  .getSnapshot(gui.timeMap.at(gui.snapshot))
671  .id());
672  }
673  }
674  }
675  int instances = 0;
676  for (MemoryID& snapshot : snapshots)
677  {
678  workingMemory().getSnapshot(snapshot).forEachInstance(
679  [this, &row, &root, &instances](const auto& instance)
680  {
681  if (gui.unlimitedInstances || instances < gui.maxInstances)
682  {
683  std::unique_lock lock(gui.visualizationMutex);
684  root.add(Label(instance.id().str()), Pos{row, 0});
685  gui.tab.checkInstances[instance.id().str()] =
686  CheckBox();
687  if (std::find(gui.visibleInstanceIds.begin(),
688  gui.visibleInstanceIds.end(),
689  instance.id()) !=
690  gui.visibleInstanceIds.end())
691  {
692  gui.tab.checkInstances.at(instance.id().str())
693  .setValue(true);
694  }
695  root.add(gui.tab.checkInstances[instance.id().str()],
696  Pos{row, 1});
697  row++;
698  instances++;
699  }
700  });
701  }
702  }
703  else
704  {
705  root.add(Label("Instances"), Pos{row, 0});
706  row++;
707  root.add(Label("Show all Instances"), Pos{row, 0});
708  root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
709  row++;
710  root.add(Label("Limit for shown Instances"), Pos{row, 0});
711  root.add(gui.tab.maxInstances, Pos{row, 1});
712  row++;
713  root.add(gui.tab.selectAll, Pos{row, 0});
714  row++;
715  root.add(gui.tab.deselectAll, Pos{row, 0});
716  row++;
717  gui.tab.checkInstances.clear();
718  }
719  }
720  else
721  {
722  gui.tab.checkInstances.clear();
723 
724  std::vector<std::string> options = {"<None>"};
725 
726  gui.tab.selectSnapshot.setOptions(options);
727 
728  root.add(Label("Snapshot"), Pos{row, 0});
729  row++;
730  root.add(gui.tab.selectSnapshot, Pos{row, 0});
731  row++;
732  root.add(Label("Show all Instances"), Pos{row, 0});
733  root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
734  row++;
735  root.add(Label("Limit for shown Instances"), Pos{row, 0});
736  root.add(gui.tab.maxInstances, Pos{row, 1});
737  row++;
738  root.add(gui.tab.selectAll, Pos{row, 0});
739  row++;
740  root.add(gui.tab.deselectAll, Pos{row, 0});
741  row++;
742  }
743  }
744  else
745  {
746  gui.tab.checkInstances.clear();
747 
748  std::vector<std::string> options = {"<None>"};
749 
750  gui.tab.selectEntity.setOptions(options);
751 
752  root.add(Label("Entity"), Pos{row, 0});
753  row++;
754  root.add(gui.tab.selectEntity, Pos{row, 0});
755  row++;
756 
757  gui.tab.selectSnapshot.setOptions(options);
758 
759  root.add(Label("Snapshot"), Pos{row, 0});
760  row++;
761  root.add(gui.tab.selectSnapshot, Pos{row, 0});
762  row++;
763  root.add(Label("Show all Instances"), Pos{row, 0});
764  root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
765  row++;
766  root.add(Label("Limit for shown Instances"), Pos{row, 0});
767  root.add(gui.tab.maxInstances, Pos{row, 1});
768  row++;
769  root.add(gui.tab.selectAll, Pos{row, 0});
770  row++;
771  root.add(gui.tab.deselectAll, Pos{row, 0});
772  row++;
773  }
774  }
775  else
776  {
777  gui.tab.checkInstances.clear();
778 
779  std::vector<std::string> options = {"<None>"};
780  gui.tab.selectProvider.setOptions(options);
781 
782  root.add(Label("Provider"), Pos{row, 0});
783  row++;
784  root.add(gui.tab.selectProvider, Pos{row, 0});
785  row++;
786 
787  gui.tab.selectEntity.setOptions(options);
788 
789  root.add(Label("Entity"), Pos{row, 0});
790  row++;
791  root.add(gui.tab.selectEntity, Pos{row, 0});
792  row++;
793 
794  gui.tab.selectSnapshot.setOptions(options);
795 
796  root.add(Label("Snapshot"), Pos{row, 0});
797  row++;
798  root.add(gui.tab.selectSnapshot, Pos{row, 0});
799  row++;
800  root.add(Label("Show all Instances"), Pos{row, 0});
801  root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
802  row++;
803  root.add(Label("Limit for shown Instances"), Pos{row, 0});
804  root.add(gui.tab.maxInstances, Pos{row, 1});
805  row++;
806  root.add(gui.tab.selectAll, Pos{row, 0});
807  row++;
808  root.add(gui.tab.deselectAll, Pos{row, 0});
809  row++;
810  }
811  }
812 
813 
814  RemoteGui_createTab(getName(), root, &gui.tab);
815  gui.tab.rebuild = false;
816  }
817 
818  inline const auto GetSide =
819  [](const armarx::armem::grasping::arondto::KnownGraspSet& graspSet) -> std::string
820  {
821  ARMARX_VERBOSE << "Grasping with " << graspSet.robot << " and with the endeffector named"
822  << graspSet.endeffector;
823  //Filter the robot befor that
824  if (graspSet.robot == "Armar6" or graspSet.robot == "ArmarDE" or graspSet.robot == "Armar7")
825  {
826  if (graspSet.endeffector == "TCP_R")
827  {
828  return "Right";
829  }
830  if (graspSet.endeffector == "TCP_L")
831  {
832  return "Left";
833  }
834  if (graspSet.endeffector == "Hand_R_EEF")
835  {
836  return "Right";
837  }
838  if (graspSet.endeffector == "Hand_L_EEF")
839  {
840  return "Left";
841  }
842  ARMARX_ERROR << "Unknown TCP `" << graspSet.endeffector << "` for robot `"
843  << graspSet.robot << "`!";
844  }
845 
846  if (graspSet.robot == "Armar3")
847  {
848  if (graspSet.endeffector == "Hand R")
849  {
850  return "Right";
851  }
852  if (graspSet.endeffector == "Hand L")
853  {
854  return "Left";
855  }
856  ARMARX_ERROR << "Unknown TCP `" << graspSet.endeffector << "` for robot `"
857  << graspSet.robot << "`!";
858  }
859 
860  ARMARX_ERROR << "Unknown TCP `" << graspSet.endeffector << "` for robot `" << graspSet.robot
861  << "`!";
862  throw std::exception();
863  };
864 
865  void
866  armarx::armem::server::grasp::GraspMemory::visualizeKnownGraspCandidates()
867  {
868  if (not enableVisualizeKnownGraspCandidates)
869  {
870  return;
871  }
872 
873  Metronome metronome(Frequency::Hertz(frequencyHzVisualizeKnownGraspCandidates));
874 
875  while (not taskVisuKnownGrasps->isStopped())
876  {
877  // do the work
878  {
879  const std::vector<robot_state::description::RobotDescription> robotDescriptions =
880  virtualRobotReaderPlugin->get().queryDescriptions(armarx::Clock::Now());
881  ARMARX_VERBOSE << VAROUT(robotDescriptions.size());
882 
883  std::map<std::string, robot_state::description::RobotDescription>
884  robotDescriptionMap;
885  for (const auto& robotDescription : robotDescriptions)
886  {
887  robotDescriptionMap.emplace(robotDescription.name, robotDescription);
888  }
889 
890  // load robot if needed
891  for (const auto& robotDescription : robotDescriptions)
892  {
893  if (robots.count(robotDescription.name) > 0)
894  {
895  // robot already loaded
896  continue;
897  }
898 
899 
900  if (auto robot = VirtualRobot::RobotIO::loadRobot(
901  robotDescription.xml.toSystemPath(), VirtualRobot::BaseIO::eStructure))
902  {
903  ARMARX_INFO << "Loaded robot `" << robotDescription.name << "`";
904  robots.emplace(robotDescription.name, robot);
905  }
906  else
907  {
908  ARMARX_INFO << "Failed to load robot `" << robotDescription.name
909  << "` from file `" << robotDescription.xml.toSystemPath()
910  << "`.";
911  }
912  }
913 
914 
915  std::map<std::string, std::vector<armarx::armem::grasping::arondto::KnownGraspInfo>>
916  graspsForObject;
917  {
918 
919  workingMemory()
920  .getCoreSegment("KnownGraspCandidate")
921  .forEachInstance(
922  [&graspsForObject](const auto& entityInstance)
923  {
924  const armarx::armem::grasping::arondto::KnownGraspInfo dto =
925  armarx::armem::grasping::arondto::KnownGraspInfo::FromAron(
926  entityInstance.data());
927 
928  const MemoryID memoryId = entityInstance.id();
929 
930  // memoryId.entityName is the objectClass
931  graspsForObject[memoryId.entityName].push_back(dto);
932  });
933  }
934 
935  // ensure old data is cleared
936  for (auto& [_, layer] : graspVisuPerObject)
937  {
938  layer.clear();
939  }
940 
941  // Query objects
942 
943  const auto objects = getObjectPoses();
944 
945  for (const auto& object : objects)
946  {
947  const std::string layerName = object.objectID.str();
948  auto layer = arviz.layer(layerName);
949 
950  // get all corresponding grasps
951  const std::string objectClassName = object.objectID.getClassID().str();
952 
953  if (graspsForObject.count(objectClassName) == 0)
954  {
955  continue;
956  }
957 
958  const auto& graspInfos = graspsForObject.at(objectClassName);
959 
960  for (const auto& graspInfo : graspInfos)
961  {
962  for (const auto& [_, graspSet] : graspInfo.graspSets)
963  {
964  // if robot is unknown, skip it
965  if (robotDescriptionMap.count(graspSet.robot) == 0)
966  {
967  continue;
968  }
969 
970  const std::string side = GetSide(graspSet);
971  const std::string bodyPartName = side + "Hand";
972 
973  const auto& robotDescription = robotDescriptionMap.at(graspSet.robot);
974  const auto& visuPart =
975  robotDescription.visualization.body_parts.at(bodyPartName);
976 
977  const armarx::PackagePath visuRobotPartXML{visuPart.xml.package,
978  visuPart.xml.path};
979 
980  for (const auto& grasp : graspSet.grasps)
981  {
982  const std::string visuName = graspSet.robot + "::" + graspSet.name +
983  "::" + object.providerName +
984  "::" + object.objectID.str() +
985  "::" + grasp.name;
986  ARMARX_VERBOSE << VAROUT(visuName);
987 
988  if (robots.count(graspSet.robot) == 0)
989  {
990  continue;
991  }
992 
993  const auto robot = robots.at(graspSet.robot);
994 
995  const auto handRootNode =
996  robot->getRobotNode(visuPart.root_node_name);
997  if (handRootNode == nullptr)
998  {
999  ARMARX_INFO << "Node `" << visuPart.root_node_name
1000  << "` not available for robot `" << graspSet.robot
1001  << "`.";
1002  continue;
1003  }
1004 
1005  const auto eefNode = robot->getEndEffector(graspSet.endeffector);
1006  if (eefNode == nullptr)
1007  {
1008  ARMARX_INFO << "End-effector `" << graspSet.endeffector
1009  << "` not available for robot `" << graspSet.robot
1010  << "`.";
1011  continue;
1012  }
1013 
1014  const auto tcp = eefNode->getTcp();
1015  if (tcp == nullptr)
1016  {
1017  ARMARX_INFO << "End-effector `" << graspSet.endeffector
1018  << "` does not provide a TCP";
1019  continue;
1020  }
1021 
1022  const Eigen::Isometry3f eef_T_hand_root{
1023  handRootNode->getPoseInFrame(tcp)};
1024 
1025  const Eigen::Isometry3f global_T_grasp_pose =
1026  Eigen::Isometry3f{object.objectPoseGlobal} *
1027  Eigen::Isometry3f{grasp.pose}.inverse();
1028 
1029  // visualize grasp pose
1030  {
1031  viz::Robot graspHandVisu(visuName + "_grasp");
1032  graspHandVisu.useCollisionModel()
1033  .file(visuRobotPartXML)
1034  .pose(
1035  Eigen::Isometry3f{global_T_grasp_pose * eef_T_hand_root}
1036  .matrix());
1037 
1038  layer.add(graspHandVisu);
1039  }
1040 
1041  // visualize prepose if available
1042  if (grasp.prepose.has_value())
1043  {
1044  const Eigen::Isometry3f global_T_prepose =
1045  Eigen::Isometry3f{object.objectPoseGlobal} *
1046  Eigen::Isometry3f{grasp.prepose.value()}.inverse();
1047 
1048  // visualize as prepose as hand
1049  if (enableVisualizeKnownGraspCandidatesPreposes)
1050  {
1051  viz::Robot graspHandVisuPrepose(visuName + "_prepose");
1052  graspHandVisuPrepose.useCollisionModel()
1053  .file(visuRobotPartXML)
1054  .pose(Eigen::Isometry3f{global_T_prepose *
1055  eef_T_hand_root}
1056  .matrix())
1057  .overrideColor(viz::Color::blue());
1058 
1059  layer.add(graspHandVisuPrepose);
1060  }
1061 
1062  // visualize approach direction (arrow between poses)
1063  if (enableVisualizeKnownGraspCandidatesApproach)
1064  {
1065  viz::Arrow arrow(visuName + "_approach");
1066  arrow
1067  .fromTo(global_T_prepose.translation(),
1068  global_T_grasp_pose.translation())
1069  .color(viz::Color::green());
1070 
1071  layer.add(arrow);
1072  }
1073  }
1074  }
1075  }
1076  }
1077 
1078  graspVisuPerObject[layerName] = layer;
1079  }
1080 
1081  arviz.commit(simox::alg::get_values(graspVisuPerObject));
1082  }
1083 
1084  metronome.waitForNextTick();
1085  }
1086  }
1087 
1088  void
1089  armarx::armem::server::grasp::GraspMemory::visualizeGraspCandidates()
1090  {
1091  std::unique_lock lock(gui.visualizationMutex);
1092  armarx::grasping::GraspCandidateVisu visu;
1093  std::map<std::string, viz::Layer> reachableLayers;
1094  std::map<std::string, viz::Layer> unreachableLayers;
1095 
1096  {
1097 
1098  for (auto& element : gui.visibleInstanceIds)
1099  {
1100  std::string entityName = element.entityName;
1101 
1102  if (reachableLayers.find(entityName) == reachableLayers.end())
1103  {
1104  reachableLayers[entityName] = arviz.layer(entityName + "_reachable");
1105  unreachableLayers[entityName] = arviz.layer(entityName + "_unreachable");
1106  gui.activeLayers.push_back(entityName);
1107  }
1108 
1109  armarx::grasping::GraspCandidate candidate;
1110 
1111  armarx::grasping::arondto::GraspCandidate aronTransform;
1112 
1113  aronTransform.fromAron(workingMemory().getInstance(element).data());
1114 
1115  fromAron(aronTransform, candidate);
1116 
1117  visu.visualize(element.str(),
1118  candidate,
1119  reachableLayers.at(entityName),
1120  unreachableLayers.at(entityName));
1121  }
1122  }
1123 
1124  std::vector<viz::Layer> layers;
1125  for (auto& [entityName, layer] : reachableLayers)
1126  {
1127  layers.push_back(layer);
1128  layers.push_back(unreachableLayers.at(entityName));
1129  }
1130  arviz.commit(layers);
1131  }
1132 
1133  void
1134  armarx::armem::server::grasp::GraspMemory::addInstanceToVisu(
1135  const armarx::armem::MemoryID& instance)
1136  {
1137  std::unique_lock lock(gui.visualizationMutex);
1138  auto position =
1139  std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance);
1140 
1141  if (position == gui.visibleInstanceIds.end())
1142  {
1143  gui.visibleInstanceIds.push_back(instance);
1144  }
1145  }
1146 
1147  void
1148  armarx::armem::server::grasp::GraspMemory::removeInstanceFromVisu(
1149  const armarx::armem::MemoryID& instance)
1150  {
1151  std::unique_lock lock(gui.visualizationMutex);
1152  auto position =
1153  std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance);
1154 
1155  if (position != gui.visibleInstanceIds.end())
1156  {
1157  gui.visibleInstanceIds.erase(position);
1158  }
1159  //check if this was the last instance of this entity and if so, mark layer for deletion
1160 
1161  std::string entityName = instance.entityName;
1162 
1163  if (std::none_of(gui.visibleInstanceIds.begin(),
1164  gui.visibleInstanceIds.end(),
1165  [&entityName](const armem::MemoryID& id)
1166  { return id.entityName == entityName; }))
1167  {
1168  arviz.commitDeleteLayer(entityName + "_reachable");
1169  arviz.commitDeleteLayer(entityName + "_unreachable");
1170  }
1171 
1172  auto layerPos = std::find(gui.activeLayers.begin(), gui.activeLayers.end(), entityName);
1173 
1174  if (layerPos != gui.activeLayers.end())
1175  {
1176  gui.activeLayers.erase(layerPos);
1177  }
1178  }
1179 
1180  void
1181  GraspMemory::addToHighlightLayer(const MemoryID& memoryID, const std::string color)
1182  {
1183  viz::Color handColor;
1184 
1185  if (color == "pink")
1186  {
1187  handColor = viz::Color::pink();
1188  }
1189  else if (color == "red")
1190  {
1191  handColor = viz::Color::red();
1192  }
1193  else if (color == "blue")
1194  {
1195  handColor = viz::Color::blue();
1196  }
1197  else if (color == "yellow")
1198  {
1199  handColor = viz::Color::yellow();
1200  }
1201  else if (color == "purple")
1202  {
1203  handColor = viz::Color::purple();
1204  }
1205 
1206  viz::Layer highlightLayer = arviz.layer(("HighlightedGrasps"));
1207  armarx::grasping::GraspCandidateVisu visu;
1208 
1209  std::vector<armem::MemoryID> instances;
1210 
1211  if (memoryID.hasInstanceIndex())
1212  {
1213  instances.push_back(memoryID);
1214  }
1215  else if (memoryID.hasTimestamp())
1216  {
1217  workingMemory().getSnapshot(memoryID).forEachInstance(
1218  [&instances](const auto& instance) { instances.push_back(instance.id()); });
1219  }
1220  else if (memoryID.hasEntityName())
1221  {
1222  workingMemory().getEntity(memoryID).forEachInstance(
1223  [&instances](const auto& instance) { instances.push_back(instance.id()); });
1224  }
1225  else if (memoryID.hasProviderSegmentName())
1226  {
1227  workingMemory().getProviderSegment(memoryID).forEachInstance(
1228  [&instances](const auto& instance) { instances.push_back(instance.id()); });
1229  }
1230  else
1231  {
1232  //currently only visualization for CoreSegment GraspCandidate available
1233  workingMemory()
1234  .getCoreSegment("GraspCandidate")
1235  .forEachInstance([&instances](const auto& instance)
1236  { instances.push_back(instance.id()); });
1237  }
1238 
1239  armarx::grasping::GraspCandidate candidate;
1240 
1241  armarx::grasping::arondto::GraspCandidate aronTransform;
1242 
1243  for (armem::MemoryID& instance : instances)
1244  {
1245  aronTransform.fromAron(workingMemory().getInstance(instance).data());
1246 
1247  fromAron(aronTransform, candidate);
1248 
1249 
1250  viz::Robot hand = visu.visualize(instance.str(), candidate);
1251  hand.color(handColor);
1252  highlightLayer.add(hand);
1253  }
1254 
1255  arviz.commit(highlightLayer);
1256  }
1257 
1258  void
1259  GraspMemory::RemoteGui_update()
1260  {
1261  // if (gui.tab.selectCoreSegment.hasValueChanged())
1262  // {
1263  // gui.coreSegment = gui.tab.selectCoreSegment.getValue();
1264  // gui.coreSegIndex = gui.tab.selectCoreSegment.getIndex();
1265  // gui.providerIndex = 0;
1266  // gui.entityIndex = 0;
1267  // gui.snapshotIndex = 0;
1268  // gui.tab.rebuild = true;
1269  // }
1270 
1271  if (gui.tab.selectProvider.hasValueChanged())
1272  {
1273  gui.provider = gui.tab.selectProvider.getValue();
1274  gui.providerIndex = gui.tab.selectProvider.getIndex();
1275  gui.entityIndex = 0;
1276  gui.snapshotIndex = 0;
1277  gui.tab.rebuild = true;
1278  }
1279 
1280  if (gui.tab.selectEntity.hasValueChanged())
1281  {
1282  gui.entity = gui.tab.selectEntity.getValue();
1283  gui.entityIndex = gui.tab.selectEntity.getIndex();
1284  gui.snapshotIndex = 0;
1285  gui.tab.rebuild = true;
1286  }
1287 
1288  if (gui.tab.selectSnapshot.hasValueChanged())
1289  {
1290  gui.snapshot = gui.tab.selectSnapshot.getValue();
1291  gui.snapshotIndex = gui.tab.selectSnapshot.getIndex();
1292  gui.tab.rebuild = true;
1293  }
1294 
1295  if (gui.tab.showUnlimitedInstances.hasValueChanged())
1296  {
1297  gui.unlimitedInstances = gui.tab.showUnlimitedInstances.getValue();
1298  gui.tab.rebuild = true;
1299  }
1300 
1301  if (gui.tab.maxInstances.hasValueChanged())
1302  {
1303  gui.maxInstances = gui.tab.maxInstances.getValue();
1304  gui.tab.rebuild = true;
1305  }
1306 
1307  if (gui.tab.selectAll.wasClicked())
1308  {
1309  for (auto& element : gui.tab.checkInstances)
1310  {
1311  element.second.setValue(true);
1312  }
1313  }
1314 
1315  if (gui.tab.deselectAll.wasClicked())
1316  {
1317  for (auto& element : gui.tab.checkInstances)
1318  {
1319  element.second.setValue(false);
1320  }
1321  }
1322 
1323  for (auto& element : gui.tab.checkInstances)
1324  {
1325  if (element.second.hasValueChanged())
1326  {
1327  if (element.second.getValue())
1328  {
1329  addInstanceToVisu(armem::MemoryID::fromString(element.first));
1330  }
1331  else
1332  {
1333  removeInstanceFromVisu(armem::MemoryID::fromString(element.first));
1334  }
1335  }
1336  }
1337 
1338  visualizeGraspCandidates();
1339 
1340  if (gui.tab.rebuild)
1341  {
1342  ARMARX_INFO << "Rebuilding remote gui tab";
1343  createRemoteGuiTab();
1344  }
1345  }
1346 
1347 } // namespace armarx::armem::server::grasp
armarx::armem::server::grasp::GraspMemory::commit
armem::data::CommitResult commit(const armem::data::Commit &commit, const Ice::Current &) override
Definition: GraspMemory.cpp:135
armarx::viz::Client::commit
CommitResult commit(StagedCommit const &commit)
Definition: Client.cpp:89
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
GraspMemory.h
Robot.h
armarx::grasping::GraspCandidateVisu
Definition: GraspCandidateVisu.h:9
armarx::viz::Color::purple
static Color purple(int p=255, int a=255)
2 Blue + 1 Red
Definition: Color.h:167
robots
std::vector< VirtualRobot::RobotPtr > robots
Definition: VisualizationRobot.cpp:88
armarx::armem::server::grasp::GraspMemory::getActions
armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq &inputs) override
Definition: GraspMemory.cpp:200
armarx::armem::base::detail::GetFindEntityMixin::getEntity
auto & getEntity(const MemoryID &entityID)
Retrieve an entity.
Definition: lookup_mixins.h:445
MemoryID.h
armarx::armem::base::MemoryBase::getCoreSegment
CoreSegmentT & getCoreSegment(const std::string &name)
Definition: MemoryBase.h:132
armarx::armem::server::grasp
Definition: GraspMemory.cpp:33
armarx::viz::Client::commitDeleteLayer
void commitDeleteLayer(std::string const &name)
Definition: Client.h:183
armarx::RemoteGui::Client::GridLayout::add
GridLayout & add(Widget const &child, Pos pos, Span span=Span{1, 1})
Definition: Widgets.cpp:438
armarx::armem::MemoryID::str
std::string str(bool escapeDelimiters=true) const
Get a string representation of this memory ID.
Definition: MemoryID.cpp:102
GraspCandidateVisu.h
armarx::viz::Color::blue
static Color blue(int b=255, int a=255)
Definition: Color.h:100
armarx::armem::server::grasp::GraspMemory::createRemoteGuiTab
void createRemoteGuiTab()
Definition: GraspMemory.cpp:448
armarx::armem::server::plugins::ReadWritePluginUser::setMemoryName
void setMemoryName(const std::string &memoryName)
Definition: ReadWritePluginUser.cpp:25
armarx::viz::Arrow
Definition: Elements.h:196
armarx::ManagedIceObject::addPlugin
PluginT * addPlugin(const std::string prefix="", ParamsT &&... params)
Definition: ManagedIceObject.h:186
armarx::grasping::GraspCandidateVisu::visualize
void visualize(const grasping::GraspCandidateDict &candidates, viz::Client &arviz)
Definition: GraspCandidateVisu.cpp:13
armarx::armem::server::grasp::GraspMemory::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: GraspMemory.cpp:90
armarx::armem::base::detail::GetFindSnapshotMixin::getSnapshot
auto & getSnapshot(const MemoryID &snapshotID)
Retrieve an entity snapshot.
Definition: lookup_mixins.h:286
armarx::viz::Layer::add
void add(ElementT const &element)
Definition: Layer.h:31
armarx::armem::contains
bool contains(const MemoryID &general, const MemoryID &specific)
Indicates whether general is "less specific" than, or equal to, specific, i.e.
Definition: MemoryID.cpp:563
armarx::armem::server::plugins::ReadWritePluginUser::workingMemory
server::wm::Memory & workingMemory()
Definition: ReadWritePluginUser.cpp:106
armarx::armem::MemoryID::coreSegmentName
std::string coreSegmentName
Definition: MemoryID.h:51
armarx::RunningTask
Definition: ArmarXMultipleObjectsScheduler.h:36
armarx::armem::server::grasp::GetSide
const auto GetSide
Definition: GraspMemory.cpp:818
Clock.h
armarx::armem::server::grasp::GraspMemory::onExitComponent
void onExitComponent() override
Hook for subclass.
Definition: GraspMemory.cpp:128
visionx::voxelgrid::Label
uint32_t Label
Type of an object label.
Definition: types.h:6
armarx::armem::server::grasp::GraspMemory::executeActions
armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq &inputs) override
Definition: GraspMemory.cpp:264
armarx::RemoteGui::Client::GridLayout
Definition: Widgets.h:186
armarx::RemoteGui::Client::Pos
Definition: Widgets.h:174
armarx::armem::MemoryID
A memory ID.
Definition: MemoryID.h:47
armarx.h
if
if(!yyvaluep)
Definition: Grammar.cpp:645
armarx::armem::toDateTimeMilliSeconds
std::string toDateTimeMilliSeconds(const Time &time, int decimals=6)
Returns timeas e.g.
Definition: Time.cpp:35
Color.h
MemoryRemoteGui.h
armarx::armem::base::detail::GetFindProviderSegmentMixin::getProviderSegment
auto & getProviderSegment(const MemoryID &providerSegmentID)
Retrieve a provider segment.
Definition: lookup_mixins.h:494
armarx::viz::Color::yellow
static Color yellow(int y=255, int a=255)
Red + Green.
Definition: Color.h:116
armarx::armem::server::grasp::GraspMemory::GraspMemory
GraspMemory()
Definition: GraspMemory.cpp:70
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:12
error.h
armarx::viz::Color
Definition: Color.h:12
armarx::armem::MemoryID::hasTimestamp
bool hasTimestamp() const
Definition: MemoryID.h:127
ice_conversions.h
armarx::armem::base::MemoryBase::addCoreSegment
CoreSegmentT & addCoreSegment(const std::string &name, aron::type::ObjectPtr coreSegmentType=nullptr, const std::vector< PredictionEngine > &predictionEngines={})
Add an empty core segment with the given name, type and prediction engines.
Definition: MemoryBase.h:259
armarx::armem::MemoryID::hasInstanceIndex
bool hasInstanceIndex() const
Definition: MemoryID.h:139
armarx::viz::Robot
Definition: Robot.h:10
Metronome.h
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::armem::MemoryID::hasGap
bool hasGap() const
Indicate whether this ID has a gap such as in 'Memory//MyProvider' (no core segment name).
Definition: MemoryID.cpp:163
armarx::viz::Color::red
static Color red(int r=255, int a=255)
Definition: Color.h:88
armarx::armem::base::detail::MemoryItem::id
MemoryID & id()
Definition: MemoryItem.h:25
armarx::armem::MemoryID::entityName
std::string entityName
Definition: MemoryID.h:53
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
forward_declarations.h
armarx::armem::grasping::segment::KnownGraspProviderSegment::init
void init() override
Definition: KnownGraspProviderSegment.cpp:48
armarx::armem::MemoryID::hasEntityName
bool hasEntityName() const
Definition: MemoryID.h:121
armarx::armem::server::grasp::GraspMemory::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: GraspMemory.cpp:76
armarx::armem::MemoryID::fromString
static MemoryID fromString(const std::string &string)
Alias for constructor from string.
Definition: MemoryID.cpp:188
armarx::LightweightRemoteGuiComponentPluginUser::RemoteGui_startRunningTask
void RemoteGui_startRunningTask()
Definition: LightweightRemoteGuiComponentPlugin.cpp:119
armarx::armem::server::plugins::ReadWritePluginUser::commit
virtual data::CommitResult commit(const data::Commit &commit, const Ice::Current &=Ice::emptyCurrent) override
Definition: ReadWritePluginUser.cpp:48
ExpressionException.h
armarx::viz::Color::pink
static Color pink(int p=255, int a=255)
2 Red + 1 Blue
Definition: Color.h:139
armarx::armem::MemoryID::clearTimestamp
void clearTimestamp()
Definition: MemoryID.h:133
armarx::control::common::MPType::hand
@ hand
armarx::armem::server::grasp::GraspMemory::createPropertyDefinitions
PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: GraspMemory.cpp:36
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:79
armarx::armem::fromAron
void fromAron(const arondto::MemoryID &dto, MemoryID &bo)
Definition: aron_conversions.cpp:8
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:69
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::ArVizComponentPluginUser::arviz
armarx::viz::Client arviz
Definition: ArVizComponentPlugin.h:42
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::core::time::Metronome
Simple rate limiter for use in loops to maintain a certain frequency given a clock.
Definition: Metronome.h:34
armarx::armem::MemoryID::hasProviderSegmentName
bool hasProviderSegmentName() const
Definition: MemoryID.h:115
armarx::armem::index::memoryID
const MemoryID memoryID
Definition: memory_ids.cpp:28
ARMARX_CHECK_NULL
#define ARMARX_CHECK_NULL(ptr)
Definition: ExpressionException.h:212
armarx::RemoteGui::Client::CheckBox
Definition: Widgets.h:129
aron_conversions.h
armarx::core::time::Clock::Now
static DateTime Now()
Current time on the virtual clock.
Definition: Clock.cpp:93
ice_conversions_templates.h
armarx::core::time::Frequency::Hertz
static Frequency Hertz(std::int64_t hertz)
Definition: Frequency.cpp:20
armarx::armem::server::grasp::GraspMemory::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: GraspMemory.cpp:106
Logging.h
armarx::viz::Color::green
static Color green(int g=255, int a=255)
Definition: Color.h:94
armarx::armem::server::grasp::GraspMemory::getDefaultName
std::string getDefaultName() const override
Definition: GraspMemory.cpp:65
armarx::viz::Client::layer
Layer layer(std::string const &name) const
Definition: Client.cpp:80
armarx::RemoteGui::Client
Definition: EigenWidgets.cpp:8
armarx::PackagePath
Definition: PackagePath.h:52
armarx::viz::Layer
Definition: Layer.h:12
armarx::armem::base::detail::GetFindProviderSegmentMixin::hasProviderSegment
bool hasProviderSegment(const MemoryID &providerSegmentID) const
Definition: lookup_mixins.h:463
armarx::armem::base::detail::GetFindEntityMixin::hasEntity
bool hasEntity(const MemoryID &entityID) const
Definition: lookup_mixins.h:414
PackagePath.h
armarx::human::MemoryID
const armem::MemoryID MemoryID
Definition: memory_ids.cpp:28