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

Visualize best grasp

Browse Source
This commit is contained in:
Michel Breyer 2021-09-03 11:50:14 +02:00
parent 98abe3789d
commit f43688023b
3 changed files with 18 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
active_grasp/nbv.py
View File

@ -24,7 +24,7 @@ class NextBestView(MultiViewPolicy):
nbv, ig = views[i], gains[i] nbv, ig = views[i], gains[i]
if self.check_stopping_criteria(ig): if self.check_stopping_criteria(ig):
self.vis.clear_views() self.vis.clear_views(len(views))
self.done = True self.done = True
else: else:
return nbv.inv() # the controller expects T_cam_base return nbv.inv() # the controller expects T_cam_base

7
active_grasp/policy.py
View File

@ -88,7 +88,7 @@ class SingleViewPolicy(Policy):
out = self.vgn.predict(tsdf_grid) out = self.vgn.predict(tsdf_grid)
self.vis.quality(self.task_frame, voxel_size, out.qual) self.vis.quality(self.task_frame, voxel_size, out.qual)
grasps = select_grid(voxel_size, out, threshold=0.95) grasps = select_grid(voxel_size, out, threshold=0.90)
grasps, scores = self.sort_grasps(grasps) grasps, scores = self.sort_grasps(grasps)
self.vis.grasps(self.base_frame, grasps, scores) self.vis.grasps(self.base_frame, grasps, scores)
@ -111,13 +111,12 @@ class MultiViewPolicy(Policy):
tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size
out = self.vgn.predict(tsdf_grid) out = self.vgn.predict(tsdf_grid)
grasps = select_grid(voxel_size, out, threshold=0.95) grasps = select_grid(voxel_size, out, threshold=0.90)
grasps, scores = self.sort_grasps(grasps) grasps, scores = self.sort_grasps(grasps)
if len(grasps) > 0: if len(grasps) > 0:
self.best_grasp = grasps[0] self.best_grasp = grasps[0]
else: self.vis.best_grasp(self.base_frame, grasps[0], scores[0])
self.best_grasp = None
self.vis.grasps(self.base_frame, grasps, scores) self.vis.grasps(self.base_frame, grasps, scores)

19
active_grasp/visualization.py
View File

@ -28,15 +28,19 @@ class Visualizer:
self.quality_pub = rospy.Publisher("quality", PointCloud2, queue_size=1) self.quality_pub = rospy.Publisher("quality", PointCloud2, queue_size=1)
def clear(self): def clear(self):
self.draw([Marker(action=Marker.DELETEALL)]) self.clear_markers()
msg = to_cloud_msg("panda_link0", np.array([])) msg = to_cloud_msg("panda_link0", np.array([]))
self.scene_cloud_pub.publish(msg) self.scene_cloud_pub.publish(msg)
self.map_cloud_pub.publish(msg) self.map_cloud_pub.publish(msg)
self.quality_pub.publish(msg) self.quality_pub.publish(msg)
rospy.sleep(0.1) rospy.sleep(0.1)
def clear_views(self): def clear_markers(self):
self.draw([Marker(action=Marker.DELETE, ns="views")]) 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): def draw(self, markers):
self.marker_pub.publish(MarkerArray(markers=markers)) self.marker_pub.publish(MarkerArray(markers=markers))
@ -48,7 +52,12 @@ class Visualizer:
marker = create_cube_marker(frame, pose, scale, color, ns="bbox") marker = create_cube_marker(frame, pose, scale, color, ns="bbox")
self.draw([marker]) self.draw([marker])
def grasps(self, frame, grasps, scores, smin=0.9, smax=1.0, alpha=0.8): def best_grasp(self, frame, grasp, score, smin=0.9, smax=1.0, alpha=1.0):
color = cmap((score - smin) / (smax - smin))
color = [color[0], color[1], color[2], alpha]
self.draw(create_grasp_markers(frame, grasp, color, "best_grasp", radius=0.01))
def grasps(self, frame, grasps, scores, smin=0.9, smax=1.0, alpha=0.6):
if len(grasps) == 0: if len(grasps) == 0:
return return
@ -188,7 +197,7 @@ def create_grasp_markers(
grasp, grasp,
color, color,
ns, ns,
id, id=0,
finger_depth=0.05, finger_depth=0.05,
radius=0.005, radius=0.005,
): ):