From 98abe3789d9cacfbe7dc28deb2bb4096df1b079b Mon Sep 17 00:00:00 2001
From: Michel Breyer <michel.p.breyer@gmail.com>
Date: Mon, 30 Aug 2021 12:51:38 +0200
Subject: [PATCH] Small touch to the visualization

---
 active_grasp/nbv.py           |  1 +
 active_grasp/policy.py        |  2 ++
 active_grasp/visualization.py | 10 +++++++---
 3 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/active_grasp/nbv.py b/active_grasp/nbv.py
index 257e185..5eb3d51 100644
--- a/active_grasp/nbv.py
+++ b/active_grasp/nbv.py
@@ -24,6 +24,7 @@ class NextBestView(MultiViewPolicy):
         nbv, ig = views[i], gains[i]
 
         if self.check_stopping_criteria(ig):
+            self.vis.clear_views()
             self.done = True
         else:
             return nbv.inv()  # the controller expects T_cam_base
diff --git a/active_grasp/policy.py b/active_grasp/policy.py
index a63c1f7..d02c39d 100644
--- a/active_grasp/policy.py
+++ b/active_grasp/policy.py
@@ -116,6 +116,8 @@ class MultiViewPolicy(Policy):
 
         if len(grasps) > 0:
             self.best_grasp = grasps[0]
+        else:
+            self.best_grasp = None
 
         self.vis.grasps(self.base_frame, grasps, scores)
 
diff --git a/active_grasp/visualization.py b/active_grasp/visualization.py
index e5cd70f..ae2076c 100644
--- a/active_grasp/visualization.py
+++ b/active_grasp/visualization.py
@@ -35,6 +35,9 @@ class Visualizer:
         self.quality_pub.publish(msg)
         rospy.sleep(0.1)
 
+    def clear_views(self):
+        self.draw([Marker(action=Marker.DELETE, ns="views")])
+
     def draw(self, markers):
         self.marker_pub.publish(MarkerArray(markers=markers))
 
@@ -45,14 +48,14 @@ class Visualizer:
         marker = create_cube_marker(frame, pose, scale, color, ns="bbox")
         self.draw([marker])
 
-    def grasps(self, frame, grasps, scores):
+    def grasps(self, frame, grasps, scores, smin=0.9, smax=1.0, alpha=0.8):
         if len(grasps) == 0:
             return
 
-        smin, smax = min(scores), max(scores)
         markers = []
         for i, (grasp, score) in enumerate(zip(grasps, scores)):
             color = cmap((score - smin) / (smax - smin))
+            color = [color[0], color[1], color[2], alpha]
             markers += create_grasp_markers(frame, grasp, color, "grasps", 4 * i)
         self.draw(markers)
 
@@ -117,13 +120,14 @@ class Visualizer:
         msg = to_cloud_msg(frame, np.asarray(cloud.points))
         self.scene_cloud_pub.publish(msg)
 
-    def views(self, frame, intrinsic, views, values):
+    def views(self, frame, intrinsic, views, values, alpha=0.8):
         vmin, vmax = min(values), max(values)
         scale = [0.002, 0.0, 0.0]
         near, far = 0.0, 0.02
         markers = []
         for i, (view, value) in enumerate(zip(views, values)):
             color = cmap((value - vmin) / (vmax - vmin))
+            color = [color[0], color[1], color[2], alpha]
             marker = create_cam_view_marker(
                 frame,
                 view,