SirBlob/RDC_Simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduPilot SITL Update

Browse Source
This commit is contained in:
default
2026-01-04 00:24:46 +00:00
parent 6c72bbf24c
commit 6804180e21
20 changed files with 2138 additions and 2970 deletions
Download Patch File Download Diff File Expand all files Collapse all files

134
gazebo/launch/ardupilot_drone.launch.py
View File

@@ -1,40 +1,52 @@
#!/usr/bin/env python3
"""
ArduPilot SITL + Gazebo Launch File
Works with Gazebo Harmonic/Garden and ArduPilot SITL.
ArduPilot ROS 2 Launch Helper
This launch file:
1. Starts Gazebo with ArduPilot-compatible drone model
2. Sets up ROS-Gazebo bridge for telemetry
3. Optionally starts ArduPilot SITL
This file provides a helper for launching the official ArduPilot ROS 2 simulation.
For the full ArduPilot + Gazebo experience, use the official ardupilot_gz packages.
NOTE: ArduPilot SITL integration requires the ardupilot_gazebo plugin.
Install from: https://github.com/ArduPilot/ardupilot_gazebo
RECOMMENDED: Use the official launch files:
ros2 launch ardupilot_gz_bringup iris_runway.launch.py
This local launch file is for custom Gazebo worlds or fallback testing.
"""
import os
import shutil
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction, LogInfo
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
"""Generate the launch description."""
"""
Generate launch description for ArduPilot simulation.
This launch file:
1. Checks for official ardupilot_gz packages
2. Falls back to local Gazebo world if packages not found
3. Starts ROS-Gazebo bridge for additional topics
"""
# Get paths
script_dir = os.path.dirname(os.path.abspath(__file__))
gazebo_dir = os.path.dirname(script_dir)
world_file = os.path.join(gazebo_dir, 'worlds', 'ardupilot_drone.sdf')
world_file = os.path.join(gazebo_dir, 'worlds', 'drone_landing.sdf')
# Check for ArduPilot Gazebo plugin
plugin_paths = [
os.path.expanduser("~/ardupilot_gazebo/build"),
"/opt/ardupilot_gazebo/lib",
os.environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", ""),
]
ardupilot_plugin_found = any(os.path.exists(p) for p in plugin_paths if p)
# Check if official ArduPilot packages are available
try:
import subprocess
result = subprocess.run(
['ros2', 'pkg', 'prefix', 'ardupilot_gz_bringup'],
capture_output=True, text=True
)
official_pkg_available = result.returncode == 0
except Exception:
official_pkg_available = False
# Use simulation time
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
# Determine Gazebo command
if shutil.which('gz'):
@@ -44,37 +56,35 @@ def generate_launch_description():
else:
gz_cmd = ['gz', 'sim', '-r', world_file]
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
start_sitl = LaunchConfiguration('start_sitl', default='false')
# Set plugin path for ArduPilot
env = os.environ.copy()
plugin_search_paths = ':'.join([p for p in plugin_paths if p and os.path.exists(p)])
if plugin_search_paths:
env['GZ_SIM_SYSTEM_PLUGIN_PATH'] = plugin_search_paths + ':' + env.get('GZ_SIM_SYSTEM_PLUGIN_PATH', '')
actions = [
DeclareLaunchArgument(
'use_sim_time',
default_value='true',
description='Use simulation clock'
),
DeclareLaunchArgument(
'start_sitl',
default_value='false',
description='Start ArduPilot SITL automatically'
),
# Start Gazebo with ArduPilot world
]
if official_pkg_available:
# Use official ArduPilot packages
actions.append(
LogInfo(msg=[
'\n',
'=' * 60,
'\n RECOMMENDED: Use official ArduPilot launch files:\n',
' ros2 launch ardupilot_gz_bringup iris_runway.launch.py\n',
'=' * 60,
'\n',
])
)
# Start Gazebo with local world (for custom scenarios)
actions.extend([
ExecuteProcess(
cmd=gz_cmd,
output='screen',
additional_env=env
),
# ROS-Gazebo Bridge for telemetry and commands
# Delayed start to wait for Gazebo
# ROS-Gazebo Bridge
TimerAction(
period=2.0,
actions=[
@@ -83,14 +93,13 @@ def generate_launch_description():
executable='parameter_bridge',
name='gz_bridge',
arguments=[
# Rover velocity commands (ROS to Gazebo)
# Rover velocity commands
'/rover/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
# Odometry (from Gazebo to ROS) - fallback if MAVLink not used
# Drone velocity commands
'/drone/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
# Odometry
'/model/drone/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
# Camera (from Gazebo to ROS)
'/drone/camera@sensor_msgs/msg/Image[gz.msgs.Image',
# IMU (from Gazebo to ROS)
'/imu@sensor_msgs/msg/Imu[gz.msgs.IMU',
'/model/rover/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
# Clock
'/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
],
@@ -99,36 +108,31 @@ def generate_launch_description():
),
]
),
]
])
return LaunchDescription(actions)
if __name__ == '__main__':
print("=" * 60)
print(" ArduPilot SITL + Gazebo Launch File")
print(" ArduPilot ROS 2 Launch Helper")
print("=" * 60)
print()
print("This is a ROS 2 launch file for ArduPilot integration.")
print("RECOMMENDED: Use official ArduPilot ROS 2 packages:")
print()
print("Prerequisites:")
print(" 1. Gazebo (gz sim or ign gazebo)")
print(" 2. ros_gz_bridge package")
print(" 3. ArduPilot Gazebo plugin (optional, for SITL)")
print(" # Install ArduPilot ROS 2")
print(" ./setup/install_ardupilot.sh")
print()
print("Usage:")
print(" # Source workspace")
print(" source ~/ardu_ws/install/setup.bash")
print()
print(" # Launch simulation (SITL + Gazebo + RViz)")
print(" ros2 launch ardupilot_gz_bringup iris_runway.launch.py")
print()
print(" # Connect MAVProxy")
print(" mavproxy.py --console --map --master=:14550")
print()
print("-" * 60)
print("Alternative: Use this local launch for custom worlds:")
print(" ros2 launch gazebo/launch/ardupilot_drone.launch.py")
print()
print("Then in another terminal:")
print(" # Start SITL")
print(" sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON")
print()
print(" # Or use the integrated runner")
print(" python run_ardupilot.py --no-sitl")
print()
print("Manual Gazebo Start:")
if shutil.which('gz'):
print(" gz sim -r gazebo/worlds/ardupilot_drone.sdf")
else:
print(" ign gazebo gazebo/worlds/ardupilot_drone.sdf")

504
gazebo/worlds/ardupilot_drone.sdf
View File

@@ -1,504 +0,0 @@
<?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>