581 lines
21 KiB
Python
581 lines
21 KiB
Python
#!/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()
|