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

Small touch to the visualization

Browse Source
This commit is contained in:
Michel Breyer 2021-08-30 12:51:38 +02:00
parent 9f0e2789cf
commit 98abe3789d
3 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
active_grasp/nbv.py
View File

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

2
active_grasp/policy.py
View File

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

10
active_grasp/visualization.py
View File

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