diff --git a/active_grasp/controller.py b/active_grasp/controller.py index 5519607..42ae435 100644 --- a/active_grasp/controller.py +++ b/active_grasp/controller.py @@ -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) diff --git a/active_grasp/nbv.py b/active_grasp/nbv.py index 67d5d97..60126f1 100644 --- a/active_grasp/nbv.py +++ b/active_grasp/nbv.py @@ -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)] diff --git a/active_grasp/policy.py b/active_grasp/policy.py index 178500a..ec1cfc4 100644 --- a/active_grasp/policy.py +++ b/active_grasp/policy.py @@ -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 diff --git a/active_grasp/visualization.py b/active_grasp/visualization.py index c999463..827e46f 100644 --- a/active_grasp/visualization.py +++ b/active_grasp/visualization.py @@ -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)