Minor
This commit is contained in:
Michel Breyer
parent
c1710eb2da
commit
6ec5272394
@ -3,9 +3,7 @@ Panels:
|
|||||||
Help Height: 0
|
Help Height: 0
|
||||||
Name: Displays
|
Name: Displays
|
||||||
Property Tree Widget:
|
Property Tree Widget:
|
||||||
Expanded:
|
Expanded: ~
|
||||||
- /Markers1/Namespaces1
|
|
||||||
- /TF1/Frames1
|
|
||||||
Splitter Ratio: 0.49705880880355835
|
Splitter Ratio: 0.49705880880355835
|
||||||
Tree Height: 814
|
Tree Height: 814
|
||||||
- Class: rviz/Selection
|
- Class: rviz/Selection
|
||||||
@ -27,7 +25,7 @@ Panels:
|
|||||||
Experimental: false
|
Experimental: false
|
||||||
Name: Time
|
Name: Time
|
||||||
SyncMode: 0
|
SyncMode: 0
|
||||||
SyncSource: SceneCloud
|
SyncSource: GraspQuality
|
||||||
Preferences:
|
Preferences:
|
||||||
PromptSaveOnExit: true
|
PromptSaveOnExit: true
|
||||||
Toolbars:
|
Toolbars:
|
||||||
@ -53,74 +51,149 @@ Visualization Manager:
|
|||||||
Plane Cell Count: 10
|
Plane Cell Count: 10
|
||||||
Reference Frame: <Fixed Frame>
|
Reference Frame: <Fixed Frame>
|
||||||
Value: true
|
Value: true
|
||||||
- Class: rviz/Image
|
- Class: rviz/TF
|
||||||
Enabled: false
|
|
||||||
Image Topic: /camera/color/image_raw
|
|
||||||
Max Value: 1
|
|
||||||
Median window: 5
|
|
||||||
Min Value: 0
|
|
||||||
Name: ColorImage
|
|
||||||
Normalize Range: true
|
|
||||||
Queue Size: 2
|
|
||||||
Transport Hint: raw
|
|
||||||
Unreliable: false
|
|
||||||
Value: false
|
|
||||||
- Class: rviz/Image
|
|
||||||
Enabled: false
|
|
||||||
Image Topic: /camera/depth/image_rect_raw
|
|
||||||
Max Value: 1
|
|
||||||
Median window: 5
|
|
||||||
Min Value: 0
|
|
||||||
Name: DepthImage
|
|
||||||
Normalize Range: true
|
|
||||||
Queue Size: 2
|
|
||||||
Transport Hint: raw
|
|
||||||
Unreliable: false
|
|
||||||
Value: false
|
|
||||||
- Alpha: 0.05000000074505806
|
|
||||||
Autocompute Intensity Bounds: false
|
|
||||||
Autocompute Value Bounds:
|
|
||||||
Max Value: 10
|
|
||||||
Min Value: -10
|
|
||||||
Value: true
|
|
||||||
Axis: Z
|
|
||||||
Channel Name: distance
|
|
||||||
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
|
|
||||||
- Class: rviz/MarkerArray
|
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Marker Topic: visualization_marker_array
|
Frame Timeout: 15
|
||||||
Name: Markers
|
Frames:
|
||||||
Namespaces:
|
All Enabled: false
|
||||||
bbox: true
|
camera_depth_optical_frame:
|
||||||
best_grasp: true
|
Value: true
|
||||||
grasps: true
|
panda_hand:
|
||||||
path: true
|
Value: true
|
||||||
rays: true
|
panda_leftfinger:
|
||||||
views: true
|
Value: false
|
||||||
workspace: true
|
panda_link0:
|
||||||
Queue Size: 100
|
Value: true
|
||||||
|
panda_link1:
|
||||||
|
Value: false
|
||||||
|
panda_link2:
|
||||||
|
Value: false
|
||||||
|
panda_link3:
|
||||||
|
Value: false
|
||||||
|
panda_link4:
|
||||||
|
Value: false
|
||||||
|
panda_link5:
|
||||||
|
Value: false
|
||||||
|
panda_link6:
|
||||||
|
Value: false
|
||||||
|
panda_link7:
|
||||||
|
Value: false
|
||||||
|
panda_link8:
|
||||||
|
Value: false
|
||||||
|
panda_rightfinger:
|
||||||
|
Value: false
|
||||||
|
task:
|
||||||
|
Value: true
|
||||||
|
world:
|
||||||
|
Value: false
|
||||||
|
Marker Alpha: 1
|
||||||
|
Marker Scale: 0.30000001192092896
|
||||||
|
Name: TF
|
||||||
|
Show Arrows: false
|
||||||
|
Show Axes: true
|
||||||
|
Show Names: false
|
||||||
|
Tree:
|
||||||
|
world:
|
||||||
|
panda_link0:
|
||||||
|
panda_link1:
|
||||||
|
panda_link2:
|
||||||
|
panda_link3:
|
||||||
|
panda_link4:
|
||||||
|
panda_link5:
|
||||||
|
panda_link6:
|
||||||
|
panda_link7:
|
||||||
|
panda_link8:
|
||||||
|
panda_hand:
|
||||||
|
camera_depth_optical_frame:
|
||||||
|
{}
|
||||||
|
panda_leftfinger:
|
||||||
|
{}
|
||||||
|
panda_rightfinger:
|
||||||
|
{}
|
||||||
|
task:
|
||||||
|
{}
|
||||||
|
Update Interval: 0
|
||||||
Value: true
|
Value: true
|
||||||
|
- Alpha: 1
|
||||||
|
Class: rviz/RobotModel
|
||||||
|
Collision Enabled: false
|
||||||
|
Enabled: true
|
||||||
|
Links:
|
||||||
|
All Links Enabled: true
|
||||||
|
Expand Joint Details: false
|
||||||
|
Expand Link Details: false
|
||||||
|
Expand Tree: false
|
||||||
|
Link Tree Style: Links in Alphabetic Order
|
||||||
|
camera_depth_optical_frame:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
panda_hand:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
panda_leftfinger:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
panda_link0:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
panda_link1:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
panda_link2:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
panda_link3:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
panda_link4:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
panda_link5:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
panda_link6:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
panda_link7:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
panda_link8:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
panda_rightfinger:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
Name: RobotModel
|
||||||
|
Robot Description: robot_description
|
||||||
|
TF Prefix: ""
|
||||||
|
Update Interval: 0
|
||||||
|
Value: true
|
||||||
|
Visual Enabled: true
|
||||||
- Acceleration_Scaling_Factor: 0.1
|
- Acceleration_Scaling_Factor: 0.1
|
||||||
Class: moveit_rviz_plugin/MotionPlanning
|
Class: moveit_rviz_plugin/MotionPlanning
|
||||||
Enabled: false
|
Enabled: false
|
||||||
@ -357,6 +430,30 @@ Visualization Manager:
|
|||||||
Show Robot Collision: false
|
Show Robot Collision: false
|
||||||
Show Robot Visual: false
|
Show Robot Visual: false
|
||||||
Value: false
|
Value: false
|
||||||
|
- Class: rviz/Image
|
||||||
|
Enabled: false
|
||||||
|
Image Topic: /camera/color/image_raw
|
||||||
|
Max Value: 1
|
||||||
|
Median window: 5
|
||||||
|
Min Value: 0
|
||||||
|
Name: ColorImage
|
||||||
|
Normalize Range: true
|
||||||
|
Queue Size: 2
|
||||||
|
Transport Hint: raw
|
||||||
|
Unreliable: false
|
||||||
|
Value: false
|
||||||
|
- Class: rviz/Image
|
||||||
|
Enabled: false
|
||||||
|
Image Topic: /camera/depth/image_rect_raw
|
||||||
|
Max Value: 1
|
||||||
|
Median window: 5
|
||||||
|
Min Value: 0
|
||||||
|
Name: DepthImage
|
||||||
|
Normalize Range: true
|
||||||
|
Queue Size: 2
|
||||||
|
Transport Hint: raw
|
||||||
|
Unreliable: false
|
||||||
|
Value: false
|
||||||
- Alpha: 1
|
- Alpha: 1
|
||||||
Autocompute Intensity Bounds: true
|
Autocompute Intensity Bounds: true
|
||||||
Autocompute Value Bounds:
|
Autocompute Value Bounds:
|
||||||
@ -385,116 +482,19 @@ Visualization Manager:
|
|||||||
Use Fixed Frame: true
|
Use Fixed Frame: true
|
||||||
Use rainbow: true
|
Use rainbow: true
|
||||||
Value: false
|
Value: false
|
||||||
- Alpha: 0.20000000298023224
|
- Class: rviz/MarkerArray
|
||||||
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: 138; 226; 52
|
|
||||||
Max Intensity: 1
|
|
||||||
Min Color: 239; 41; 41
|
|
||||||
Min Intensity: 0.800000011920929
|
|
||||||
Name: GraspQuality
|
|
||||||
Position Transformer: XYZ
|
|
||||||
Queue Size: 10
|
|
||||||
Selectable: true
|
|
||||||
Size (Pixels): 3
|
|
||||||
Size (m): 0.007499999832361937
|
|
||||||
Style: Boxes
|
|
||||||
Topic: /quality
|
|
||||||
Unreliable: false
|
|
||||||
Use Fixed Frame: true
|
|
||||||
Use rainbow: false
|
|
||||||
Value: false
|
|
||||||
- Alpha: 1
|
|
||||||
Class: rviz/RobotModel
|
|
||||||
Collision Enabled: false
|
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Links:
|
Marker Topic: visualization_marker_array
|
||||||
All Links Enabled: true
|
Name: Markers
|
||||||
Expand Joint Details: false
|
Namespaces:
|
||||||
Expand Link Details: false
|
bbox: true
|
||||||
Expand Tree: false
|
grasp: true
|
||||||
Link Tree Style: Links in Alphabetic Order
|
grasps: true
|
||||||
camera_depth_optical_frame:
|
path: true
|
||||||
Alpha: 1
|
views: true
|
||||||
Show Axes: false
|
workspace: true
|
||||||
Show Trail: false
|
Queue Size: 100
|
||||||
panda_hand:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
panda_leftfinger:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
panda_link0:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
panda_link1:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
panda_link2:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
panda_link3:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
panda_link4:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
panda_link5:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
panda_link6:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
panda_link7:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
panda_link8:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
panda_rightfinger:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
Name: RobotModel
|
|
||||||
Robot Description: robot_description
|
|
||||||
TF Prefix: ""
|
|
||||||
Update Interval: 0
|
|
||||||
Value: true
|
Value: true
|
||||||
Visual Enabled: true
|
|
||||||
- Alpha: 1
|
- Alpha: 1
|
||||||
Autocompute Intensity Bounds: true
|
Autocompute Intensity Bounds: true
|
||||||
Autocompute Value Bounds:
|
Autocompute Value Bounds:
|
||||||
@ -523,68 +523,65 @@ Visualization Manager:
|
|||||||
Use Fixed Frame: true
|
Use Fixed Frame: true
|
||||||
Use rainbow: true
|
Use rainbow: true
|
||||||
Value: true
|
Value: true
|
||||||
- Class: rviz/TF
|
- Alpha: 0.05000000074505806
|
||||||
|
Autocompute Intensity Bounds: false
|
||||||
|
Autocompute Value Bounds:
|
||||||
|
Max Value: 10
|
||||||
|
Min Value: -10
|
||||||
|
Value: true
|
||||||
|
Axis: Z
|
||||||
|
Channel Name: distance
|
||||||
|
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: 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: true
|
Enabled: true
|
||||||
Frame Timeout: 15
|
Invert Rainbow: false
|
||||||
Frames:
|
Max Color: 0; 255; 0
|
||||||
All Enabled: false
|
Max Intensity: 1
|
||||||
camera_depth_optical_frame:
|
Min Color: 255; 0; 0
|
||||||
Value: true
|
Min Intensity: 0.5
|
||||||
panda_hand:
|
Name: GraspQuality
|
||||||
Value: true
|
Position Transformer: XYZ
|
||||||
panda_leftfinger:
|
Queue Size: 10
|
||||||
Value: false
|
Selectable: true
|
||||||
panda_link0:
|
Size (Pixels): 3
|
||||||
Value: true
|
Size (m): 0.007499999832361937
|
||||||
panda_link1:
|
Style: Boxes
|
||||||
Value: false
|
Topic: /quality
|
||||||
panda_link2:
|
Unreliable: false
|
||||||
Value: false
|
Use Fixed Frame: true
|
||||||
panda_link3:
|
Use rainbow: false
|
||||||
Value: false
|
|
||||||
panda_link4:
|
|
||||||
Value: false
|
|
||||||
panda_link5:
|
|
||||||
Value: false
|
|
||||||
panda_link6:
|
|
||||||
Value: false
|
|
||||||
panda_link7:
|
|
||||||
Value: false
|
|
||||||
panda_link8:
|
|
||||||
Value: false
|
|
||||||
panda_rightfinger:
|
|
||||||
Value: false
|
|
||||||
task:
|
|
||||||
Value: true
|
|
||||||
world:
|
|
||||||
Value: false
|
|
||||||
Marker Alpha: 1
|
|
||||||
Marker Scale: 0.30000001192092896
|
|
||||||
Name: TF
|
|
||||||
Show Arrows: false
|
|
||||||
Show Axes: true
|
|
||||||
Show Names: false
|
|
||||||
Tree:
|
|
||||||
world:
|
|
||||||
panda_link0:
|
|
||||||
panda_link1:
|
|
||||||
panda_link2:
|
|
||||||
panda_link3:
|
|
||||||
panda_link4:
|
|
||||||
panda_link5:
|
|
||||||
panda_link6:
|
|
||||||
panda_link7:
|
|
||||||
panda_link8:
|
|
||||||
panda_hand:
|
|
||||||
camera_depth_optical_frame:
|
|
||||||
{}
|
|
||||||
panda_leftfinger:
|
|
||||||
{}
|
|
||||||
panda_rightfinger:
|
|
||||||
{}
|
|
||||||
task:
|
|
||||||
{}
|
|
||||||
Update Interval: 0
|
|
||||||
Value: true
|
Value: true
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Global Options:
|
Global Options:
|
||||||
@ -614,7 +611,7 @@ Visualization Manager:
|
|||||||
Views:
|
Views:
|
||||||
Current:
|
Current:
|
||||||
Class: rviz/Orbit
|
Class: rviz/Orbit
|
||||||
Distance: 1.710027813911438
|
Distance: 1.7602970600128174
|
||||||
Enable Stereo Rendering:
|
Enable Stereo Rendering:
|
||||||
Stereo Eye Separation: 0.05999999865889549
|
Stereo Eye Separation: 0.05999999865889549
|
||||||
Stereo Focal Distance: 1
|
Stereo Focal Distance: 1
|
||||||
@ -622,17 +619,17 @@ Visualization Manager:
|
|||||||
Value: false
|
Value: false
|
||||||
Field of View: 0.7853981852531433
|
Field of View: 0.7853981852531433
|
||||||
Focal Point:
|
Focal Point:
|
||||||
X: 0.17848753929138184
|
X: 0.23816421627998352
|
||||||
Y: 0.3464738428592682
|
Y: 0.3857060968875885
|
||||||
Z: 0.36626341938972473
|
Z: 0.35086870193481445
|
||||||
Focal Shape Fixed Size: false
|
Focal Shape Fixed Size: false
|
||||||
Focal Shape Size: 0.05000000074505806
|
Focal Shape Size: 0.05000000074505806
|
||||||
Invert Z Axis: false
|
Invert Z Axis: false
|
||||||
Name: Current View
|
Name: Current View
|
||||||
Near Clip Distance: 0.009999999776482582
|
Near Clip Distance: 0.009999999776482582
|
||||||
Pitch: 0.25500035285949707
|
Pitch: 0.22000055015087128
|
||||||
Target Frame: <Fixed Frame>
|
Target Frame: <Fixed Frame>
|
||||||
Yaw: 5.125002384185791
|
Yaw: 5.094988822937012
|
||||||
Saved: ~
|
Saved: ~
|
||||||
Window Geometry:
|
Window Geometry:
|
||||||
ColorImage:
|
ColorImage:
|
||||||
@ -643,12 +640,12 @@ Window Geometry:
|
|||||||
collapsed: false
|
collapsed: false
|
||||||
Height: 965
|
Height: 965
|
||||||
Hide Left Dock: false
|
Hide Left Dock: false
|
||||||
Hide Right Dock: false
|
Hide Right Dock: true
|
||||||
MotionPlanning:
|
MotionPlanning:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
MotionPlanning - Trajectory Slider:
|
MotionPlanning - Trajectory Slider:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
QMainWindow State: 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
|
QMainWindow State: 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
|
||||||
Selection:
|
Selection:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Time:
|
Time:
|
||||||
@ -658,5 +655,5 @@ Window Geometry:
|
|||||||
Views:
|
Views:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Width: 1379
|
Width: 1379
|
||||||
X: 824
|
X: 1070
|
||||||
Y: 205
|
Y: 288
|
||||||
|
|||||||
@ -1,2 +1 @@
|
|||||||
numba
|
numba
|
||||||
scikit-image
|
|
||||||
|
|||||||
@ -9,7 +9,6 @@ from geometry_msgs.msg import Twist
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
import rospy
|
import rospy
|
||||||
from sensor_msgs.msg import JointState, Image, CameraInfo
|
from sensor_msgs.msg import JointState, Image, CameraInfo
|
||||||
import skimage.transform
|
|
||||||
from scipy import interpolate
|
from scipy import interpolate
|
||||||
from threading import Thread
|
from threading import Thread
|
||||||
|
|
||||||
@ -17,6 +16,7 @@ from active_grasp.bbox import to_bbox_msg
|
|||||||
from active_grasp.srv import *
|
from active_grasp.srv import *
|
||||||
from active_grasp.simulation import Simulation
|
from active_grasp.simulation import Simulation
|
||||||
from robot_helpers.ros.conversions import *
|
from robot_helpers.ros.conversions import *
|
||||||
|
from vgn.simulation import apply_noise
|
||||||
|
|
||||||
|
|
||||||
class BtSimNode:
|
class BtSimNode:
|
||||||
@ -318,20 +318,9 @@ class CameraPlugin(Plugin):
|
|||||||
self.depth_pub.publish(msg)
|
self.depth_pub.publish(msg)
|
||||||
|
|
||||||
|
|
||||||
def apply_noise(img, k=1000, theta=0.001, sigma=0.005, l=4.0):
|
|
||||||
# Multiplicative and additive noise
|
|
||||||
img *= np.random.gamma(k, theta)
|
|
||||||
h, w = img.shape
|
|
||||||
noise = np.random.randn(int(h / l), int(w / l)) * sigma
|
|
||||||
img += skimage.transform.resize(noise, img.shape, order=1, mode="constant")
|
|
||||||
return img
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
def main():
|
||||||
rospy.init_node("bt_sim")
|
rospy.init_node("bt_sim")
|
||||||
server = BtSimNode()
|
server = BtSimNode()
|
||||||
# server.seed(SeedRequest(1))
|
|
||||||
# server.reset(ResetRequest())
|
|
||||||
server.run()
|
server.run()
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -4,7 +4,7 @@ from pathlib import Path
|
|||||||
import rospy
|
import rospy
|
||||||
|
|
||||||
from .timer import Timer
|
from .timer import Timer
|
||||||
from .visualization import Visualizer
|
from .rviz import Visualizer
|
||||||
from robot_helpers.model import KDLModel
|
from robot_helpers.model import KDLModel
|
||||||
from robot_helpers.ros import tf
|
from robot_helpers.ros import tf
|
||||||
from robot_helpers.ros.conversions import *
|
from robot_helpers.ros.conversions import *
|
||||||
@ -62,36 +62,33 @@ class Policy:
|
|||||||
self.T_task_base = self.T_base_task.inv()
|
self.T_task_base = self.T_base_task.inv()
|
||||||
tf.broadcast(self.T_base_task, self.base_frame, self.task_frame)
|
tf.broadcast(self.T_base_task, self.base_frame, self.task_frame)
|
||||||
rospy.sleep(1.0) # Wait for tf tree to be updated
|
rospy.sleep(1.0) # Wait for tf tree to be updated
|
||||||
self.vis.workspace(self.task_frame, 0.3)
|
|
||||||
|
|
||||||
def update(self, img, x, q):
|
def update(self, img, x, q):
|
||||||
raise NotImplementedError
|
raise NotImplementedError
|
||||||
|
|
||||||
def sort_grasps(self, in_grasps, q):
|
def sort_grasps(self, grasps, qualities, q):
|
||||||
# Transforms grasps into base frame, checks whether they lie on the target, and sorts by their score
|
"""
|
||||||
grasps, scores = [], []
|
1. Transform grasp configurations into base_frame
|
||||||
|
2. Check whether the finger tips lie within the bounding box
|
||||||
for grasp in in_grasps:
|
3. Remove grasps for which no IK solution was found
|
||||||
|
4. Sort grasps according to score_fn
|
||||||
|
"""
|
||||||
|
filtered_grasps, scores = [], []
|
||||||
|
for grasp, quality in zip(grasps, qualities):
|
||||||
pose = self.T_base_task * grasp.pose
|
pose = self.T_base_task * grasp.pose
|
||||||
R, t = pose.rotation, pose.translation
|
R, t = pose.rotation, pose.translation
|
||||||
tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation
|
tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation
|
||||||
|
|
||||||
# Filter out artifacts close to the support
|
|
||||||
if t[2] < self.bbox.min[2] + 0.05 or tip[2] < self.bbox.min[2] + 0.02:
|
|
||||||
continue
|
|
||||||
|
|
||||||
if self.bbox.is_inside(tip):
|
if self.bbox.is_inside(tip):
|
||||||
grasp.pose = pose
|
grasp.pose = pose
|
||||||
q_grasp = self.ee_model.ik(q, pose * self.T_grasp_ee)
|
q_grasp = self.ee_model.ik(q, pose * self.T_grasp_ee)
|
||||||
if q_grasp is not None:
|
if q_grasp is not None:
|
||||||
grasps.append(grasp)
|
filtered_grasps.append(grasp)
|
||||||
scores.append(self.score_fn(grasp, q, q_grasp))
|
scores.append(self.score_fn(grasp, quality, q, q_grasp))
|
||||||
|
filtered_grasps, scores = np.asarray(filtered_grasps), np.asarray(scores)
|
||||||
|
i = np.argsort(-scores)
|
||||||
|
return filtered_grasps[i], qualities[i], scores[i]
|
||||||
|
|
||||||
grasps, scores = np.asarray(grasps), np.asarray(scores)
|
def score_fn(self, grasp, quality, q, q_grasp):
|
||||||
indices = np.argsort(-scores)
|
|
||||||
return grasps[indices], scores[indices]
|
|
||||||
|
|
||||||
def score_fn(self, grasp, q, q_grasp):
|
|
||||||
return -np.linalg.norm(q - q_grasp)
|
return -np.linalg.norm(q - q_grasp)
|
||||||
|
|
||||||
|
|
||||||
@ -108,8 +105,8 @@ class SingleViewPolicy(Policy):
|
|||||||
out = self.vgn.predict(tsdf_grid)
|
out = self.vgn.predict(tsdf_grid)
|
||||||
self.vis.quality(self.task_frame, voxel_size, out.qual, 0.5)
|
self.vis.quality(self.task_frame, voxel_size, out.qual, 0.5)
|
||||||
|
|
||||||
grasps = select_grid(voxel_size, out, threshold=self.qual_threshold)
|
grasps, qualities = select_grid(voxel_size, out, self.qual_threshold)
|
||||||
grasps, _ = self.sort_grasps(grasps, q)
|
grasps, _ = self.sort_grasps(grasps, qualities, q)
|
||||||
|
|
||||||
if len(grasps) > 0:
|
if len(grasps) > 0:
|
||||||
self.best_grasp = grasps[0]
|
self.best_grasp = grasps[0]
|
||||||
@ -143,17 +140,16 @@ class MultiViewPolicy(Policy):
|
|||||||
self.qual_hist[t, ...] = out.qual
|
self.qual_hist[t, ...] = out.qual
|
||||||
|
|
||||||
with Timer("grasp_selection"):
|
with Timer("grasp_selection"):
|
||||||
grasps = select_grid(voxel_size, out, threshold=self.qual_threshold)
|
grasps, qualities = select_grid(voxel_size, out, self.qual_threshold)
|
||||||
grasps, _ = self.sort_grasps(grasps, q)
|
grasps, qualities, _ = self.sort_grasps(grasps, qualities, q)
|
||||||
|
|
||||||
self.vis.clear_grasps()
|
|
||||||
|
|
||||||
if len(grasps) > 0:
|
if len(grasps) > 0:
|
||||||
self.best_grasp = grasps[0]
|
self.best_grasp = grasps[0]
|
||||||
self.vis.grasps(self.base_frame, grasps)
|
# self.vis.grasps(self.base_frame, grasps, qualities)
|
||||||
self.vis.best_grasp(self.base_frame, self.best_grasp)
|
self.vis.grasp(self.base_frame, self.best_grasp, qualities[0])
|
||||||
else:
|
else:
|
||||||
self.best_grasp = None
|
self.best_grasp = None
|
||||||
|
self.vis.clear_grasp()
|
||||||
|
|
||||||
|
|
||||||
def compute_error(x_d, x):
|
def compute_error(x_d, x):
|
||||||
|
|||||||
143
src/active_grasp/rviz.py
Normal file
143
src/active_grasp/rviz.py
Normal file
@ -0,0 +1,143 @@
|
|||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from robot_helpers.ros.rviz import *
|
||||||
|
from robot_helpers.spatial import Transform
|
||||||
|
import vgn.rviz
|
||||||
|
|
||||||
|
cm = lambda s: tuple([float(1 - s), float(s), float(0)])
|
||||||
|
red = np.r_[1.0, 0.0, 0.0]
|
||||||
|
blue = np.r_[0, 0.6, 1.0]
|
||||||
|
|
||||||
|
|
||||||
|
class Visualizer(vgn.rviz.Visualizer):
|
||||||
|
def bbox(self, frame, bbox):
|
||||||
|
pose = Transform.identity()
|
||||||
|
scale = [0.004, 0.0, 0.0]
|
||||||
|
color = red
|
||||||
|
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, "bbox")
|
||||||
|
self.draw([marker])
|
||||||
|
|
||||||
|
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(
|
||||||
|
frame,
|
||||||
|
Transform.identity(),
|
||||||
|
[0.001, 0.0, 0.0],
|
||||||
|
[0.9, 0.9, 0.9],
|
||||||
|
lines,
|
||||||
|
"rays",
|
||||||
|
)
|
||||||
|
self.draw([marker])
|
||||||
|
|
||||||
|
def path(self, frame, poses):
|
||||||
|
color = blue
|
||||||
|
points = [p.translation for p in poses]
|
||||||
|
spheres = create_sphere_list_marker(
|
||||||
|
frame,
|
||||||
|
Transform.identity(),
|
||||||
|
np.full(3, 0.01),
|
||||||
|
color,
|
||||||
|
points,
|
||||||
|
"path",
|
||||||
|
0,
|
||||||
|
)
|
||||||
|
markers = [spheres]
|
||||||
|
if len(poses) > 1:
|
||||||
|
lines = create_line_strip_marker(
|
||||||
|
frame,
|
||||||
|
Transform.identity(),
|
||||||
|
[0.005, 0.0, 0.0],
|
||||||
|
color,
|
||||||
|
points,
|
||||||
|
"path",
|
||||||
|
1,
|
||||||
|
)
|
||||||
|
markers.append(lines)
|
||||||
|
self.draw(markers)
|
||||||
|
|
||||||
|
def point(self, frame, point):
|
||||||
|
marker = create_sphere_marker(
|
||||||
|
frame,
|
||||||
|
Transform.translation(point),
|
||||||
|
np.full(3, 0.01),
|
||||||
|
[0, 0, 1],
|
||||||
|
"point",
|
||||||
|
)
|
||||||
|
self.draw([marker])
|
||||||
|
|
||||||
|
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 = cm((value - vmin) / (vmax - vmin))
|
||||||
|
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(
|
||||||
|
frame, pose, scale, color, intrinsic, near, far, ns="", id=0
|
||||||
|
):
|
||||||
|
marker = create_marker(Marker.LINE_LIST, frame, pose, scale, color, ns, id)
|
||||||
|
x_n = near * intrinsic.width / (2.0 * intrinsic.fx)
|
||||||
|
y_n = near * intrinsic.height / (2.0 * intrinsic.fy)
|
||||||
|
z_n = near
|
||||||
|
x_f = far * intrinsic.width / (2.0 * intrinsic.fx)
|
||||||
|
y_f = far * intrinsic.height / (2.0 * intrinsic.fy)
|
||||||
|
z_f = far
|
||||||
|
points = [
|
||||||
|
[x_n, y_n, z_n],
|
||||||
|
[-x_n, y_n, z_n],
|
||||||
|
[-x_n, y_n, z_n],
|
||||||
|
[-x_n, -y_n, z_n],
|
||||||
|
[-x_n, -y_n, z_n],
|
||||||
|
[x_n, -y_n, z_n],
|
||||||
|
[x_n, -y_n, z_n],
|
||||||
|
[x_n, y_n, z_n],
|
||||||
|
[x_f, y_f, z_f],
|
||||||
|
[-x_f, y_f, z_f],
|
||||||
|
[-x_f, y_f, z_f],
|
||||||
|
[-x_f, -y_f, z_f],
|
||||||
|
[-x_f, -y_f, z_f],
|
||||||
|
[x_f, -y_f, z_f],
|
||||||
|
[x_f, -y_f, z_f],
|
||||||
|
[x_f, y_f, z_f],
|
||||||
|
[x_n, y_n, z_n],
|
||||||
|
[x_f, y_f, z_f],
|
||||||
|
[-x_n, y_n, z_n],
|
||||||
|
[-x_f, y_f, z_f],
|
||||||
|
[-x_n, -y_n, z_n],
|
||||||
|
[-x_f, -y_f, z_f],
|
||||||
|
[x_n, -y_n, z_n],
|
||||||
|
[x_f, -y_f, z_f],
|
||||||
|
]
|
||||||
|
marker.points = [to_point_msg(p) for p in points]
|
||||||
|
return marker
|
||||||
@ -1,244 +0,0 @@
|
|||||||
from geometry_msgs.msg import PoseStamped
|
|
||||||
import matplotlib.colors
|
|
||||||
import numpy as np
|
|
||||||
import rospy
|
|
||||||
|
|
||||||
from robot_helpers.ros.rviz import *
|
|
||||||
from robot_helpers.spatial import Transform
|
|
||||||
from vgn.utils import *
|
|
||||||
|
|
||||||
cmap = matplotlib.colors.LinearSegmentedColormap.from_list("RedGreen", ["r", "g"])
|
|
||||||
red = np.r_[1.0, 0.0, 0.0]
|
|
||||||
blue = np.r_[0, 0.6, 1.0]
|
|
||||||
|
|
||||||
|
|
||||||
class Visualizer:
|
|
||||||
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,
|
|
||||||
latch=True,
|
|
||||||
queue_size=1,
|
|
||||||
)
|
|
||||||
self.map_cloud_pub = rospy.Publisher(
|
|
||||||
"map_cloud",
|
|
||||||
PointCloud2,
|
|
||||||
latch=True,
|
|
||||||
queue_size=1,
|
|
||||||
)
|
|
||||||
self.pose_pub = rospy.Publisher("pose", PoseStamped, queue_size=1)
|
|
||||||
self.quality_pub = rospy.Publisher("quality", PointCloud2, queue_size=1)
|
|
||||||
|
|
||||||
self.grasp_marker_count = 0
|
|
||||||
|
|
||||||
def clear(self):
|
|
||||||
self.clear_markers()
|
|
||||||
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)
|
|
||||||
rospy.sleep(0.1)
|
|
||||||
|
|
||||||
def clear_markers(self):
|
|
||||||
self.draw([Marker(action=Marker.DELETEALL)])
|
|
||||||
|
|
||||||
def draw(self, markers):
|
|
||||||
self.marker_pub.publish(MarkerArray(markers=markers))
|
|
||||||
|
|
||||||
def bbox(self, frame, bbox):
|
|
||||||
pose = Transform.identity()
|
|
||||||
scale = [0.004, 0.0, 0.0]
|
|
||||||
color = red
|
|
||||||
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])
|
|
||||||
|
|
||||||
def best_grasp(self, frame, grasp, qmin=0.5, qmax=1.0):
|
|
||||||
color = cmap((grasp.quality - qmin) / (qmax - qmin))
|
|
||||||
self.draw(create_grasp_markers(frame, grasp, color, "best_grasp", radius=0.006))
|
|
||||||
|
|
||||||
def grasps(self, frame, grasps, qmin=0.5, qmax=1.0):
|
|
||||||
markers = []
|
|
||||||
for i, grasp in enumerate(grasps):
|
|
||||||
color = cmap((grasp.quality - qmin) / (qmax - qmin))
|
|
||||||
markers += create_grasp_markers(frame, grasp, color, "grasps", 4 * i)
|
|
||||||
self.grasp_marker_count = len(markers)
|
|
||||||
self.draw(markers)
|
|
||||||
|
|
||||||
def clear_grasps(self):
|
|
||||||
if self.grasp_marker_count > 0:
|
|
||||||
markers = [
|
|
||||||
Marker(action=Marker.DELETE, ns="grasps", id=i)
|
|
||||||
for i in range(self.grasp_marker_count)
|
|
||||||
]
|
|
||||||
markers += [
|
|
||||||
Marker(action=Marker.DELETE, ns="best_grasp", id=i) for i in range(4)
|
|
||||||
]
|
|
||||||
self.draw(markers)
|
|
||||||
self.grasp_marker_count = 0
|
|
||||||
|
|
||||||
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(
|
|
||||||
frame,
|
|
||||||
Transform.identity(),
|
|
||||||
[0.001, 0.0, 0.0],
|
|
||||||
[0.9, 0.9, 0.9],
|
|
||||||
lines,
|
|
||||||
"rays",
|
|
||||||
)
|
|
||||||
self.draw([marker])
|
|
||||||
|
|
||||||
def map_cloud(self, frame, cloud):
|
|
||||||
points = np.asarray(cloud.points)
|
|
||||||
distances = np.expand_dims(np.asarray(cloud.colors)[:, 0], 1)
|
|
||||||
msg = to_cloud_msg(frame, points, distances=distances)
|
|
||||||
self.map_cloud_pub.publish(msg)
|
|
||||||
|
|
||||||
def path(self, frame, poses):
|
|
||||||
color = blue
|
|
||||||
points = [p.translation for p in poses]
|
|
||||||
spheres = create_sphere_list_marker(
|
|
||||||
frame,
|
|
||||||
Transform.identity(),
|
|
||||||
np.full(3, 0.01),
|
|
||||||
color,
|
|
||||||
points,
|
|
||||||
"path",
|
|
||||||
0,
|
|
||||||
)
|
|
||||||
markers = [spheres]
|
|
||||||
if len(poses) > 1:
|
|
||||||
lines = create_line_strip_marker(
|
|
||||||
frame,
|
|
||||||
Transform.identity(),
|
|
||||||
[0.005, 0.0, 0.0],
|
|
||||||
color,
|
|
||||||
points,
|
|
||||||
"path",
|
|
||||||
1,
|
|
||||||
)
|
|
||||||
markers.append(lines)
|
|
||||||
self.draw(markers)
|
|
||||||
|
|
||||||
def point(self, frame, point):
|
|
||||||
marker = create_sphere_marker(
|
|
||||||
frame,
|
|
||||||
Transform.translation(point),
|
|
||||||
np.full(3, 0.01),
|
|
||||||
[0, 0, 1],
|
|
||||||
"point",
|
|
||||||
)
|
|
||||||
self.draw([marker])
|
|
||||||
|
|
||||||
def pose(self, frame, pose):
|
|
||||||
msg = to_pose_stamped_msg(pose, frame)
|
|
||||||
self.pose_pub.publish(msg)
|
|
||||||
|
|
||||||
def quality(self, frame, voxel_size, quality, threshold=0.9):
|
|
||||||
points, values = grid_to_map_cloud(voxel_size, quality, threshold)
|
|
||||||
msg = to_cloud_msg(frame, points, intensities=values)
|
|
||||||
self.quality_pub.publish(msg)
|
|
||||||
|
|
||||||
def scene_cloud(self, frame, cloud):
|
|
||||||
msg = to_cloud_msg(frame, np.asarray(cloud.points))
|
|
||||||
self.scene_cloud_pub.publish(msg)
|
|
||||||
|
|
||||||
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))
|
|
||||||
marker = create_cam_view_marker(
|
|
||||||
frame,
|
|
||||||
view,
|
|
||||||
scale,
|
|
||||||
color,
|
|
||||||
intrinsic,
|
|
||||||
near,
|
|
||||||
far,
|
|
||||||
ns="views",
|
|
||||||
id=i,
|
|
||||||
)
|
|
||||||
markers.append(marker)
|
|
||||||
self.draw(markers)
|
|
||||||
|
|
||||||
def workspace(self, frame, size):
|
|
||||||
scale = size * 0.005
|
|
||||||
pose = Transform.identity()
|
|
||||||
scale = [scale, 0.0, 0.0]
|
|
||||||
color = [0.5, 0.5, 0.5]
|
|
||||||
lines = [
|
|
||||||
([0.0, 0.0, 0.0], [size, 0.0, 0.0]),
|
|
||||||
([size, 0.0, 0.0], [size, size, 0.0]),
|
|
||||||
([size, size, 0.0], [0.0, size, 0.0]),
|
|
||||||
([0.0, size, 0.0], [0.0, 0.0, 0.0]),
|
|
||||||
([0.0, 0.0, size], [size, 0.0, size]),
|
|
||||||
([size, 0.0, size], [size, size, size]),
|
|
||||||
([size, size, size], [0.0, size, size]),
|
|
||||||
([0.0, size, size], [0.0, 0.0, size]),
|
|
||||||
([0.0, 0.0, 0.0], [0.0, 0.0, size]),
|
|
||||||
([size, 0.0, 0.0], [size, 0.0, size]),
|
|
||||||
([size, size, 0.0], [size, size, size]),
|
|
||||||
([0.0, size, 0.0], [0.0, size, size]),
|
|
||||||
]
|
|
||||||
msg = create_line_list_marker(frame, pose, scale, color, lines, ns="workspace")
|
|
||||||
self.draw([msg])
|
|
||||||
|
|
||||||
|
|
||||||
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)
|
|
||||||
x_n = near * intrinsic.width / (2.0 * intrinsic.fx)
|
|
||||||
y_n = near * intrinsic.height / (2.0 * intrinsic.fy)
|
|
||||||
z_n = near
|
|
||||||
x_f = far * intrinsic.width / (2.0 * intrinsic.fx)
|
|
||||||
y_f = far * intrinsic.height / (2.0 * intrinsic.fy)
|
|
||||||
z_f = far
|
|
||||||
points = [
|
|
||||||
[x_n, y_n, z_n],
|
|
||||||
[-x_n, y_n, z_n],
|
|
||||||
[-x_n, y_n, z_n],
|
|
||||||
[-x_n, -y_n, z_n],
|
|
||||||
[-x_n, -y_n, z_n],
|
|
||||||
[x_n, -y_n, z_n],
|
|
||||||
[x_n, -y_n, z_n],
|
|
||||||
[x_n, y_n, z_n],
|
|
||||||
[x_f, y_f, z_f],
|
|
||||||
[-x_f, y_f, z_f],
|
|
||||||
[-x_f, y_f, z_f],
|
|
||||||
[-x_f, -y_f, z_f],
|
|
||||||
[-x_f, -y_f, z_f],
|
|
||||||
[x_f, -y_f, z_f],
|
|
||||||
[x_f, -y_f, z_f],
|
|
||||||
[x_f, y_f, z_f],
|
|
||||||
[x_n, y_n, z_n],
|
|
||||||
[x_f, y_f, z_f],
|
|
||||||
[-x_n, y_n, z_n],
|
|
||||||
[-x_f, y_f, z_f],
|
|
||||||
[-x_n, -y_n, z_n],
|
|
||||||
[-x_f, -y_f, z_f],
|
|
||||||
[x_n, -y_n, z_n],
|
|
||||||
[x_f, -y_f, z_f],
|
|
||||||
]
|
|
||||||
marker.points = [to_point_msg(p) for p in points]
|
|
||||||
return marker
|
|
||||||
Loading…
x
Reference in New Issue
Block a user