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

Improve bbox visualization

Browse Source
This commit is contained in:
Michel Breyer 2021-09-08 18:45:13 +02:00
parent b781c47328
commit d955931590
1 changed files with 25 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
active_grasp/visualization.py
View File

@ -8,6 +8,8 @@ from robot_helpers.spatial import Transform
from vgn.utils import * from vgn.utils import *
cmap = matplotlib.colors.LinearSegmentedColormap.from_list("RedGreen", ["r", "g"]) cmap = matplotlib.colors.LinearSegmentedColormap.from_list("RedGreen", ["r", "g"])
red = np.r_[1.0, 0.0, 0.0]
blue = np.r_[31, 119, 180] / 255.0
class Visualizer: class Visualizer:
@ -46,16 +48,32 @@ class Visualizer:
self.marker_pub.publish(MarkerArray(markers=markers)) self.marker_pub.publish(MarkerArray(markers=markers))
def bbox(self, frame, bbox): def bbox(self, frame, bbox):
pose = Transform.translation((bbox.min + bbox.max) / 2.0) pose = Transform.identity()
scale = bbox.max - bbox.min scale = [0.004, 0.0, 0.0]
color = np.r_[0.8, 0.2, 0.2, 0.6] color = red
marker = create_cube_marker(frame, pose, scale, color, ns="bbox") corners = bbox.corners
edges = [
(0, 1),
(1, 3),
(3, 2),
(2, 0),
(4, 5),
(5, 7),
(7, 6),
(6, 4),
(0, 4),
(1, 5),
(3, 7),
(2, 6),
]
lines = [(corners[s], corners[e]) for s, e in edges]
marker = create_line_list_marker(frame, pose, scale, color, lines, ns="bbox")
self.draw([marker]) self.draw([marker])
def best_grasp(self, frame, grasp, score, smin=0.9, smax=1.0, alpha=1.0): def best_grasp(self, frame, grasp, score, smin=0.9, smax=1.0, alpha=1.0):
color = cmap((score - smin) / (smax - smin)) color = cmap((score - smin) / (smax - smin))
color = [color[0], color[1], color[2], alpha] color = [color[0], color[1], color[2], alpha]
self.draw(create_grasp_markers(frame, grasp, color, "best_grasp", radius=0.01)) self.draw(create_grasp_markers(frame, grasp, color, "best_grasp", radius=0.006))
def grasps(self, frame, grasps, scores, smin=0.9, smax=1.0, alpha=0.6): def grasps(self, frame, grasps, scores, smin=0.9, smax=1.0, alpha=0.6):
if len(grasps) == 0: if len(grasps) == 0:
@ -87,7 +105,7 @@ class Visualizer:
self.map_cloud_pub.publish(msg) self.map_cloud_pub.publish(msg)
def path(self, frame, poses): def path(self, frame, poses):
color = np.r_[31, 119, 180] / 255.0 color = blue
points = [p.translation for p in poses] points = [p.translation for p in poses]
spheres = create_sphere_list_marker( spheres = create_sphere_list_marker(
frame, frame,
@ -199,7 +217,7 @@ def create_grasp_markers(
ns, ns,
id=0, id=0,
finger_depth=0.05, finger_depth=0.05,
radius=0.005, radius=0.003,
): ):
w, d = grasp.width, finger_depth w, d = grasp.width, finger_depth