diff --git a/active_grasp/policy.py b/active_grasp/policy.py index 63ba005..e257321 100644 --- a/active_grasp/policy.py +++ b/active_grasp/policy.py @@ -147,6 +147,8 @@ class MultiViewPolicy(Policy): grasps = select_grid(voxel_size, out, threshold=self.qual_threshold) grasps, scores = self.sort_grasps(grasps, q) + self.vis.clear_grasps() + if len(grasps) > 0: smin, smax = np.min(scores), np.max(scores) self.best_grasp = grasps[0] @@ -154,7 +156,6 @@ class MultiViewPolicy(Policy): self.vis.best_grasp(self.base_frame, grasps[0], scores[0], smin, smax) else: self.best_grasp = None - # TODO clear grasp markers def compute_error(x_d, x): diff --git a/active_grasp/simulation.py b/active_grasp/simulation.py index 59eef06..8537d5c 100644 --- a/active_grasp/simulation.py +++ b/active_grasp/simulation.py @@ -126,7 +126,7 @@ def find_urdfs(root): class RandomScene(Scene): def __init__(self): super().__init__() - self.center = np.r_[0.6, 0.0, 0.1] + self.center = np.r_[0.5, 0.0, 0.1] self.length = 0.3 self.origin = self.center - np.r_[0.5 * self.length, 0.5 * self.length, 0.0] self.object_urdfs = find_urdfs(self.vgn_urdfs_dir / "packed" / "test") diff --git a/active_grasp/visualization.py b/active_grasp/visualization.py index ce429d6..f9599d7 100644 --- a/active_grasp/visualization.py +++ b/active_grasp/visualization.py @@ -30,6 +30,8 @@ class Visualizer: self.pose_pub = rospy.Publisher("pose", PoseStamped, queue_size=1) self.quality_pub = rospy.Publisher("quality", PointCloud2, queue_size=1) + self.grasp_marker_count = 0 + def clear(self): self.clear_markers() msg = to_cloud_msg("panda_link0", np.array([])) @@ -41,10 +43,6 @@ class Visualizer: def clear_markers(self): self.draw([Marker(action=Marker.DELETEALL)]) - def clear_views(self, n): - markers = [Marker(action=Marker.DELETE, ns="views", id=i) for i in range(n)] - self.draw(markers) - def draw(self, markers): self.marker_pub.publish(MarkerArray(markers=markers)) @@ -82,8 +80,21 @@ class Visualizer: 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.grasp_marker_count = len(markers) self.draw(markers) + def clear_grasps(self): + if self.grasp_marker_count > 0: + markers = [ + Marker(action=Marker.DELETE, ns="grasps", id=i) + for i in range(self.grasp_marker_count) + ] + markers += [ + Marker(action=Marker.DELETE, ns="best_grasp", id=i) for i in range(4) + ] + self.draw(markers) + self.grasp_marker_count = 0 + def rays(self, frame, origin, directions, t_max=1.0): lines = [[origin, origin + t_max * direction] for direction in directions] marker = create_line_list_marker(