505 lines
17 KiB
XML
505 lines
17 KiB
XML
<?xml version="1.0" ?>
|
|
<!--
|
|
ArduPilot SITL + Gazebo Drone Landing World
|
|
|
|
This world integrates with ArduPilot SITL via the ardupilot_gazebo plugin.
|
|
The drone receives motor commands from ArduPilot and sends sensor data back.
|
|
|
|
Prerequisites:
|
|
1. ArduPilot Gazebo Plugin: https://github.com/ArduPilot/ardupilot_gazebo
|
|
2. ArduPilot SITL with JSON backend
|
|
|
|
Usage:
|
|
1. Start Gazebo: gz sim -r gazebo/worlds/ardupilot_drone.sdf
|
|
2. Start SITL: sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON
|
|
3. Run bridge: python run_ardupilot.py --no-sitl
|
|
-->
|
|
<sdf version="1.9">
|
|
<world name="ardupilot_landing_world">
|
|
|
|
<!-- Physics - match ArduPilot defaults -->
|
|
<physics name="1ms" type="ode">
|
|
<max_step_size>0.001</max_step_size>
|
|
<real_time_factor>1.0</real_time_factor>
|
|
<real_time_update_rate>1000</real_time_update_rate>
|
|
</physics>
|
|
|
|
<!-- Gazebo Plugins -->
|
|
<plugin filename="gz-sim-physics-system"
|
|
name="gz::sim::systems::Physics"/>
|
|
<plugin filename="gz-sim-user-commands-system"
|
|
name="gz::sim::systems::UserCommands"/>
|
|
<plugin filename="gz-sim-scene-broadcaster-system"
|
|
name="gz::sim::systems::SceneBroadcaster"/>
|
|
<plugin filename="gz-sim-sensors-system"
|
|
name="gz::sim::systems::Sensors">
|
|
<render_engine>ogre2</render_engine>
|
|
</plugin>
|
|
<plugin filename="gz-sim-imu-system"
|
|
name="gz::sim::systems::Imu"/>
|
|
<plugin filename="gz-sim-air-pressure-system"
|
|
name="gz::sim::systems::AirPressure"/>
|
|
<plugin filename="gz-sim-altimeter-system"
|
|
name="gz::sim::systems::Altimeter"/>
|
|
<plugin filename="gz-sim-navsat-system"
|
|
name="gz::sim::systems::NavSat"/>
|
|
|
|
<!-- Lighting -->
|
|
<light type="directional" name="sun">
|
|
<cast_shadows>true</cast_shadows>
|
|
<pose>0 0 10 0 0 0</pose>
|
|
<diffuse>0.8 0.8 0.8 1</diffuse>
|
|
<specular>0.2 0.2 0.2 1</specular>
|
|
<direction>-0.5 0.1 -0.9</direction>
|
|
</light>
|
|
|
|
<light type="directional" name="fill_light">
|
|
<cast_shadows>false</cast_shadows>
|
|
<pose>0 0 10 0 0 0</pose>
|
|
<diffuse>0.3 0.3 0.3 1</diffuse>
|
|
<specular>0.0 0.0 0.0 1</specular>
|
|
<direction>0.5 0.3 -0.5</direction>
|
|
</light>
|
|
|
|
<!-- Ground Plane -->
|
|
<model name="ground_plane">
|
|
<static>true</static>
|
|
<link name="link">
|
|
<collision name="collision">
|
|
<geometry>
|
|
<plane>
|
|
<normal>0 0 1</normal>
|
|
<size>100 100</size>
|
|
</plane>
|
|
</geometry>
|
|
<surface>
|
|
<friction>
|
|
<ode>
|
|
<mu>1</mu>
|
|
<mu2>1</mu2>
|
|
</ode>
|
|
</friction>
|
|
</surface>
|
|
</collision>
|
|
<visual name="visual">
|
|
<geometry>
|
|
<plane>
|
|
<normal>0 0 1</normal>
|
|
<size>100 100</size>
|
|
</plane>
|
|
</geometry>
|
|
<material>
|
|
<ambient>0.5 0.5 0.5 1</ambient>
|
|
<diffuse>0.7 0.7 0.7 1</diffuse>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
</model>
|
|
|
|
<!-- Landing Pad (Rover) - Velocity controlled for moving target -->
|
|
<model name="landing_pad">
|
|
<pose>0 0 0.15 0 0 0</pose>
|
|
|
|
<link name="base_link">
|
|
<inertial>
|
|
<mass>10.0</mass>
|
|
<inertia>
|
|
<ixx>1.0</ixx><ixy>0</ixy><ixz>0</ixz>
|
|
<iyy>1.0</iyy><iyz>0</iyz>
|
|
<izz>1.0</izz>
|
|
</inertia>
|
|
</inertial>
|
|
|
|
<visual name="visual">
|
|
<geometry>
|
|
<box><size>1.0 1.0 0.3</size></box>
|
|
</geometry>
|
|
<material>
|
|
<ambient>0.1 0.5 0.1 1</ambient>
|
|
<diffuse>0.2 0.7 0.2 1</diffuse>
|
|
</material>
|
|
</visual>
|
|
<collision name="collision">
|
|
<geometry>
|
|
<box><size>1.0 1.0 0.3</size></box>
|
|
</geometry>
|
|
</collision>
|
|
|
|
<!-- H marker -->
|
|
<visual name="h_center">
|
|
<pose>0 0 0.151 0 0 0</pose>
|
|
<geometry><box><size>0.6 0.1 0.001</size></box></geometry>
|
|
<material>
|
|
<ambient>1 1 1 1</ambient>
|
|
<diffuse>1 1 1 1</diffuse>
|
|
</material>
|
|
</visual>
|
|
<visual name="h_left">
|
|
<pose>-0.25 0 0.151 0 0 0</pose>
|
|
<geometry><box><size>0.1 0.5 0.001</size></box></geometry>
|
|
<material>
|
|
<ambient>1 1 1 1</ambient>
|
|
<diffuse>1 1 1 1</diffuse>
|
|
</material>
|
|
</visual>
|
|
<visual name="h_right">
|
|
<pose>0.25 0 0.151 0 0 0</pose>
|
|
<geometry><box><size>0.1 0.5 0.001</size></box></geometry>
|
|
<material>
|
|
<ambient>1 1 1 1</ambient>
|
|
<diffuse>1 1 1 1</diffuse>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
|
|
<!-- Velocity control for rover -->
|
|
<plugin filename="gz-sim-velocity-control-system"
|
|
name="gz::sim::systems::VelocityControl">
|
|
<topic>/rover/cmd_vel</topic>
|
|
<initial_linear>0 0 0</initial_linear>
|
|
</plugin>
|
|
</model>
|
|
|
|
<!--
|
|
ArduPilot Iris Quadcopter
|
|
|
|
This model uses the ArduPilot Gazebo plugin for flight control.
|
|
Motor commands come from ArduPilot SITL, and sensor data is sent back.
|
|
-->
|
|
<model name="iris_with_ardupilot">
|
|
<pose>0 0 0.194923 0 0 0</pose>
|
|
|
|
<link name="base_link">
|
|
<inertial>
|
|
<pose>0 0 0 0 0 0</pose>
|
|
<mass>1.5</mass>
|
|
<inertia>
|
|
<ixx>0.029125</ixx>
|
|
<ixy>0</ixy>
|
|
<ixz>0</ixz>
|
|
<iyy>0.029125</iyy>
|
|
<iyz>0</iyz>
|
|
<izz>0.055225</izz>
|
|
</inertia>
|
|
</inertial>
|
|
|
|
<collision name="base_link_collision">
|
|
<geometry>
|
|
<box><size>0.47 0.47 0.11</size></box>
|
|
</geometry>
|
|
</collision>
|
|
|
|
<!-- Main body -->
|
|
<visual name="base_link_visual">
|
|
<geometry>
|
|
<box><size>0.3 0.3 0.1</size></box>
|
|
</geometry>
|
|
<material>
|
|
<ambient>0.8 0.1 0.1 1</ambient>
|
|
<diffuse>0.9 0.2 0.2 1</diffuse>
|
|
</material>
|
|
</visual>
|
|
|
|
<!-- Arms -->
|
|
<visual name="arm_fl">
|
|
<pose>0.12 0.12 0 0 0 0.785</pose>
|
|
<geometry><box><size>0.2 0.02 0.02</size></box></geometry>
|
|
<material><diffuse>0.3 0.3 0.3 1</diffuse></material>
|
|
</visual>
|
|
<visual name="arm_fr">
|
|
<pose>0.12 -0.12 0 0 0 -0.785</pose>
|
|
<geometry><box><size>0.2 0.02 0.02</size></box></geometry>
|
|
<material><diffuse>0.3 0.3 0.3 1</diffuse></material>
|
|
</visual>
|
|
<visual name="arm_bl">
|
|
<pose>-0.12 0.12 0 0 0 -0.785</pose>
|
|
<geometry><box><size>0.2 0.02 0.02</size></box></geometry>
|
|
<material><diffuse>0.3 0.3 0.3 1</diffuse></material>
|
|
</visual>
|
|
<visual name="arm_br">
|
|
<pose>-0.12 -0.12 0 0 0 0.785</pose>
|
|
<geometry><box><size>0.2 0.02 0.02</size></box></geometry>
|
|
<material><diffuse>0.3 0.3 0.3 1</diffuse></material>
|
|
</visual>
|
|
|
|
<!-- GPS antenna indicator -->
|
|
<visual name="gps_antenna">
|
|
<pose>0 0 0.08 0 0 0</pose>
|
|
<geometry><cylinder><radius>0.02</radius><length>0.04</length></cylinder></geometry>
|
|
<material><diffuse>0.2 0.2 0.8 1</diffuse></material>
|
|
</visual>
|
|
|
|
<!-- IMU Sensor -->
|
|
<sensor name="imu_sensor" type="imu">
|
|
<always_on>true</always_on>
|
|
<update_rate>400</update_rate>
|
|
<topic>imu</topic>
|
|
<imu>
|
|
<angular_velocity>
|
|
<x><noise type="gaussian"><mean>0</mean><stddev>0.0002</stddev></noise></x>
|
|
<y><noise type="gaussian"><mean>0</mean><stddev>0.0002</stddev></noise></y>
|
|
<z><noise type="gaussian"><mean>0</mean><stddev>0.0002</stddev></noise></z>
|
|
</angular_velocity>
|
|
<linear_acceleration>
|
|
<x><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></x>
|
|
<y><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></y>
|
|
<z><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></z>
|
|
</linear_acceleration>
|
|
</imu>
|
|
</sensor>
|
|
|
|
<!-- Air Pressure Sensor (Barometer) -->
|
|
<sensor name="air_pressure" type="air_pressure">
|
|
<always_on>true</always_on>
|
|
<update_rate>50</update_rate>
|
|
<topic>air_pressure</topic>
|
|
<air_pressure>
|
|
<reference_altitude>0</reference_altitude>
|
|
<noise type="gaussian">
|
|
<mean>0</mean>
|
|
<stddev>0.01</stddev>
|
|
</noise>
|
|
</air_pressure>
|
|
</sensor>
|
|
|
|
<!-- Downward Camera for Landing Pad Detection -->
|
|
<sensor name="camera" type="camera">
|
|
<pose>0 0 -0.05 0 1.5708 0</pose>
|
|
<always_on>true</always_on>
|
|
<update_rate>30</update_rate>
|
|
<topic>/drone/camera</topic>
|
|
<camera>
|
|
<horizontal_fov>1.047</horizontal_fov>
|
|
<image>
|
|
<width>320</width>
|
|
<height>240</height>
|
|
<format>R8G8B8</format>
|
|
</image>
|
|
<clip>
|
|
<near>0.1</near>
|
|
<far>100</far>
|
|
</clip>
|
|
</camera>
|
|
</sensor>
|
|
</link>
|
|
|
|
<!-- Rotor 0: Front Right (CCW) -->
|
|
<link name="rotor_0">
|
|
<pose>0.13 -0.22 0.023 0 0 0</pose>
|
|
<inertial>
|
|
<mass>0.025</mass>
|
|
<inertia>
|
|
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
|
|
<iyy>1.66704e-04</iyy><iyz>0</iyz>
|
|
<izz>1.66704e-04</izz>
|
|
</inertia>
|
|
</inertial>
|
|
<visual name="visual">
|
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
|
<material><diffuse>0.1 0.1 0.1 1</diffuse></material>
|
|
</visual>
|
|
<collision name="collision">
|
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
|
</collision>
|
|
</link>
|
|
|
|
<joint name="rotor_0_joint" type="revolute">
|
|
<parent>base_link</parent>
|
|
<child>rotor_0</child>
|
|
<axis>
|
|
<xyz>0 0 1</xyz>
|
|
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
|
|
</axis>
|
|
</joint>
|
|
|
|
<!-- Rotor 1: Back Left (CCW) -->
|
|
<link name="rotor_1">
|
|
<pose>-0.13 0.2 0.023 0 0 0</pose>
|
|
<inertial>
|
|
<mass>0.025</mass>
|
|
<inertia>
|
|
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
|
|
<iyy>1.66704e-04</iyy><iyz>0</iyz>
|
|
<izz>1.66704e-04</izz>
|
|
</inertia>
|
|
</inertial>
|
|
<visual name="visual">
|
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
|
<material><diffuse>0.1 0.1 0.1 1</diffuse></material>
|
|
</visual>
|
|
<collision name="collision">
|
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
|
</collision>
|
|
</link>
|
|
|
|
<joint name="rotor_1_joint" type="revolute">
|
|
<parent>base_link</parent>
|
|
<child>rotor_1</child>
|
|
<axis>
|
|
<xyz>0 0 1</xyz>
|
|
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
|
|
</axis>
|
|
</joint>
|
|
|
|
<!-- Rotor 2: Front Left (CW) -->
|
|
<link name="rotor_2">
|
|
<pose>0.13 0.22 0.023 0 0 0</pose>
|
|
<inertial>
|
|
<mass>0.025</mass>
|
|
<inertia>
|
|
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
|
|
<iyy>1.66704e-04</iyy><iyz>0</iyz>
|
|
<izz>1.66704e-04</izz>
|
|
</inertia>
|
|
</inertial>
|
|
<visual name="visual">
|
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
|
<material><diffuse>0.1 0.1 0.1 1</diffuse></material>
|
|
</visual>
|
|
<collision name="collision">
|
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
|
</collision>
|
|
</link>
|
|
|
|
<joint name="rotor_2_joint" type="revolute">
|
|
<parent>base_link</parent>
|
|
<child>rotor_2</child>
|
|
<axis>
|
|
<xyz>0 0 1</xyz>
|
|
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
|
|
</axis>
|
|
</joint>
|
|
|
|
<!-- Rotor 3: Back Right (CW) -->
|
|
<link name="rotor_3">
|
|
<pose>-0.13 -0.2 0.023 0 0 0</pose>
|
|
<inertial>
|
|
<mass>0.025</mass>
|
|
<inertia>
|
|
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
|
|
<iyy>1.66704e-04</iyy><iyz>0</iyz>
|
|
<izz>1.66704e-04</izz>
|
|
</inertia>
|
|
</inertial>
|
|
<visual name="visual">
|
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
|
<material><diffuse>0.1 0.1 0.1 1</diffuse></material>
|
|
</visual>
|
|
<collision name="collision">
|
|
<geometry><cylinder><radius>0.1</radius><length>0.005</length></cylinder></geometry>
|
|
</collision>
|
|
</link>
|
|
|
|
<joint name="rotor_3_joint" type="revolute">
|
|
<parent>base_link</parent>
|
|
<child>rotor_3</child>
|
|
<axis>
|
|
<xyz>0 0 1</xyz>
|
|
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
|
|
</axis>
|
|
</joint>
|
|
|
|
<!--
|
|
ArduPilot Plugin Configuration
|
|
|
|
This plugin connects to ArduPilot SITL via the JSON interface.
|
|
It receives motor commands and sends back sensor data.
|
|
|
|
fdm_addr: Address of ArduPilot SITL (default 127.0.0.1)
|
|
fdm_port_in: Port to receive motor commands (default 9002)
|
|
listen_addr: Our address for sending sensor data (127.0.0.1)
|
|
listen_port_out: SITL port for receiving sensor data (9003)
|
|
|
|
If ardupilot_gazebo plugin is not available, this will be ignored.
|
|
-->
|
|
<plugin filename="ArduPilotPlugin"
|
|
name="gz::sim::systems::ArduPilotPlugin">
|
|
<!-- Connection settings -->
|
|
<fdm_addr>127.0.0.1</fdm_addr>
|
|
<fdm_port_in>9002</fdm_port_in>
|
|
<listen_addr>127.0.0.1</listen_addr>
|
|
<listen_port_out>9003</listen_port_out>
|
|
|
|
<!-- Model reference link -->
|
|
<modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
|
|
<gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
|
|
|
|
<!-- IMU configuration -->
|
|
<imuName>imu_sensor</imuName>
|
|
|
|
<!-- Motor configuration (CCW positive, CW negative) -->
|
|
<!-- Rotor 0: FR CCW -->
|
|
<control channel="0">
|
|
<jointName>rotor_0_joint</jointName>
|
|
<type>VELOCITY</type>
|
|
<offset>0</offset>
|
|
<multiplier>838</multiplier>
|
|
<p_gain>0.2</p_gain>
|
|
<i_gain>0</i_gain>
|
|
<d_gain>0</d_gain>
|
|
<i_max>0</i_max>
|
|
<i_min>0</i_min>
|
|
<cmd_max>2.5</cmd_max>
|
|
<cmd_min>-2.5</cmd_min>
|
|
</control>
|
|
|
|
<!-- Rotor 1: BL CCW -->
|
|
<control channel="1">
|
|
<jointName>rotor_1_joint</jointName>
|
|
<type>VELOCITY</type>
|
|
<offset>0</offset>
|
|
<multiplier>838</multiplier>
|
|
<p_gain>0.2</p_gain>
|
|
<i_gain>0</i_gain>
|
|
<d_gain>0</d_gain>
|
|
<i_max>0</i_max>
|
|
<i_min>0</i_min>
|
|
<cmd_max>2.5</cmd_max>
|
|
<cmd_min>-2.5</cmd_min>
|
|
</control>
|
|
|
|
<!-- Rotor 2: FL CW -->
|
|
<control channel="2">
|
|
<jointName>rotor_2_joint</jointName>
|
|
<type>VELOCITY</type>
|
|
<offset>0</offset>
|
|
<multiplier>-838</multiplier>
|
|
<p_gain>0.2</p_gain>
|
|
<i_gain>0</i_gain>
|
|
<d_gain>0</d_gain>
|
|
<i_max>0</i_max>
|
|
<i_min>0</i_min>
|
|
<cmd_max>2.5</cmd_max>
|
|
<cmd_min>-2.5</cmd_min>
|
|
</control>
|
|
|
|
<!-- Rotor 3: BR CW -->
|
|
<control channel="3">
|
|
<jointName>rotor_3_joint</jointName>
|
|
<type>VELOCITY</type>
|
|
<offset>0</offset>
|
|
<multiplier>-838</multiplier>
|
|
<p_gain>0.2</p_gain>
|
|
<i_gain>0</i_gain>
|
|
<d_gain>0</d_gain>
|
|
<i_max>0</i_max>
|
|
<i_min>0</i_min>
|
|
<cmd_max>2.5</cmd_max>
|
|
<cmd_min>-2.5</cmd_min>
|
|
</control>
|
|
</plugin>
|
|
|
|
<!-- Odometry publisher for compatibility -->
|
|
<plugin filename="gz-sim-odometry-publisher-system"
|
|
name="gz::sim::systems::OdometryPublisher">
|
|
<odom_frame>odom</odom_frame>
|
|
<robot_base_frame>base_link</robot_base_frame>
|
|
<odom_topic>/model/drone/odometry</odom_topic>
|
|
<odom_publish_frequency>50</odom_publish_frequency>
|
|
</plugin>
|
|
|
|
</model>
|
|
|
|
</world>
|
|
</sdf>
|