[Mini Quadrotor] add downward camera plugin

sugihara-16 · Nov 10, 2023 · 9df52cf · 9df52cf
1 parent a29d490
commit 9df52cf
Showing 1 changed file with 107 additions and 0 deletions.
diff --git a/robots/mini_quadrotor/urdf/robot.gazebo.xacro b/robots/mini_quadrotor/urdf/robot.gazebo.xacro
@@ -16,4 +16,111 @@
     <mu2>0.1</mu2>
   </gazebo>
 
+
+  <!-- <gazebo reference="livox_link"> -->
+  <!--   <sensor type="ray" name="livox_mid360_sensor"> -->
+  <!--     <update_rate>10.0</update_rate> -->
+  <!--     <pose>0 0 0 0 0 0</pose> -->
+  <!--     <visualize>false</visualize> -->
+  <!--     <ray> -->
+  <!--       <scan> -->
+  <!--         <horizontal> -->
+  <!--           <samples>400</samples> <!-\- TODO: check the true parameter -\-> -->
+  <!--           <resolution>1</resolution> -->
+  <!--           <min_angle>-${pi}</min_angle> -->
+  <!--           <max_angle>${pi}</max_angle> -->
+  <!--         </horizontal> -->
+  <!--         <vertical> -->
+  <!--           <samples>64</samples> <!-\- TODO: check the true parameter -\-> -->
+  <!--           <resolution>1</resolution> -->
+  <!--           <min_angle>-${7.0*pi/180.0}</min_angle> -->
+  <!--           <max_angle> ${52.0*pi/180.0}</max_angle> -->
+  <!--         </vertical> -->
+  <!--       </scan> -->
+  <!--       <range> -->
+  <!--         <min>0.1</min> -->
+  <!--         <max>40.0</max> -->
+  <!--         <resolution>0.001</resolution> -->
+  <!--       </range> -->
+  <!--       <noise> -->
+  <!--         <type>gaussian</type> -->
+  <!--         <mean>0.0</mean> -->
+  <!--         <stddev>0.0</stddev> -->
+  <!--       </noise> -->
+  <!--     </ray> -->
+  <!--     <plugin name="livox_lidar_link_block_laser_controller" filename="libgazebo_ros_block_laser.so"> -->
+  <!--       <robotNamespace>$(arg robot_name)</robotNamespace> -->
+  <!--       <topicName>livox/scan</topicName> -->
+  <!--       <frameName>$(arg robot_name)/livox_link</frameName> -->
+  <!--       <min_range>0.1</min_range> -->
+  <!--       <max_range>40.0</max_range> -->
+  <!--       <gaussianNoise>0.001</gaussianNoise> -->
+  <!--     </plugin> -->
+  <!--   </sensor> -->
+  <!-- </gazebo> -->
+
+  <!-- sensors -->
+  <!-- 1. stereo camera: depth, color based on left camera -->
+  <xacro:extra_module name = "downward_camera_frame" parent = "main_body">
+    <origin xyz="0.0 0.00 -0.05" rpy="0 ${pi/2} 0"/>
+    <inertial>
+      <mass value = "0.00001" />
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertia
+          ixx="0.000001" ixy="0.0" ixz="0.0"
+          iyy="0.000001" iyz="0.0"
+          izz="0.000002"/>
+    </inertial>
+  </xacro:extra_module>
+  <xacro:extra_module name = "downward_camera_optical_frame" parent = "downward_camera_frame">
+    <origin xyz="0 0 0" rpy="${-pi / 2} 0 ${-pi / 2}"/>
+    <inertial>
+      <mass value = "0.00001" />
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertia
+          ixx="0.000001" ixy="0.0" ixz="0.0"
+          iyy="0.000001" iyz="0.0"
+          izz="0.000002"/>
+    </inertial>
+  </xacro:extra_module>
+
+  <!-- gazebo plugin -->
+  <gazebo reference="downward_camera_frame">
+    <sensor type="camera" name="downward_camera">
+      <update_rate>30.0</update_rate>
+      <camera name="head">
+        <horizontal_fov>1.57</horizontal_fov>
+        <image>
+          <width>640</width>
+          <height>480</height>
+          <format>RGB</format>
+        </image>
+        <clip>
+          <near>0.02</near>
+          <far>300</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
+        <robotNamespace>$(arg robot_name)</robotNamespace>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>0.0</updateRate>
+        <cameraName>downword_camera</cameraName>
+        <imageTopicName>image_raw</imageTopicName>
+        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+        <frameName>downward_camera_optical_frame</frameName>
+        <hackBaseline>0.07</hackBaseline>
+        <distortionK1>0.0</distortionK1>
+        <distortionK2>0.0</distortionK2>
+        <distortionK3>0.0</distortionK3>
+        <distortionT1>0.0</distortionT1>
+        <distortionT2>0.0</distortionT2>
+      </plugin>
+    </sensor>
+  </gazebo>
+
 </robot>