Skip to content

Commit 7f6b552

Browse files
michaelpanticmichaelpantic
authored
Merge pull request #589 from ethz-asl/feature/pmd_depth_cam
Adds PMD Monstar depthcam as plugin
2 parents bd65153 + ca56b03 commit 7f6b552

4 files changed

Lines changed: 198 additions & 2 deletions

File tree

‎rotors_description/urdf/pmd_monstar.xacro‎

Lines changed: 160 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,160 @@
1+
<?xml version="1.0"?>
2+
<robot name="$(arg mav_name)" xmlns:xacro="http://ros.org/wiki/xacro">
3+
<xacro:include filename="$(find rotors_description)/urdf/component_snippets.xacro" />
4+
5+
<link name="rgbd_camera/base_link">
6+
<collision>
7+
<origin xyz="0 -0.0175 0" rpy="0 0 0" />
8+
<geometry>
9+
<box size="0.04 0.04 0.02" />
10+
</geometry>
11+
</collision>
12+
<visual>
13+
<origin xyz="0 0 0" rpy="0 0 0" />
14+
<geometry>
15+
<box size="0.04 0.04 0.02"/>
16+
</geometry>
17+
<material name="black" />
18+
</visual>
19+
</link>
20+
21+
<link name="rgbd_camera/base_link_gazebo"/>
22+
23+
<joint name="rgbd_camera/base_link_gazebo_joint" type="fixed">
24+
<origin xyz="0 0 0" rpy="${pi/2} ${-pi/2} 0" />
25+
<parent link="rgbd_camera/base_link"/>
26+
<child link="rgbd_camera/base_link_gazebo"/>
27+
28+
</joint>
29+
30+
31+
<!-- Define Macro to add a depth camera using noisy depth plugin -->
32+
<xacro:macro name="rgbd_sensor_depth_macro"
33+
params="namespace parent_link camera_suffix frame_rate width height fov max_range min_range baseline *origin">
34+
<link name="${namespace}/camera_${camera_suffix}_link">
35+
<collision>
36+
<origin xyz="0 0 0" rpy="0 0 0" />
37+
<geometry>
38+
<cylinder length="0.01" radius="0.007" />
39+
</geometry>
40+
</collision>
41+
<inertial>
42+
<mass value="1e-5" />
43+
<origin xyz="0 0 0" rpy="0 0 0" />
44+
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
45+
</inertial>
46+
</link>
47+
<joint name="${namespace}/camera_${camera_suffix}_joint" type="fixed">
48+
<xacro:insert_block name="origin" />
49+
<parent link="${parent_link}" />
50+
<child link="${namespace}/camera_${camera_suffix}_link" />
51+
<limit upper="0" lower="0" effort="0" velocity="0" />
52+
</joint>
53+
54+
<link name="${namespace}/camera_${camera_suffix}_optical_center_link" />
55+
<joint name="${namespace}/camera_${camera_suffix}_optical_center_joint" type="fixed">
56+
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
57+
<parent link="${namespace}/camera_${camera_suffix}_link" />
58+
<child link="${namespace}/camera_${camera_suffix}_optical_center_link" />
59+
<limit upper="0" lower="0" effort="0" velocity="0" />
60+
</joint>
61+
62+
<!-- Call rgb sensor macro-->
63+
64+
<gazebo reference="${namespace}/camera_${camera_suffix}_link">
65+
<sensor type="depth" name="${namespace}_camera_${camera_suffix}">
66+
<always_on>true</always_on>
67+
<update_rate>${frame_rate}</update_rate>
68+
<camera>
69+
<horizontal_fov>${fov}</horizontal_fov>
70+
<image>
71+
<format>L8</format>
72+
<width>${width}</width>
73+
<height>${height}</height>
74+
</image>
75+
<clip>
76+
<near>${min_range}</near>
77+
<far>${max_range}</far>
78+
</clip>
79+
</camera>
80+
<plugin name="${namespace}_camera_${camera_suffix}" filename="librotors_gazebo_noisydepth_plugin.so">
81+
<robotNamespace>${namespace}</robotNamespace>
82+
<alwaysOn>true</alwaysOn>
83+
<baseline>${baseline}</baseline>
84+
<updateRate>${frame_rate}</updateRate>
85+
<cameraName>camera_${camera_suffix}</cameraName>
86+
<horizontal_fov>${fov}</horizontal_fov>
87+
<depthNoiseModelName>PMD</depthNoiseModelName>
88+
<depthNoiseMinDist>0.1</depthNoiseMinDist>
89+
<depthNoiseMaxDist>6.0</depthNoiseMaxDist>
90+
91+
<imageTopicName>camera/image_raw</imageTopicName>
92+
<cameraInfoTopicName>camera/camera_info</cameraInfoTopicName>
93+
<depthImageTopicName>depth/image_raw</depthImageTopicName>
94+
<depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
95+
<pointCloudTopicName>depth/points</pointCloudTopicName>
96+
<frameName>camera_${camera_suffix}_optical_center_link</frameName>
97+
<distortionK1>0.0</distortionK1>
98+
<distortionK2>0.0</distortionK2>
99+
<distortionK3>0.0</distortionK3>
100+
<distortionT1>0.0</distortionT1>
101+
<distortionT2>0.0</distortionT2>
102+
</plugin>
103+
</sensor>
104+
105+
<sensor type="camera" name="${namespace}_camera_${camera_suffix}">
106+
<update_rate>${frame_rate}</update_rate>
107+
<camera name="intensity">
108+
<horizontal_fov>${fov}</horizontal_fov>
109+
<image>
110+
<width>${width}</width>
111+
<height>${height}</height>
112+
<format>L8</format>
113+
</image>
114+
<clip>
115+
<near>0.01</near>
116+
<far>100.0</far>
117+
</clip>
118+
<noise>
119+
<type>gaussian</type>
120+
<!-- Noise is sampled independently per pixel on each frame.
121+
That pixel's noise value is added to each of its color
122+
channels, which at that point lie in the range [0,1]. -->
123+
<mean>0.07</mean>
124+
<stddev>0.0</stddev>
125+
</noise>
126+
</camera>
127+
<plugin name="${namespace}_camera_${camera_suffix}_controller" filename="libgazebo_ros_camera.so">
128+
<robotNamespace>${namespace}</robotNamespace>
129+
<alwaysOn>true</alwaysOn>
130+
<updateRate>${frame_rate}</updateRate>
131+
<cameraName>camera_${camera_suffix}</cameraName>
132+
<imageTopicName>intensity/image_raw</imageTopicName>
133+
<cameraInfoTopicName>intensity/camera_info</cameraInfoTopicName>
134+
<frameName>camera_${camera_suffix}_optical_center_link</frameName>
135+
<hackBaseline>0.0</hackBaseline>
136+
<distortionK1>0.0</distortionK1>
137+
<distortionK2>0.0</distortionK2>
138+
<distortionK3>0.0</distortionK3>
139+
<distortionT1>0.0</distortionT1>
140+
<distortionT2>0.0</distortionT2>
141+
</plugin>
142+
</sensor>
143+
</gazebo>
144+
145+
</xacro:macro>
146+
147+
<xacro:rgbd_sensor_depth_macro
148+
namespace="${namespace}/rgbd" parent_link="rgbd_camera/base_link_gazebo"
149+
camera_suffix="depth"
150+
frame_rate="5.0"
151+
fov = "1.59174"
152+
baseline = "0.05"
153+
width = "352"
154+
height = "287"
155+
max_range="7.0"
156+
min_range="0.05">
157+
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
158+
</xacro:rgbd_sensor_depth_macro>
159+
160+
</robot>

‎rotors_gazebo_plugins/include/rotors_gazebo_plugins/depth_noise_model.hpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,14 @@ class KinectDepthNoiseModel : public DepthNoiseModel {
4545
void ApplyNoise(uint32_t width, uint32_t height, float *data);
4646
};
4747

48+
class PMDDepthNoiseModel : public DepthNoiseModel {
49+
public:
50+
PMDDepthNoiseModel() : DepthNoiseModel() {}
51+
52+
void ApplyNoise(uint32_t width, uint32_t height, float *data);
53+
};
54+
55+
4856
class D435DepthNoiseModel : public DepthNoiseModel {
4957
public:
5058
D435DepthNoiseModel()

‎rotors_gazebo_plugins/src/depth_noise_model.cpp‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -71,3 +71,25 @@ void KinectDepthNoiseModel::ApplyNoise(const uint32_t width,
7171
}
7272
}
7373
}
74+
75+
76+
77+
void PMDDepthNoiseModel::ApplyNoise(const uint32_t width,
78+
const uint32_t height, float *data) {
79+
if (data == nullptr) {
80+
return;
81+
}
82+
83+
// 1% error claimed by PMD
84+
Eigen::Map<Eigen::VectorXf> data_vector_map(data, width * height);
85+
Eigen::VectorXf var_noise = data_vector_map.array() * 0.01;
86+
87+
// Sample noise for each pixel and transform variance according to error at this depth.
88+
for (int i = 0; i < width * height; ++i) {
89+
if (InRange(data_vector_map[i])) {
90+
data_vector_map[i] += this->dist(this->gen) * var_noise(i);
91+
} else {
92+
data_vector_map[i] = this->bad_point;
93+
}
94+
}
95+
}

‎rotors_gazebo_plugins/src/gazebo_noisydepth_plugin.cpp‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -94,7 +94,13 @@ void GazeboNoisyDepth::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) {
9494
this->noise_model.reset(new KinectDepthNoiseModel());
9595

9696
/* no other properties for Kinect */
97-
} else if (boost::iequals(noise_model, "D435")) {
97+
} else if(boost::iequals(noise_model, "PMD")) {
98+
this->noise_model.reset(new PMDDepthNoiseModel());
99+
100+
/* no other properties for PMD */
101+
}
102+
103+
else if (boost::iequals(noise_model, "D435")) {
98104
D435DepthNoiseModel *model = new D435DepthNoiseModel();
99105
this->noise_model.reset(model);
100106

@@ -295,4 +301,4 @@ void GazeboNoisyDepth::PublishCameraInfo() {
295301
}
296302
}
297303
}
298-
}
304+
}

0 commit comments

Comments
 (0)