diff --git a/active_grasp/visualization.py b/active_grasp/visualization.py index 827e46f..e4ee50b 100644 --- a/active_grasp/visualization.py +++ b/active_grasp/visualization.py @@ -16,7 +16,6 @@ class Visualizer: 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)]) @@ -24,9 +23,6 @@ class Visualizer: self.scene_cloud_pub.publish(msg) self.map_cloud_pub.publish(msg) self.quality_pub.publish(msg) - msg = PoseArray() - msg.header.frame_id = "panda_link0" - self.grasps_pub.publish(msg) def draw(self, markers): self.marker_pub.publish(MarkerArray(markers=markers)) @@ -39,10 +35,11 @@ class Visualizer: self.draw([marker]) def grasps(self, frame, grasps): - msg = PoseArray() - msg.header.frame_id = frame - msg.poses = [to_pose_msg(grasp.pose) for grasp in grasps] - self.grasps_pub.publish(msg) + markers = [] + for i, grasp in enumerate(grasps): + color = [1.0, 0.0, 0.0] # TODO choose color based on score + markers += create_grasp_markers(frame, grasp, color, "grasps", 4 * i) + self.draw(markers) def rays(self, frame, origin, directions, t_max=1.0): lines = [[origin, origin + t_max * direction] for direction in directions] @@ -163,3 +160,33 @@ def create_cam_view_marker( ] marker.points = [to_point_msg(p) for p in points] return marker + + +def create_grasp_markers( + frame, + grasp, + color, + ns, + id, + finger_depth=0.05, + radius=0.005, +): + w, d = grasp.width, finger_depth + + pose = grasp.pose * Transform.translation([0.0, -w / 2, d / 2]) + scale = [radius, radius, d] + left = create_marker(Marker.CYLINDER, frame, pose, scale, color, ns, id) + + pose = grasp.pose * Transform.translation([0.0, w / 2, d / 2]) + scale = [radius, radius, d] + right = create_marker(Marker.CYLINDER, frame, pose, scale, color, ns, id + 1) + + pose = grasp.pose * Transform.translation([0.0, 0.0, -d / 4]) + scale = [radius, radius, d / 2] + wrist = create_marker(Marker.CYLINDER, frame, pose, scale, color, ns, id + 2) + + pose = grasp.pose * Transform.rotation(Rotation.from_rotvec([np.pi / 2, 0, 0])) + scale = [radius, radius, w] + palm = create_marker(Marker.CYLINDER, frame, pose, scale, color, ns, id + 3) + + return [left, right, wrist, palm] diff --git a/launch/active_grasp.rviz b/launch/active_grasp.rviz index f514357..c4469a7 100644 --- a/launch/active_grasp.rviz +++ b/launch/active_grasp.rviz @@ -1,14 +1,13 @@ Panels: - Class: rviz/Displays - Help Height: 78 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - - /Grasps1/Status1 - - /Markers1 + - /TF1/Frames1 - /Markers1/Namespaces1 Splitter Ratio: 0.5 - Tree Height: 465 + Tree Height: 543 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -139,13 +138,13 @@ Visualization Manager: Frames: All Enabled: false camera_optical_frame: - Value: false + Value: true panda_hand: Value: true panda_leftfinger: Value: false panda_link0: - Value: false + Value: true panda_link1: Value: false panda_link2: @@ -193,6 +192,18 @@ Visualization Manager: {} Update Interval: 0 Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /camera/depth/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -221,6 +232,36 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.007499999832361937 + Style: Boxes + Topic: /map_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false - Alpha: 0.20000000298023224 Autocompute Intensity Bounds: false Autocompute Value Bounds: @@ -251,23 +292,6 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: false Value: true - - Alpha: 1 - Arrow Length: 0.30000001192092896 - Axes Length: 0.05000000074505806 - Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray - Color: 255; 25; 0 - Enabled: true - Head Length: 0.07000000029802322 - Head Radius: 0.029999999329447746 - Name: Grasps - Queue Size: 10 - Shaft Length: 0.23000000417232513 - Shaft Radius: 0.009999999776482582 - Shape: Axes - Topic: /grasps - Unreliable: false - Value: true - Alpha: 1 Axes Length: 0.05000000074505806 Axes Radius: 0.009999999776482582 @@ -289,8 +313,12 @@ Visualization Manager: Marker Topic: visualization_marker_array Name: Markers Namespaces: - bbox: false + bbox: true + grasps: true path: true + point: true + rays: true + views: true Queue Size: 100 Value: true Enabled: true @@ -321,7 +349,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.2231600284576416 + Distance: 1.4553453922271729 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -329,25 +357,27 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.2673089802265167 - Y: 0.08313889056444168 - Z: 0.4073459804058075 + X: 0.43946000933647156 + Y: 0.016814040020108223 + Z: 0.40760287642478943 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.2547985911369324 + Pitch: 0.19979602098464966 Target Frame: - Yaw: 5.173501968383789 + Yaw: 5.38097620010376 Saved: ~ Window Geometry: + Depth: + collapsed: false Displays: collapsed: false Height: 762 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000025cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000025c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004000000003efc0100000002fb0000000800540069006d0065010000000000000400000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002a40000025c00000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001560000025cfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000025c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0044006500700074006800000001e4000000b50000001600ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004000000003efc0100000002fb0000000800540069006d0065010000000000000400000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002a40000025c00000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -357,5 +387,5 @@ Window Geometry: Views: collapsed: true Width: 1024 - X: 208 - Y: 205 + X: 1440 + Y: 268