Ardupilot Update

gazebo/launch/ardupilot_drone.launch.py

@@ -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")