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]