#!/usr/bin/env python3 """ MAVLink Bridge for ArduPilot SITL Integration. Connects Gazebo simulation to ArduPilot SITL via MAVProxy. This bridge: - Connects to ArduPilot SITL via MAVLink (UDP) - Receives telemetry from ArduPilot (position, attitude, battery, etc.) - Sends commands to ArduPilot (velocity, position, etc.) - Publishes ROS 2 telemetry for the drone controller """ import json import math import time import threading from typing import Optional, Dict, Any, Tuple import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist from std_msgs.msg import String try: from pymavlink import mavutil from pymavlink.dialects.v20 import ardupilotmega as mavlink MAVLINK_AVAILABLE = True except ImportError: MAVLINK_AVAILABLE = False print("WARNING: pymavlink not installed. Run: pip install pymavlink") class MAVLinkBridge(Node): """ ROS 2 Node that bridges between ROS topics and ArduPilot SITL via MAVLink. """ # MAVLink connection defaults DEFAULT_SITL_HOST = "127.0.0.1" DEFAULT_SITL_PORT = 14550 # MAVProxy output port DEFAULT_SYSTEM_ID = 1 DEFAULT_COMPONENT_ID = 191 # MAV_COMP_ID_MISSIONPLANNER # Telemetry rate (Hz) TELEMETRY_RATE = 50.0 def __init__( self, sitl_host: str = None, sitl_port: int = None, system_id: int = None, component_id: int = None ): super().__init__('mavlink_bridge') # Connection parameters (with parameter overrides) self.declare_parameter('sitl_host', sitl_host or self.DEFAULT_SITL_HOST) self.declare_parameter('sitl_port', sitl_port or self.DEFAULT_SITL_PORT) self.declare_parameter('system_id', system_id or self.DEFAULT_SYSTEM_ID) self.declare_parameter('component_id', component_id or self.DEFAULT_COMPONENT_ID) self._sitl_host = self.get_parameter('sitl_host').value self._sitl_port = self.get_parameter('sitl_port').value self._system_id = self.get_parameter('system_id').value self._component_id = self.get_parameter('component_id').value self.get_logger().info('=' * 60) self.get_logger().info('MAVLink Bridge Starting...') self.get_logger().info(f' Connecting to SITL at {self._sitl_host}:{self._sitl_port}') self.get_logger().info('=' * 60) # MAVLink connection self._mav_conn: Optional[mavutil.mavlink_connection] = None self._connected = False self._armed = False self._flight_mode = "UNKNOWN" # Telemetry state self._position = [0.0, 0.0, 0.0] # x, y, z (NED frame, converted to ENU) self._velocity = [0.0, 0.0, 0.0] # vx, vy, vz self._attitude = [0.0, 0.0, 0.0] # roll, pitch, yaw (radians) self._angular_velocity = [0.0, 0.0, 0.0] # wx, wy, wz self._battery_voltage = 0.0 self._battery_remaining = 100 self._gps_fix = 0 self._landed = False self._home_position = None self._last_heartbeat = 0.0 # Rover position (for landing pad detection) self._rover_position = [0.0, 0.0, 0.15] # Thread safety self._lock = threading.Lock() # ROS 2 Subscriptions self._cmd_vel_sub = self.create_subscription( Twist, '/cmd_vel', self._cmd_vel_callback, 10 ) self.get_logger().info(' Subscribed to: /cmd_vel') self._rover_sub = self.create_subscription( String, '/rover/telemetry', self._rover_callback, 10 ) self.get_logger().info(' Subscribed to: /rover/telemetry') # ROS 2 Publishers self._telemetry_pub = self.create_publisher( String, '/drone/telemetry', 10 ) self.get_logger().info(' Publishing to: /drone/telemetry') self._status_pub = self.create_publisher( String, '/mavlink/status', 10 ) self.get_logger().info(' Publishing to: /mavlink/status') # Initialize MAVLink connection self._connect_mavlink() # Start telemetry thread self._running = True self._mavlink_thread = threading.Thread(target=self._mavlink_loop, daemon=True) self._mavlink_thread.start() # Telemetry timer telemetry_period = 1.0 / self.TELEMETRY_RATE self._telemetry_timer = self.create_timer(telemetry_period, self._publish_telemetry) self.get_logger().info('MAVLink Bridge Ready!') def _connect_mavlink(self) -> bool: """Establish MAVLink connection to ArduPilot SITL.""" if not MAVLINK_AVAILABLE: self.get_logger().error('pymavlink not available!') return False try: connection_string = f'udpin:{self._sitl_host}:{self._sitl_port}' self.get_logger().info(f'Connecting to: {connection_string}') self._mav_conn = mavutil.mavlink_connection( connection_string, source_system=self._system_id, source_component=self._component_id ) # Wait for heartbeat self.get_logger().info('Waiting for heartbeat...') self._mav_conn.wait_heartbeat(timeout=30) self._connected = True self._last_heartbeat = time.time() self.get_logger().info( f'Connected! System: {self._mav_conn.target_system}, ' f'Component: {self._mav_conn.target_component}' ) # Request data streams self._request_data_streams() return True except Exception as e: self.get_logger().error(f'MAVLink connection failed: {e}') self._connected = False return False def _request_data_streams(self): """Request required data streams from ArduPilot.""" if not self._mav_conn: return # Request all data streams at high rate streams = [ (mavutil.mavlink.MAV_DATA_STREAM_ALL, 10), (mavutil.mavlink.MAV_DATA_STREAM_RAW_SENSORS, 50), (mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, 5), (mavutil.mavlink.MAV_DATA_STREAM_POSITION, 50), (mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, 50), # Attitude (mavutil.mavlink.MAV_DATA_STREAM_EXTRA2, 10), # VFR_HUD ] for stream_id, rate in streams: self._mav_conn.mav.request_data_stream_send( self._mav_conn.target_system, self._mav_conn.target_component, stream_id, rate, 1 # Start ) def _mavlink_loop(self): """Background thread for receiving MAVLink messages.""" reconnect_delay = 2.0 while self._running: if not self._connected: time.sleep(reconnect_delay) self._connect_mavlink() continue try: # Non-blocking message receive msg = self._mav_conn.recv_match(blocking=True, timeout=0.1) if msg is None: # Check for connection timeout if time.time() - self._last_heartbeat > 5.0: self.get_logger().warning('Heartbeat timeout, reconnecting...') self._connected = False continue self._process_mavlink_message(msg) except Exception as e: self.get_logger().error(f'MAVLink receive error: {e}') self._connected = False def _process_mavlink_message(self, msg): """Process incoming MAVLink messages.""" msg_type = msg.get_type() with self._lock: if msg_type == 'HEARTBEAT': self._last_heartbeat = time.time() self._armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED self._flight_mode = mavutil.mode_string_v10(msg) elif msg_type == 'LOCAL_POSITION_NED': # Convert NED to ENU (ROS convention) self._position = [msg.y, msg.x, -msg.z] # NED to ENU self._velocity = [msg.vy, msg.vx, -msg.vz] elif msg_type == 'GLOBAL_POSITION_INT': # Backup position from GPS if self._home_position is None: self._home_position = (msg.lat, msg.lon, msg.alt) elif msg_type == 'ATTITUDE': self._attitude = [msg.roll, msg.pitch, msg.yaw] self._angular_velocity = [msg.rollspeed, msg.pitchspeed, msg.yawspeed] elif msg_type == 'SYS_STATUS': self._battery_voltage = msg.voltage_battery / 1000.0 # mV to V self._battery_remaining = msg.battery_remaining elif msg_type == 'GPS_RAW_INT': self._gps_fix = msg.fix_type elif msg_type == 'EXTENDED_SYS_STATE': # Check landed state self._landed = (msg.landed_state == mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND) def _cmd_vel_callback(self, msg: Twist): """Handle velocity commands from drone controller.""" if not self._connected or not self._mav_conn: return try: # Convert ROS Twist to MAVLink velocity command # linear.x = forward (pitch), linear.y = left (roll), linear.z = up (thrust) # angular.z = yaw rate # Use SET_POSITION_TARGET_LOCAL_NED for velocity control # Type mask: velocity only (ignore position and acceleration) type_mask = ( mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE ) # Convert velocities (ENU to NED) vx = msg.linear.x # Forward (body frame pitch command) vy = msg.linear.y # Left (body frame roll command) vz = -msg.linear.z # Down (negative because NED) yaw_rate = msg.angular.z self._mav_conn.mav.set_position_target_local_ned_send( 0, # time_boot_ms (not used) self._mav_conn.target_system, self._mav_conn.target_component, mavutil.mavlink.MAV_FRAME_LOCAL_NED, type_mask, 0, 0, 0, # position (ignored) vx, vy, vz, # velocity 0, 0, 0, # acceleration (ignored) 0, yaw_rate # yaw, yaw_rate ) except Exception as e: self.get_logger().warning(f'Failed to send velocity command: {e}') def _rover_callback(self, msg: String): """Receive rover position for landing pad detection.""" try: data = json.loads(msg.data) pos = data.get('position', {}) with self._lock: self._rover_position = [ pos.get('x', 0.0), pos.get('y', 0.0), pos.get('z', 0.15) ] except json.JSONDecodeError: pass def _get_landing_pad_detection(self) -> Optional[Dict[str, float]]: """Simulate landing pad detection based on relative position.""" # Camera parameters CAMERA_FOV = 60.0 CAMERA_RANGE = 10.0 dx = self._rover_position[0] - self._position[0] dy = self._rover_position[1] - self._position[1] dz = self._rover_position[2] - self._position[2] horizontal_dist = math.sqrt(dx**2 + dy**2) vertical_dist = -dz # Height above rover if vertical_dist <= 0 or vertical_dist > CAMERA_RANGE: return None fov_rad = math.radians(CAMERA_FOV / 2) max_horizontal = vertical_dist * math.tan(fov_rad) if horizontal_dist > max_horizontal: return None # Transform to body frame using yaw yaw = self._attitude[2] cos_yaw = math.cos(-yaw) sin_yaw = math.sin(-yaw) relative_x = dx * cos_yaw - dy * sin_yaw relative_y = dx * sin_yaw + dy * cos_yaw # Calculate confidence angle = math.atan2(horizontal_dist, vertical_dist) confidence = max(0.0, 1.0 - (angle / fov_rad)) confidence *= max(0.0, 1.0 - (vertical_dist / CAMERA_RANGE)) return { "relative_x": round(relative_x, 4), "relative_y": round(relative_y, 4), "distance": round(vertical_dist, 4), "confidence": round(confidence, 4) } def _publish_telemetry(self): """Publish drone telemetry to ROS 2 topic.""" with self._lock: # Landing pad detection pad_detection = self._get_landing_pad_detection() telemetry = { "imu": { "orientation": { "roll": round(self._attitude[0], 4), "pitch": round(self._attitude[1], 4), "yaw": round(self._attitude[2], 4) }, "angular_velocity": { "x": round(self._angular_velocity[0], 4), "y": round(self._angular_velocity[1], 4), "z": round(self._angular_velocity[2], 4) }, "linear_acceleration": { "x": 0.0, "y": 0.0, "z": 9.81 } }, "altimeter": { "altitude": round(self._position[2], 4), "vertical_velocity": round(self._velocity[2], 4) }, "velocity": { "x": round(self._velocity[0], 4), "y": round(self._velocity[1], 4), "z": round(self._velocity[2], 4) }, "position": { "x": round(self._position[0], 4), "y": round(self._position[1], 4), "z": round(self._position[2], 4) }, "landing_pad": pad_detection, "camera": { "width": 320, "height": 240, "fov": 60.0, "image": None # Would need camera integration }, "battery": { "voltage": round(self._battery_voltage, 2), "remaining": self._battery_remaining }, "landed": self._landed, "armed": self._armed, "flight_mode": self._flight_mode, "connected": self._connected, "timestamp": round(time.time(), 4) } telemetry_msg = String() telemetry_msg.data = json.dumps(telemetry) self._telemetry_pub.publish(telemetry_msg) # Publish status status_msg = String() status_msg.data = json.dumps({ "connected": self._connected, "armed": self._armed, "mode": self._flight_mode, "gps_fix": self._gps_fix }) self._status_pub.publish(status_msg) # ========================================================================= # ArduPilot Control Commands # ========================================================================= def arm(self, force: bool = False) -> bool: """Arm the drone.""" if not self._connected or not self._mav_conn: return False self.get_logger().info('Arming drone...') self._mav_conn.arducopter_arm() return self._wait_for_arm(True, timeout=5.0) def disarm(self, force: bool = False) -> bool: """Disarm the drone.""" if not self._connected or not self._mav_conn: return False self.get_logger().info('Disarming drone...') self._mav_conn.arducopter_disarm() return self._wait_for_arm(False, timeout=5.0) def _wait_for_arm(self, expected: bool, timeout: float = 5.0) -> bool: """Wait for arm/disarm to complete.""" start = time.time() while time.time() - start < timeout: if self._armed == expected: return True time.sleep(0.1) return False def set_mode(self, mode: str) -> bool: """Set flight mode (e.g., 'GUIDED', 'LAND', 'LOITER').""" if not self._connected or not self._mav_conn: return False self.get_logger().info(f'Setting mode to: {mode}') mode_mapping = self._mav_conn.mode_mapping() if mode.upper() not in mode_mapping: self.get_logger().error(f'Unknown mode: {mode}') return False mode_id = mode_mapping[mode.upper()] self._mav_conn.set_mode(mode_id) # Wait for mode change start = time.time() while time.time() - start < 5.0: if self._flight_mode.upper() == mode.upper(): return True time.sleep(0.1) return False def takeoff(self, altitude: float = 5.0) -> bool: """Take off to specified altitude.""" if not self._connected or not self._mav_conn: return False self.get_logger().info(f'Taking off to {altitude}m...') # Set GUIDED mode if not self.set_mode('GUIDED'): self.get_logger().error('Failed to set GUIDED mode') return False # Arm if not armed if not self._armed: if not self.arm(): self.get_logger().error('Failed to arm') return False # Command takeoff self._mav_conn.mav.command_long_send( self._mav_conn.target_system, self._mav_conn.target_component, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, # confirmation 0, 0, 0, 0, # params 1-4 (unused) 0, 0, # lat, lon (use current) altitude # altitude ) return True def land(self) -> bool: """Command the drone to land.""" if not self._connected or not self._mav_conn: return False self.get_logger().info('Landing...') return self.set_mode('LAND') def goto_position(self, x: float, y: float, z: float, yaw: float = 0.0): """Go to a local position (ENU frame).""" if not self._connected or not self._mav_conn: return # Convert ENU to NED type_mask = ( mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE ) self._mav_conn.mav.set_position_target_local_ned_send( 0, # time_boot_ms self._mav_conn.target_system, self._mav_conn.target_component, mavutil.mavlink.MAV_FRAME_LOCAL_NED, type_mask, y, x, -z, # ENU to NED position 0, 0, 0, # velocity (ignored) 0, 0, 0, # acceleration (ignored) yaw, 0 # yaw, yaw_rate ) def destroy_node(self): """Clean shutdown.""" self._running = False if self._mavlink_thread.is_alive(): self._mavlink_thread.join(timeout=2.0) if self._mav_conn: self._mav_conn.close() super().destroy_node() def main(args=None): if not MAVLINK_AVAILABLE: print("ERROR: pymavlink is required. Install with: pip install pymavlink") return print("\n" + "=" * 60) print(" MAVLink Bridge for ArduPilot SITL") print("=" * 60 + "\n") rclpy.init(args=args) bridge = None try: bridge = MAVLinkBridge() rclpy.spin(bridge) except KeyboardInterrupt: print('\nShutting down...') finally: if bridge: bridge.destroy_node() if rclpy.ok(): rclpy.shutdown() if __name__ == '__main__': main()