Ardupilot Update
This commit is contained in:
580
mavlink_bridge.py
Normal file
580
mavlink_bridge.py
Normal file
@@ -0,0 +1,580 @@
|
||||
#!/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()
|
||||
Reference in New Issue
Block a user