Pass frame explicitly to each visualization method
This commit is contained in:
Michel Breyer
parent
69e6acf00b
commit
26f7103237
@ -89,7 +89,7 @@ class GraspController:
|
||||
self.send_cmd(
|
||||
T_base_grasp * Transform.translation([0, 0, -0.05]) * self.T_grasp_ee
|
||||
)
|
||||
rospy.sleep(3.0)
|
||||
rospy.sleep(4.0) # TODO
|
||||
|
||||
# Approach grasp pose.
|
||||
self.send_cmd(T_base_grasp * self.T_grasp_ee)
|
||||
|
||||
@ -37,7 +37,7 @@ class NextBestView(BasePolicy):
|
||||
utilities = gains / np.sum(gains) - costs / np.sum(costs)
|
||||
|
||||
# Visualize
|
||||
self.visualizer.views(self.intrinsic, views, utilities)
|
||||
self.visualizer.views(self.base_frame, self.intrinsic, views, utilities)
|
||||
|
||||
# Determine next-best-view
|
||||
nbv = views[np.argmax(utilities)]
|
||||
|
||||
@ -36,7 +36,7 @@ class BasePolicy(Policy):
|
||||
self.score_fn = lambda g: g.pose.translation[2] # TODO
|
||||
|
||||
def init_visualizer(self):
|
||||
self.visualizer = Visualizer(self.base_frame)
|
||||
self.visualizer = Visualizer()
|
||||
|
||||
def activate(self, bbox):
|
||||
self.bbox = bbox
|
||||
@ -61,11 +61,12 @@ class BasePolicy(Policy):
|
||||
self.best_grasp = None
|
||||
|
||||
self.visualizer.clear()
|
||||
self.visualizer.bbox(bbox)
|
||||
self.visualizer.bbox(self.base_frame, bbox)
|
||||
|
||||
def integrate_img(self, img, extrinsic):
|
||||
self.viewpoints.append(extrinsic.inv())
|
||||
self.tsdf.integrate(img, self.intrinsic, extrinsic * self.T_base_task)
|
||||
tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size
|
||||
|
||||
if self.filter_grasps:
|
||||
out = self.vgn.predict(self.tsdf.get_grid())
|
||||
@ -75,10 +76,11 @@ class BasePolicy(Policy):
|
||||
self.width_hist[t, ...] = out.width
|
||||
|
||||
mean_qual = self.compute_mean_quality()
|
||||
self.visualizer.quality(self.task_frame, self.tsdf.voxel_size, mean_qual)
|
||||
self.visualizer.quality(self.task_frame, voxel_size, mean_qual)
|
||||
|
||||
self.visualizer.scene_cloud(self.task_frame, self.tsdf.get_scene_cloud())
|
||||
self.visualizer.path(self.viewpoints)
|
||||
self.visualizer.map_cloud(self.task_frame, voxel_size, tsdf_grid)
|
||||
self.visualizer.path(self.base_frame, self.viewpoints)
|
||||
|
||||
def compute_best_grasp(self):
|
||||
if self.filter_grasps:
|
||||
@ -96,7 +98,7 @@ class BasePolicy(Policy):
|
||||
grasps = sort_grasps(grasps, self.score_fn)
|
||||
|
||||
self.visualizer.quality(self.task_frame, self.tsdf.voxel_size, qual)
|
||||
self.visualizer.grasps(grasps)
|
||||
self.visualizer.grasps(self.base_frame, grasps)
|
||||
|
||||
return grasps[0] if len(grasps) > 0 else None
|
||||
|
||||
|
||||
@ -11,55 +11,61 @@ cmap = matplotlib.colors.LinearSegmentedColormap.from_list("RedGreen", ["r", "g"
|
||||
|
||||
|
||||
class Visualizer:
|
||||
def __init__(self, frame, topic="visualization_marker_array"):
|
||||
self.frame = frame
|
||||
def __init__(self, topic="visualization_marker_array"):
|
||||
self.marker_pub = rospy.Publisher(topic, MarkerArray, queue_size=1)
|
||||
self.scene_cloud_pub = rospy.Publisher("scene_cloud", PointCloud2, queue_size=1)
|
||||
self.map_cloud_pub = rospy.Publisher("map_cloud", PointCloud2, queue_size=1)
|
||||
self.quality_pub = rospy.Publisher("quality", PointCloud2, queue_size=1)
|
||||
self.grasps_pub = rospy.Publisher("grasps", PoseArray, queue_size=1)
|
||||
|
||||
def clear(self):
|
||||
self.draw([Marker(action=Marker.DELETEALL)])
|
||||
msg = to_cloud_msg(self.frame, np.array([]))
|
||||
msg = to_cloud_msg("panda_link0", np.array([]))
|
||||
self.scene_cloud_pub.publish(msg)
|
||||
self.map_cloud_pub.publish(msg)
|
||||
self.quality_pub.publish(msg)
|
||||
msg = PoseArray()
|
||||
msg.header.frame_id = self.frame
|
||||
msg.header.frame_id = "panda_link0"
|
||||
self.grasps_pub.publish(msg)
|
||||
|
||||
def draw(self, markers):
|
||||
self.marker_pub.publish(MarkerArray(markers=markers))
|
||||
|
||||
def bbox(self, bbox):
|
||||
def bbox(self, frame, bbox):
|
||||
pose = Transform.translation((bbox.min + bbox.max) / 2.0)
|
||||
scale = bbox.max - bbox.min
|
||||
color = np.r_[0.8, 0.2, 0.2, 0.6]
|
||||
marker = create_cube_marker(self.frame, pose, scale, color, ns="bbox")
|
||||
marker = create_cube_marker(frame, pose, scale, color, ns="bbox")
|
||||
self.draw([marker])
|
||||
|
||||
def grasps(self, grasps):
|
||||
def grasps(self, frame, grasps):
|
||||
msg = PoseArray()
|
||||
msg.header.frame_id = self.frame
|
||||
msg.header.frame_id = frame
|
||||
msg.poses = [to_pose_msg(grasp.pose) for grasp in grasps]
|
||||
self.grasps_pub.publish(msg)
|
||||
|
||||
def lines(self, lines):
|
||||
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(
|
||||
self.frame,
|
||||
frame,
|
||||
Transform.identity(),
|
||||
[0.005, 0.0, 0.0],
|
||||
[1.0, 0.0, 0.0],
|
||||
[0.6, 0.6, 0.6],
|
||||
lines,
|
||||
"rays",
|
||||
0,
|
||||
)
|
||||
self.draw([marker])
|
||||
|
||||
def path(self, poses):
|
||||
def map_cloud(self, frame, voxel_size, tsdf):
|
||||
points, values = grid_to_map_cloud(voxel_size, tsdf, threshold=0.0)
|
||||
msg = to_cloud_msg(frame, points, intensities=values)
|
||||
self.map_cloud_pub.publish(msg)
|
||||
|
||||
def path(self, frame, poses):
|
||||
color = np.r_[31, 119, 180] / 255.0
|
||||
points = [p.translation for p in poses]
|
||||
spheres = create_sphere_list_marker(
|
||||
self.frame,
|
||||
frame,
|
||||
Transform.identity(),
|
||||
np.full(3, 0.01),
|
||||
color,
|
||||
@ -68,7 +74,7 @@ class Visualizer:
|
||||
0,
|
||||
)
|
||||
lines = create_line_strip_marker(
|
||||
self.frame,
|
||||
frame,
|
||||
Transform.identity(),
|
||||
[0.005, 0.0, 0.0],
|
||||
color,
|
||||
@ -78,6 +84,16 @@ class Visualizer:
|
||||
)
|
||||
self.draw([spheres, lines])
|
||||
|
||||
def point(self, frame, point):
|
||||
marker = create_sphere_marker(
|
||||
frame,
|
||||
Transform.translation(point),
|
||||
np.full(3, 0.01),
|
||||
[1, 0, 0],
|
||||
"point",
|
||||
)
|
||||
self.draw([marker])
|
||||
|
||||
def quality(self, frame, voxel_size, quality):
|
||||
points, values = grid_to_map_cloud(voxel_size, quality, threshold=0.8)
|
||||
msg = to_cloud_msg(frame, points, intensities=values)
|
||||
@ -87,30 +103,29 @@ class Visualizer:
|
||||
msg = to_cloud_msg(frame, np.asarray(cloud.points))
|
||||
self.scene_cloud_pub.publish(msg)
|
||||
|
||||
def views(self, intrinsic, views, values):
|
||||
def views(self, frame, intrinsic, views, values):
|
||||
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))
|
||||
markers.append(
|
||||
_create_cam_view_marker(
|
||||
self.frame,
|
||||
view,
|
||||
scale,
|
||||
color,
|
||||
intrinsic,
|
||||
near,
|
||||
far,
|
||||
ns="views",
|
||||
id=i,
|
||||
)
|
||||
marker = create_cam_view_marker(
|
||||
frame,
|
||||
view,
|
||||
scale,
|
||||
color,
|
||||
intrinsic,
|
||||
near,
|
||||
far,
|
||||
ns="views",
|
||||
id=i,
|
||||
)
|
||||
markers.append(marker)
|
||||
self.draw(markers)
|
||||
|
||||
|
||||
def _create_cam_view_marker(
|
||||
def create_cam_view_marker(
|
||||
frame, pose, scale, color, intrinsic, near, far, ns="", id=0
|
||||
):
|
||||
marker = create_marker(Marker.LINE_LIST, frame, pose, scale, color, ns, id)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user