Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:38:27 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
conf/simulator/gazebo/airframes/bebop2.xml
+14
View File
@@ -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>
conf/simulator/gazebo/models/bebop2/bebop2.sdf
+299
View File
@@ -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>
conf/simulator/gazebo/models/bebop2/model.config
+15
View File
@@ -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>
conf/simulator/gazebo/models/bebop2_with_slamdunk/bebop2_with_slamdunk.sdf
+310
View File
@@ -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>
conf/simulator/gazebo/models/bebop2_with_slamdunk/model.config
+15
View File
@@ -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>
conf/simulator/gazebo/models/slamdunk/model.config
+15
View File
@@ -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>
conf/simulator/gazebo/models/slamdunk/slamdunk.sdf
+134
View File
@@ -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>
sw/simulator/nps/nps_autopilot_rotorcraft.c
+3 -1
View File
@@ -131,7 +131,9 @@ void nps_autopilot_run_step(double time)
#if USE_SONAR #if USE_SONAR
if (nps_sensors_sonar_available()) { if (nps_sensors_sonar_available()) {
float dist = (float) sensors.sonar.value; float dist = (float) sensors.sonar.value;
AbiSendMsgAGL(AGL_SONAR_NPS_ID, dist); if (dist >= 0.0) {
AbiSendMsgAGL(AGL_SONAR_NPS_ID, dist);
}
#ifdef SENSOR_SYNC_SEND_SONAR #ifdef SENSOR_SYNC_SEND_SONAR
uint16_t foo = 0; uint16_t foo = 0;
sw/simulator/nps/nps_fdm_gazebo.cpp
+43 -2
View File
@@ -121,6 +121,8 @@ static void gazebo_read_range_sensors(void);
#endif #endif
std::shared_ptr<gazebo::sensors::SonarSensor> sonar = NULL;
/// Holds all necessary NPS FDM state information /// Holds all necessary NPS FDM state information
struct NpsFdm fdm; struct NpsFdm fdm;
@@ -328,6 +330,14 @@ static void init_gazebo(void)
string gazebodir = pprz_home + gazebo_home; string gazebodir = pprz_home + gazebo_home;
cout << "Gazebo directory: " << gazebodir << endl; cout << "Gazebo directory: " << gazebodir << endl;
if (getenv("ROS_MASTER_URI")) {
// Launch with ROS support
cout << "Add ROS plugins... ";
gazebo::addPlugin("libgazebo_ros_paths_plugin.so");
gazebo::addPlugin("libgazebo_ros_api_plugin.so");
cout << "ok" << endl;
}
if (!gazebo::setupServer(0, NULL)) { if (!gazebo::setupServer(0, NULL)) {
cout << "Failed to start Gazebo, exiting." << endl; cout << "Failed to start Gazebo, exiting." << endl;
std::exit(-1); std::exit(-1);
@@ -431,10 +441,34 @@ static void init_gazebo(void)
gazebo::runWorld(world, 1); gazebo::runWorld(world, 1);
cout << "Sensors initialized..." << endl; cout << "Sensors initialized..." << endl;
// activate collision sensor // Find sensors
// Contact sensor
gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance(); gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
ct = static_pointer_cast < gazebo::sensors::ContactSensor > (mgr->GetSensor("contactsensor")); ct = static_pointer_cast < gazebo::sensors::ContactSensor > (mgr->GetSensor("contactsensor"));
ct->SetActive(true); ct->SetActive(true);
// Sonar
sonar = static_pointer_cast<gazebo::sensors::SonarSensor>(mgr->GetSensor("sonarsensor"));
if(sonar) {
cout << "Found sonar" << endl;
}
gazebo::physics::LinkPtr link = model->GetLink("sonar");
if (link) {
// Get a pointer to the sensor using its full name
if (link->GetSensorCount() != 1) {
cout << "ERROR: Link '" << link->GetName()
<< "' should only contain 1 sensor!" << endl;
} else {
string name = link->GetSensorName(0);
sonar = static_pointer_cast< gazebo::sensors::SonarSensor > (mgr->GetSensor(name));
if (!sonar) {
cout << "ERROR: Could not get pointer to '" << name << "'!" << endl;
} else {
// Activate sensor
sonar->SetActive(true);
}
}
}
// Overwrite motor directions as defined by motor_mixing // Overwrite motor directions as defined by motor_mixing
#ifdef MOTOR_MIXING_YAW_COEF #ifdef MOTOR_MIXING_YAW_COEF
@@ -498,7 +532,14 @@ static void gazebo_read(void)
fdm.lla_pos_pprz = fdm.lla_pos; // Don't really care... fdm.lla_pos_pprz = fdm.lla_pos; // Don't really care...
fdm.lla_pos_geod = fdm.lla_pos; fdm.lla_pos_geod = fdm.lla_pos;
fdm.lla_pos_geoc = fdm.lla_pos; fdm.lla_pos_geoc = fdm.lla_pos;
fdm.agl = pose.Pos().Z(); // TODO Measure with sensor
if(sonar) {
double agl = sonar->Range();
if (agl > sonar->RangeMax()) agl = -1.0;
fdm.agl = agl;
} else {
fdm.agl = pose.Pos().Z(); // TODO Measure with sensor
}
/* velocity */ /* velocity */
fdm.ecef_ecef_vel = to_pprz_ecef(sphere->VelocityTransform(vel, gazebo::common::SphericalCoordinates::LOCAL, fdm.ecef_ecef_vel = to_pprz_ecef(sphere->VelocityTransform(vel, gazebo::common::SphericalCoordinates::LOCAL,
sw/tools/gzserver_ros_launcher.sh
Executable
+15
View File
@@ -0,0 +1,15 @@
#!/bin/sh
# This script sets up environment variables for gazebo_ros
# Paparazzi paths
export GAZEBO_MODEL_PATH="$PAPARAZZI_HOME/conf/simulator/gazebo/models:$GAZEBO_MODEL_PATH"
export GAZEBO_MODEL_PATH="$PAPARAZZI_HOME/sw/ext/tudelft_gazebo_models/models:$GAZEBO_MODEL_PATH"
# ROS and Gazebo defaults
ROS_SETUP=`locate --regex 'ros/[a-z]*/setup.sh$'`
GAZEBO_SETUP=`locate --regex 'gazebo/setup.sh$'`
. $ROS_SETUP
. $GAZEBO_SETUP
# Launch NPS-Gazebo
exec $PAPARAZZI_HOME/sw/simulator/pprzsim-launch $@