Visualization of Single Handed Grasp Candidates via MemoryServer (WIP)

source/RobotAPI/components/armem/server/GraspMemory/CMakeLists.txt

@@ -6,7 +6,7 @@ set(COMPONENT_LIBS
     ArmarXGuiComponentPlugins
     RobotAPICore RobotAPIInterfaces armem
     RobotAPIComponentPlugins  # for ArViz and other plugins
-
+    GraspingUtility
     ${IVT_LIBRARIES}
 )
 

source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp

@@ -9,6 +9,8 @@
 #include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
 
 #include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
+#include <RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h>
+#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
 
 namespace armarx::armem::server::grasp
 {
@@ -79,20 +81,57 @@ namespace armarx::armem::server::grasp
     void GraspMemory::createRemoteGuiTab()
     {
         using namespace armarx::RemoteGui::Client;
-
+        GridLayout root;
         {
 
-            tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory());
+            int row = 0;
+
+            workingMemory().getCoreSegment("GraspCandidate").forEachInstance([this](const auto & instance)
+            {
+                if (tab.checkBoxes.find(instance.id().str()) == tab.checkBoxes.end()){
+                    tab.checkBoxes[instance.id().str()] = CheckBox();
+                }             
+            });
+            for(std::pair<std::string, CheckBox> element : tab.checkBoxes )
+            {
+                root.add(Label(element.first) , Pos{row, 0});
+                root.add(element.second, Pos{row, 1});
+                row += 1;
+            }
         }
 
-        VBoxLayout root = {tab.memoryGroup, VSpacer()};
         RemoteGui_createTab(getName(), root, &tab);
+        tab.rebuild = false;
     }
 
 
     void GraspMemory::RemoteGui_update()
     {
-        if (tab.rebuild.exchange(false))
+        grasping::GraspCandidateVisu visu = grasping::GraspCandidateVisu();
+
+        grasping::GraspCandidateDict candidatesToVisualize;
+
+        for(std::pair<std::string, RemoteGui::Client::CheckBox> element : tab.checkBoxes )
+        {
+            //draw candidates
+            if (element.second.getValue())
+            {
+                armarx::grasping::GraspCandidate candidate;
+
+                grasping::arondto::GraspCandidate aronTransform;
+
+                armem::wm::EntityInstance instance = workingMemory().getInstance(armem::MemoryID::fromString(element.first));
+
+                aronTransform.fromAron(workingMemory().getInstance(armem::MemoryID::fromString(element.first)).data());
+
+                fromAron(aronTransform, candidate);
+
+                candidatesToVisualize[element.first] = new grasping::GraspCandidate(candidate);
+            }
+        }
+        visu.visualize(candidatesToVisualize, arviz);
+
+        if (tab.rebuild)
         {
             createRemoteGuiTab();
         }

source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h

@@ -66,6 +66,7 @@ namespace armarx::armem::server::grasp
             std::atomic_bool rebuild = false;
 
             RemoteGui::Client::GroupBox memoryGroup;
+            std::map<std::string, RemoteGui::Client::CheckBox> checkBoxes;
         };
         RemoteGuiTab tab;
 

source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp

@@ -30,7 +30,7 @@ namespace armarx::grasping
     {
         for (auto& [id, candidate] : candidates)
         {
-            int alpha = alpha;
+            int alpha = alpha_default;
             if (auto it = alphasByKey.find(id); it != alphasByKey.end())
             {
                 alpha = it->second;
@@ -54,7 +54,7 @@ namespace armarx::grasping
             const std::string& name,
             const GraspCandidate& candidate)
     {
-        return visualize(name, candidate, alpha);
+        return visualize(name, candidate, alpha_default);
     }
 
 
@@ -69,9 +69,9 @@ namespace armarx::grasping
         color.a = alpha;
 
         Eigen::Matrix4f tcp2handRoot = fromIce(candidate.tcpPoseInHandRoot).inverse();
-
         Eigen::Matrix4f graspPose = PosePtr::dynamicCast(candidate.graspPose)->toEigen();
         std::string modelFile = "Armar6RT/robotmodel/Armar6-SH/Armar6-" + candidate.side + "Hand-v3.xml";
+
         viz::Robot hand = viz::RobotHand(name)
                 .file("Armar6RT", modelFile)
                 .pose(fromIce(candidate.robotPose) * graspPose * tcp2handRoot)

source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h

@@ -34,7 +34,7 @@ namespace armarx::grasping
 
     public:
 
-        int alpha = 255;
+        int alpha_default = 255;
         std::map<std::string, int> alphasByKey = {};