ArduPilot SITL Update

gazebo/launch/ardupilot_drone.launch.py

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