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