GraspMemory.cpp
Go to the documentation of this file.
1 
2 #include "GraspMemory.h"
3 
4 #include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
5 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
6 
7 #include <SimoxUtility/algorithm/string.h>
8 
9 #include <RobotAPI/libraries/armem/core/error.h>
10 #include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
11 #include <RobotAPI/libraries/armem/core/ice_conversions.h>
12 
13 #include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
14 #include <RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h>
15 #include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
16 #include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
17 
18 
19 namespace armarx::armem::server::grasp
20 {
21  armarx::PropertyDefinitionsPtr GraspMemory::createPropertyDefinitions()
22  {
23  setMemoryName("Grasp");
24 
25  armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
26 
27  defs->topic(debugObserver);
28 
29  defs->optional(enableRemoteGui, "remoteGui.enable", "Enable/Disable Remote GUI");
30  defs->optional(gui.trackNewEntities, "EnableTrackingOfNewEntities", "Enable/Disable the automatic visual tracking of newly commited Entities");
31  return defs;
32  }
33 
34 
35  std::string GraspMemory::getDefaultName() const
36  {
37  return "GraspMemory";
38  }
39 
40  GraspMemory::GraspMemory() :
41  knownGraspProviderSegment(iceAdapter())
42  {
43  }
44 
45  void GraspMemory::onInitComponent()
46  {
47  workingMemory().addCoreSegment("GraspCandidate",
48  armarx::grasping::arondto::GraspCandidate::ToAronType());
49  workingMemory().addCoreSegment("BimanualGraspCandidate",
50  armarx::grasping::arondto::BimanualGraspCandidate::ToAronType());
51  workingMemory().addCoreSegment("KnownGraspCandidate",
52  armarx::armem::grasping::arondto::KnownGraspInfo::ToAronType());
53 
54  knownGraspProviderSegment.init();
55  }
56 
57  void GraspMemory::onConnectComponent()
58  {
59  if (enableRemoteGui)
60  {
61  createRemoteGuiTab();
62  RemoteGui_startRunningTask();
63  }
64  }
65 
66  void GraspMemory::onDisconnectComponent()
67  {
68  }
69 
70  void GraspMemory::onExitComponent()
71  {
72  }
73 
74 
75 
76  // WRITING
77 
78  armem::data::CommitResult GraspMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
79  {
80  std::vector<armem::MemoryID> trackedEntityIds;
81  {
82  std::unique_lock lock(gui.visualizationMutex);
83  trackedEntityIds = gui.trackedEntityIds;
84  }
85 
86  if(! trackedEntityIds.empty())
87  {
88  for (auto &update : commit.updates)
89  {
90  armem::MemoryID entityID = armarx::fromIce<armem::MemoryID>(update.entityID);
91  entityID.clearTimestamp();
92  if(std::find(trackedEntityIds.begin(), trackedEntityIds.end(), entityID) != trackedEntityIds.end())
93  {
94  workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance([this](const auto & instance)
95  {
96  removeInstanceFromVisu(instance.id());
97 
98  });
99  }
100 
101  }
102  }
103 
104  if(gui.trackNewEntities)
105  {
106  for (auto &update : commit.updates)
107  {
108  armem::MemoryID entityID = armarx::fromIce<armem::MemoryID>(update.entityID);
109  entityID.clearTimestamp();
110  if(!workingMemory().findEntity(entityID))
111  {
112  std::unique_lock lock(gui.visualizationMutex);
113  gui.trackedEntityIds.emplace_back(entityID);
114  trackedEntityIds.emplace_back(entityID);
115  }
116  }
117  }
118 
119  // This function is overloaded to trigger the remote gui rebuild.
120  armem::data::CommitResult result = ReadWritePluginUser::commit(commit);
121  gui.tab.rebuild = true;
122 
123 
124  if (! trackedEntityIds.empty())
125  {
126  for (auto &update : commit.updates)
127  {
128  armem::MemoryID entityID = armarx::fromIce<armem::MemoryID>(update.entityID);
129  entityID.clearTimestamp();
130  if(std::find(trackedEntityIds.begin(), trackedEntityIds.end(), entityID) != trackedEntityIds.end())
131  {
132  workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance([this](const auto & instance)
133  {
134  addInstanceToVisu(instance.id());
135 
136  });
137  }
138 
139  }
140  }
141  return result;
142  }
143 
144  // READING
145 
146  // Inherited from Plugin
147 
148  armem::actions::data::GetActionsOutputSeq
149  GraspMemory::getActions(
150  const armem::actions::data::GetActionsInputSeq &inputs)
151  {
152  using namespace armem::actions;
153 
154  GetActionsOutputSeq outputs;
155  for (const auto& input : inputs)
156  {
157  auto memoryID = armarx::fromIce<armem::MemoryID>(input.id);
158  if (armem::contains(armem::MemoryID("Grasp"), memoryID) and not memoryID.hasGap())
159  {
160  if(armem::contains(armem::MemoryID("Grasp", "BimanualGraspCandidate"), memoryID)
161  || armem::contains(armem::MemoryID("Grasp", "KnownGraspCandidate"), memoryID))
162  {
163  continue; // Not supported, ignore.
164  }
165 
166  std::vector<MenuEntry> actions;
167 
168  if ( !memoryID.hasTimestamp() && !memoryID.hasInstanceIndex())
169  {
170  if(memoryID.hasEntityName())
171  {
172  if(std::find(gui.trackedEntityIds.begin(), gui.trackedEntityIds.end(), memoryID) == gui.trackedEntityIds.end())
173  {
174  actions.push_back(Action{"track","Track this entity in arviz"});
175  }
176  else
177  {
178  actions.push_back(Action{"untrack","Stop tracking this entity in arviz"});
179  }
180 
181  }
182  else
183  {
184  actions.push_back(Action{"track","Track all underlying entities in arviz"});
185  actions.push_back(Action{"untrack","Stop tracking all underlying entities in arviz"});
186  }
187  }
188 
189  actions.push_back(Action{"vis", "Visualize all contained grasp candidates"});
190  actions.push_back(Action{"rem", "Remove all contained grasp candidates from visualization"});
191  actions.push_back(SubMenu{"high", "Highlight all contain grasp candidates", {
192  Action{"pink", "in pink"},
193  Action{"red", "in red"},
194  Action{"blue", "in blue"},
195  Action{"yellow", "in yellow"},
196  Action{"purple", "in purple"}
197  }});
198  actions.push_back(Action{"reset", "Reset highlight layer"});
199 
200  Menu menu{actions};
201  outputs.push_back({ menu.toIce() });
202 
203  }
204  }
205 
206  return outputs;
207  }
208 
209  armem::actions::data::ExecuteActionOutputSeq
210  GraspMemory::executeActions(
211  const armem::actions::data::ExecuteActionInputSeq &inputs)
212  {
213  using namespace armem::actions;
214  ExecuteActionOutputSeq outputs;
215 
216  for (const auto& input : inputs)
217  {
218  auto memoryID = armarx::fromIce<armem::MemoryID>(input.id);
219  if (input.actionPath == ActionPath{"vis"} || input.actionPath == ActionPath{"rem"})
220  {
221  if (armem::contains(armem::MemoryID("Grasp"), memoryID) and not memoryID.hasGap())
222  {
223  {
224  if(armem::contains(armem::MemoryID("Grasp", "BimanualGraspCandidate"), memoryID)
225  || armem::contains(armem::MemoryID("Grasp", "KnownGraspCandidate"), memoryID))
226  {
227  std::stringstream sstream;
228  sstream << "Currently visualization for CoreSegment " << memoryID.coreSegmentName << " is not yet supported";
229  outputs.emplace_back(false, sstream.str());
230  }
231  else
232  {
233  if (memoryID.hasInstanceIndex())
234  {
235  if (input.actionPath == ActionPath{"vis"})
236  {
237  addInstanceToVisu(memoryID);
238  }
239  else if (input.actionPath == ActionPath{"rem"})
240  {
241  removeInstanceFromVisu(memoryID);
242  }
243 
244  }
245  else if (memoryID.hasTimestamp())
246  {
247  workingMemory().getSnapshot(memoryID).forEachInstance([this, &input](const auto & instance)
248  {
249  if (input.actionPath == ActionPath{"vis"})
250  {
251  addInstanceToVisu(instance.id());
252  }
253  else if (input.actionPath == ActionPath{"rem"})
254  {
255  removeInstanceFromVisu(instance.id());
256  }
257  });
258  }
259  else if (memoryID.hasEntityName())
260  {
261  workingMemory().getEntity(memoryID).forEachInstance([this, &input](const auto & instance)
262  {
263  if (input.actionPath == ActionPath{"vis"})
264  {
265  addInstanceToVisu(instance.id());
266  }
267  else if (input.actionPath == ActionPath{"rem"})
268  {
269  removeInstanceFromVisu(instance.id());
270  }
271  });
272  }
273  else if (memoryID.hasProviderSegmentName())
274  {
275  workingMemory().getProviderSegment(memoryID).forEachInstance([this, &input](const auto & instance)
276  {
277  if (input.actionPath == ActionPath{"vis"})
278  {
279  addInstanceToVisu(instance.id());
280  }
281  else if (input.actionPath == ActionPath{"rem"})
282  {
283  removeInstanceFromVisu(instance.id());
284  }
285  });
286  }
287  else
288  {
289  if (input.actionPath == ActionPath{"rem"})
290  {
291  std::unique_lock lock(gui.visualizationMutex);
292  gui.visibleInstanceIds.clear();
293  // mark all layers for deletion
294  for (std::string & layer : gui.activeLayers)
295  {
296  arviz.commitDeleteLayer(layer + "_reachable");
297  arviz.commitDeleteLayer(layer + "_unreachable");
298  }
299  gui.activeLayers.clear();
300  }
301  else if (input.actionPath == ActionPath{"vis"})
302  {
303  //currently only visualization for CoreSegment GraspCandidate available
304  workingMemory().getCoreSegment("GraspCandidate").forEachInstance([this](const auto & instance)
305  {
306  addInstanceToVisu(instance.id());
307  });
308  }
309  }
310 
311  visualizeGraspCandidates();
312  outputs.emplace_back(true, "");
313  }
314  }
315  }
316  else
317  {
318  std::stringstream sstream;
319  sstream << "MemoryID " << memoryID
320  << " does not refer to a valid Grasp Memory Part.";
321  outputs.emplace_back(false, sstream.str());
322  }
323  }
324  else if(input.actionPath == ActionPath{"track"} || input.actionPath == ActionPath{"untrack"} )
325  {
326  std::vector<armem::MemoryID> entityIDs;
327  if(memoryID.hasEntityName())
328  {
329  entityIDs.emplace_back(memoryID);
330  }
331  else if(memoryID.hasProviderSegmentName()){
332  workingMemory().getProviderSegment(memoryID).forEachEntity([&entityIDs](const auto & entity)
333  {
334  entityIDs.emplace_back(entity.id());
335  });
336  }
337  else
338  {
339  //currently only visualization for CoreSegment GraspCandidate available
340  workingMemory().getCoreSegment("GraspCandidate").forEachEntity([&entityIDs](const auto & entity)
341  {
342  entityIDs.emplace_back(entity.id());
343  });
344  }
345  for(auto & entityID : entityIDs)
346  {
347  if(input.actionPath == ActionPath{"track"})
348  {
349  //visualize latest snapshot of entity and refresh if necessary
350  workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance([this](const auto & instance)
351  {
352  addInstanceToVisu(instance.id());
353  });
354  gui.trackedEntityIds.push_back(entityID);
355  ARMARX_INFO << "starting to track " << entityID;
356  outputs.emplace_back(true, "");
357  }
358  else if(input.actionPath == ActionPath{"untrack"})
359  {
360  auto pos = std::find(gui.trackedEntityIds.begin(), gui.trackedEntityIds.end(), entityID);
361  if(pos != gui.trackedEntityIds.end())
362  {
363  gui.trackedEntityIds.erase(pos);
364  ARMARX_INFO << "Stop tracking of " << entityID;
365  }
366  outputs.emplace_back(true, "");
367  }
368  }
369  }
370  else if(input.actionPath.front() == "high")
371  {
372  addToHighlightLayer(memoryID, input.actionPath.back());
373  }
374  else if(input.actionPath == ActionPath{"reset"})
375  {
376  arviz.commit(arviz.layer("HighlightedGrasps"));
377  }
378  else
379  {
380  std::stringstream sstream;
381  sstream << "Action path " << input.actionPath << " is not defined.";
382  outputs.emplace_back(false, sstream.str());
383  }
384  }
385  return outputs;
386  }
387 
388 
389 
390  // REMOTE GUI
391 
392  void GraspMemory::createRemoteGuiTab()
393  {
394  using namespace armarx::RemoteGui::Client;
395  GridLayout root;
396  {
397  gui.tab.selectAll.setLabel("Select All");
398  gui.tab.deselectAll.setLabel("Deselect All");
399  gui.tab.showUnlimitedInstances.setValue(gui.unlimitedInstances);
400  gui.tab.maxInstances.setRange(0,1000);
401  gui.tab.maxInstances.setValue(gui.maxInstances);
402  int row = 0;
403 
404  // bimanual candidates are not available for visualization for now
405 // {
406 // root.add(Label("CoreSegment"), Pos{row, 0});
407 // row++;
408 // std::vector<std::string> options = workingMemory().getCoreSegmentNames();
409 // if (options.empty())
410 // {
411 // options.push_back("<None>");
412 // }
413 // gui.tab.selectCoreSegment.setOptions(options);
414 // if (gui.coreSegIndex >= 0 && gui.coreSegIndex < static_cast<int>(options.size()))
415 // {
416 // gui.tab.selectCoreSegment.setIndex(gui.coreSegIndex);
417 // gui.coreSegment = options[gui.coreSegIndex];
418 // }
419 // root.add(gui.tab.selectCoreSegment, Pos{row,0});
420 // row++;
421 // }
422  if(workingMemory().hasCoreSegment(gui.coreSegment))
423  {
424  {
425  root.add(Label("Provider"), Pos{row, 0});
426  row++;
427  std::vector<std::string> options = workingMemory().getCoreSegment(gui.coreSegment).getProviderSegmentNames();
428  if (options.empty())
429  {
430  options.push_back("<None>");
431  }
432  else
433  {
434  options.insert(options.begin(), "<All>");
435  }
436 
437  gui.tab.selectProvider.setOptions(options);
438 
439  if (gui.providerIndex >= 0 && gui.providerIndex < static_cast<int>(options.size()))
440  {
441  gui.tab.selectProvider.setIndex(gui.providerIndex);
442  gui.provider = options[gui.providerIndex];
443  }
444  root.add(gui.tab.selectProvider, Pos{row, 0});
445  row++;
446  }
447 
448  if(gui.provider == "<All>"
449  || workingMemory().getCoreSegment(gui.coreSegment).hasProviderSegment(gui.provider))
450  {
451  std::vector<MemoryID> providers;
452  {
453  root.add(Label("Entity"), Pos{row,0});
454  row++;
455  std::vector<std::string> options;
456 
457  if (gui.provider == "<All>")
458  {
459  workingMemory().getCoreSegment(gui.coreSegment).forEachProviderSegment([&providers](const auto & provider)
460  {
461  providers.push_back(provider.id());
462  });
463  }
464  else
465  {
466  providers.push_back(MemoryID(workingMemory().name(), gui.coreSegment, gui.provider));
467 
468  }
469  for (MemoryID & provider : providers)
470  {
471  for(std::string & entity : workingMemory().getProviderSegment(provider).getEntityNames())
472  {
473  options.push_back(entity);
474  }
475  }
476  if (options.empty())
477  {
478  options.push_back("<None>");
479  }
480  else
481  {
482  options.insert(options.begin(), "<All>");
483  }
484  gui.tab.selectEntity.setOptions(options);
485  if (gui.entityIndex >= 0 && gui.entityIndex < static_cast<int>(options.size()))
486  {
487  gui.tab.selectEntity.setIndex(gui.entityIndex);
488  gui.entity = options[gui.entityIndex];
489  }
490  root.add(gui.tab.selectEntity, Pos{row, 0});
491  row++;
492  }
493 
494  if( gui.entity == "<All>"
495  || (gui.provider == "<All>" && workingMemory().getCoreSegment(gui.coreSegment).hasEntity(gui.entity))
496  || workingMemory().getCoreSegment(gui.coreSegment).getProviderSegment(gui.provider).hasEntity(gui.entity))
497  {
498  std::vector<MemoryID> entities;
499  {
500  root.add(Label("Snapshot"), Pos{row, 0});
501  row++;
502  std::vector<std::string> options;
503  for (MemoryID & provider : providers)
504  {
505  if (gui.entity == "<All>")
506  {
507  workingMemory().getProviderSegment(provider).forEachEntity([&entities](auto & entity)
508  {
509  entities.push_back(entity.id());
510  });
511  }
512  else
513  {
514  if (workingMemory().getProviderSegment(provider).hasEntity(gui.entity))
515  {
516  entities.push_back(MemoryID(workingMemory().name(), gui.coreSegment, provider.providerSegmentName, gui.entity));
517  }
518  }
519  }
520 
521  for (MemoryID & entity : entities)
522  {
523  workingMemory().getEntity(entity).forEachSnapshot([this, &options](const auto & snapshot)
524  {
525  std::string time = armem::toDateTimeMilliSeconds(snapshot.time());
526  gui.timeMap[time] = snapshot.time();
527  options.push_back(time);
528  });
529  }
530 
531  if (options.empty())
532  {
533  options.push_back("<None>");
534  }
535  else
536  {
537  options.insert(options.begin(), "<All>");
538  }
539  gui.tab.selectSnapshot.setOptions(options);
540  if (gui.snapshotIndex >= 0 && gui.snapshotIndex < static_cast<int>(options.size()))
541  {
542  gui.tab.selectSnapshot.setIndex(gui.snapshotIndex);
543  if(options[gui.snapshotIndex] != "<None>")
544  {
545  gui.snapshot = options[gui.snapshotIndex];
546  }
547  }
548  root.add(gui.tab.selectSnapshot, Pos{row, 0});
549  row++;
550 
551 
552  }
553  bool comboExists;
554  if (gui.snapshot == "<All>")
555  {
556  comboExists = true;
557  }
558  else
559  {
560  for (MemoryID & entity : entities)
561  {
562  if (workingMemory().getEntity(entity).hasSnapshot(gui.timeMap.at(gui.snapshot)))
563  {
564  comboExists = true;
565  }
566  }
567  }
568  if(comboExists)
569  {
570  root.add(Label("Instances"), Pos{row,0});
571  row++;
572  root.add(Label("Show all Instances"), Pos{row, 0});
573  root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
574  row++;
575  root.add(Label("Limit for shown Instances"), Pos{row, 0});
576  root.add(gui.tab.maxInstances, Pos{row, 1});
577  row++;
578  root.add(gui.tab.selectAll, Pos{row, 0});
579  row++;
580  root.add(gui.tab.deselectAll, Pos{row, 0});
581  row++;
582  gui.tab.checkInstances.clear();
583  std::vector<MemoryID> snapshots;
584  for (MemoryID & entity : entities)
585  {
586  if(gui.snapshot == "<All>")
587  {
588  workingMemory().getEntity(entity).forEachSnapshot([&snapshots](auto & snapshot)
589  {
590  snapshots.push_back(snapshot.id());
591  });
592  }
593  else
594  {
595  if (workingMemory().getEntity(entity).hasSnapshot(gui.timeMap.at(gui.snapshot)))
596  {
597  snapshots.push_back(workingMemory().getEntity(entity).getSnapshot(gui.timeMap.at(gui.snapshot)).id());
598  }
599  }
600  }
601  int instances = 0;
602  for (MemoryID & snapshot : snapshots)
603  {
604  workingMemory().getSnapshot(snapshot).forEachInstance([this, &row, &root, &instances](const auto & instance)
605  {
606  if (gui.unlimitedInstances || instances < gui.maxInstances)
607  {
608  std::unique_lock lock(gui.visualizationMutex);
609  root.add(Label(instance.id().str()) , Pos{row, 0});
610  gui.tab.checkInstances[instance.id().str()] = CheckBox();
611  if(std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance.id()) != gui.visibleInstanceIds.end())
612  {
613  gui.tab.checkInstances.at(instance.id().str()).setValue(true);
614  }
615  root.add(gui.tab.checkInstances[instance.id().str()], Pos{row, 1});
616  row++;
617  instances++;
618  }
619  });
620  }
621  }
622  else
623  {
624  root.add(Label("Instances"), Pos{row,0});
625  row++;
626  root.add(Label("Show all Instances"), Pos{row, 0});
627  root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
628  row++;
629  root.add(Label("Limit for shown Instances"), Pos{row, 0});
630  root.add(gui.tab.maxInstances, Pos{row, 1});
631  row++;
632  root.add(gui.tab.selectAll, Pos{row, 0});
633  row++;
634  root.add(gui.tab.deselectAll, Pos{row, 0});
635  row++;
636  gui.tab.checkInstances.clear();
637  }
638  }
639  else
640  {
641  gui.tab.checkInstances.clear();
642 
643  std::vector<std::string> options = {"<None>"};
644 
645  gui.tab.selectSnapshot.setOptions(options);
646 
647  root.add(Label("Snapshot"), Pos{row, 0});
648  row++;
649  root.add(gui.tab.selectSnapshot, Pos{row, 0});
650  row++;
651  root.add(Label("Show all Instances"), Pos{row, 0});
652  root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
653  row++;
654  root.add(Label("Limit for shown Instances"), Pos{row, 0});
655  root.add(gui.tab.maxInstances, Pos{row, 1});
656  row++;
657  root.add(gui.tab.selectAll, Pos{row, 0});
658  row++;
659  root.add(gui.tab.deselectAll, Pos{row, 0});
660  row++;
661  }
662  }
663  else
664  {
665  gui.tab.checkInstances.clear();
666 
667  std::vector<std::string> options = {"<None>"};
668 
669  gui.tab.selectEntity.setOptions(options);
670 
671  root.add(Label("Entity"), Pos{row,0});
672  row++;
673  root.add(gui.tab.selectEntity, Pos{row, 0});
674  row++;
675 
676  gui.tab.selectSnapshot.setOptions(options);
677 
678  root.add(Label("Snapshot"), Pos{row, 0});
679  row++;
680  root.add(gui.tab.selectSnapshot, Pos{row, 0});
681  row++;
682  root.add(Label("Show all Instances"), Pos{row, 0});
683  root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
684  row++;
685  root.add(Label("Limit for shown Instances"), Pos{row, 0});
686  root.add(gui.tab.maxInstances, Pos{row, 1});
687  row++;
688  root.add(gui.tab.selectAll, Pos{row, 0});
689  row++;
690  root.add(gui.tab.deselectAll, Pos{row, 0});
691  row++;
692  }
693  }
694  else {
695  gui.tab.checkInstances.clear();
696 
697  std::vector<std::string> options = {"<None>"};
698  gui.tab.selectProvider.setOptions(options);
699 
700  root.add(Label("Provider"), Pos{row, 0});
701  row++;
702  root.add(gui.tab.selectProvider, Pos{row, 0});
703  row++;
704 
705  gui.tab.selectEntity.setOptions(options);
706 
707  root.add(Label("Entity"), Pos{row,0});
708  row++;
709  root.add(gui.tab.selectEntity, Pos{row, 0});
710  row++;
711 
712  gui.tab.selectSnapshot.setOptions(options);
713 
714  root.add(Label("Snapshot"), Pos{row, 0});
715  row++;
716  root.add(gui.tab.selectSnapshot, Pos{row, 0});
717  row++;
718  root.add(Label("Show all Instances"), Pos{row, 0});
719  root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
720  row++;
721  root.add(Label("Limit for shown Instances"), Pos{row, 0});
722  root.add(gui.tab.maxInstances, Pos{row, 1});
723  row++;
724  root.add(gui.tab.selectAll, Pos{row, 0});
725  row++;
726  root.add(gui.tab.deselectAll, Pos{row, 0});
727  row++;
728 
729  }
730  }
731 
732 
733  RemoteGui_createTab(getName(), root, &gui.tab);
734  gui.tab.rebuild = false;
735 
736  }
737 
738 
739  void armarx::armem::server::grasp::GraspMemory::visualizeGraspCandidates()
740  {
741  std::unique_lock lock(gui.visualizationMutex);
742  armarx::grasping::GraspCandidateVisu visu;
743  std::map<std::string, viz::Layer> reachableLayers;
744  std::map<std::string, viz::Layer> unreachableLayers;
745 
746  {
747 
748  for (auto & element : gui.visibleInstanceIds)
749  {
750  std::string entityName = element.entityName;
751 
752  if (reachableLayers.find(entityName) == reachableLayers.end())
753  {
754  reachableLayers[entityName] = arviz.layer(entityName + "_reachable");
755  unreachableLayers[entityName] = arviz.layer(entityName + "_unreachable");
756  gui.activeLayers.push_back(entityName);
757  }
758 
759  armarx::grasping::GraspCandidate candidate;
760 
761  armarx::grasping::arondto::GraspCandidate aronTransform;
762 
763  aronTransform.fromAron(workingMemory().getInstance(element).data());
764 
765  fromAron(aronTransform, candidate);
766 
767  visu.visualize(element.str(), candidate, reachableLayers.at(entityName), unreachableLayers.at(entityName));
768 
769  }
770  }
771 
772  std::vector<viz::Layer> layers;
773  for(auto & [entityName, layer] : reachableLayers)
774  {
775  layers.push_back(layer);
776  layers.push_back(unreachableLayers.at(entityName));
777  }
778  arviz.commit(layers);
779  }
780 
781  void armarx::armem::server::grasp::GraspMemory::addInstanceToVisu(const armarx::armem::MemoryID &instance)
782  {
783  std::unique_lock lock(gui.visualizationMutex);
784  auto position = std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance);
785 
786  if(position == gui.visibleInstanceIds.end())
787  {
788  gui.visibleInstanceIds.push_back(instance);
789  }
790 
791  }
792 
793 
794  void armarx::armem::server::grasp::GraspMemory::removeInstanceFromVisu(const armarx::armem::MemoryID &instance)
795  {
796  std::unique_lock lock(gui.visualizationMutex);
797  auto position = std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance);
798 
799  if (position != gui.visibleInstanceIds.end())
800  {
801  gui.visibleInstanceIds.erase(position);
802  }
803  //check if this was the last instance of this entity and if so, mark layer for deletion
804 
805  std::string entityName = instance.entityName;
806 
807  if (std::none_of(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), [&entityName](const armem::MemoryID& id)
808  { return id.entityName == entityName; }))
809  {
810  arviz.commitDeleteLayer(entityName + "_reachable");
811  arviz.commitDeleteLayer(entityName + "_unreachable");
812  }
813 
814  auto layerPos = std::find(gui.activeLayers.begin(), gui.activeLayers.end(), entityName);
815 
816  if (layerPos != gui.activeLayers.end())
817  {
818  gui.activeLayers.erase(layerPos);
819  }
820  }
821 
822  void GraspMemory::addToHighlightLayer(const MemoryID &memoryID, const std::string color)
823  {
824  viz::Color handColor;
825 
826  if (color == "pink")
827  {
828  handColor = viz::Color::pink();
829  }
830  else if (color == "red")
831  {
832  handColor = viz::Color::red();
833  }
834  else if (color == "blue")
835  {
836  handColor = viz::Color::blue();
837  }
838  else if (color == "yellow")
839  {
840  handColor = viz::Color::yellow();
841  }
842  else if (color == "purple")
843  {
844  handColor = viz::Color::purple();
845  }
846 
847  viz::Layer highlightLayer = arviz.layer(("HighlightedGrasps"));
848  armarx::grasping::GraspCandidateVisu visu;
849 
850  std::vector<armem::MemoryID> instances;
851 
852  if (memoryID.hasInstanceIndex())
853  {
854  instances.push_back(memoryID);
855  }
856  else if (memoryID.hasTimestamp())
857  {
858  workingMemory().getSnapshot(memoryID).forEachInstance([&instances](const auto & instance)
859  {
860  instances.push_back(instance.id());
861  });
862  }
863  else if (memoryID.hasEntityName())
864  {
865  workingMemory().getEntity(memoryID).forEachInstance([&instances](const auto & instance)
866  {
867  instances.push_back(instance.id());
868  });
869  }
870  else if (memoryID.hasProviderSegmentName())
871  {
872  workingMemory().getProviderSegment(memoryID).forEachInstance([&instances](const auto & instance)
873  {
874  instances.push_back(instance.id());
875  });
876  }
877  else
878  {
879  //currently only visualization for CoreSegment GraspCandidate available
880  workingMemory().getCoreSegment("GraspCandidate").forEachInstance([&instances](const auto & instance)
881  {
882  instances.push_back(instance.id());
883  });
884  }
885 
886  armarx::grasping::GraspCandidate candidate;
887 
888  armarx::grasping::arondto::GraspCandidate aronTransform;
889 
890  for (armem::MemoryID &instance : instances)
891  {
892  aronTransform.fromAron(workingMemory().getInstance(instance).data());
893 
894  fromAron(aronTransform, candidate);
895 
896 
897  viz::Robot hand = visu.visualize(instance.str(), candidate);
898  hand.color(handColor);
899  highlightLayer.add(hand);
900  }
901 
902  arviz.commit(highlightLayer);
903 
904  }
905 
906  void GraspMemory::RemoteGui_update()
907  {
908 // if (gui.tab.selectCoreSegment.hasValueChanged())
909 // {
910 // gui.coreSegment = gui.tab.selectCoreSegment.getValue();
911 // gui.coreSegIndex = gui.tab.selectCoreSegment.getIndex();
912 // gui.providerIndex = 0;
913 // gui.entityIndex = 0;
914 // gui.snapshotIndex = 0;
915 // gui.tab.rebuild = true;
916 // }
917 
918  if (gui.tab.selectProvider.hasValueChanged())
919  {
920  gui.provider = gui.tab.selectProvider.getValue();
921  gui.providerIndex = gui.tab.selectProvider.getIndex();
922  gui.entityIndex = 0;
923  gui.snapshotIndex = 0;
924  gui.tab.rebuild = true;
925  }
926 
927  if (gui.tab.selectEntity.hasValueChanged())
928  {
929  gui.entity = gui.tab.selectEntity.getValue();
930  gui.entityIndex = gui.tab.selectEntity.getIndex();
931  gui.snapshotIndex = 0;
932  gui.tab.rebuild = true;
933  }
934 
935  if (gui.tab.selectSnapshot.hasValueChanged())
936  {
937  gui.snapshot = gui.tab.selectSnapshot.getValue();
938  gui.snapshotIndex = gui.tab.selectSnapshot.getIndex();
939  gui.tab.rebuild = true;
940  }
941 
942  if(gui.tab.showUnlimitedInstances.hasValueChanged())
943  {
944  gui.unlimitedInstances = gui.tab.showUnlimitedInstances.getValue();
945  gui.tab.rebuild = true;
946  }
947 
948  if(gui.tab.maxInstances.hasValueChanged())
949  {
950  gui.maxInstances = gui.tab.maxInstances.getValue();
951  gui.tab.rebuild = true;
952  }
953 
954  if(gui.tab.selectAll.wasClicked())
955  {
956  for(auto & element : gui.tab.checkInstances)
957  {
958  element.second.setValue(true);
959  }
960  }
961 
962  if(gui.tab.deselectAll.wasClicked())
963  {
964  for(auto & element : gui.tab.checkInstances)
965  {
966  element.second.setValue(false);
967  }
968  }
969 
970  for(auto & element : gui.tab.checkInstances)
971  {
972  if(element.second.hasValueChanged())
973  {
974  if (element.second.getValue())
975  {
976  addInstanceToVisu(armem::MemoryID::fromString(element.first));
977  }
978  else
979  {
980  removeInstanceFromVisu(armem::MemoryID::fromString(element.first));
981  }
982 
983  }
984  }
985 
986  visualizeGraspCandidates();
987 
988  if (gui.tab.rebuild)
989  {
990  ARMARX_INFO << "Rebuilding remote gui tab";
991  createRemoteGuiTab();
992  }
993  }
994 
995 }
armarx::armem::server::grasp::GraspMemory::commit
armem::data::CommitResult commit(const armem::data::Commit &commit, const Ice::Current &) override
Definition: GraspMemory.cpp:78
armarx::viz::Client::commit
CommitResult commit(StagedCommit const &commit)
Definition: Client.cpp:80
GraspMemory.h
armarx::grasping::GraspCandidateVisu
Definition: GraspCandidateVisu.h:11
armarx::viz::Color::purple
static Color purple(int p=255, int a=255)
2 Blue + 1 Red
Definition: Color.h:149
armarx::armem::server::grasp::GraspMemory::getActions
armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq &inputs) override
Definition: GraspMemory.cpp:149
armarx::armem::base::detail::GetFindEntityMixin::getEntity
auto & getEntity(const MemoryID &entityID)
Retrieve an entity.
Definition: lookup_mixins.h:445
armarx::armem::base::MemoryBase::getCoreSegment
CoreSegmentT & getCoreSegment(const std::string &name)
Definition: MemoryBase.h:132
armarx::armem::server::grasp
Definition: GraspMemory.cpp:19
armarx::viz::Client::commitDeleteLayer
void commitDeleteLayer(std::string const &name)
Definition: Client.h:169
armarx::RemoteGui::Client::GridLayout::add
GridLayout & add(Widget const &child, Pos pos, Span span=Span{1, 1})
Definition: Widgets.cpp:412
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:89
armarx::armem::server::grasp::GraspMemory::createRemoteGuiTab
void createRemoteGuiTab()
Definition: GraspMemory.cpp:392
armarx::armem::server::plugins::ReadWritePluginUser::setMemoryName
void setMemoryName(const std::string &memoryName)
Definition: ReadWritePluginUser.cpp:25
armarx::grasping::GraspCandidateVisu::visualize
void visualize(const grasping::GraspCandidateDict &candidates, viz::Client &arviz)
Definition: GraspCandidateVisu.cpp:14
armarx::armem::server::grasp::GraspMemory::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: GraspMemory.cpp:57
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:29
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:558
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::armem::server::grasp::GraspMemory::onExitComponent
void onExitComponent() override
Hook for subclass.
Definition: GraspMemory.cpp:70
visionx::voxelgrid::Label
uint32_t Label
Type of an object label.
Definition: types.h:7
armarx::armem::server::grasp::GraspMemory::executeActions
armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq &inputs) override
Definition: GraspMemory.cpp:210
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
if
if(!yyvaluep)
Definition: Grammar.cpp:724
armarx::armem::toDateTimeMilliSeconds
std::string toDateTimeMilliSeconds(const Time &time, int decimals=6)
Returns timeas e.g.
Definition: Time.cpp:35
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:104
armarx::armem::server::grasp::GraspMemory::GraspMemory
GraspMemory()
Definition: GraspMemory.cpp:40
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:19
error.h
armarx::viz::Color
Definition: Color.h:13
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:9
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:158
armarx::viz::Color::red
static Color red(int r=255, int a=255)
Definition: Color.h:79
armarx::armem::base::detail::MemoryItem::id
MemoryID & id()
Definition: MemoryItem.h:27
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:67
armarx::armem::grasping::segment::KnownGraspProviderSegment::init
void init() override
Definition: KnownGraspProviderSegment.cpp:16
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:45
armarx::armem::MemoryID::fromString
static MemoryID fromString(const std::string &string)
Alias for constructor from string.
Definition: MemoryID.cpp:183
armarx::LightweightRemoteGuiComponentPluginUser::RemoteGui_startRunningTask
void RemoteGui_startRunningTask()
Definition: LightweightRemoteGuiComponentPlugin.cpp:110
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:125
armarx::armem::MemoryID::clearTimestamp
void clearTimestamp()
Definition: MemoryID.h:133
armarx::armem::server::grasp::GraspMemory::createPropertyDefinitions
PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: GraspMemory.cpp:21
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
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:70
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::ArVizComponentPluginUser::arviz
armarx::viz::Client arviz
Definition: ArVizComponentPlugin.h:43
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::armem::MemoryID::hasProviderSegmentName
bool hasProviderSegmentName() const
Definition: MemoryID.h:115
armarx::viz::ElementOps::color
DerivedT & color(Color color)
Definition: ElementOps.h:195
armarx::armem::index::memoryID
const MemoryID memoryID
Definition: memory_ids.cpp:29
armarx::RemoteGui::Client::CheckBox
Definition: Widgets.h:129
aron_conversions.h
ice_conversions_templates.h
armarx::armem::server::grasp::GraspMemory::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: GraspMemory.cpp:66
armarx::armem::server::grasp::GraspMemory::getDefaultName
std::string getDefaultName() const override
Definition: GraspMemory.cpp:35
armarx::viz::Client::layer
Layer layer(std::string const &name) const
Definition: Client.cpp:73
armarx::RemoteGui::Client
Definition: EigenWidgets.cpp:8
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
armarx::human::MemoryID
const armem::MemoryID MemoryID
Definition: memory_ids.cpp:29