From 7584b4c210a492c8df0cd70e17082b0a2ae708bd Mon Sep 17 00:00:00 2001
From: alissa <alissamueller@outlook.de>
Date: Sun, 22 May 2022 15:02:22 +0200
Subject: [PATCH] Added action for highlighting grasp candidates
---
.../armem/server/GraspMemory/GraspMemory.cpp | 105 +++++++++++++++++-
.../armem/server/GraspMemory/GraspMemory.h | 1 +
.../aron/converter/eigen/EigenConverter.h | 2 +-
3 files changed, 104 insertions(+), 4 deletions(-)
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index dc4a356c9..f0f9ba77b 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -180,9 +180,16 @@ namespace armarx::armem::server::grasp
}
}
-
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(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() });
@@ -215,7 +222,6 @@ namespace armarx::armem::server::grasp
}
else
{
-
if (memoryID.hasInstanceIndex())
{
if (input.actionPath == ActionPath{"vis"})
@@ -226,6 +232,7 @@ namespace armarx::armem::server::grasp
{
removeInstanceFromVisu(memoryID);
}
+
}
else if (memoryID.hasTimestamp())
{
@@ -352,6 +359,14 @@ namespace armarx::armem::server::grasp
}
}
}
+ else if(input.actionPath.front() == "high")
+ {
+ addToHighlightLayer(memoryID, input.actionPath.back());
+ }
+ else if(input.actionPath == ActionPath{"reset"})
+ {
+ arviz.commit(arviz.layer("HighlightedGrasps"));
+ }
else
{
std::stringstream sstream;
@@ -761,6 +776,90 @@ namespace armarx::armem::server::grasp
}
}
+ void GraspMemory::addToHighlightLayer(const MemoryID &memoryID, const std::string color)
+ {
+ viz::Color handColor;
+
+ if (color == "pink")
+ {
+ handColor = viz::Color::pink();
+ }
+ else if (color == "red")
+ {
+ handColor = viz::Color::red();
+ }
+ else if (color == "blue")
+ {
+ handColor = viz::Color::blue();
+ }
+ else if (color == "yellow")
+ {
+ handColor = viz::Color::yellow();
+ }
+ else if (color == "purple")
+ {
+ handColor = viz::Color::purple();
+ }
+
+ viz::Layer highlightLayer = arviz.layer(("HighlightedGrasps"));
+ armarx::grasping::GraspCandidateVisu visu;
+
+ std::vector<armem::MemoryID> instances;
+
+ if (memoryID.hasInstanceIndex())
+ {
+ instances.push_back(memoryID);
+ }
+ else if (memoryID.hasTimestamp())
+ {
+ 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());
+ });
+ }
+ else if (memoryID.hasProviderSegmentName())
+ {
+ 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());
+ });
+ }
+
+ armarx::grasping::GraspCandidate candidate;
+
+ armarx::grasping::arondto::GraspCandidate aronTransform;
+
+ for (armem::MemoryID &instance : instances)
+ {
+ aronTransform.fromAron(workingMemory().getInstance(instance).data());
+
+ fromAron(aronTransform, candidate);
+
+
+ viz::Robot hand = visu.visualize(instance.str(), candidate);
+ hand.color(handColor);
+ highlightLayer.add(hand);
+ }
+
+ arviz.commit(highlightLayer);
+
+ }
+
void GraspMemory::RemoteGui_update()
{
// if (gui.tab.selectCoreSegment.hasValueChanged())
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index 47960bcf7..1aa999b62 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -112,5 +112,6 @@ namespace armarx::armem::server::grasp
void visualizeGraspCandidates();
void addInstanceToVisu(const armem::MemoryID& instance);
void removeInstanceFromVisu(const armem::MemoryID& instance);
+ void addToHighlightLayer(const armem::MemoryID& memoryID, const std::string color);
};
}
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 8d6ccf594..d1e5c6cd5 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -129,7 +129,7 @@ namespace armarx::aron::converter
auto dims = nav.getShape();
Eigen::Matrix<T, Rows, Cols> ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), sizeof(T), std::multiplies<int>()));
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
return ret;
}
--
GitLab