diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index deded9bcbcfee433d99146452f9a9425ec7a3797..ff55654b049adfe37980d243b1717e69c5938d73 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -1,81 +1,132 @@
#include "GraspMemory.h"
-#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <string>
-#include <SimoxUtility/algorithm/string.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string.h>
+#include <VirtualRobot/XML/BaseIO.h>
-#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/Clock.h>
+#include <ArmarXCore/core/time/Metronome.h>
+#include <ArmarXCore/core/time/forward_declarations.h>
+
+#include "RobotAPI/components/ArViz/Client/elements/Robot.h"
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h>
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h>
+#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
-
namespace armarx::armem::server::grasp
{
- armarx::PropertyDefinitionsPtr GraspMemory::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ GraspMemory::createPropertyDefinitions()
{
setMemoryName("Grasp");
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
defs->topic(debugObserver);
defs->optional(enableRemoteGui, "remoteGui.enable", "Enable/Disable Remote GUI");
- defs->optional(gui.trackNewEntities, "EnableTrackingOfNewEntities", "Enable/Disable the automatic visual tracking of newly commited Entities");
+ defs->optional(gui.trackNewEntities,
+ "EnableTrackingOfNewEntities",
+ "Enable/Disable the automatic visual tracking of newly commited Entities");
+
+ defs->optional(enableVisualizeKnownGraspCandidates,
+ "p.enableVisualizeKnownGraspCandidates");
+ defs->optional(frequencyHzVisualizeKnownGraspCandidates,
+ "p.frequencyHzVisualizeKnownGraspCandidates");
+
+
return defs;
}
-
- std::string GraspMemory::getDefaultName() const
+ std::string
+ GraspMemory::getDefaultName() const
{
return "GraspMemory";
}
- GraspMemory::GraspMemory() :
- knownGraspProviderSegment(iceAdapter())
+ GraspMemory::GraspMemory() : knownGraspProviderSegment(iceAdapter())
{
+ addPlugin(virtualRobotReaderPlugin);
}
- void GraspMemory::onInitComponent()
+ void
+ GraspMemory::onInitComponent()
{
workingMemory().addCoreSegment("GraspCandidate",
- armarx::grasping::arondto::GraspCandidate::ToAronType());
- workingMemory().addCoreSegment("BimanualGraspCandidate",
- armarx::grasping::arondto::BimanualGraspCandidate::ToAronType());
- workingMemory().addCoreSegment("KnownGraspCandidate",
- armarx::armem::grasping::arondto::KnownGraspInfo::ToAronType());
+ armarx::grasping::arondto::GraspCandidate::ToAronType());
+ workingMemory().addCoreSegment(
+ "BimanualGraspCandidate",
+ armarx::grasping::arondto::BimanualGraspCandidate::ToAronType());
+ workingMemory().addCoreSegment(
+ "KnownGraspCandidate", armarx::armem::grasping::arondto::KnownGraspInfo::ToAronType());
knownGraspProviderSegment.init();
}
- void GraspMemory::onConnectComponent()
+ void
+ GraspMemory::onConnectComponent()
{
if (enableRemoteGui)
{
createRemoteGuiTab();
RemoteGui_startRunningTask();
}
+
+ ARMARX_CHECK_NULL(taskVisuKnownGrasps);
+
+ taskVisuKnownGrasps =
+ new RunningTask<GraspMemory>(this, &GraspMemory::visualizeKnownGraspCandidates);
+ taskVisuKnownGrasps->start();
}
- void GraspMemory::onDisconnectComponent()
+ void
+ GraspMemory::onDisconnectComponent()
{
+ if (taskVisuKnownGrasps)
+ {
+ taskVisuKnownGrasps->stop();
+ taskVisuKnownGrasps = nullptr;
+ }
+
+ // try to clear all layers
+ {
+ for (auto& [_, layer] : graspVisuPerObject)
+ {
+ layer.clear();
+ layer.markForDeletion();
+ }
+
+ arviz.commit(simox::alg::get_values(graspVisuPerObject));
+ graspVisuPerObject.clear();
+ }
}
- void GraspMemory::onExitComponent()
+ void
+ GraspMemory::onExitComponent()
{
}
-
-
// WRITING
- armem::data::CommitResult GraspMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
+ armem::data::CommitResult
+ GraspMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
{
std::vector<armem::MemoryID> trackedEntityIds;
{
@@ -83,31 +134,28 @@ namespace armarx::armem::server::grasp
trackedEntityIds = gui.trackedEntityIds;
}
- if(! trackedEntityIds.empty())
+ if (!trackedEntityIds.empty())
{
- for (auto &update : commit.updates)
+ for (auto& update : commit.updates)
{
armem::MemoryID entityID = armarx::fromIce<armem::MemoryID>(update.entityID);
entityID.clearTimestamp();
- if(std::find(trackedEntityIds.begin(), trackedEntityIds.end(), entityID) != trackedEntityIds.end())
+ if (std::find(trackedEntityIds.begin(), trackedEntityIds.end(), entityID) !=
+ trackedEntityIds.end())
{
- workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance([this](const auto & instance)
- {
- removeInstanceFromVisu(instance.id());
-
- });
+ workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance(
+ [this](const auto& instance) { removeInstanceFromVisu(instance.id()); });
}
-
}
}
- if(gui.trackNewEntities)
+ if (gui.trackNewEntities)
{
- for (auto &update : commit.updates)
+ for (auto& update : commit.updates)
{
armem::MemoryID entityID = armarx::fromIce<armem::MemoryID>(update.entityID);
entityID.clearTimestamp();
- if(!workingMemory().findEntity(entityID))
+ if (!workingMemory().findEntity(entityID))
{
std::unique_lock lock(gui.visualizationMutex);
gui.trackedEntityIds.emplace_back(entityID);
@@ -121,21 +169,18 @@ namespace armarx::armem::server::grasp
gui.tab.rebuild = true;
- if (! trackedEntityIds.empty())
+ if (!trackedEntityIds.empty())
{
- for (auto &update : commit.updates)
+ for (auto& update : commit.updates)
{
armem::MemoryID entityID = armarx::fromIce<armem::MemoryID>(update.entityID);
entityID.clearTimestamp();
- if(std::find(trackedEntityIds.begin(), trackedEntityIds.end(), entityID) != trackedEntityIds.end())
+ if (std::find(trackedEntityIds.begin(), trackedEntityIds.end(), entityID) !=
+ trackedEntityIds.end())
{
- workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance([this](const auto & instance)
- {
- addInstanceToVisu(instance.id());
-
- });
+ workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance(
+ [this](const auto& instance) { addInstanceToVisu(instance.id()); });
}
-
}
}
return result;
@@ -146,8 +191,7 @@ namespace armarx::armem::server::grasp
// Inherited from Plugin
armem::actions::data::GetActionsOutputSeq
- GraspMemory::getActions(
- const armem::actions::data::GetActionsInputSeq &inputs)
+ GraspMemory::getActions(const armem::actions::data::GetActionsInputSeq& inputs)
{
using namespace armem::actions;
@@ -157,49 +201,53 @@ namespace armarx::armem::server::grasp
auto memoryID = armarx::fromIce<armem::MemoryID>(input.id);
if (armem::contains(armem::MemoryID("Grasp"), memoryID) and not memoryID.hasGap())
{
- if(armem::contains(armem::MemoryID("Grasp", "BimanualGraspCandidate"), memoryID)
- || armem::contains(armem::MemoryID("Grasp", "KnownGraspCandidate"), memoryID))
+ if (armem::contains(armem::MemoryID("Grasp", "BimanualGraspCandidate"), memoryID) ||
+ armem::contains(armem::MemoryID("Grasp", "KnownGraspCandidate"), memoryID))
{
- continue; // Not supported, ignore.
+ continue; // Not supported, ignore.
}
std::vector<MenuEntry> actions;
- if ( !memoryID.hasTimestamp() && !memoryID.hasInstanceIndex())
+ if (!memoryID.hasTimestamp() && !memoryID.hasInstanceIndex())
{
- if(memoryID.hasEntityName())
+ if (memoryID.hasEntityName())
{
- if(std::find(gui.trackedEntityIds.begin(), gui.trackedEntityIds.end(), memoryID) == gui.trackedEntityIds.end())
+ if (std::find(gui.trackedEntityIds.begin(),
+ gui.trackedEntityIds.end(),
+ memoryID) == gui.trackedEntityIds.end())
{
- actions.push_back(Action{"track","Track this entity in arviz"});
+ actions.push_back(Action{"track", "Track this entity in arviz"});
}
else
{
- actions.push_back(Action{"untrack","Stop tracking this entity in arviz"});
+ actions.push_back(
+ Action{"untrack", "Stop tracking this entity in arviz"});
}
-
}
else
{
- actions.push_back(Action{"track","Track all underlying entities in arviz"});
- actions.push_back(Action{"untrack","Stop tracking all underlying entities in arviz"});
+ actions.push_back(
+ Action{"track", "Track all underlying entities in arviz"});
+ actions.push_back(
+ Action{"untrack", "Stop tracking all underlying entities in arviz"});
}
}
actions.push_back(Action{"vis", "Visualize all contained grasp candidates"});
- actions.push_back(Action{"rem", "Remove all contained grasp candidates from visualization"});
- actions.push_back(SubMenu{"high", "Highlight all contain grasp candidates", {
- Action{"pink", "in pink"},
- Action{"red", "in red"},
- Action{"blue", "in blue"},
- Action{"yellow", "in yellow"},
- Action{"purple", "in purple"}
- }});
+ actions.push_back(
+ Action{"rem", "Remove all contained grasp candidates from visualization"});
+ actions.push_back(SubMenu{"high",
+ "Highlight all contain grasp candidates",
+ {Action{"pink", "in pink"},
+ Action{"red", "in red"},
+ Action{"blue", "in blue"},
+ Action{"yellow", "in yellow"},
+ Action{"purple", "in purple"}}});
actions.push_back(Action{"reset", "Reset highlight layer"});
Menu menu{actions};
- outputs.push_back({ menu.toIce() });
-
+ outputs.push_back({menu.toIce()});
}
}
@@ -207,8 +255,7 @@ namespace armarx::armem::server::grasp
}
armem::actions::data::ExecuteActionOutputSeq
- GraspMemory::executeActions(
- const armem::actions::data::ExecuteActionInputSeq &inputs)
+ GraspMemory::executeActions(const armem::actions::data::ExecuteActionInputSeq& inputs)
{
using namespace armem::actions;
ExecuteActionOutputSeq outputs;
@@ -221,11 +268,14 @@ namespace armarx::armem::server::grasp
if (armem::contains(armem::MemoryID("Grasp"), memoryID) and not memoryID.hasGap())
{
{
- if(armem::contains(armem::MemoryID("Grasp", "BimanualGraspCandidate"), memoryID)
- || armem::contains(armem::MemoryID("Grasp", "KnownGraspCandidate"), memoryID))
+ if (armem::contains(armem::MemoryID("Grasp", "BimanualGraspCandidate"),
+ memoryID) ||
+ armem::contains(armem::MemoryID("Grasp", "KnownGraspCandidate"),
+ memoryID))
{
std::stringstream sstream;
- sstream << "Currently visualization for CoreSegment " << memoryID.coreSegmentName << " is not yet supported";
+ sstream << "Currently visualization for CoreSegment "
+ << memoryID.coreSegmentName << " is not yet supported";
outputs.emplace_back(false, sstream.str());
}
else
@@ -240,49 +290,51 @@ namespace armarx::armem::server::grasp
{
removeInstanceFromVisu(memoryID);
}
-
}
else if (memoryID.hasTimestamp())
{
- workingMemory().getSnapshot(memoryID).forEachInstance([this, &input](const auto & instance)
- {
- if (input.actionPath == ActionPath{"vis"})
- {
- addInstanceToVisu(instance.id());
- }
- else if (input.actionPath == ActionPath{"rem"})
+ workingMemory().getSnapshot(memoryID).forEachInstance(
+ [this, &input](const auto& instance)
{
- removeInstanceFromVisu(instance.id());
- }
- });
+ if (input.actionPath == ActionPath{"vis"})
+ {
+ addInstanceToVisu(instance.id());
+ }
+ else if (input.actionPath == ActionPath{"rem"})
+ {
+ removeInstanceFromVisu(instance.id());
+ }
+ });
}
else if (memoryID.hasEntityName())
{
- workingMemory().getEntity(memoryID).forEachInstance([this, &input](const auto & instance)
- {
- if (input.actionPath == ActionPath{"vis"})
- {
- addInstanceToVisu(instance.id());
- }
- else if (input.actionPath == ActionPath{"rem"})
+ workingMemory().getEntity(memoryID).forEachInstance(
+ [this, &input](const auto& instance)
{
- removeInstanceFromVisu(instance.id());
- }
- });
+ if (input.actionPath == ActionPath{"vis"})
+ {
+ addInstanceToVisu(instance.id());
+ }
+ else if (input.actionPath == ActionPath{"rem"})
+ {
+ removeInstanceFromVisu(instance.id());
+ }
+ });
}
else if (memoryID.hasProviderSegmentName())
{
- workingMemory().getProviderSegment(memoryID).forEachInstance([this, &input](const auto & instance)
- {
- if (input.actionPath == ActionPath{"vis"})
- {
- addInstanceToVisu(instance.id());
- }
- else if (input.actionPath == ActionPath{"rem"})
+ workingMemory().getProviderSegment(memoryID).forEachInstance(
+ [this, &input](const auto& instance)
{
- removeInstanceFromVisu(instance.id());
- }
- });
+ if (input.actionPath == ActionPath{"vis"})
+ {
+ addInstanceToVisu(instance.id());
+ }
+ else if (input.actionPath == ActionPath{"rem"})
+ {
+ removeInstanceFromVisu(instance.id());
+ }
+ });
}
else
{
@@ -291,7 +343,7 @@ namespace armarx::armem::server::grasp
std::unique_lock lock(gui.visualizationMutex);
gui.visibleInstanceIds.clear();
// mark all layers for deletion
- for (std::string & layer : gui.activeLayers)
+ for (std::string& layer : gui.activeLayers)
{
arviz.commitDeleteLayer(layer + "_reachable");
arviz.commitDeleteLayer(layer + "_unreachable");
@@ -301,15 +353,15 @@ namespace armarx::armem::server::grasp
else if (input.actionPath == ActionPath{"vis"})
{
//currently only visualization for CoreSegment GraspCandidate available
- workingMemory().getCoreSegment("GraspCandidate").forEachInstance([this](const auto & instance)
- {
- addInstanceToVisu(instance.id());
- });
+ workingMemory()
+ .getCoreSegment("GraspCandidate")
+ .forEachInstance([this](const auto& instance)
+ { addInstanceToVisu(instance.id()); });
}
}
- visualizeGraspCandidates();
- outputs.emplace_back(true, "");
+ visualizeGraspCandidates();
+ outputs.emplace_back(true, "");
}
}
}
@@ -321,44 +373,43 @@ namespace armarx::armem::server::grasp
outputs.emplace_back(false, sstream.str());
}
}
- else if(input.actionPath == ActionPath{"track"} || input.actionPath == ActionPath{"untrack"} )
+ else if (input.actionPath == ActionPath{"track"} ||
+ input.actionPath == ActionPath{"untrack"})
{
std::vector<armem::MemoryID> entityIDs;
- if(memoryID.hasEntityName())
+ if (memoryID.hasEntityName())
{
entityIDs.emplace_back(memoryID);
}
- else if(memoryID.hasProviderSegmentName()){
- workingMemory().getProviderSegment(memoryID).forEachEntity([&entityIDs](const auto & entity)
- {
- entityIDs.emplace_back(entity.id());
- });
+ else if (memoryID.hasProviderSegmentName())
+ {
+ workingMemory().getProviderSegment(memoryID).forEachEntity(
+ [&entityIDs](const auto& entity) { entityIDs.emplace_back(entity.id()); });
}
else
{
//currently only visualization for CoreSegment GraspCandidate available
- workingMemory().getCoreSegment("GraspCandidate").forEachEntity([&entityIDs](const auto & entity)
- {
- entityIDs.emplace_back(entity.id());
- });
+ workingMemory()
+ .getCoreSegment("GraspCandidate")
+ .forEachEntity([&entityIDs](const auto& entity)
+ { entityIDs.emplace_back(entity.id()); });
}
- for(auto & entityID : entityIDs)
+ for (auto& entityID : entityIDs)
{
- if(input.actionPath == ActionPath{"track"})
+ if (input.actionPath == ActionPath{"track"})
{
//visualize latest snapshot of entity and refresh if necessary
- workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance([this](const auto & instance)
- {
- addInstanceToVisu(instance.id());
- });
+ workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance(
+ [this](const auto& instance) { addInstanceToVisu(instance.id()); });
gui.trackedEntityIds.push_back(entityID);
ARMARX_INFO << "starting to track " << entityID;
outputs.emplace_back(true, "");
}
- else if(input.actionPath == ActionPath{"untrack"})
+ else if (input.actionPath == ActionPath{"untrack"})
{
- auto pos = std::find(gui.trackedEntityIds.begin(), gui.trackedEntityIds.end(), entityID);
- if(pos != gui.trackedEntityIds.end())
+ auto pos = std::find(
+ gui.trackedEntityIds.begin(), gui.trackedEntityIds.end(), entityID);
+ if (pos != gui.trackedEntityIds.end())
{
gui.trackedEntityIds.erase(pos);
ARMARX_INFO << "Stop tracking of " << entityID;
@@ -367,11 +418,11 @@ namespace armarx::armem::server::grasp
}
}
}
- else if(input.actionPath.front() == "high")
+ else if (input.actionPath.front() == "high")
{
addToHighlightLayer(memoryID, input.actionPath.back());
}
- else if(input.actionPath == ActionPath{"reset"})
+ else if (input.actionPath == ActionPath{"reset"})
{
arviz.commit(arviz.layer("HighlightedGrasps"));
}
@@ -385,11 +436,10 @@ namespace armarx::armem::server::grasp
return outputs;
}
-
-
// REMOTE GUI
- void GraspMemory::createRemoteGuiTab()
+ void
+ GraspMemory::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
GridLayout root;
@@ -397,34 +447,35 @@ namespace armarx::armem::server::grasp
gui.tab.selectAll.setLabel("Select All");
gui.tab.deselectAll.setLabel("Deselect All");
gui.tab.showUnlimitedInstances.setValue(gui.unlimitedInstances);
- gui.tab.maxInstances.setRange(0,1000);
+ gui.tab.maxInstances.setRange(0, 1000);
gui.tab.maxInstances.setValue(gui.maxInstances);
int row = 0;
// bimanual candidates are not available for visualization for now
-// {
-// root.add(Label("CoreSegment"), Pos{row, 0});
-// row++;
-// std::vector<std::string> options = workingMemory().getCoreSegmentNames();
-// if (options.empty())
-// {
-// options.push_back("<None>");
-// }
-// gui.tab.selectCoreSegment.setOptions(options);
-// if (gui.coreSegIndex >= 0 && gui.coreSegIndex < static_cast<int>(options.size()))
-// {
-// gui.tab.selectCoreSegment.setIndex(gui.coreSegIndex);
-// gui.coreSegment = options[gui.coreSegIndex];
-// }
-// root.add(gui.tab.selectCoreSegment, Pos{row,0});
-// row++;
-// }
- if(workingMemory().hasCoreSegment(gui.coreSegment))
+ // {
+ // root.add(Label("CoreSegment"), Pos{row, 0});
+ // row++;
+ // std::vector<std::string> options = workingMemory().getCoreSegmentNames();
+ // if (options.empty())
+ // {
+ // options.push_back("<None>");
+ // }
+ // gui.tab.selectCoreSegment.setOptions(options);
+ // if (gui.coreSegIndex >= 0 && gui.coreSegIndex < static_cast<int>(options.size()))
+ // {
+ // gui.tab.selectCoreSegment.setIndex(gui.coreSegIndex);
+ // gui.coreSegment = options[gui.coreSegIndex];
+ // }
+ // root.add(gui.tab.selectCoreSegment, Pos{row,0});
+ // row++;
+ // }
+ if (workingMemory().hasCoreSegment(gui.coreSegment))
{
{
root.add(Label("Provider"), Pos{row, 0});
row++;
- std::vector<std::string> options = workingMemory().getCoreSegment(gui.coreSegment).getProviderSegmentNames();
+ std::vector<std::string> options =
+ workingMemory().getCoreSegment(gui.coreSegment).getProviderSegmentNames();
if (options.empty())
{
options.push_back("<None>");
@@ -436,7 +487,8 @@ namespace armarx::armem::server::grasp
gui.tab.selectProvider.setOptions(options);
- if (gui.providerIndex >= 0 && gui.providerIndex < static_cast<int>(options.size()))
+ if (gui.providerIndex >= 0 &&
+ gui.providerIndex < static_cast<int>(options.size()))
{
gui.tab.selectProvider.setIndex(gui.providerIndex);
gui.provider = options[gui.providerIndex];
@@ -445,30 +497,32 @@ namespace armarx::armem::server::grasp
row++;
}
- if(gui.provider == "<All>"
- || workingMemory().getCoreSegment(gui.coreSegment).hasProviderSegment(gui.provider))
+ if (gui.provider == "<All>" || workingMemory()
+ .getCoreSegment(gui.coreSegment)
+ .hasProviderSegment(gui.provider))
{
std::vector<MemoryID> providers;
{
- root.add(Label("Entity"), Pos{row,0});
+ root.add(Label("Entity"), Pos{row, 0});
row++;
std::vector<std::string> options;
if (gui.provider == "<All>")
{
- workingMemory().getCoreSegment(gui.coreSegment).forEachProviderSegment([&providers](const auto & provider)
- {
- providers.push_back(provider.id());
- });
+ workingMemory()
+ .getCoreSegment(gui.coreSegment)
+ .forEachProviderSegment([&providers](const auto& provider)
+ { providers.push_back(provider.id()); });
}
else
{
- providers.push_back(MemoryID(workingMemory().name(), gui.coreSegment, gui.provider));
-
+ providers.push_back(
+ MemoryID(workingMemory().name(), gui.coreSegment, gui.provider));
}
- for (MemoryID & provider : providers)
+ for (MemoryID& provider : providers)
{
- for(std::string & entity : workingMemory().getProviderSegment(provider).getEntityNames())
+ for (std::string& entity :
+ workingMemory().getProviderSegment(provider).getEntityNames())
{
options.push_back(entity);
}
@@ -482,7 +536,8 @@ namespace armarx::armem::server::grasp
options.insert(options.begin(), "<All>");
}
gui.tab.selectEntity.setOptions(options);
- if (gui.entityIndex >= 0 && gui.entityIndex < static_cast<int>(options.size()))
+ if (gui.entityIndex >= 0 &&
+ gui.entityIndex < static_cast<int>(options.size()))
{
gui.tab.selectEntity.setIndex(gui.entityIndex);
gui.entity = options[gui.entityIndex];
@@ -491,41 +546,50 @@ namespace armarx::armem::server::grasp
row++;
}
- if( gui.entity == "<All>"
- || (gui.provider == "<All>" && workingMemory().getCoreSegment(gui.coreSegment).hasEntity(gui.entity))
- || workingMemory().getCoreSegment(gui.coreSegment).getProviderSegment(gui.provider).hasEntity(gui.entity))
+ if (gui.entity == "<All>" ||
+ (gui.provider == "<All>" &&
+ workingMemory().getCoreSegment(gui.coreSegment).hasEntity(gui.entity)) ||
+ workingMemory()
+ .getCoreSegment(gui.coreSegment)
+ .getProviderSegment(gui.provider)
+ .hasEntity(gui.entity))
{
std::vector<MemoryID> entities;
{
root.add(Label("Snapshot"), Pos{row, 0});
row++;
std::vector<std::string> options;
- for (MemoryID & provider : providers)
+ for (MemoryID& provider : providers)
{
if (gui.entity == "<All>")
{
- workingMemory().getProviderSegment(provider).forEachEntity([&entities](auto & entity)
- {
- entities.push_back(entity.id());
- });
+ workingMemory().getProviderSegment(provider).forEachEntity(
+ [&entities](auto& entity)
+ { entities.push_back(entity.id()); });
}
else
{
- if (workingMemory().getProviderSegment(provider).hasEntity(gui.entity))
+ if (workingMemory().getProviderSegment(provider).hasEntity(
+ gui.entity))
{
- entities.push_back(MemoryID(workingMemory().name(), gui.coreSegment, provider.providerSegmentName, gui.entity));
+ entities.push_back(MemoryID(workingMemory().name(),
+ gui.coreSegment,
+ provider.providerSegmentName,
+ gui.entity));
}
}
}
- for (MemoryID & entity : entities)
+ for (MemoryID& entity : entities)
{
- workingMemory().getEntity(entity).forEachSnapshot([this, &options](const auto & snapshot)
- {
- std::string time = armem::toDateTimeMilliSeconds(snapshot.time());
- gui.timeMap[time] = snapshot.time();
- options.push_back(time);
- });
+ workingMemory().getEntity(entity).forEachSnapshot(
+ [this, &options](const auto& snapshot)
+ {
+ std::string time =
+ armem::toDateTimeMilliSeconds(snapshot.time());
+ gui.timeMap[time] = snapshot.time();
+ options.push_back(time);
+ });
}
if (options.empty())
@@ -537,18 +601,17 @@ namespace armarx::armem::server::grasp
options.insert(options.begin(), "<All>");
}
gui.tab.selectSnapshot.setOptions(options);
- if (gui.snapshotIndex >= 0 && gui.snapshotIndex < static_cast<int>(options.size()))
+ if (gui.snapshotIndex >= 0 &&
+ gui.snapshotIndex < static_cast<int>(options.size()))
{
gui.tab.selectSnapshot.setIndex(gui.snapshotIndex);
- if(options[gui.snapshotIndex] != "<None>")
+ if (options[gui.snapshotIndex] != "<None>")
{
gui.snapshot = options[gui.snapshotIndex];
}
}
root.add(gui.tab.selectSnapshot, Pos{row, 0});
row++;
-
-
}
bool comboExists;
if (gui.snapshot == "<All>")
@@ -557,17 +620,18 @@ namespace armarx::armem::server::grasp
}
else
{
- for (MemoryID & entity : entities)
+ for (MemoryID& entity : entities)
{
- if (workingMemory().getEntity(entity).hasSnapshot(gui.timeMap.at(gui.snapshot)))
+ if (workingMemory().getEntity(entity).hasSnapshot(
+ gui.timeMap.at(gui.snapshot)))
{
comboExists = true;
}
}
}
- if(comboExists)
+ if (comboExists)
{
- root.add(Label("Instances"), Pos{row,0});
+ root.add(Label("Instances"), Pos{row, 0});
row++;
root.add(Label("Show all Instances"), Pos{row, 0});
root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
@@ -581,47 +645,58 @@ namespace armarx::armem::server::grasp
row++;
gui.tab.checkInstances.clear();
std::vector<MemoryID> snapshots;
- for (MemoryID & entity : entities)
+ for (MemoryID& entity : entities)
{
- if(gui.snapshot == "<All>")
+ if (gui.snapshot == "<All>")
{
- workingMemory().getEntity(entity).forEachSnapshot([&snapshots](auto & snapshot)
- {
- snapshots.push_back(snapshot.id());
- });
+ workingMemory().getEntity(entity).forEachSnapshot(
+ [&snapshots](auto& snapshot)
+ { snapshots.push_back(snapshot.id()); });
}
else
{
- if (workingMemory().getEntity(entity).hasSnapshot(gui.timeMap.at(gui.snapshot)))
+ if (workingMemory().getEntity(entity).hasSnapshot(
+ gui.timeMap.at(gui.snapshot)))
{
- snapshots.push_back(workingMemory().getEntity(entity).getSnapshot(gui.timeMap.at(gui.snapshot)).id());
+ snapshots.push_back(
+ workingMemory()
+ .getEntity(entity)
+ .getSnapshot(gui.timeMap.at(gui.snapshot))
+ .id());
}
}
}
int instances = 0;
- for (MemoryID & snapshot : snapshots)
+ for (MemoryID& snapshot : snapshots)
{
- workingMemory().getSnapshot(snapshot).forEachInstance([this, &row, &root, &instances](const auto & instance)
- {
- if (gui.unlimitedInstances || instances < gui.maxInstances)
+ workingMemory().getSnapshot(snapshot).forEachInstance(
+ [this, &row, &root, &instances](const auto& instance)
{
- std::unique_lock lock(gui.visualizationMutex);
- root.add(Label(instance.id().str()) , Pos{row, 0});
- gui.tab.checkInstances[instance.id().str()] = CheckBox();
- if(std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance.id()) != gui.visibleInstanceIds.end())
+ if (gui.unlimitedInstances || instances < gui.maxInstances)
{
- gui.tab.checkInstances.at(instance.id().str()).setValue(true);
+ std::unique_lock lock(gui.visualizationMutex);
+ root.add(Label(instance.id().str()), Pos{row, 0});
+ gui.tab.checkInstances[instance.id().str()] =
+ CheckBox();
+ if (std::find(gui.visibleInstanceIds.begin(),
+ gui.visibleInstanceIds.end(),
+ instance.id()) !=
+ gui.visibleInstanceIds.end())
+ {
+ gui.tab.checkInstances.at(instance.id().str())
+ .setValue(true);
+ }
+ root.add(gui.tab.checkInstances[instance.id().str()],
+ Pos{row, 1});
+ row++;
+ instances++;
}
- root.add(gui.tab.checkInstances[instance.id().str()], Pos{row, 1});
- row++;
- instances++;
- }
- });
+ });
}
}
else
{
- root.add(Label("Instances"), Pos{row,0});
+ root.add(Label("Instances"), Pos{row, 0});
row++;
root.add(Label("Show all Instances"), Pos{row, 0});
root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
@@ -668,7 +743,7 @@ namespace armarx::armem::server::grasp
gui.tab.selectEntity.setOptions(options);
- root.add(Label("Entity"), Pos{row,0});
+ root.add(Label("Entity"), Pos{row, 0});
row++;
root.add(gui.tab.selectEntity, Pos{row, 0});
row++;
@@ -691,7 +766,8 @@ namespace armarx::armem::server::grasp
row++;
}
}
- else {
+ else
+ {
gui.tab.checkInstances.clear();
std::vector<std::string> options = {"<None>"};
@@ -704,7 +780,7 @@ namespace armarx::armem::server::grasp
gui.tab.selectEntity.setOptions(options);
- root.add(Label("Entity"), Pos{row,0});
+ root.add(Label("Entity"), Pos{row, 0});
row++;
root.add(gui.tab.selectEntity, Pos{row, 0});
row++;
@@ -725,18 +801,247 @@ namespace armarx::armem::server::grasp
row++;
root.add(gui.tab.deselectAll, Pos{row, 0});
row++;
-
}
}
RemoteGui_createTab(getName(), root, &gui.tab);
gui.tab.rebuild = false;
-
}
+ inline const auto GetSide =
+ [](const armarx::armem::grasping::arondto::KnownGraspSet& graspSet) -> std::string
+ {
+ ARMARX_VERBOSE << "Grasping with " << graspSet.robot << " and with the endeffector named"
+ << graspSet.endeffector;
+ //Filter the robot befor that
+ if (graspSet.robot == "Armar6" or graspSet.robot == "ArmarDE" or graspSet.robot == "Armar7")
+ {
+ if (graspSet.endeffector == "TCP_R")
+ {
+ return "Right";
+ }
+ if (graspSet.endeffector == "TCP_L")
+ {
+ return "Left";
+ }
+ if (graspSet.endeffector == "Hand_R_EEF")
+ {
+ return "Right";
+ }
+ if (graspSet.endeffector == "Hand_L_EEF")
+ {
+ return "Left";
+ }
+ ARMARX_ERROR << "Unknown TCP `" << graspSet.endeffector << "` for robot `"
+ << graspSet.robot << "`!";
+ }
+
+ if (graspSet.robot == "Armar3")
+ {
+ if (graspSet.endeffector == "Hand R")
+ {
+ return "Right";
+ }
+ if (graspSet.endeffector == "Hand L")
+ {
+ return "Left";
+ }
+ ARMARX_ERROR << "Unknown TCP `" << graspSet.endeffector << "` for robot `"
+ << graspSet.robot << "`!";
+ }
+
+ ARMARX_ERROR << "Unknown TCP `" << graspSet.endeffector << "` for robot `" << graspSet.robot
+ << "`!";
+ throw std::exception();
+ };
+
+ void
+ armarx::armem::server::grasp::GraspMemory::visualizeKnownGraspCandidates()
+ {
+ if (not enableVisualizeKnownGraspCandidates)
+ {
+ return;
+ }
+
+ Metronome metronome(Frequency::Hertz(frequencyHzVisualizeKnownGraspCandidates));
+
+ while (not taskVisuKnownGrasps->isStopped())
+ {
+ // do the work
+ {
+ const std::vector<robot_state::description::RobotDescription> robotDescriptions =
+ virtualRobotReaderPlugin->get().queryDescriptions(armarx::Clock::Now());
+ ARMARX_VERBOSE << VAROUT(robotDescriptions.size());
+
+ std::map<std::string, robot_state::description::RobotDescription>
+ robotDescriptionMap;
+ for (const auto& robotDescription : robotDescriptions)
+ {
+ robotDescriptionMap.emplace(robotDescription.name, robotDescription);
+ }
+
+ // load robot if needed
+ for (const auto& robotDescription : robotDescriptions)
+ {
+ if (robots.count(robotDescription.name) > 0)
+ {
+ // robot already loaded
+ continue;
+ }
+
+
+ if (auto robot = VirtualRobot::RobotIO::loadRobot(
+ robotDescription.xml.toSystemPath(), VirtualRobot::BaseIO::eStructure))
+ {
+ ARMARX_INFO << "Loaded robot `" << robotDescription.name << "`";
+ robots.emplace(robotDescription.name, robot);
+ }
+ else
+ {
+ ARMARX_INFO << "Failed to load robot `" << robotDescription.name
+ << "` from file `" << robotDescription.xml.toSystemPath()
+ << "`.";
+ }
+ }
+
+
+ std::map<std::string, std::vector<armarx::armem::grasping::arondto::KnownGraspInfo>>
+ graspsForObject;
+ {
+
+ workingMemory()
+ .getCoreSegment("KnownGraspCandidate")
+ .forEachInstance(
+ [&graspsForObject](const auto& entityInstance)
+ {
+ const armarx::armem::grasping::arondto::KnownGraspInfo dto =
+ armarx::armem::grasping::arondto::KnownGraspInfo::FromAron(
+ entityInstance.data());
+
+ const MemoryID memoryId = entityInstance.id();
+
+ // memoryId.entityName is the objectClass
+ graspsForObject[memoryId.entityName].push_back(dto);
+ });
+ }
+
+ // ensure old data is cleared
+ for (auto& [_, layer] : graspVisuPerObject)
+ {
+ layer.clear();
+ }
+
+ // Query objects
+
+ const auto objects = getObjectPoses();
+
+ for (const auto& object : objects)
+ {
+ const std::string layerName = object.objectID.str();
+ auto layer = arviz.layer(layerName);
+
+ // get all corresponding grasps
+ const std::string objectClassName = object.objectID.getClassID().str();
+
+ if (graspsForObject.count(objectClassName) == 0)
+ {
+ continue;
+ }
+
+ const auto& graspInfos = graspsForObject.at(objectClassName);
+
+ for (const auto& graspInfo : graspInfos)
+ {
+ for (const auto& [_, graspSet] : graspInfo.graspSets)
+ {
+ // if robot is unknown, skip it
+ if (robotDescriptionMap.count(graspSet.robot) == 0)
+ {
+ continue;
+ }
+
+ const std::string side = GetSide(graspSet);
+ const std::string bodyPartName = side + "Hand";
+
+ const auto& robotDescription = robotDescriptionMap.at(graspSet.robot);
+ const auto& visuPart =
+ robotDescription.visualization.body_parts.at(bodyPartName);
+
+ const armarx::PackagePath visuRobotPartXML{visuPart.xml.package,
+ visuPart.xml.path};
- void armarx::armem::server::grasp::GraspMemory::visualizeGraspCandidates()
+ for (const auto& grasp : graspSet.grasps)
+ {
+ const std::string visuName = graspSet.robot + "::" + graspSet.name +
+ "::" + object.providerName +
+ "::" + object.objectID.str() +
+ "::" + grasp.name;
+ ARMARX_VERBOSE << VAROUT(visuName);
+
+ if (robots.count(graspSet.robot) == 0)
+ {
+ continue;
+ }
+
+ const auto robot = robots.at(graspSet.robot);
+
+ const auto handRootNode =
+ robot->getRobotNode(visuPart.root_node_name);
+ if (handRootNode == nullptr)
+ {
+ ARMARX_INFO << "Node `" << visuPart.root_node_name
+ << "` not available for robot `" << graspSet.robot
+ << "`.";
+ continue;
+ }
+
+ const auto eefNode = robot->getEndEffector(graspSet.endeffector);
+ if (eefNode == nullptr)
+ {
+ ARMARX_INFO << "End-effector `" << graspSet.endeffector
+ << "` not available for robot `" << graspSet.robot
+ << "`.";
+ continue;
+ }
+
+ const auto tcp = eefNode->getTcp();
+ if (tcp == nullptr)
+ {
+ ARMARX_INFO << "End-effector `" << graspSet.endeffector
+ << "` does not provide a TCP";
+ continue;
+ }
+
+ const Eigen::Isometry3f eef_T_hand_root{
+ handRootNode->getPoseInFrame(tcp)};
+
+ const Eigen::Isometry3f pose =
+ Eigen::Isometry3f{object.objectPoseGlobal} *
+ Eigen::Isometry3f{grasp.pose}.inverse() * eef_T_hand_root;
+
+ viz::Robot handVisu(visuName);
+ handVisu.useCollisionModel()
+ .file(visuRobotPartXML)
+ .pose(pose.matrix());
+
+ layer.add(handVisu);
+ }
+ }
+ }
+
+ graspVisuPerObject[layerName] = layer;
+ }
+
+ arviz.commit(simox::alg::get_values(graspVisuPerObject));
+ }
+
+ metronome.waitForNextTick();
+ }
+ }
+
+ void
+ armarx::armem::server::grasp::GraspMemory::visualizeGraspCandidates()
{
std::unique_lock lock(gui.visualizationMutex);
armarx::grasping::GraspCandidateVisu visu;
@@ -745,7 +1050,7 @@ namespace armarx::armem::server::grasp
{
- for (auto & element : gui.visibleInstanceIds)
+ for (auto& element : gui.visibleInstanceIds)
{
std::string entityName = element.entityName;
@@ -764,13 +1069,15 @@ namespace armarx::armem::server::grasp
fromAron(aronTransform, candidate);
- visu.visualize(element.str(), candidate, reachableLayers.at(entityName), unreachableLayers.at(entityName));
-
+ visu.visualize(element.str(),
+ candidate,
+ reachableLayers.at(entityName),
+ unreachableLayers.at(entityName));
}
}
std::vector<viz::Layer> layers;
- for(auto & [entityName, layer] : reachableLayers)
+ for (auto& [entityName, layer] : reachableLayers)
{
layers.push_back(layer);
layers.push_back(unreachableLayers.at(entityName));
@@ -778,23 +1085,27 @@ namespace armarx::armem::server::grasp
arviz.commit(layers);
}
- void armarx::armem::server::grasp::GraspMemory::addInstanceToVisu(const armarx::armem::MemoryID &instance)
+ void
+ armarx::armem::server::grasp::GraspMemory::addInstanceToVisu(
+ const armarx::armem::MemoryID& instance)
{
std::unique_lock lock(gui.visualizationMutex);
- auto position = std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance);
+ auto position =
+ std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance);
- if(position == gui.visibleInstanceIds.end())
+ if (position == gui.visibleInstanceIds.end())
{
gui.visibleInstanceIds.push_back(instance);
}
-
}
-
- void armarx::armem::server::grasp::GraspMemory::removeInstanceFromVisu(const armarx::armem::MemoryID &instance)
+ void
+ armarx::armem::server::grasp::GraspMemory::removeInstanceFromVisu(
+ const armarx::armem::MemoryID& instance)
{
std::unique_lock lock(gui.visualizationMutex);
- auto position = std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance);
+ auto position =
+ std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance);
if (position != gui.visibleInstanceIds.end())
{
@@ -804,8 +1115,10 @@ namespace armarx::armem::server::grasp
std::string entityName = instance.entityName;
- if (std::none_of(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), [&entityName](const armem::MemoryID& id)
- { return id.entityName == entityName; }))
+ if (std::none_of(gui.visibleInstanceIds.begin(),
+ gui.visibleInstanceIds.end(),
+ [&entityName](const armem::MemoryID& id)
+ { return id.entityName == entityName; }))
{
arviz.commitDeleteLayer(entityName + "_reachable");
arviz.commitDeleteLayer(entityName + "_unreachable");
@@ -819,7 +1132,8 @@ namespace armarx::armem::server::grasp
}
}
- void GraspMemory::addToHighlightLayer(const MemoryID &memoryID, const std::string color)
+ void
+ GraspMemory::addToHighlightLayer(const MemoryID& memoryID, const std::string color)
{
viz::Color handColor;
@@ -855,39 +1169,33 @@ namespace armarx::armem::server::grasp
}
else if (memoryID.hasTimestamp())
{
- workingMemory().getSnapshot(memoryID).forEachInstance([&instances](const auto & instance)
- {
- instances.push_back(instance.id());
- });
+ workingMemory().getSnapshot(memoryID).forEachInstance(
+ [&instances](const auto& instance) { instances.push_back(instance.id()); });
}
else if (memoryID.hasEntityName())
{
- workingMemory().getEntity(memoryID).forEachInstance([&instances](const auto & instance)
- {
- instances.push_back(instance.id());
- });
+ workingMemory().getEntity(memoryID).forEachInstance(
+ [&instances](const auto& instance) { instances.push_back(instance.id()); });
}
else if (memoryID.hasProviderSegmentName())
{
- workingMemory().getProviderSegment(memoryID).forEachInstance([&instances](const auto & instance)
- {
- instances.push_back(instance.id());
- });
+ workingMemory().getProviderSegment(memoryID).forEachInstance(
+ [&instances](const auto& instance) { instances.push_back(instance.id()); });
}
else
{
- //currently only visualization for CoreSegment GraspCandidate available
- workingMemory().getCoreSegment("GraspCandidate").forEachInstance([&instances](const auto & instance)
- {
- instances.push_back(instance.id());
- });
+ //currently only visualization for CoreSegment GraspCandidate available
+ workingMemory()
+ .getCoreSegment("GraspCandidate")
+ .forEachInstance([&instances](const auto& instance)
+ { instances.push_back(instance.id()); });
}
armarx::grasping::GraspCandidate candidate;
armarx::grasping::arondto::GraspCandidate aronTransform;
- for (armem::MemoryID &instance : instances)
+ for (armem::MemoryID& instance : instances)
{
aronTransform.fromAron(workingMemory().getInstance(instance).data());
@@ -900,20 +1208,20 @@ namespace armarx::armem::server::grasp
}
arviz.commit(highlightLayer);
-
}
- void GraspMemory::RemoteGui_update()
+ void
+ GraspMemory::RemoteGui_update()
{
-// if (gui.tab.selectCoreSegment.hasValueChanged())
-// {
-// gui.coreSegment = gui.tab.selectCoreSegment.getValue();
-// gui.coreSegIndex = gui.tab.selectCoreSegment.getIndex();
-// gui.providerIndex = 0;
-// gui.entityIndex = 0;
-// gui.snapshotIndex = 0;
-// gui.tab.rebuild = true;
-// }
+ // if (gui.tab.selectCoreSegment.hasValueChanged())
+ // {
+ // gui.coreSegment = gui.tab.selectCoreSegment.getValue();
+ // gui.coreSegIndex = gui.tab.selectCoreSegment.getIndex();
+ // gui.providerIndex = 0;
+ // gui.entityIndex = 0;
+ // gui.snapshotIndex = 0;
+ // gui.tab.rebuild = true;
+ // }
if (gui.tab.selectProvider.hasValueChanged())
{
@@ -939,37 +1247,37 @@ namespace armarx::armem::server::grasp
gui.tab.rebuild = true;
}
- if(gui.tab.showUnlimitedInstances.hasValueChanged())
+ if (gui.tab.showUnlimitedInstances.hasValueChanged())
{
gui.unlimitedInstances = gui.tab.showUnlimitedInstances.getValue();
gui.tab.rebuild = true;
}
- if(gui.tab.maxInstances.hasValueChanged())
+ if (gui.tab.maxInstances.hasValueChanged())
{
gui.maxInstances = gui.tab.maxInstances.getValue();
gui.tab.rebuild = true;
}
- if(gui.tab.selectAll.wasClicked())
+ if (gui.tab.selectAll.wasClicked())
{
- for(auto & element : gui.tab.checkInstances)
+ for (auto& element : gui.tab.checkInstances)
{
element.second.setValue(true);
}
}
- if(gui.tab.deselectAll.wasClicked())
+ if (gui.tab.deselectAll.wasClicked())
{
- for(auto & element : gui.tab.checkInstances)
+ for (auto& element : gui.tab.checkInstances)
{
element.second.setValue(false);
}
}
- for(auto & element : gui.tab.checkInstances)
+ for (auto& element : gui.tab.checkInstances)
{
- if(element.second.hasValueChanged())
+ if (element.second.hasValueChanged())
{
if (element.second.getValue())
{
@@ -979,7 +1287,6 @@ namespace armarx::armem::server::grasp
{
removeInstanceFromVisu(armem::MemoryID::fromString(element.first));
}
-
}
}
@@ -992,4 +1299,4 @@ namespace armarx::armem::server::grasp
}
}
-}
+} // namespace armarx::armem::server::grasp
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index 20e0055141b0d679455c77b290b661da68fdf227..40ef25686fc4c8b545bd0aa49ed787c4035ab7c5 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -2,13 +2,19 @@
#include <memory>
+#include <VirtualRobot/Robot.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
+
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h"
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
-
#include <RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h>
namespace armarx::armem::server::grasp
@@ -25,13 +31,13 @@ namespace armarx::armem::server::grasp
* Detailed description of class GraspMemory.
*/
class GraspMemory :
- virtual public armarx::Component
- , virtual public armem::server::ReadWritePluginUser
- , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- , virtual public armarx::ArVizComponentPluginUser
+ virtual public armarx::Component,
+ virtual public armem::server::ReadWritePluginUser,
+ virtual public armarx::LightweightRemoteGuiComponentPluginUser,
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public armarx::ObjectPoseClientPluginUser
{
public:
-
GraspMemory();
/// @see armarx::ManagedIceObject::getDefaultName()
@@ -39,19 +45,21 @@ namespace armarx::armem::server::grasp
public:
- armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current&) override;
+ armem::data::CommitResult commit(const armem::data::Commit& commit,
+ const Ice::Current&) override;
public:
- armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& inputs) override;
- armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& inputs) override;
+ armem::actions::GetActionsOutputSeq
+ getActions(const armem::actions::GetActionsInputSeq& inputs) override;
+ armem::actions::ExecuteActionOutputSeq
+ executeActions(const armem::actions::ExecuteActionInputSeq& inputs) override;
void createRemoteGuiTab();
void RemoteGui_update() override;
protected:
-
PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -61,7 +69,6 @@ namespace armarx::armem::server::grasp
private:
-
DebugObserverInterfacePrx debugObserver;
// segments
@@ -82,7 +89,6 @@ namespace armarx::armem::server::grasp
std::map<std::string, RemoteGui::Client::CheckBox> checkInstances;
};
-
struct GuiInfo
{
// int coreSegIndex = 0;
@@ -108,13 +114,31 @@ namespace armarx::armem::server::grasp
std::vector<armem::MemoryID> trackedEntityIds;
bool trackNewEntities{true};
};
+
GuiInfo gui;
bool enableRemoteGui{true};
+ bool enableVisualizeKnownGraspCandidates = false;
+ std::size_t frequencyHzVisualizeKnownGraspCandidates = 5;
+
void visualizeGraspCandidates();
+ void visualizeKnownGraspCandidates();
+
void addInstanceToVisu(const armem::MemoryID& instance);
void removeInstanceFromVisu(const armem::MemoryID& instance);
void addToHighlightLayer(const armem::MemoryID& memoryID, const std::string color);
+
+
+ armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>*
+ virtualRobotReaderPlugin = nullptr;
+
+
+ std::map<std::string, VirtualRobot::RobotPtr> robots;
+
+ RunningTask<GraspMemory>::pointer_type taskVisuKnownGrasps;
+
+ std::map<std::string, viz::Layer> graspVisuPerObject;
+
};
-}
+} // namespace armarx::armem::server::grasp
diff --git a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
index 3247c4cc45133132a1346fa42b46166c183a60ad..f143e1436df1b25e521aa3a3b98ae03caf2afb57 100644
--- a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
@@ -1,21 +1,29 @@
#include "MemoryNameSystem.h"
-
namespace armarx::armem::mns
{
// dto::WaitForServerResult MemoryNameSystem::waitForServer(const dto::WaitForServerInput& input)
- void MemoryNameSystem::waitForServer_async(
+ void
+ MemoryNameSystem::waitForServer_async(
const AMD_MemoryNameSystemInterface_waitForServerPtr& future,
const dto::WaitForServerInput& input)
{
- waitForServerFutures[input.name].push_back(future);
+ ARMARX_INFO << "Waiting for server `" << input.name << "`.";
+
+ {
+ std::lock_guard g{futuresMtx};
+ waitForServerFutures[input.name].push_back(future);
+ }
+
waitForServer_processOnce();
}
-
- void MemoryNameSystem::waitForServer_processOnce()
+ void
+ MemoryNameSystem::waitForServer_processOnce()
{
+ std::lock_guard g{futuresMtx};
+
for (auto it = waitForServerFutures.begin(); it != waitForServerFutures.end();)
{
auto& [name, futures] = *it;
@@ -32,16 +40,20 @@ namespace armarx::armem::mns
{
future->ice_response(result);
}
+
+ // it: iterator following the last removed element (https://en.cppreference.com/w/cpp/container/map/erase)
it = waitForServerFutures.erase(it);
}
else
{
- ++it; // Skip.
+ ++it; // Skip.
+ ARMARX_INFO << "Server `" << name << "` not available yet.";
}
}
}
-
- dto::RegisterServerResult MemoryNameSystem::registerServer(const dto::RegisterServerInput& input)
+
+ dto::RegisterServerResult
+ MemoryNameSystem::registerServer(const dto::RegisterServerInput& input)
{
const auto result = Registry::registerServer(input);
waitForServer_processOnce();
@@ -49,8 +61,8 @@ namespace armarx::armem::mns
return result;
}
-
- armarx::RemoteGui::Client::GridLayout MemoryNameSystem::RemoteGui_buildInfoGrid()
+ armarx::RemoteGui::Client::GridLayout
+ MemoryNameSystem::RemoteGui_buildInfoGrid()
{
using namespace armarx::RemoteGui::Client;
@@ -58,10 +70,9 @@ namespace armarx::armem::mns
int row = 0;
grid.add(Label("Memory Name"), {row, 0})
- .add(Label("Component Name"), {row, 1})
- .add(Label("R/W/P/A"), {row, 2})
- .add(Label("Registration Time"), {row, 3})
- ;
+ .add(Label("Component Name"), {row, 1})
+ .add(Label("R/W/P/A"), {row, 2})
+ .add(Label("Registration Time"), {row, 3});
row++;
for (const auto& [name, info] : servers)
@@ -109,4 +120,4 @@ namespace armarx::armem::mns
return grid;
}
-}
+} // namespace armarx::armem::mns
diff --git a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h
index e0cf5b88753cb4a96caad2c004a853dcd4d11a34..9ca9cc260ead437543d8cc2903f42fceb9009e0b 100644
--- a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h
+++ b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h
@@ -1,14 +1,14 @@
#pragma once
-#include "Registry.h"
-
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <map>
+#include <mutex>
+#include <string>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include <map>
-#include <string>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include "Registry.h"
namespace armarx::armem::mns
{
@@ -16,18 +16,15 @@ namespace armarx::armem::mns
class MemoryNameSystem : public Registry
{
public:
-
using WaitForServerFuturePtr = AMD_MemoryNameSystemInterface_waitForServerPtr;
public:
-
/**
* @brief Store the call in a container for later response.
*/
- void waitForServer_async(
- const AMD_MemoryNameSystemInterface_waitForServerPtr& future,
- const dto::WaitForServerInput& input);
+ void waitForServer_async(const AMD_MemoryNameSystemInterface_waitForServerPtr& future,
+ const dto::WaitForServerInput& input);
void waitForServer_processOnce();
@@ -38,11 +35,11 @@ namespace armarx::armem::mns
armarx::RemoteGui::Client::GridLayout RemoteGui_buildInfoGrid();
- public:
-
+ private:
/// Queued calls to `waitForServer`.
std::map<std::string, std::vector<WaitForServerFuturePtr>> waitForServerFutures;
+ std::mutex futuresMtx;
};
-}
+} // namespace armarx::armem::mns
diff --git a/source/RobotAPI/libraries/armem/mns/Registry.h b/source/RobotAPI/libraries/armem/mns/Registry.h
index 3167bf841ba33227a3e5ec2864720e4f5d1610b9..e95f0ba9cd8f6589cfb552ab7e4e25db1923f7aa 100644
--- a/source/RobotAPI/libraries/armem/mns/Registry.h
+++ b/source/RobotAPI/libraries/armem/mns/Registry.h
@@ -1,18 +1,16 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/Time.h>
+#include <map>
+#include <string>
+
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/ActionsInterface.h>
#include <RobotAPI/interface/armem/server/PredictingMemoryInterface.h>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.h>
-
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include <map>
-#include <string>
-
+#include <RobotAPI/libraries/armem/core/Time.h>
namespace armarx::armem::mns
{
@@ -20,10 +18,9 @@ namespace armarx::armem::mns
/**
* @brief A registry for memory servers.
*/
- class Registry : armarx::Logging
+ class Registry : public armarx::Logging
{
public:
-
Registry(const std::string& logTag = "MemoryNameSystem Registry");
@@ -53,7 +50,6 @@ namespace armarx::armem::mns
public:
-
/// Information about a memory entry.
struct ServerInfo
{
@@ -64,7 +60,6 @@ namespace armarx::armem::mns
/// The registered memories.
std::map<std::string, ServerInfo> servers;
-
};
-}
+} // namespace armarx::armem::mns
diff --git a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
index ff955d28e1778f3d30fa28ca9dc56345b44273bd..7fc46a57672e594333de3d178655c326bffce5da 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
@@ -193,7 +193,7 @@ namespace armarx::armem::server::ltm
}
else
{
- ARMARX_INFO << "CoreSegment does not have aron type, so aron type information "
+ ARMARX_DEBUG << "CoreSegment does not have aron type, so aron type information "
"cannot be exported";
/*writeForeignKeyToPreviousDocument();*/
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
index f568b32137ab22872076c72c32e6078b03a2e99c..7d2cabfc613dea511df6bf91e8b76a12a8a770cd 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
@@ -193,7 +193,7 @@ namespace armarx::armem::server::ltm
}
else
{
- ARMARX_INFO << "ProviderSegment does not seem to have an aron type, so aron type information connot be exported";
+ ARMARX_DEBUG << "ProviderSegment does not seem to have an aron type, so aron type information connot be exported";
//writeForeignKeyToPreviousDocument();
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
index 0a17f6adcac3f7d491e55047247b83deb10f3aa9..2f3ab8022851c7fa5a66ef3d263b6d4b0189e3c6 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
@@ -108,7 +108,7 @@ namespace armarx::armem::server::ltm::detail::mixin
if (to_store->empty())
{
- ARMARX_INFO << deactivateSpam() << "Cannot store an empty buffer. Ignoring.";
+ ARMARX_DEBUG << deactivateSpam(120) << "Cannot store an empty buffer. Ignoring.";
return;
}
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index a3a55ac0fafa59c5b0997dc08b4ac2065b616a15..c805d2ee70a55356bfd46c463cd2f9b315aec583 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -1,26 +1,33 @@
#include "KnownGraspProviderSegment.h"
+
#include <VirtualRobot/Grasping/GraspSet.h>
#include <VirtualRobot/XML/ObjectIO.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
namespace armarx::armem::grasping::segment
{
- KnownGraspProviderSegment::KnownGraspProviderSegment(armem::server::MemoryToIceAdapter& iceMemory) :
+
+ KnownGraspProviderSegment::KnownGraspProviderSegment(
+ armem::server::MemoryToIceAdapter& iceMemory) :
Base(iceMemory, PROVIDER_SEGMENT_NAME, CORE_SEGMENT_NAME)
{
-
}
- void KnownGraspProviderSegment::init()
+ void
+ KnownGraspProviderSegment::init()
{
Base::init();
loadMemory();
}
- std::optional<arondto::KnownGraspInfo> KnownGraspProviderSegment::knownGraspInfoFromObjectInfo(const ObjectInfo& info)
+ std::optional<arondto::KnownGraspInfo>
+ KnownGraspProviderSegment::knownGraspInfoFromObjectInfo(const ObjectInfo& info)
{
std::string objectClassName = info.className();
auto fileLocInfo = info.file(".xml", "_Grasps");
@@ -29,10 +36,12 @@ namespace armarx::armem::grasping::segment
if (std::filesystem::is_regular_file(graspFilePath))
{
ARMARX_INFO << "loading " << graspFilePath;
- try {
- auto manipulationObject = VirtualRobot::ObjectIO::loadManipulationObject(graspFilePath);
-
- if(manipulationObject == nullptr)
+ try
+ {
+ auto manipulationObject =
+ VirtualRobot::ObjectIO::loadManipulationObject(graspFilePath);
+
+ if (manipulationObject == nullptr)
{
ARMARX_WARNING << "Invalid file content: " << graspFilePath;
return std::nullopt;
@@ -46,7 +55,8 @@ namespace armarx::armem::grasping::segment
ret.xml.package = fileLocInfo.package;
ret.xml.path = fileLocInfo.relativePath;
- for (const VirtualRobot::GraspSetPtr& graspSet : manipulationObject->getAllGraspSets())
+ for (const VirtualRobot::GraspSetPtr& graspSet :
+ manipulationObject->getAllGraspSets())
{
ARMARX_CHECK_NOT_NULL(graspSet);
@@ -67,23 +77,35 @@ namespace armarx::armem::grasping::segment
retGrasp.creator = grasp->getCreationMethod();
retGrasp.pose = grasp->getTransformation();
- ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
+ ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '"
+ << retGraspSet.name << "' for obj '" << objectClassName
+ << "' with pose \n"
+ << retGrasp.pose;
retGraspSet.grasps.push_back(retGrasp);
}
- ret.graspSets[retGraspSet.name] = retGraspSet;
+
+ ARMARX_CHECK(ret.graspSets.count(retGraspSet.robot + "/" + retGraspSet.name) ==
+ 0)
+ << "The grasp set `" << retGraspSet.robot + "/" + retGraspSet.name
+ << "` was defined twice!";
+
+ ret.graspSets[retGraspSet.robot + "/" + retGraspSet.name] = retGraspSet;
}
return ret;
-
- } catch (...) {
- ARMARX_WARNING << graspFilePath << " is not a manipulation object!";
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << graspFilePath << " is not a manipulation object!"
+ << GetHandledExceptionString();
return std::nullopt;
}
}
return std::nullopt;
}
- void KnownGraspProviderSegment::loadMemory()
+ void
+ KnownGraspProviderSegment::loadMemory()
{
// load data from prior knowledge
ObjectFinder objectFinder;
@@ -92,8 +114,10 @@ namespace armarx::armem::grasping::segment
const bool checkPaths = false;
std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
- const MemoryID providerID = segmentPtr->id().withProviderSegmentName(objectFinder.getPackageName());
- ARMARX_INFO << "Checking up to " << infos.size() << " object classes from '" << objectFinder.getPackageName() << "' ...";
+ const MemoryID providerID =
+ segmentPtr->id().withProviderSegmentName(objectFinder.getPackageName());
+ ARMARX_INFO << "Checking up to " << infos.size() << " object classes from '"
+ << objectFinder.getPackageName() << "' ...";
Commit commit;
for (ObjectInfo& info : infos)
@@ -103,19 +127,29 @@ namespace armarx::armem::grasping::segment
{
EntityUpdate& update = commit.add();
update.entityID = providerID.withEntityName(info.id().str());
- update.entityID.timestamp = update.arrivedTime = update.referencedTime = update.sentTime = now;
+ update.entityID.timestamp = update.arrivedTime = update.referencedTime =
+ update.sentTime = now;
- update.instancesData =
+ update.instancesData = {knownGraspCandidate->toAron()};
+
+ ARMARX_VERBOSE << VAROUT(knownGraspCandidate->graspSets.size());
+ for (const auto& gs : knownGraspCandidate->graspSets)
{
- knownGraspCandidate->toAron()
- };
+ ARMARX_VERBOSE << VAROUT(gs.second.grasps.size());
+ for (const auto& grasp : gs.second.grasps)
+ {
+ ARMARX_VERBOSE << VAROUT(grasp.name);
+ }
+ }
}
}
- ARMARX_INFO << "Loaded " << commit.updates.size() << " grasp candidates from object classes from '" << objectFinder.getPackageName() << "'.";
+ ARMARX_INFO << "Loaded " << commit.updates.size()
+ << " grasp candidates from object classes from '"
+ << objectFinder.getPackageName() << "'.";
auto result = iceMemory.commit(commit);
if (!result.allSuccess())
{
ARMARX_WARNING << "Got errors for commit: " << result.allErrorMessages();
}
}
-}
+} // namespace armarx::armem::grasping::segment