From eca61e7297db64721f5faf465623f67a996f5133 Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Mon, 6 Dec 2021 10:22:39 +0100
Subject: [PATCH] Clear IG markers after exploring

---
 src/active_grasp/nbv.py  | 3 +++
 src/active_grasp/rviz.py | 4 ++++
 2 files changed, 7 insertions(+)

diff --git a/src/active_grasp/nbv.py b/src/active_grasp/nbv.py
index 443db27..ea15421 100644
--- a/src/active_grasp/nbv.py
+++ b/src/active_grasp/nbv.py
@@ -104,6 +104,9 @@ class NextBestView(MultiViewPolicy):
             if gain < self.min_gain and len(self.views) > self.T:
                 self.done = True
 
+            if self.done:
+                self.vis.clear_ig_views()
+
             self.x_d = nbv
 
     def best_grasp_prediction_is_stable(self):
diff --git a/src/active_grasp/rviz.py b/src/active_grasp/rviz.py
index 5b2410e..b71ba21 100644
--- a/src/active_grasp/rviz.py
+++ b/src/active_grasp/rviz.py
@@ -13,6 +13,10 @@ grey = [0.9, 0.9, 0.9]
 
 
 class Visualizer(vgn.rviz.Visualizer):
+    def clear_ig_views(self):
+        markers = [Marker(action=Marker.DELETE, ns="ig_views", id=i) for i in range(24)]
+        self.draw(markers)
+
     def bbox(self, frame, bbox):
         pose = Transform.identity()
         scale = [0.004, 0.0, 0.0]