Improve grasp visualization
This commit is contained in:
Michel Breyer
parent
685e41dde2
commit
231e337c0f
@ -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]
|
||||
|
||||
@ -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: <Fixed 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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user