[gazebo] add ROS support and sonar sensor (#2399)

This commit is contained in:
Tom van Dijk
2019-02-28 09:58:12 +01:00
committed by Gautier Hattenberger
parent a807bf7afe
commit 2430b31f0f
10 changed files with 863 additions and 3 deletions
@@ -0,0 +1,14 @@
<!DOCTYPE airframe SYSTEM "../../../airframes/airframe.dtd">
<airframe>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_THRUSTS" value="2.80, 2.80, 2.80, 2.80" type="float[]"/>
<define name="ACTUATOR_TORQUES" value="0.05, 0.05, 0.05, 0.05" type="float[]"/>
<define name="ACTUATOR_TIME_CONSTANTS" value="0.02, 0.02, 0.02, 0.02" type="float[]"/>
<define name="ACTUATOR_MAX_ANGULAR_MOMENTUM" value="0.5, 0.5, 0.5, 0.5" type="float[]"/>
<define name="GAZEBO_AC_NAME" value="bebop2" type="string"/>
<define name="BYPASS_AHRS" value="1"/>
<define name="BYPASS_INS" value="1"/>
<!-- <define name="SIMULATE_MT9F002" value="1"/> --><!-- TODO -->
</section>
</airframe>
@@ -0,0 +1,299 @@
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="bebop2">
<pose>0 0 .1 0 0 0</pose>
<link name="chassis">
<velocity_decay>
<linear>0.001</linear>
</velocity_decay>
<inertial>
<mass>0.536</mass>
<inertia>
<ixx> 0.000906 </ixx>
<iyy> 0.002225 </iyy>
<izz> 0.002500 </izz>
<ixy> 0. </ixy>
<ixz> 0. </ixz>
<iyz> 0. </iyz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.4 0.4 0.05</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.15 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.82 0.82 0.82 1</diffuse>
<ambient>0.82 0.82 0.82 1</ambient>
</material>
</visual>
<sensor name="contactsensor" type="contact">
<contact>
<collision>collision</collision>
</contact>
</sensor>
</link>
<!-- MOTORS -->
<link name="nw_motor">
<pose>0.0875 0.1125 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.02</length>
</cylinder>
</geometry>
<material>
<diffuse>0.82 0.82 0.82 1</diffuse>
<ambient>0.82 0.82 0.82 1</ambient>
</material>
</visual>
</link>
<joint type="fixed" name="nw_motor_joint">
<parent>chassis</parent>
<child>nw_motor</child>
</joint>
<link name="se_motor">
<pose>-0.0875 -0.1125 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.02</length>
</cylinder>
</geometry>
<material>
<diffuse>0.1 0.1 0.1 1</diffuse>
<ambient>0.1 0.1 0.1 1</ambient>
</material>
</visual>
</link>
<joint type="fixed" name="se_motor_joint">
<parent>chassis</parent>
<child>se_motor</child>
</joint>
<link name="ne_motor">
<pose>0.0875 -0.1125 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.02</length>
</cylinder>
</geometry>
<material>
<diffuse>0.82 0.82 0.82 1</diffuse>
<ambient>0.82 0.82 0.82 1</ambient>
</material>
</visual>
</link>
<joint type="fixed" name="ne_motor_joint">
<parent>chassis</parent>
<child>ne_motor</child>
</joint>
<link name="sw_motor">
<pose>-0.0875 0.1125 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.02</length>
</cylinder>
</geometry>
<material>
<diffuse>0.1 0.1 0.1 1</diffuse>
<ambient>0.1 0.1 0.1 1</ambient>
</material>
</visual>
</link>
<joint type="fixed" name="sw_motor_joint">
<parent>chassis</parent>
<child>sw_motor</child>
</joint>
<link name="sonar">
<pose>0 0 0 0 0 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor name="sonarsensor" type="sonar">
<sonar>
<min>0.20</min>
<max>5.0</max>
<radius>0.50</radius>
</sonar>
</sensor>
</link>
<joint name="sonar_joint" type="fixed">
<parent>chassis</parent>
<child>sonar</child>
</joint>
<!-- CAMERAS -->
<link name="front_camera">
<pose>0.15 0 0 -1.57 0.33 0</pose><!-- Bebop camera output is rotated 90 degrees and pitched slightly downwards -->
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="wideanglecamera" name="front_camera">
<update_rate>15.0</update_rate><!-- adjust with MT9F002_TARGET_FPS -->
<camera name="front_camera">
<image>
<width>3746</width><!-- with MT9F002_OUTPUT_SCALER = 1.00, will be scaled by NPS --><!-- Reduced to 3 rad FoV for Gazebo 7 compatibility -->
<height>3288</height>
<format>R8G8B8</format>
</image>
<horizontal_fov>3.00</horizontal_fov>
<lens>
<type>equisolid_angle</type>
<scale_to_hfov>true</scale_to_hfov>
<env_texture_size>2048</env_texture_size><!-- with MT9F002_OUTPUT_SCALER = 1.00, will be scaled by NPS -->
</lens>
<clip>
<near>0.01</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="front_camera_joint" type="fixed">
<parent>chassis</parent>
<child>front_camera</child>
</joint>
<link name="bottom_camera">
<pose>0 0 0 0 1.57 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="bottom_camera">
<update_rate>30.0</update_rate>
<camera name="bottom_camera">
<horizontal_fov>0.7175</horizontal_fov>
<image>
<width>240</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="bottom_camera_joint" type="fixed">
<parent>chassis</parent>
<child>bottom_camera</child>
</joint>
</model>
</sdf>
@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<model>
<name>Bebop 2 (Paparazzi)</name>
<version>1.0</version>
<sdf version='1.4'>bebop2.sdf</sdf>
<author>
<name>Tom van Dijk</name>
<email>tomvand@users.noreply.github.com</email>
</author>
<description>
Simple Bebop 2 model for use with Paparazzi's NPS (http://wiki.paparazziuav.org).
</description>
</model>
@@ -0,0 +1,310 @@
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="bebop2_with_slamdunk">
<pose>0 0 .1 0 0 0</pose>
<link name="chassis">
<velocity_decay>
<linear>0.001</linear>
</velocity_decay>
<inertial>
<mass>0.536</mass>
<inertia>
<ixx> 0.000906 </ixx>
<iyy> 0.002225 </iyy>
<izz> 0.002500 </izz>
<ixy> 0. </ixy>
<ixz> 0. </ixz>
<iyz> 0. </iyz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.4 0.4 0.05</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.15 0.05 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.82 0.82 0.82 1</diffuse>
<ambient>0.82 0.82 0.82 1</ambient>
</material>
</visual>
<sensor name="contactsensor" type="contact">
<contact>
<collision>collision</collision>
</contact>
</sensor>
</link>
<!-- SLAMDUNK -->
<include name="slamdunk">
<uri>model://slamdunk</uri>
<pose>0 0 .05 0 0 0</pose>
</include>
<joint type="fixed" name="slamdunk_joint">
<parent>chassis</parent>
<child>slamdunk::slamdunk_link</child>
</joint>
<!-- MOTORS -->
<link name="nw_motor">
<pose>0.0875 0.1125 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.02</length>
</cylinder>
</geometry>
<material>
<diffuse>0.82 0.82 0.82 1</diffuse>
<ambient>0.82 0.82 0.82 1</ambient>
</material>
</visual>
</link>
<joint type="fixed" name="nw_motor_joint">
<parent>chassis</parent>
<child>nw_motor</child>
</joint>
<link name="se_motor">
<pose>-0.0875 -0.1125 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.02</length>
</cylinder>
</geometry>
<material>
<diffuse>0.1 0.1 0.1 1</diffuse>
<ambient>0.1 0.1 0.1 1</ambient>
</material>
</visual>
</link>
<joint type="fixed" name="se_motor_joint">
<parent>chassis</parent>
<child>se_motor</child>
</joint>
<link name="ne_motor">
<pose>0.0875 -0.1125 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.02</length>
</cylinder>
</geometry>
<material>
<diffuse>0.82 0.82 0.82 1</diffuse>
<ambient>0.82 0.82 0.82 1</ambient>
</material>
</visual>
</link>
<joint type="fixed" name="ne_motor_joint">
<parent>chassis</parent>
<child>ne_motor</child>
</joint>
<link name="sw_motor">
<pose>-0.0875 0.1125 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.02</length>
</cylinder>
</geometry>
<material>
<diffuse>0.1 0.1 0.1 1</diffuse>
<ambient>0.1 0.1 0.1 1</ambient>
</material>
</visual>
</link>
<joint type="fixed" name="sw_motor_joint">
<parent>chassis</parent>
<child>sw_motor</child>
</joint>
<link name="sonar">
<pose>0 0 0 0 0 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor name="sonarsensor" type="sonar">
<sonar>
<min>0.20</min>
<max>5.0</max>
<radius>0.50</radius>
</sonar>
</sensor>
</link>
<joint name="sonar_joint" type="fixed">
<parent>chassis</parent>
<child>sonar</child>
</joint>
<!-- CAMERAS -->
<link name="front_camera">
<pose>0.15 0 0 -1.57 0.33 0</pose><!-- Bebop camera output is rotated 90 degrees and pitched slightly downwards -->
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="wideanglecamera" name="front_camera">
<update_rate>15.0</update_rate><!-- adjust with MT9F002_TARGET_FPS -->
<camera name="front_camera">
<image>
<width>3746</width><!-- with MT9F002_OUTPUT_SCALER = 1.00, will be scaled by NPS --><!-- Reduced to 3 rad FoV for Gazebo 7 compatibility -->
<height>3288</height>
<format>R8G8B8</format>
</image>
<horizontal_fov>3.00</horizontal_fov>
<lens>
<type>equisolid_angle</type>
<scale_to_hfov>true</scale_to_hfov>
<env_texture_size>2048</env_texture_size><!-- with MT9F002_OUTPUT_SCALER = 1.00, will be scaled by NPS -->
</lens>
<clip>
<near>0.01</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="front_camera_joint" type="fixed">
<parent>chassis</parent>
<child>front_camera</child>
</joint>
<link name="bottom_camera">
<pose>0 0 0 0 1.57 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="bottom_camera">
<update_rate>30.0</update_rate>
<camera name="bottom_camera">
<horizontal_fov>0.7175</horizontal_fov>
<image>
<width>240</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="bottom_camera_joint" type="fixed">
<parent>chassis</parent>
<child>bottom_camera</child>
</joint>
</model>
</sdf>
@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<model>
<name>Bebop 2 (Paparazzi) with SLAMDunk</name>
<version>1.0</version>
<sdf version='1.4'>bebop2_with_slamdunk.sdf</sdf>
<author>
<name>Tom van Dijk</name>
<email>tomvand@users.noreply.github.com</email>
</author>
<description>
Bebop 2 with SLAMDunk
</description>
</model>
@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<model>
<name>SLAMDunk</name>
<version>1.0</version>
<sdf version='1.4'>slamdunk.sdf</sdf>
<author>
<name>Tom van Dijk</name>
<email>tomvand@users.noreply.github.com</email>
</author>
<description>
Approximate parrot SLAMDunk model
</description>
</model>
@@ -0,0 +1,134 @@
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="slamdunk">
<link name="slamdunk_link">
<visual name="visual">
<geometry>
<box>
<size>0.12 0.06 0.025</size>
</box>
</geometry>
<material>
<diffuse>0.1 0.1 0.1 1</diffuse>
<ambient>0.1 0.1 0.1 1</ambient>
</material>
</visual>
<visual name="visual2">
<pose>0.04 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.005 0.2 0.025</size>
</box>
</geometry>
<material>
<diffuse>0.1 0.1 0.1 1</diffuse>
<ambient>0.1 0.1 0.1 1</ambient>
</material>
</visual>
<inertial>
<mass>0.135</mass>
<inertia>
<ixx> 0.00010 </ixx>
<iyy> 0.00020 </iyy>
<izz> 0.00020 </izz>
</inertia>
</inertial>
<collision name="slamdunk_collision">
<geometry>
<box>
<size>0.12 0.06 0.025</size>
</box>
</geometry>
</collision>
<!-- <sensor type="depth" name="depth_camera">
<pose>0.05 0.1 0 0 0 0</pose>
<update_rate>30.0</update_rate>
<camera name="depth_cam">
<horizontal_fov>1.19</horizontal_fov>
<image>
<width>96</width>
<height>96</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>1000</far>
</clip>
</camera>
<plugin name="depth_controller" filename="libgazebo_ros_depth_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>slamdunk_camera</cameraName>
<imageTopicName>rgb/image_rect_color</imageTopicName>
<cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
<depthImageTopicName>/depth_map/image</depthImageTopicName>
<depthImageCameraInfoTopicName>/depth_map/camera_info</depthImageCameraInfoTopicName>
<pointCloudTopicName>points</pointCloudTopicName>
<frameName>cam_optical_frame</frameName>
<pointCloudCutoff>0.02</pointCloudCutoff>
</plugin>
</sensor> -->
<sensor type="camera" name="left_rgb_rect">
<pose>0.05 0.1 0 0 0 0</pose>
<update_rate>30.0</update_rate>
<camera name="left_rgb_rect_cam">
<horizontal_fov>1.19</horizontal_fov>
<image>
<width>576</width>
<height>576</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>1000</far>
</clip>
</camera>
<plugin name="left_cam_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>30.0</updateRate>
<cameraName>left_rgb_rect</cameraName>
<imageTopicName>image_rect_color</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>cam_optical_frame2</frameName>
</plugin>
</sensor>
<sensor type="camera" name="right_rgb_rect">
<pose>0.05 -0.1 0 0 0 0</pose>
<update_rate>30.0</update_rate>
<camera name="right_rgb_rect_cam">
<horizontal_fov>1.19</horizontal_fov>
<image>
<width>576</width>
<height>576</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>1000</far>
</clip>
</camera>
<plugin name="right_cam_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>30.0</updateRate>
<cameraName>right_rgb_rect</cameraName>
<imageTopicName>image_rect_color</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>cam_optical_frame3</frameName>
</plugin>
</sensor>
<sensor type="imu" name="imu_sensor">
<update_rate>200</update_rate>
<imu>
</imu>
<plugin name="imu_controller" filename="libgazebo_ros_imu_sensor.so">
<alwaysOn>true</alwaysOn>
<updateRateHZ>0.0</updateRateHZ>
<robotNamespace></robotNamespace>
<topicName>imu</topicName>
<bodyName>slamdunk_link</bodyName>
<frameName>slamdunk_link</frameName>
</plugin>
</sensor>
</link>
</model>
</sdf>