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