/
kinect.gazebo.xacro
93 lines (90 loc) · 3.32 KB
/
kinect.gazebo.xacro
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
<?xml version="1.0"?>
<root xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="kinect_ir_sensor" params="link_name frame_name camera_name">
<gazebo reference="${link_name}">
<sensor type="depth" name="${link_name}_ir_sensor">
<always_on>true</always_on>
<update_rate>1.0</update_rate>
<camera>
<horizontal_fov>${57.0*PI/180.0}</horizontal_fov>
<image>
<format>L8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>1.2</near>
<far>3.5</far>
</clip>
</camera>
<plugin name="${link_name}_controller" filename="libgazebo_ros_openni_kinect.so">
<baseline>0.2</baseline>
<alwaysOn>true</alwaysOn>
<updateRate>1.0</updateRate>
<cameraName>${camera_name}_ir</cameraName>
<imageTopicName>/${camera_name}/depth/image_raw</imageTopicName>
<cameraInfoTopicName>/${camera_name}/depth/camera_info</cameraInfoTopicName>
<depthImageTopicName>/${camera_name}/depth/image_raw</depthImageTopicName>
<depthImageCameraInfoTopicName>/${camera_name}/depth/camera_info</depthImageCameraInfoTopicName>
<pointCloudTopicName>/${camera_name}/depth/points</pointCloudTopicName>
<frameName>${frame_name}</frameName>
<pointCloudCutoff>0.5</pointCloudCutoff>
<distortionK1>0.00000001</distortionK1>
<distortionK2>0.00000001</distortionK2>
<distortionK3>0.00000001</distortionK3>
<distortionT1>0.00000001</distortionT1>
<distortionT2>0.00000001</distortionT2>
<CxPrime>0</CxPrime>
<Cx>0</Cx>
<Cy>0</Cy>
<focalLength>0</focalLength>
<hackBaseline>0</hackBaseline>
</plugin>
</sensor>
<material value="Gazebo/Red"/>
</gazebo>
</xacro:macro>
<xacro:macro name="kinect_rgb_sensor" params="link_name frame_name camera_name">
<gazebo reference="${link_name}">
<sensor type="depth" name="${link_name}_rgb_sensor">
<always_on>true</always_on>
<update_rate>1.0</update_rate>
<camera>
<horizontal_fov>${57.0*PI/180.0}</horizontal_fov>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>1.2</near>
<far>3.5</far>
</clip>
</camera>
<plugin name="${link_name}_controller" filename="libgazebo_ros_openni_kinect.so">
<alwaysOn>true</alwaysOn>
<updateRate>1.0</updateRate>
<cameraName>${camera_name}_rgb</cameraName>
<imageTopicName>/${camera_name}/rgb/image_raw</imageTopicName>
<cameraInfoTopicName>/${camera_name}/rgb/camera_info</cameraInfoTopicName>
<depthImageTopicName>/${camera_name}/depth/image_raw</depthImageTopicName>
<depthImageCameraInfoTopicName>/${camera_name}/depth/camera_info</depthImageCameraInfoTopicName>
<pointCloudTopicName>/${camera_name}/depth_registered/points</pointCloudTopicName>
<frameName>${frame_name}</frameName>
<pointCloudCutoff>0.5</pointCloudCutoff>
<distortionK1>0.00000001</distortionK1>
<distortionK2>0.00000001</distortionK2>
<distortionK3>0.00000001</distortionK3>
<distortionT1>0.00000001</distortionT1>
<distortionT2>0.00000001</distortionT2>
<CxPrime>0</CxPrime>
<Cx>0</Cx>
<Cy>0</Cy>
<focalLength>0</focalLength>
<hackBaseline>0</hackBaseline>
</plugin>
</sensor>
<material value="Gazebo/Red"/>
</gazebo>
</xacro:macro>
</root>