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

Ardupilot Update

Browse Source
This commit is contained in:
default
2026-01-02 21:45:16 +00:00
parent 61c47f82fc
commit 6c72bbf24c
13 changed files with 2189 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -0,0 +1,134 @@
#!/usr/bin/env python3
"""
ArduPilot SITL + Gazebo Launch File
Works with Gazebo Harmonic/Garden and ArduPilot SITL.
This launch file:
1. Starts Gazebo with ArduPilot-compatible drone model
2. Sets up ROS-Gazebo bridge for telemetry
3. Optionally starts ArduPilot SITL
NOTE: ArduPilot SITL integration requires the ardupilot_gazebo plugin.
Install from: https://github.com/ArduPilot/ardupilot_gazebo
"""
import os
import shutil
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
"""Generate the launch description."""
# 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')
# 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)
# Determine Gazebo command
if shutil.which('gz'):
gz_cmd = ['gz', 'sim', '-r', world_file]
elif shutil.which('ign'):
gz_cmd = ['ign', 'gazebo', '-r', world_file]
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
ExecuteProcess(
cmd=gz_cmd,
output='screen',
additional_env=env
),
# ROS-Gazebo Bridge for telemetry and commands
# Delayed start to wait for Gazebo
TimerAction(
period=2.0,
actions=[
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
name='gz_bridge',
arguments=[
# Rover velocity commands (ROS to Gazebo)
'/rover/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
# Odometry (from Gazebo to ROS) - fallback if MAVLink not used
'/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',
# Clock
'/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
],
parameters=[{'use_sim_time': use_sim_time}],
output='screen'
),
]
),
]
return LaunchDescription(actions)
if __name__ == '__main__':
print("=" * 60)
print(" ArduPilot SITL + Gazebo Launch File")
print("=" * 60)
print()
print("This is a ROS 2 launch file for ArduPilot integration.")
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()
print("Usage:")
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 Normal file
View File

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