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