From dbbd9955fd1ff4acda656a744c95ebebf65715c6 Mon Sep 17 00:00:00 2001
From: jean_patrick_mathes <uomnk@student.kit.edu>
Date: Thu, 29 Sep 2022 15:54:19 +0200
Subject: [PATCH] Add option to commit all changes to each object

---
 .../InteractiveMemoryEditor/Editor.cpp        | 40 ++++++-------------
 .../InteractiveMemoryEditor/Editor.h          |  9 ++---
 .../drivers/SickLaserUnit/SickScanAdapter.cpp |  2 +-
 3 files changed, 18 insertions(+), 33 deletions(-)

diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
index 13dc8abce..003b5d456 100644
--- a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
+++ b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
@@ -14,7 +14,7 @@ namespace armarx
         : client(client)
         , pushToMemory(std::move(pushToMemory))
         , pullFromMemory(std::move(pullFromMemory))
-        , isPushRequired(false)
+        , isCommitRequired(false)
         , isPullRequired(true)
         , isMemoryVizRequired(true)
         , isMetaVizRequired(true)
@@ -49,11 +49,11 @@ namespace armarx
             isMetaVizRequired = false;
         }
 
-        if (isPushRequired)
+        if (isCommitRequired)
         {
-            push();
+            commit();
 
-            isPushRequired = false;
+            isCommitRequired = false;
         }
 
         observer.requestInteractions(stage);
@@ -75,7 +75,7 @@ namespace armarx
         return newRequestedPoses;
     }
 
-    void Editor::push()
+    void Editor::commit()
     {
         changes.moveNewObjectsTo(storedPoses);
 
@@ -176,6 +176,10 @@ namespace armarx
                 changes.resetObject(objectPose);
                 isMemoryVizRequired = true;
             })
+            .onContextMenu(description.commitIndex, [&]
+            {
+                isCommitRequired = true;
+            })
             .onTransformEnd([&](const Eigen::Matrix4f& transform)
             {
                 changes.moveObject(objectPose, transform);
@@ -185,28 +189,7 @@ namespace armarx
 
     void Editor::visualizeMeta()
     {
-        const float size = 1000;
-
         observer.clearObservedLayer(metaLayer);
-
-        observer.addObserved(
-                metaLayer,
-                viz::Box("meta")
-                .pose(metaCubePose)
-                .size(size)
-                .color(simox::color::Color(1.0, 0.0, 0.0))
-                .enable(viz::interaction()
-                    .selection()
-                    .transform()
-                    .hideDuringTransform()
-                    .contextMenu({"Push", "Pull"})))
-            .onContextMenu(0, [this] { isPushRequired = true; })
-            .onContextMenu(1, [this] { isPullRequired = true; })
-            .onTransformEnd([&](const Eigen::Matrix4f& transform)
-                            {
-                                metaCubePose = transform * metaCubePose;
-                                isMetaVizRequired = true;
-                            });
     }
 
     void Editor::ChangeState::clear()
@@ -282,7 +265,7 @@ namespace armarx
             auto& [name, change] = *iterator;
 
             description.options.emplace_back("Reset");
-            description.resetIndex = currentIndex; // ++
+            description.resetIndex = currentIndex++;
 
             description.transform = change.transform;
 
@@ -304,6 +287,9 @@ namespace armarx
             }
         }
 
+        description.options.emplace_back("Commit All Changes");
+        description.commitIndex = currentIndex++;
+
         return description;
     }
 
diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
index cd2c5cf54..6e0fe6d32 100644
--- a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
+++ b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
@@ -46,6 +46,7 @@ namespace armarx
             size_t cloneIndex = std::numeric_limits<size_t>::max();
             size_t deleteIndex = std::numeric_limits<size_t>::max();
             size_t resetIndex = std::numeric_limits<size_t>::max();
+            size_t commitIndex = std::numeric_limits<size_t>::max();
         };
 
         class ChangeState
@@ -85,19 +86,17 @@ namespace armarx
 
         ChangeState changes;
 
-        bool isPushRequired;
+        bool isCommitRequired;
         bool isPullRequired;
         bool isMemoryVizRequired;
         bool isMetaVizRequired;
 
-        Eigen::Matrix4f metaCubePose {Eigen::Matrix4f::Identity()};
-
         void visualizeMemory();
         void visualizeMeta();
 
-        void push();
+        void commit();
         objpose::ObjectPoseSeq pull();
 
         void visualizeObject(objpose::ObjectPose &objectPose);
     };
-}
+}  // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 40250b2b1..5cabc89dd 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -685,7 +685,7 @@ namespace armarx
                 frame.getRawData() +
                 frame.size()));
         // recvQueue.push(std::vector<unsigned char>(frame.getRawData(), frame.getRawData() + frame.size()));
-        recvQueue.push(dataGramWidthTimeStamp);
+        recvQueue.commit(dataGramWidthTimeStamp);
     }
 
     void SickScanAdapter::readCallbackFunction(UINT8* buffer, UINT32& numOfBytes)
-- 
GitLab