hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Clear old grasp grasp markers

Browse Source
This commit is contained in:
Michel Breyer 2021-09-12 18:19:10 +02:00
parent f2a12efb0f
commit b41d70df66
3 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
active_grasp/policy.py
View File

@ -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):

2
active_grasp/simulation.py
View File

@ -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")

19
active_grasp/visualization.py
View File

@ -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(