ArduPilot SITL Update
This commit is contained in:
@@ -1,16 +1,24 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
DroneController - Template for GPS-denied landing logic.
|
||||
|
||||
Supports multiple telemetry sources:
|
||||
1. Custom JSON telemetry (/drone/telemetry) - PyBullet/Gazebo bridge
|
||||
2. ArduPilot ROS 2 DDS topics (/ap/*) - Official ArduPilot integration
|
||||
|
||||
Implement your algorithm in calculate_landing_maneuver().
|
||||
Uses sensors: IMU, altimeter, camera, and landing pad detection.
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
from typing import Dict, Any, Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
from geometry_msgs.msg import Twist, PoseStamped, TwistStamped
|
||||
from sensor_msgs.msg import Imu, BatteryState, NavSatFix
|
||||
from std_msgs.msg import String
|
||||
|
||||
# Load configuration
|
||||
@@ -24,10 +32,38 @@ except ImportError:
|
||||
LANDING = {"height_threshold": 0.1, "success_velocity": 0.1}
|
||||
|
||||
|
||||
class DroneController(Node):
|
||||
"""Drone controller for GPS-denied landing."""
|
||||
def quaternion_to_euler(x: float, y: float, z: float, w: float) -> tuple:
|
||||
"""Convert quaternion to Euler angles (roll, pitch, yaw)."""
|
||||
# Roll (x-axis rotation)
|
||||
sinr_cosp = 2.0 * (w * x + y * z)
|
||||
cosr_cosp = 1.0 - 2.0 * (x * x + y * y)
|
||||
roll = math.atan2(sinr_cosp, cosr_cosp)
|
||||
|
||||
# Pitch (y-axis rotation)
|
||||
sinp = 2.0 * (w * y - z * x)
|
||||
if abs(sinp) >= 1:
|
||||
pitch = math.copysign(math.pi / 2, sinp)
|
||||
else:
|
||||
pitch = math.asin(sinp)
|
||||
|
||||
# Yaw (z-axis rotation)
|
||||
siny_cosp = 2.0 * (w * z + x * y)
|
||||
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
|
||||
yaw = math.atan2(siny_cosp, cosy_cosp)
|
||||
|
||||
return roll, pitch, yaw
|
||||
|
||||
def __init__(self):
|
||||
|
||||
class DroneController(Node):
|
||||
"""
|
||||
Drone controller for GPS-denied landing.
|
||||
|
||||
Supports multiple telemetry sources:
|
||||
- Legacy: /drone/telemetry (JSON String)
|
||||
- ArduPilot DDS: /ap/pose/filtered, /ap/imu/filtered, etc.
|
||||
"""
|
||||
|
||||
def __init__(self, use_ardupilot_topics: bool = True):
|
||||
super().__init__('drone_controller')
|
||||
|
||||
# Load from config
|
||||
@@ -42,37 +78,189 @@ class DroneController(Node):
|
||||
self._landing_height = LANDING.get("height_threshold", 0.1)
|
||||
self._landing_velocity = LANDING.get("success_velocity", 0.1)
|
||||
|
||||
# Mode selection
|
||||
self._use_ardupilot = use_ardupilot_topics
|
||||
|
||||
self.get_logger().info('=' * 50)
|
||||
self.get_logger().info('Drone Controller Starting (GPS-Denied)...')
|
||||
if CONFIG_LOADED:
|
||||
self.get_logger().info(' Configuration loaded from config.py')
|
||||
self.get_logger().info(f' Mode: {"ArduPilot DDS" if use_ardupilot_topics else "Legacy JSON"}')
|
||||
self.get_logger().info('=' * 50)
|
||||
|
||||
# State variables
|
||||
self._latest_telemetry: Optional[Dict[str, Any]] = None
|
||||
self._rover_telemetry: Optional[Dict[str, Any]] = None
|
||||
self._telemetry_received = False
|
||||
self._landing_complete = False
|
||||
|
||||
self._telemetry_sub = self.create_subscription(
|
||||
String, '/drone/telemetry', self._telemetry_callback, 10
|
||||
)
|
||||
self.get_logger().info(' Subscribed to: /drone/telemetry')
|
||||
# ArduPilot state (built from DDS topics)
|
||||
self._ap_pose: Optional[PoseStamped] = None
|
||||
self._ap_twist: Optional[TwistStamped] = None
|
||||
self._ap_imu: Optional[Imu] = None
|
||||
self._ap_battery: Optional[BatteryState] = None
|
||||
|
||||
# QoS for sensor topics
|
||||
sensor_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1
|
||||
)
|
||||
|
||||
if use_ardupilot_topics:
|
||||
self._setup_ardupilot_subscriptions(sensor_qos)
|
||||
else:
|
||||
self._setup_legacy_subscriptions()
|
||||
|
||||
# Always subscribe to rover telemetry (for moving landing pad)
|
||||
self._rover_telemetry_sub = self.create_subscription(
|
||||
String, '/rover/telemetry', self._rover_telemetry_callback, 10
|
||||
)
|
||||
self.get_logger().info(' Subscribed to: /rover/telemetry')
|
||||
|
||||
# Command publisher
|
||||
self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
|
||||
self.get_logger().info(' Publishing to: /cmd_vel')
|
||||
|
||||
# Control loop
|
||||
control_period = 1.0 / self._control_rate
|
||||
self._control_timer = self.create_timer(control_period, self._control_loop)
|
||||
self.get_logger().info(f' Control loop: {self._control_rate} Hz')
|
||||
|
||||
self.get_logger().info('Drone Controller Ready!')
|
||||
self.get_logger().info('Sensors: IMU, Altimeter, Camera, Landing Pad Detection')
|
||||
|
||||
self.get_logger().info('Sensors: IMU, Altimeter, Velocity, Landing Pad Detection')
|
||||
|
||||
def _setup_ardupilot_subscriptions(self, qos: QoSProfile):
|
||||
"""Set up subscriptions to official ArduPilot ROS 2 topics."""
|
||||
|
||||
# Pose (position + orientation)
|
||||
self._ap_pose_sub = self.create_subscription(
|
||||
PoseStamped, '/ap/pose/filtered', self._ap_pose_callback, qos
|
||||
)
|
||||
self.get_logger().info(' Subscribed to: /ap/pose/filtered')
|
||||
|
||||
# Twist (velocity)
|
||||
self._ap_twist_sub = self.create_subscription(
|
||||
TwistStamped, '/ap/twist/filtered', self._ap_twist_callback, qos
|
||||
)
|
||||
self.get_logger().info(' Subscribed to: /ap/twist/filtered')
|
||||
|
||||
# IMU
|
||||
self._ap_imu_sub = self.create_subscription(
|
||||
Imu, '/ap/imu/filtered', self._ap_imu_callback, qos
|
||||
)
|
||||
self.get_logger().info(' Subscribed to: /ap/imu/filtered')
|
||||
|
||||
# Battery
|
||||
self._ap_battery_sub = self.create_subscription(
|
||||
BatteryState, '/ap/battery', self._ap_battery_callback, qos
|
||||
)
|
||||
self.get_logger().info(' Subscribed to: /ap/battery')
|
||||
|
||||
def _setup_legacy_subscriptions(self):
|
||||
"""Set up subscriptions to legacy JSON telemetry."""
|
||||
self._telemetry_sub = self.create_subscription(
|
||||
String, '/drone/telemetry', self._telemetry_callback, 10
|
||||
)
|
||||
self.get_logger().info(' Subscribed to: /drone/telemetry')
|
||||
|
||||
# ArduPilot topic callbacks
|
||||
def _ap_pose_callback(self, msg: PoseStamped):
|
||||
self._ap_pose = msg
|
||||
if not self._telemetry_received:
|
||||
self._telemetry_received = True
|
||||
self._warmup_count = 0
|
||||
self.get_logger().info('First ArduPilot pose received!')
|
||||
self._build_telemetry_from_ardupilot()
|
||||
|
||||
def _ap_twist_callback(self, msg: TwistStamped):
|
||||
self._ap_twist = msg
|
||||
self._build_telemetry_from_ardupilot()
|
||||
|
||||
def _ap_imu_callback(self, msg: Imu):
|
||||
self._ap_imu = msg
|
||||
self._build_telemetry_from_ardupilot()
|
||||
|
||||
def _ap_battery_callback(self, msg: BatteryState):
|
||||
self._ap_battery = msg
|
||||
self._build_telemetry_from_ardupilot()
|
||||
|
||||
def _build_telemetry_from_ardupilot(self):
|
||||
"""Build telemetry dict from ArduPilot topics."""
|
||||
telemetry = {}
|
||||
|
||||
# Position and orientation from pose
|
||||
if self._ap_pose:
|
||||
pos = self._ap_pose.pose.position
|
||||
ori = self._ap_pose.pose.orientation
|
||||
roll, pitch, yaw = quaternion_to_euler(ori.x, ori.y, ori.z, ori.w)
|
||||
|
||||
telemetry['position'] = {
|
||||
'x': pos.x,
|
||||
'y': pos.y,
|
||||
'z': pos.z
|
||||
}
|
||||
telemetry['altimeter'] = {
|
||||
'altitude': pos.z,
|
||||
'vertical_velocity': self._ap_twist.twist.linear.z if self._ap_twist else 0.0
|
||||
}
|
||||
telemetry['imu'] = {
|
||||
'orientation': {'roll': roll, 'pitch': pitch, 'yaw': yaw},
|
||||
'angular_velocity': {'x': 0, 'y': 0, 'z': 0}
|
||||
}
|
||||
|
||||
# Velocity from twist
|
||||
if self._ap_twist:
|
||||
twist = self._ap_twist.twist
|
||||
telemetry['velocity'] = {
|
||||
'x': twist.linear.x,
|
||||
'y': twist.linear.y,
|
||||
'z': twist.linear.z
|
||||
}
|
||||
if 'altimeter' in telemetry:
|
||||
telemetry['altimeter']['vertical_velocity'] = twist.linear.z
|
||||
|
||||
# Angular velocity from IMU
|
||||
if self._ap_imu:
|
||||
if 'imu' not in telemetry:
|
||||
telemetry['imu'] = {}
|
||||
telemetry['imu']['angular_velocity'] = {
|
||||
'x': self._ap_imu.angular_velocity.x,
|
||||
'y': self._ap_imu.angular_velocity.y,
|
||||
'z': self._ap_imu.angular_velocity.z
|
||||
}
|
||||
|
||||
# Battery
|
||||
if self._ap_battery:
|
||||
telemetry['battery'] = {
|
||||
'voltage': self._ap_battery.voltage,
|
||||
'remaining': self._ap_battery.percentage * 100
|
||||
}
|
||||
|
||||
# Landing pad detection (placeholder - would need vision processing)
|
||||
# For now, calculate from rover telemetry if available
|
||||
if self._rover_telemetry and self._ap_pose:
|
||||
rover_pos = self._rover_telemetry.get('position', {})
|
||||
rx = rover_pos.get('x', 0)
|
||||
ry = rover_pos.get('y', 0)
|
||||
dx = self._ap_pose.pose.position.x
|
||||
dy = self._ap_pose.pose.position.y
|
||||
dz = self._ap_pose.pose.position.z
|
||||
|
||||
rel_x = rx - dx
|
||||
rel_y = ry - dy
|
||||
distance = math.sqrt(rel_x**2 + rel_y**2 + dz**2)
|
||||
|
||||
telemetry['landing_pad'] = {
|
||||
'relative_x': rel_x,
|
||||
'relative_y': rel_y,
|
||||
'distance': distance,
|
||||
'confidence': 1.0 if distance < 10.0 else 0.0
|
||||
}
|
||||
|
||||
self._latest_telemetry = telemetry
|
||||
|
||||
# Legacy callbacks
|
||||
def _telemetry_callback(self, msg: String) -> None:
|
||||
try:
|
||||
self._latest_telemetry = json.loads(msg.data)
|
||||
@@ -211,11 +399,19 @@ class DroneController(Node):
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
import sys
|
||||
|
||||
# Check for --ardupilot flag
|
||||
use_ardupilot = '--ardupilot' in sys.argv or '-a' in sys.argv
|
||||
|
||||
# Remove our custom args before passing to rclpy
|
||||
filtered_args = [a for a in (args or sys.argv) if a not in ['--ardupilot', '-a']]
|
||||
|
||||
rclpy.init(args=filtered_args)
|
||||
controller = None
|
||||
|
||||
try:
|
||||
controller = DroneController()
|
||||
controller = DroneController(use_ardupilot_topics=use_ardupilot)
|
||||
rclpy.spin(controller)
|
||||
except KeyboardInterrupt:
|
||||
print('\nShutting down...')
|
||||
|
||||
Reference in New Issue
Block a user