7 #include <SimoxUtility/algorithm/string.h>
13 #include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
16 #include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
27 defs->topic(debugObserver);
29 defs->optional(enableRemoteGui,
"remoteGui.enable",
"Enable/Disable Remote GUI");
30 defs->optional(gui.trackNewEntities,
"EnableTrackingOfNewEntities",
"Enable/Disable the automatic visual tracking of newly commited Entities");
41 knownGraspProviderSegment(iceAdapter())
48 armarx::grasping::arondto::GraspCandidate::ToAronType());
50 armarx::grasping::arondto::BimanualGraspCandidate::ToAronType());
52 armarx::armem::grasping::arondto::KnownGraspInfo::ToAronType());
54 knownGraspProviderSegment.
init();
80 std::vector<armem::MemoryID> trackedEntityIds;
82 std::unique_lock lock(gui.visualizationMutex);
83 trackedEntityIds = gui.trackedEntityIds;
86 if(! trackedEntityIds.empty())
92 if(std::find(trackedEntityIds.begin(), trackedEntityIds.end(), entityID) != trackedEntityIds.end())
96 removeInstanceFromVisu(instance.id());
104 if(gui.trackNewEntities)
112 std::unique_lock lock(gui.visualizationMutex);
113 gui.trackedEntityIds.emplace_back(entityID);
114 trackedEntityIds.emplace_back(entityID);
121 gui.tab.rebuild =
true;
124 if (! trackedEntityIds.empty())
130 if(std::find(trackedEntityIds.begin(), trackedEntityIds.end(), entityID) != trackedEntityIds.end())
134 addInstanceToVisu(instance.id());
148 armem::actions::data::GetActionsOutputSeq
150 const armem::actions::data::GetActionsInputSeq &inputs)
152 using namespace armem::actions;
154 GetActionsOutputSeq outputs;
155 for (
const auto&
input : inputs)
166 std::vector<MenuEntry> actions;
172 if(std::find(gui.trackedEntityIds.begin(), gui.trackedEntityIds.end(),
memoryID) == gui.trackedEntityIds.end())
174 actions.push_back(Action{
"track",
"Track this entity in arviz"});
178 actions.push_back(Action{
"untrack",
"Stop tracking this entity in arviz"});
184 actions.push_back(Action{
"track",
"Track all underlying entities in arviz"});
185 actions.push_back(Action{
"untrack",
"Stop tracking all underlying entities in arviz"});
189 actions.push_back(Action{
"vis",
"Visualize all contained grasp candidates"});
190 actions.push_back(Action{
"rem",
"Remove all contained grasp candidates from visualization"});
191 actions.push_back(SubMenu{
"high",
"Highlight all contain grasp candidates", {
192 Action{
"pink",
"in pink"},
193 Action{
"red",
"in red"},
194 Action{
"blue",
"in blue"},
195 Action{
"yellow",
"in yellow"},
196 Action{
"purple",
"in purple"}
198 actions.push_back(Action{
"reset",
"Reset highlight layer"});
201 outputs.push_back({ menu.toIce() });
209 armem::actions::data::ExecuteActionOutputSeq
211 const armem::actions::data::ExecuteActionInputSeq &inputs)
213 using namespace armem::actions;
214 ExecuteActionOutputSeq outputs;
216 for (
const auto&
input : inputs)
219 if (
input.actionPath == ActionPath{
"vis"} ||
input.actionPath == ActionPath{
"rem"})
227 std::stringstream sstream;
229 outputs.emplace_back(
false, sstream.str());
235 if (
input.actionPath == ActionPath{
"vis"})
239 else if (
input.actionPath == ActionPath{
"rem"})
249 if (
input.actionPath == ActionPath{
"vis"})
251 addInstanceToVisu(instance.id());
253 else if (
input.actionPath == ActionPath{
"rem"})
255 removeInstanceFromVisu(instance.id());
263 if (
input.actionPath == ActionPath{
"vis"})
265 addInstanceToVisu(instance.id());
267 else if (
input.actionPath == ActionPath{
"rem"})
269 removeInstanceFromVisu(instance.id());
277 if (
input.actionPath == ActionPath{
"vis"})
279 addInstanceToVisu(instance.id());
281 else if (
input.actionPath == ActionPath{
"rem"})
283 removeInstanceFromVisu(instance.id());
289 if (
input.actionPath == ActionPath{
"rem"})
291 std::unique_lock lock(gui.visualizationMutex);
292 gui.visibleInstanceIds.clear();
294 for (std::string & layer : gui.activeLayers)
299 gui.activeLayers.clear();
301 else if (
input.actionPath == ActionPath{
"vis"})
306 addInstanceToVisu(instance.id());
311 visualizeGraspCandidates();
312 outputs.emplace_back(
true,
"");
318 std::stringstream sstream;
320 <<
" does not refer to a valid Grasp Memory Part.";
321 outputs.emplace_back(
false, sstream.str());
324 else if(
input.actionPath == ActionPath{
"track"} ||
input.actionPath == ActionPath{
"untrack"} )
326 std::vector<armem::MemoryID> entityIDs;
334 entityIDs.emplace_back(entity.id());
342 entityIDs.emplace_back(entity.id());
345 for(
auto & entityID : entityIDs)
347 if(
input.actionPath == ActionPath{
"track"})
352 addInstanceToVisu(instance.id());
354 gui.trackedEntityIds.push_back(entityID);
356 outputs.emplace_back(
true,
"");
358 else if(
input.actionPath == ActionPath{
"untrack"})
360 auto pos = std::find(gui.trackedEntityIds.begin(), gui.trackedEntityIds.end(), entityID);
361 if(pos != gui.trackedEntityIds.end())
363 gui.trackedEntityIds.erase(pos);
366 outputs.emplace_back(
true,
"");
370 else if(
input.actionPath.front() ==
"high")
374 else if(
input.actionPath == ActionPath{
"reset"})
380 std::stringstream sstream;
381 sstream <<
"Action path " <<
input.actionPath <<
" is not defined.";
382 outputs.emplace_back(
false, sstream.str());
397 gui.tab.selectAll.setLabel(
"Select All");
398 gui.tab.deselectAll.setLabel(
"Deselect All");
399 gui.tab.showUnlimitedInstances.setValue(gui.unlimitedInstances);
400 gui.tab.maxInstances.setRange(0,1000);
401 gui.tab.maxInstances.setValue(gui.maxInstances);
430 options.push_back(
"<None>");
434 options.insert(options.begin(),
"<All>");
437 gui.tab.selectProvider.setOptions(options);
439 if (gui.providerIndex >= 0 && gui.providerIndex <
static_cast<int>(options.size()))
441 gui.tab.selectProvider.setIndex(gui.providerIndex);
442 gui.provider = options[gui.providerIndex];
444 root.
add(gui.tab.selectProvider,
Pos{row, 0});
448 if(gui.provider ==
"<All>"
451 std::vector<MemoryID> providers;
455 std::vector<std::string> options;
457 if (gui.provider ==
"<All>")
461 providers.push_back(provider.id());
469 for (
MemoryID & provider : providers)
471 for(std::string & entity :
workingMemory().getProviderSegment(provider).getEntityNames())
473 options.push_back(entity);
478 options.push_back(
"<None>");
482 options.insert(options.begin(),
"<All>");
484 gui.tab.selectEntity.setOptions(options);
485 if (gui.entityIndex >= 0 && gui.entityIndex <
static_cast<int>(options.size()))
487 gui.tab.selectEntity.setIndex(gui.entityIndex);
488 gui.entity = options[gui.entityIndex];
490 root.
add(gui.tab.selectEntity,
Pos{row, 0});
494 if( gui.entity ==
"<All>"
498 std::vector<MemoryID> entities;
502 std::vector<std::string> options;
503 for (
MemoryID & provider : providers)
505 if (gui.entity ==
"<All>")
509 entities.push_back(entity.id());
514 if (
workingMemory().getProviderSegment(provider).hasEntity(gui.entity))
516 entities.push_back(
MemoryID(
workingMemory().name(), gui.coreSegment, provider.providerSegmentName, gui.entity));
526 gui.timeMap[time] = snapshot.time();
527 options.push_back(time);
533 options.push_back(
"<None>");
537 options.insert(options.begin(),
"<All>");
539 gui.tab.selectSnapshot.setOptions(options);
540 if (gui.snapshotIndex >= 0 && gui.snapshotIndex <
static_cast<int>(options.size()))
542 gui.tab.selectSnapshot.setIndex(gui.snapshotIndex);
543 if(options[gui.snapshotIndex] !=
"<None>")
545 gui.snapshot = options[gui.snapshotIndex];
548 root.
add(gui.tab.selectSnapshot,
Pos{row, 0});
554 if (gui.snapshot ==
"<All>")
572 root.
add(
Label(
"Show all Instances"),
Pos{row, 0});
573 root.
add(gui.tab.showUnlimitedInstances,
Pos{row, 1});
575 root.
add(
Label(
"Limit for shown Instances"),
Pos{row, 0});
576 root.
add(gui.tab.maxInstances,
Pos{row, 1});
578 root.
add(gui.tab.selectAll,
Pos{row, 0});
580 root.
add(gui.tab.deselectAll,
Pos{row, 0});
582 gui.tab.checkInstances.clear();
583 std::vector<MemoryID> snapshots;
586 if(gui.snapshot ==
"<All>")
590 snapshots.push_back(snapshot.id());
595 if (
workingMemory().getEntity(entity).hasSnapshot(gui.timeMap.at(gui.snapshot)))
597 snapshots.push_back(
workingMemory().getEntity(entity).getSnapshot(gui.timeMap.at(gui.snapshot)).
id());
602 for (
MemoryID & snapshot : snapshots)
606 if (gui.unlimitedInstances || instances < gui.maxInstances)
608 std::unique_lock lock(gui.visualizationMutex);
609 root.add(Label(instance.id().str()) , Pos{row, 0});
610 gui.tab.checkInstances[instance.id().str()] =
CheckBox();
611 if(std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance.id()) != gui.visibleInstanceIds.end())
613 gui.tab.checkInstances.at(instance.id().str()).setValue(
true);
615 root.
add(gui.tab.checkInstances[instance.id().str()],
Pos{row, 1});
626 root.
add(
Label(
"Show all Instances"),
Pos{row, 0});
627 root.
add(gui.tab.showUnlimitedInstances,
Pos{row, 1});
629 root.
add(
Label(
"Limit for shown Instances"),
Pos{row, 0});
630 root.
add(gui.tab.maxInstances,
Pos{row, 1});
632 root.
add(gui.tab.selectAll,
Pos{row, 0});
634 root.
add(gui.tab.deselectAll,
Pos{row, 0});
636 gui.tab.checkInstances.clear();
641 gui.tab.checkInstances.clear();
643 std::vector<std::string> options = {
"<None>"};
645 gui.tab.selectSnapshot.setOptions(options);
649 root.
add(gui.tab.selectSnapshot,
Pos{row, 0});
651 root.
add(
Label(
"Show all Instances"),
Pos{row, 0});
652 root.
add(gui.tab.showUnlimitedInstances,
Pos{row, 1});
654 root.
add(
Label(
"Limit for shown Instances"),
Pos{row, 0});
655 root.
add(gui.tab.maxInstances,
Pos{row, 1});
657 root.
add(gui.tab.selectAll,
Pos{row, 0});
659 root.
add(gui.tab.deselectAll,
Pos{row, 0});
665 gui.tab.checkInstances.clear();
667 std::vector<std::string> options = {
"<None>"};
669 gui.tab.selectEntity.setOptions(options);
673 root.
add(gui.tab.selectEntity,
Pos{row, 0});
676 gui.tab.selectSnapshot.setOptions(options);
680 root.
add(gui.tab.selectSnapshot,
Pos{row, 0});
682 root.
add(
Label(
"Show all Instances"),
Pos{row, 0});
683 root.
add(gui.tab.showUnlimitedInstances,
Pos{row, 1});
685 root.
add(
Label(
"Limit for shown Instances"),
Pos{row, 0});
686 root.
add(gui.tab.maxInstances,
Pos{row, 1});
688 root.
add(gui.tab.selectAll,
Pos{row, 0});
690 root.
add(gui.tab.deselectAll,
Pos{row, 0});
695 gui.tab.checkInstances.clear();
697 std::vector<std::string> options = {
"<None>"};
698 gui.tab.selectProvider.setOptions(options);
702 root.
add(gui.tab.selectProvider,
Pos{row, 0});
705 gui.tab.selectEntity.setOptions(options);
709 root.
add(gui.tab.selectEntity,
Pos{row, 0});
712 gui.tab.selectSnapshot.setOptions(options);
716 root.
add(gui.tab.selectSnapshot,
Pos{row, 0});
718 root.
add(
Label(
"Show all Instances"),
Pos{row, 0});
719 root.
add(gui.tab.showUnlimitedInstances,
Pos{row, 1});
721 root.
add(
Label(
"Limit for shown Instances"),
Pos{row, 0});
722 root.
add(gui.tab.maxInstances,
Pos{row, 1});
724 root.
add(gui.tab.selectAll,
Pos{row, 0});
726 root.
add(gui.tab.deselectAll,
Pos{row, 0});
733 RemoteGui_createTab(getName(), root, &gui.tab);
734 gui.tab.rebuild =
false;
739 void armarx::armem::server::grasp::GraspMemory::visualizeGraspCandidates()
741 std::unique_lock lock(gui.visualizationMutex);
743 std::map<std::string, viz::Layer> reachableLayers;
744 std::map<std::string, viz::Layer> unreachableLayers;
748 for (
auto & element : gui.visibleInstanceIds)
750 std::string entityName = element.entityName;
752 if (reachableLayers.find(entityName) == reachableLayers.end())
754 reachableLayers[entityName] = arviz.layer(entityName +
"_reachable");
755 unreachableLayers[entityName] = arviz.layer(entityName +
"_unreachable");
756 gui.activeLayers.push_back(entityName);
759 armarx::grasping::GraspCandidate candidate;
761 armarx::grasping::arondto::GraspCandidate aronTransform;
763 aronTransform.fromAron(workingMemory().getInstance(element).
data());
767 visu.
visualize(element.str(), candidate, reachableLayers.at(entityName), unreachableLayers.at(entityName));
772 std::vector<viz::Layer> layers;
773 for(
auto & [entityName, layer] : reachableLayers)
775 layers.push_back(layer);
776 layers.push_back(unreachableLayers.at(entityName));
778 arviz.commit(layers);
783 std::unique_lock lock(gui.visualizationMutex);
784 auto position = std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance);
786 if(position == gui.visibleInstanceIds.end())
788 gui.visibleInstanceIds.push_back(instance);
794 void armarx::armem::server::grasp::GraspMemory::removeInstanceFromVisu(
const armarx::armem::MemoryID &instance)
796 std::unique_lock lock(gui.visualizationMutex);
797 auto position = std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance);
799 if (position != gui.visibleInstanceIds.end())
801 gui.visibleInstanceIds.erase(position);
807 if (std::none_of(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), [&entityName](
const armem::MemoryID&
id)
808 { return id.entityName == entityName; }))
810 arviz.commitDeleteLayer(entityName +
"_reachable");
811 arviz.commitDeleteLayer(entityName +
"_unreachable");
814 auto layerPos = std::find(gui.activeLayers.begin(), gui.activeLayers.end(), entityName);
816 if (layerPos != gui.activeLayers.end())
818 gui.activeLayers.erase(layerPos);
822 void GraspMemory::addToHighlightLayer(
const MemoryID &
memoryID,
const std::string color)
830 else if (color ==
"red")
834 else if (color ==
"blue")
838 else if (color ==
"yellow")
842 else if (color ==
"purple")
847 viz::Layer highlightLayer = arviz.layer((
"HighlightedGrasps"));
850 std::vector<armem::MemoryID> instances;
858 workingMemory().getSnapshot(
memoryID).forEachInstance([&instances](
const auto & instance)
860 instances.push_back(instance.id());
865 workingMemory().getEntity(
memoryID).forEachInstance([&instances](
const auto & instance)
867 instances.push_back(instance.id());
872 workingMemory().getProviderSegment(
memoryID).forEachInstance([&instances](
const auto & instance)
874 instances.push_back(instance.id());
880 workingMemory().getCoreSegment(
"GraspCandidate").forEachInstance([&instances](
const auto & instance)
882 instances.push_back(instance.id());
886 armarx::grasping::GraspCandidate candidate;
888 armarx::grasping::arondto::GraspCandidate aronTransform;
892 aronTransform.fromAron(workingMemory().getInstance(instance).
data());
898 hand.color(handColor);
899 highlightLayer.
add(hand);
902 arviz.commit(highlightLayer);
906 void GraspMemory::RemoteGui_update()
918 if (gui.tab.selectProvider.hasValueChanged())
920 gui.provider = gui.tab.selectProvider.getValue();
921 gui.providerIndex = gui.tab.selectProvider.getIndex();
923 gui.snapshotIndex = 0;
924 gui.tab.rebuild =
true;
927 if (gui.tab.selectEntity.hasValueChanged())
929 gui.entity = gui.tab.selectEntity.getValue();
930 gui.entityIndex = gui.tab.selectEntity.getIndex();
931 gui.snapshotIndex = 0;
932 gui.tab.rebuild =
true;
935 if (gui.tab.selectSnapshot.hasValueChanged())
937 gui.snapshot = gui.tab.selectSnapshot.getValue();
938 gui.snapshotIndex = gui.tab.selectSnapshot.getIndex();
939 gui.tab.rebuild =
true;
942 if(gui.tab.showUnlimitedInstances.hasValueChanged())
944 gui.unlimitedInstances = gui.tab.showUnlimitedInstances.getValue();
945 gui.tab.rebuild =
true;
948 if(gui.tab.maxInstances.hasValueChanged())
950 gui.maxInstances = gui.tab.maxInstances.getValue();
951 gui.tab.rebuild =
true;
954 if(gui.tab.selectAll.wasClicked())
956 for(
auto & element : gui.tab.checkInstances)
958 element.second.setValue(
true);
962 if(gui.tab.deselectAll.wasClicked())
964 for(
auto & element : gui.tab.checkInstances)
966 element.second.setValue(
false);
970 for(
auto & element : gui.tab.checkInstances)
972 if(element.second.hasValueChanged())
974 if (element.second.getValue())
986 visualizeGraspCandidates();
991 createRemoteGuiTab();