Code reorganization and Drone Logic Update
This commit is contained in:
233
legacy/build_exe.py
Normal file
233
legacy/build_exe.py
Normal file
@@ -0,0 +1,233 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
PyInstaller build script for drone simulation executables.
|
||||
Creates standalone executables that include PyBullet and dependencies.
|
||||
|
||||
Usage:
|
||||
python build_exe.py # Build standalone_simulation
|
||||
python build_exe.py simulation_host # Build simulation_host
|
||||
python build_exe.py standalone # Build standalone_simulation
|
||||
python build_exe.py ardupilot # Build ArduPilot launcher
|
||||
python build_exe.py camera_viewer # Build camera feed viewer
|
||||
python build_exe.py all # Build all
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import platform
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
try:
|
||||
import PyInstaller.__main__
|
||||
import pybullet_data
|
||||
except ImportError as e:
|
||||
print(f"Missing dependency: {e}")
|
||||
print("Install with: pip install pyinstaller pybullet")
|
||||
sys.exit(1)
|
||||
|
||||
# Check for pymavlink (optional for ArduPilot builds)
|
||||
try:
|
||||
from pymavlink import mavutil
|
||||
PYMAVLINK_AVAILABLE = True
|
||||
except ImportError:
|
||||
PYMAVLINK_AVAILABLE = False
|
||||
|
||||
|
||||
def get_pybullet_data_path() -> str:
|
||||
return pybullet_data.getDataPath()
|
||||
|
||||
|
||||
def build_executable(
|
||||
source_name: str,
|
||||
output_name: str,
|
||||
console: bool = True,
|
||||
hidden_imports: list = None,
|
||||
collect_data: list = None
|
||||
):
|
||||
"""Build a single executable."""
|
||||
script_dir = Path(__file__).parent
|
||||
source_file = script_dir / source_name
|
||||
|
||||
if not source_file.exists():
|
||||
print(f"Error: {source_file} not found!")
|
||||
return False
|
||||
|
||||
print(f"\nBuilding: {source_name} -> {output_name}")
|
||||
print("-" * 40)
|
||||
|
||||
system = platform.system().lower()
|
||||
pybullet_path = get_pybullet_data_path()
|
||||
|
||||
if system == 'windows':
|
||||
separator = ';'
|
||||
else:
|
||||
separator = ':'
|
||||
|
||||
data_spec = f"{pybullet_path}{separator}pybullet_data"
|
||||
|
||||
build_args = [
|
||||
str(source_file),
|
||||
'--onefile',
|
||||
'--clean',
|
||||
f'--name={output_name}',
|
||||
f'--add-data={data_spec}',
|
||||
]
|
||||
|
||||
# Add hidden imports if specified
|
||||
if hidden_imports:
|
||||
for imp in hidden_imports:
|
||||
build_args.append(f'--hidden-import={imp}')
|
||||
|
||||
# Add data collection for packages
|
||||
if collect_data:
|
||||
for pkg in collect_data:
|
||||
build_args.append(f'--collect-data={pkg}')
|
||||
|
||||
if console:
|
||||
build_args.append('--console')
|
||||
else:
|
||||
if system in ['windows', 'darwin']:
|
||||
build_args.append('--windowed')
|
||||
else:
|
||||
build_args.append('--console')
|
||||
|
||||
try:
|
||||
PyInstaller.__main__.run(build_args)
|
||||
|
||||
dist_dir = script_dir / "dist"
|
||||
if system == 'windows':
|
||||
exe_path = dist_dir / f"{output_name}.exe"
|
||||
elif system == 'darwin' and not console:
|
||||
exe_path = dist_dir / f"{output_name}.app"
|
||||
else:
|
||||
exe_path = dist_dir / output_name
|
||||
|
||||
print(f" Created: {exe_path}")
|
||||
return True
|
||||
|
||||
except Exception as e:
|
||||
print(f" Build failed: {e}")
|
||||
return False
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description='Build simulation executables')
|
||||
parser.add_argument(
|
||||
'target',
|
||||
nargs='?',
|
||||
default='standalone',
|
||||
choices=['standalone', 'simulation_host', 'ardupilot', 'mavlink_bridge', 'camera_viewer', 'all'],
|
||||
help='What to build (default: standalone)'
|
||||
)
|
||||
args = parser.parse_args()
|
||||
|
||||
print("=" * 60)
|
||||
print(" DRONE SIMULATION - BUILD EXECUTABLES")
|
||||
print("=" * 60)
|
||||
print(f"Platform: {platform.system()}")
|
||||
print(f"PyBullet data: {get_pybullet_data_path()}")
|
||||
print(f"pymavlink: {'Available' if PYMAVLINK_AVAILABLE else 'Not installed'}")
|
||||
|
||||
success = True
|
||||
|
||||
# Build standalone simulation
|
||||
if args.target in ['standalone', 'all']:
|
||||
success &= build_executable(
|
||||
'standalone_simulation.py',
|
||||
'drone_simulation',
|
||||
console=False
|
||||
)
|
||||
|
||||
# Build simulation host
|
||||
if args.target in ['simulation_host', 'all']:
|
||||
success &= build_executable(
|
||||
'simulation_host.py',
|
||||
'simulation_host',
|
||||
console=True
|
||||
)
|
||||
|
||||
# Build MAVLink bridge (requires pymavlink)
|
||||
if args.target in ['mavlink_bridge', 'all']:
|
||||
if not PYMAVLINK_AVAILABLE:
|
||||
print("\nWarning: pymavlink not installed, skipping mavlink_bridge build")
|
||||
print("Install with: pip install pymavlink")
|
||||
if args.target == 'mavlink_bridge':
|
||||
success = False
|
||||
else:
|
||||
success &= build_executable(
|
||||
'mavlink_bridge.py',
|
||||
'mavlink_bridge',
|
||||
console=True,
|
||||
hidden_imports=[
|
||||
'pymavlink',
|
||||
'pymavlink.mavutil',
|
||||
'pymavlink.dialects.v20.ardupilotmega',
|
||||
],
|
||||
collect_data=['pymavlink']
|
||||
)
|
||||
|
||||
# Build ArduPilot runner (requires pymavlink)
|
||||
if args.target in ['ardupilot', 'all']:
|
||||
if not PYMAVLINK_AVAILABLE:
|
||||
print("\nWarning: pymavlink not installed, skipping ardupilot build")
|
||||
print("Install with: pip install pymavlink")
|
||||
if args.target == 'ardupilot':
|
||||
success = False
|
||||
else:
|
||||
success &= build_executable(
|
||||
'run_ardupilot.py',
|
||||
'run_ardupilot',
|
||||
console=True,
|
||||
hidden_imports=[
|
||||
'pymavlink',
|
||||
'pymavlink.mavutil',
|
||||
'pymavlink.dialects.v20.ardupilotmega',
|
||||
'mavlink_bridge',
|
||||
'drone_controller',
|
||||
'rover_controller',
|
||||
],
|
||||
collect_data=['pymavlink']
|
||||
)
|
||||
|
||||
# Build camera viewer (requires opencv)
|
||||
if args.target in ['camera_viewer', 'all']:
|
||||
try:
|
||||
import cv2
|
||||
success &= build_executable(
|
||||
'camera_viewer.py',
|
||||
'camera_viewer',
|
||||
console=True,
|
||||
hidden_imports=[
|
||||
'cv2',
|
||||
'numpy',
|
||||
],
|
||||
collect_data=['cv2']
|
||||
)
|
||||
except ImportError:
|
||||
print("\nWarning: opencv-python not installed, skipping camera_viewer build")
|
||||
print("Install with: pip install opencv-python")
|
||||
if args.target == 'camera_viewer':
|
||||
success = False
|
||||
|
||||
print()
|
||||
print("=" * 60)
|
||||
if success:
|
||||
print(" BUILD COMPLETE!")
|
||||
print(" Executables in: dist/")
|
||||
print()
|
||||
print(" Available executables:")
|
||||
dist_dir = Path(__file__).parent / "dist"
|
||||
if dist_dir.exists():
|
||||
for exe in dist_dir.iterdir():
|
||||
if exe.is_file() and not exe.name.startswith('.'):
|
||||
print(f" - {exe.name}")
|
||||
else:
|
||||
print(" BUILD FAILED!")
|
||||
sys.exit(1)
|
||||
print("=" * 60)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
109
legacy/controllers.py
Normal file
109
legacy/controllers.py
Normal file
@@ -0,0 +1,109 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Controllers - Runs DroneController and RoverController together.
|
||||
Usage: python controllers.py [--pattern PATTERN] [--speed SPEED] [--amplitude AMP]
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
|
||||
import rclpy
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
|
||||
from drone_controller import DroneController
|
||||
from rover_controller import RoverController, MovementPattern
|
||||
|
||||
|
||||
def parse_args():
|
||||
parser = argparse.ArgumentParser(
|
||||
description='Run drone and rover controllers together',
|
||||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||||
epilog="""
|
||||
Rover Movement Patterns:
|
||||
stationary - Rover stays at origin (easiest)
|
||||
linear - Back and forth along X axis
|
||||
circular - Circular path around origin
|
||||
random - Random movement within bounds
|
||||
square - Square pattern around origin
|
||||
|
||||
Examples:
|
||||
python controllers.py
|
||||
python controllers.py --pattern circular --speed 0.3
|
||||
python controllers.py --pattern random --speed 0.5 --amplitude 3.0
|
||||
"""
|
||||
)
|
||||
parser.add_argument(
|
||||
'--pattern', '-p', type=str,
|
||||
choices=[p.value for p in MovementPattern],
|
||||
default='stationary',
|
||||
help='Rover movement pattern (default: stationary)'
|
||||
)
|
||||
parser.add_argument(
|
||||
'--speed', '-s', type=float, default=0.5,
|
||||
help='Rover speed in m/s (default: 0.5)'
|
||||
)
|
||||
parser.add_argument(
|
||||
'--amplitude', '-a', type=float, default=2.0,
|
||||
help='Rover movement amplitude in meters (default: 2.0)'
|
||||
)
|
||||
args, _ = parser.parse_known_args()
|
||||
return args
|
||||
|
||||
|
||||
def main(args=None):
|
||||
cli_args = parse_args()
|
||||
|
||||
print("\n" + "=" * 60)
|
||||
print(" DRONE LANDING SIMULATION - CONTROLLERS")
|
||||
print("=" * 60)
|
||||
print(f" Rover Pattern: {cli_args.pattern}")
|
||||
print(f" Rover Speed: {cli_args.speed} m/s")
|
||||
print(f" Rover Amplitude: {cli_args.amplitude} m")
|
||||
print("=" * 60 + "\n")
|
||||
|
||||
rclpy.init(args=args)
|
||||
|
||||
drone_controller = None
|
||||
rover_controller = None
|
||||
executor = None
|
||||
|
||||
try:
|
||||
# Create drone controller
|
||||
drone_controller = DroneController()
|
||||
|
||||
# Create rover controller with specified pattern
|
||||
pattern = MovementPattern(cli_args.pattern)
|
||||
rover_controller = RoverController(
|
||||
pattern=pattern,
|
||||
speed=cli_args.speed,
|
||||
amplitude=cli_args.amplitude
|
||||
)
|
||||
|
||||
# Use multi-threaded executor to run both nodes
|
||||
executor = MultiThreadedExecutor(num_threads=4)
|
||||
executor.add_node(drone_controller)
|
||||
executor.add_node(rover_controller)
|
||||
|
||||
print("\nBoth controllers running. Press Ctrl+C to stop.\n")
|
||||
|
||||
executor.spin()
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print('\nShutting down controllers...')
|
||||
except Exception as e:
|
||||
print(f'Error: {e}')
|
||||
sys.exit(1)
|
||||
finally:
|
||||
if executor:
|
||||
executor.shutdown()
|
||||
if drone_controller:
|
||||
drone_controller.destroy_node()
|
||||
if rover_controller:
|
||||
rover_controller.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
print('Controllers stopped.')
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
246
legacy/ros_bridge.py
Normal file
246
legacy/ros_bridge.py
Normal file
@@ -0,0 +1,246 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
ROS 2 Bridge for Drone Simulation - Connects PyBullet simulator to ROS 2.
|
||||
Usage: python ros_bridge.py --host <SIMULATOR_IP> --port <PORT>
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import socket
|
||||
import threading
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from geometry_msgs.msg import Twist
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
class ROS2SimulatorBridge(Node):
|
||||
"""Bridges ROS 2 topics to UDP-based simulator."""
|
||||
|
||||
DEFAULT_SIMULATOR_HOST = '0.0.0.0'
|
||||
DEFAULT_SIMULATOR_PORT = 5555
|
||||
SOCKET_TIMEOUT = 0.1
|
||||
RECEIVE_BUFFER_SIZE = 4096
|
||||
CMD_VEL_TOPIC = '/cmd_vel'
|
||||
TELEMETRY_TOPIC = '/drone/telemetry'
|
||||
|
||||
def __init__(self, simulator_host: str = None, simulator_port: int = None):
|
||||
super().__init__('ros2_simulator_bridge')
|
||||
|
||||
self._simulator_host = simulator_host or self.DEFAULT_SIMULATOR_HOST
|
||||
self._simulator_port = simulator_port or self.DEFAULT_SIMULATOR_PORT
|
||||
self._telemetry_port = self._simulator_port + 1
|
||||
|
||||
# Rover position (updated from /rover/telemetry)
|
||||
self._rover_pos = {'x': 0.0, 'y': 0.0, 'z': 0.15}
|
||||
|
||||
self.get_logger().info('=' * 60)
|
||||
self.get_logger().info('ROS 2 Simulator Bridge Starting...')
|
||||
self.get_logger().info(f' Simulator Host: {self._simulator_host}')
|
||||
self.get_logger().info(f' Simulator Port: {self._simulator_port}')
|
||||
self.get_logger().info(f' Telemetry Port: {self._telemetry_port}')
|
||||
self.get_logger().info('=' * 60)
|
||||
|
||||
self._socket: Optional[socket.socket] = None
|
||||
self._socket_lock = threading.Lock()
|
||||
self._init_socket()
|
||||
|
||||
self._cmd_vel_sub = self.create_subscription(
|
||||
Twist, self.CMD_VEL_TOPIC, self._cmd_vel_callback, 10
|
||||
)
|
||||
self.get_logger().info(f' Subscribed to: {self.CMD_VEL_TOPIC}')
|
||||
|
||||
# Subscribe to rover telemetry
|
||||
self._rover_sub = self.create_subscription(
|
||||
String, '/rover/telemetry', self._rover_callback, 10
|
||||
)
|
||||
self.get_logger().info(' Subscribed to: /rover/telemetry')
|
||||
|
||||
self._telemetry_pub = self.create_publisher(
|
||||
String, self.TELEMETRY_TOPIC, 10
|
||||
)
|
||||
self.get_logger().info(f' Publishing to: {self.TELEMETRY_TOPIC}')
|
||||
|
||||
self._running = True
|
||||
self._receiver_thread = threading.Thread(
|
||||
target=self._telemetry_receiver_loop,
|
||||
name='TelemetryReceiver',
|
||||
daemon=True
|
||||
)
|
||||
self._receiver_thread.start()
|
||||
self.get_logger().info(' Telemetry receiver thread started')
|
||||
self.get_logger().info('ROS 2 Simulator Bridge Ready!')
|
||||
|
||||
def _init_socket(self) -> None:
|
||||
try:
|
||||
self._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
self._socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
self._socket.settimeout(self.SOCKET_TIMEOUT)
|
||||
self._socket.bind(('0.0.0.0', self._telemetry_port))
|
||||
self.get_logger().info(
|
||||
f' UDP socket initialized (listening on port {self._telemetry_port})'
|
||||
)
|
||||
except socket.error as e:
|
||||
self.get_logger().error(f'Failed to initialize socket: {e}')
|
||||
raise
|
||||
|
||||
def _cmd_vel_callback(self, msg: Twist) -> None:
|
||||
command = {
|
||||
'thrust': msg.linear.z,
|
||||
'yaw': msg.angular.z,
|
||||
'pitch': msg.linear.x,
|
||||
'roll': msg.linear.y,
|
||||
'rover': self._rover_pos # Include current rover position
|
||||
}
|
||||
self._send_command_to_simulator(command)
|
||||
|
||||
def _rover_callback(self, msg: String) -> None:
|
||||
"""Update rover position from RoverController telemetry."""
|
||||
try:
|
||||
data = json.loads(msg.data)
|
||||
pos = data.get('position', {})
|
||||
self._rover_pos = {
|
||||
'x': pos.get('x', 0.0),
|
||||
'y': pos.get('y', 0.0),
|
||||
'z': pos.get('z', 0.15)
|
||||
}
|
||||
except json.JSONDecodeError:
|
||||
pass
|
||||
|
||||
def _send_command_to_simulator(self, command: dict) -> bool:
|
||||
try:
|
||||
json_data = json.dumps(command).encode('utf-8')
|
||||
with self._socket_lock:
|
||||
if self._socket is None:
|
||||
return False
|
||||
self._socket.sendto(
|
||||
json_data,
|
||||
(self._simulator_host, self._simulator_port)
|
||||
)
|
||||
return True
|
||||
except socket.error as e:
|
||||
self.get_logger().warning(f'Failed to send command: {e}')
|
||||
return False
|
||||
except json.JSONEncodeError as e:
|
||||
self.get_logger().error(f'JSON encoding error: {e}')
|
||||
return False
|
||||
|
||||
def _telemetry_receiver_loop(self) -> None:
|
||||
self.get_logger().info('Telemetry receiver loop started')
|
||||
|
||||
while self._running and rclpy.ok():
|
||||
try:
|
||||
with self._socket_lock:
|
||||
if self._socket is None:
|
||||
continue
|
||||
sock = self._socket
|
||||
|
||||
data, addr = sock.recvfrom(self.RECEIVE_BUFFER_SIZE)
|
||||
|
||||
if data:
|
||||
json_str = data.decode('utf-8')
|
||||
try:
|
||||
json.loads(json_str)
|
||||
except json.JSONDecodeError as e:
|
||||
self.get_logger().warning(
|
||||
f'Received invalid JSON from simulator: {e}'
|
||||
)
|
||||
continue
|
||||
|
||||
msg = String()
|
||||
msg.data = json_str
|
||||
self._telemetry_pub.publish(msg)
|
||||
|
||||
except socket.timeout:
|
||||
continue
|
||||
except socket.error as e:
|
||||
if self._running:
|
||||
self.get_logger().warning(f'Socket receive error: {e}')
|
||||
continue
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Unexpected error in receiver loop: {e}')
|
||||
continue
|
||||
|
||||
def shutdown(self) -> None:
|
||||
self.get_logger().info('Shutting down ROS 2 Simulator Bridge...')
|
||||
self._running = False
|
||||
|
||||
if self._receiver_thread.is_alive():
|
||||
self._receiver_thread.join(timeout=1.0)
|
||||
|
||||
with self._socket_lock:
|
||||
if self._socket is not None:
|
||||
try:
|
||||
self._socket.close()
|
||||
except socket.error:
|
||||
pass
|
||||
self._socket = None
|
||||
|
||||
self.get_logger().info('Bridge shutdown complete')
|
||||
|
||||
|
||||
def parse_args():
|
||||
parser = argparse.ArgumentParser(
|
||||
description='ROS 2 Bridge for Drone Simulation',
|
||||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||||
epilog="""
|
||||
Examples:
|
||||
python ros_bridge.py
|
||||
python ros_bridge.py --host 192.168.1.100
|
||||
python ros_bridge.py --host 192.168.1.100 --port 5555
|
||||
"""
|
||||
)
|
||||
parser.add_argument(
|
||||
'--host', '-H',
|
||||
type=str,
|
||||
default='0.0.0.0',
|
||||
help='Simulator host IP address (default: 0.0.0.0)'
|
||||
)
|
||||
parser.add_argument(
|
||||
'--port', '-p', type=int, default=5555,
|
||||
help='UDP port of the simulator (default: 5555)'
|
||||
)
|
||||
args, _ = parser.parse_known_args()
|
||||
return args
|
||||
|
||||
|
||||
def main(args=None):
|
||||
cli_args = parse_args()
|
||||
|
||||
print(f"\n{'='*60}")
|
||||
print(f" Connecting to simulator at {cli_args.host}:{cli_args.port}")
|
||||
print(f"{'='*60}\n")
|
||||
|
||||
rclpy.init(args=args)
|
||||
bridge_node = None
|
||||
|
||||
try:
|
||||
bridge_node = ROS2SimulatorBridge(
|
||||
simulator_host=cli_args.host,
|
||||
simulator_port=cli_args.port
|
||||
)
|
||||
executor = MultiThreadedExecutor(num_threads=2)
|
||||
executor.add_node(bridge_node)
|
||||
|
||||
try:
|
||||
executor.spin()
|
||||
finally:
|
||||
executor.shutdown()
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print('\nKeyboard interrupt received, shutting down...')
|
||||
except Exception as e:
|
||||
print(f'Error: {e}')
|
||||
finally:
|
||||
if bridge_node is not None:
|
||||
bridge_node.shutdown()
|
||||
bridge_node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
136
legacy/run_bridge.py
Normal file
136
legacy/run_bridge.py
Normal file
@@ -0,0 +1,136 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Run Bridge - Runs ROS bridge and controllers together.
|
||||
Connects to PyBullet simulator running on another machine or terminal.
|
||||
|
||||
Usage: python run_bridge.py [--host HOST] [--pattern PATTERN] [--speed SPEED]
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import signal
|
||||
import sys
|
||||
|
||||
import rclpy
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
|
||||
from ros_bridge import ROS2SimulatorBridge
|
||||
from drone_controller import DroneController
|
||||
from rover_controller import RoverController, MovementPattern
|
||||
|
||||
|
||||
def parse_args():
|
||||
parser = argparse.ArgumentParser(
|
||||
description='Run ROS bridge and controllers together',
|
||||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||||
epilog="""
|
||||
Examples:
|
||||
python run_bridge.py # Connect to local simulator
|
||||
python run_bridge.py --host 192.168.1.100 # Connect to remote simulator
|
||||
python run_bridge.py --pattern circular # With moving rover
|
||||
"""
|
||||
)
|
||||
parser.add_argument(
|
||||
'--host', '-H', type=str, default='0.0.0.0',
|
||||
help='Simulator host IP (default: 0.0.0.0)'
|
||||
)
|
||||
parser.add_argument(
|
||||
'--port', '-p', type=int, default=5555,
|
||||
help='Simulator port (default: 5555)'
|
||||
)
|
||||
parser.add_argument(
|
||||
'--pattern', type=str, default='stationary',
|
||||
choices=['stationary', 'linear', 'circular', 'random', 'square'],
|
||||
help='Rover movement pattern (default: stationary)'
|
||||
)
|
||||
parser.add_argument(
|
||||
'--speed', '-s', type=float, default=0.5,
|
||||
help='Rover speed in m/s (default: 0.5)'
|
||||
)
|
||||
parser.add_argument(
|
||||
'--amplitude', '-a', type=float, default=2.0,
|
||||
help='Rover movement amplitude in meters (default: 2.0)'
|
||||
)
|
||||
parser.add_argument(
|
||||
'--no-rover', action='store_true',
|
||||
help='Disable rover controller'
|
||||
)
|
||||
args, _ = parser.parse_known_args()
|
||||
return args
|
||||
|
||||
|
||||
def main():
|
||||
args = parse_args()
|
||||
|
||||
print("=" * 60)
|
||||
print(" ROS Bridge + Controllers")
|
||||
print("=" * 60)
|
||||
print(f" Simulator: {args.host}:{args.port}")
|
||||
print(f" Rover Pattern: {args.pattern}")
|
||||
print(f" Rover Speed: {args.speed} m/s")
|
||||
print("=" * 60)
|
||||
print()
|
||||
|
||||
rclpy.init()
|
||||
|
||||
nodes = []
|
||||
executor = MultiThreadedExecutor(num_threads=4)
|
||||
|
||||
try:
|
||||
# Create bridge
|
||||
bridge = ROS2SimulatorBridge(
|
||||
simulator_host=args.host,
|
||||
simulator_port=args.port
|
||||
)
|
||||
nodes.append(bridge)
|
||||
executor.add_node(bridge)
|
||||
print("[OK] ROS Bridge started")
|
||||
|
||||
# Create drone controller
|
||||
drone = DroneController()
|
||||
nodes.append(drone)
|
||||
executor.add_node(drone)
|
||||
print("[OK] Drone Controller started")
|
||||
|
||||
# Create rover controller (optional)
|
||||
if not args.no_rover:
|
||||
pattern = MovementPattern[args.pattern.upper()]
|
||||
rover = RoverController(
|
||||
pattern=pattern,
|
||||
speed=args.speed,
|
||||
amplitude=args.amplitude
|
||||
)
|
||||
nodes.append(rover)
|
||||
executor.add_node(rover)
|
||||
print("[OK] Rover Controller started")
|
||||
|
||||
print()
|
||||
print("All systems running. Press Ctrl+C to stop.")
|
||||
print()
|
||||
|
||||
# Handle Ctrl+C gracefully
|
||||
def signal_handler(sig, frame):
|
||||
print("\nShutting down...")
|
||||
executor.shutdown()
|
||||
|
||||
signal.signal(signal.SIGINT, signal_handler)
|
||||
|
||||
executor.spin()
|
||||
|
||||
except Exception as e:
|
||||
print(f"Error: {e}")
|
||||
raise
|
||||
finally:
|
||||
print("Cleaning up...")
|
||||
for node in nodes:
|
||||
if hasattr(node, 'shutdown'):
|
||||
node.shutdown()
|
||||
node.destroy_node()
|
||||
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
print("Shutdown complete.")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
122
legacy/run_gazebo.py
Normal file
122
legacy/run_gazebo.py
Normal file
@@ -0,0 +1,122 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Run Gazebo Bridge - Runs Gazebo bridge and controllers together.
|
||||
Connects to Gazebo simulation running in another terminal.
|
||||
|
||||
Usage: python run_gazebo.py [--pattern PATTERN] [--speed SPEED]
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import signal
|
||||
import sys
|
||||
|
||||
import rclpy
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
|
||||
from gazebo_bridge import GazeboBridge
|
||||
from drone_controller import DroneController
|
||||
from rover_controller import RoverController, MovementPattern
|
||||
|
||||
|
||||
def parse_args():
|
||||
parser = argparse.ArgumentParser(
|
||||
description='Run Gazebo bridge and controllers together',
|
||||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||||
epilog="""
|
||||
Examples:
|
||||
python run_gazebo.py # Stationary rover
|
||||
python run_gazebo.py --pattern circular # Circular movement
|
||||
python run_gazebo.py --pattern square --speed 0.3
|
||||
"""
|
||||
)
|
||||
parser.add_argument(
|
||||
'--pattern', type=str, default='stationary',
|
||||
choices=['stationary', 'linear', 'circular', 'random', 'square'],
|
||||
help='Rover movement pattern (default: stationary)'
|
||||
)
|
||||
parser.add_argument(
|
||||
'--speed', '-s', type=float, default=0.5,
|
||||
help='Rover speed in m/s (default: 0.5)'
|
||||
)
|
||||
parser.add_argument(
|
||||
'--amplitude', '-a', type=float, default=2.0,
|
||||
help='Rover movement amplitude in meters (default: 2.0)'
|
||||
)
|
||||
parser.add_argument(
|
||||
'--no-rover', action='store_true',
|
||||
help='Disable rover controller'
|
||||
)
|
||||
args, _ = parser.parse_known_args()
|
||||
return args
|
||||
|
||||
|
||||
def main():
|
||||
args = parse_args()
|
||||
|
||||
print("=" * 60)
|
||||
print(" Gazebo Bridge + Controllers")
|
||||
print("=" * 60)
|
||||
print(f" Rover Pattern: {args.pattern}")
|
||||
print(f" Rover Speed: {args.speed} m/s")
|
||||
print("=" * 60)
|
||||
print()
|
||||
|
||||
rclpy.init()
|
||||
|
||||
nodes = []
|
||||
executor = MultiThreadedExecutor(num_threads=4)
|
||||
|
||||
try:
|
||||
# Create Gazebo bridge
|
||||
bridge = GazeboBridge()
|
||||
nodes.append(bridge)
|
||||
executor.add_node(bridge)
|
||||
print("[OK] Gazebo Bridge started")
|
||||
|
||||
# Create drone controller
|
||||
drone = DroneController()
|
||||
nodes.append(drone)
|
||||
executor.add_node(drone)
|
||||
print("[OK] Drone Controller started")
|
||||
|
||||
# Create rover controller (optional)
|
||||
if not args.no_rover:
|
||||
pattern = MovementPattern[args.pattern.upper()]
|
||||
rover = RoverController(
|
||||
pattern=pattern,
|
||||
speed=args.speed,
|
||||
amplitude=args.amplitude
|
||||
)
|
||||
nodes.append(rover)
|
||||
executor.add_node(rover)
|
||||
print("[OK] Rover Controller started")
|
||||
|
||||
print()
|
||||
print("All systems running. Press Ctrl+C to stop.")
|
||||
print()
|
||||
|
||||
# Handle Ctrl+C gracefully
|
||||
def signal_handler(sig, frame):
|
||||
print("\nShutting down...")
|
||||
executor.shutdown()
|
||||
|
||||
signal.signal(signal.SIGINT, signal_handler)
|
||||
|
||||
executor.spin()
|
||||
|
||||
except Exception as e:
|
||||
print(f"Error: {e}")
|
||||
raise
|
||||
finally:
|
||||
print("Cleaning up...")
|
||||
for node in nodes:
|
||||
node.destroy_node()
|
||||
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
print("Shutdown complete.")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
474
legacy/simulation_host.py
Normal file
474
legacy/simulation_host.py
Normal file
@@ -0,0 +1,474 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Drone Simulation Host - PyBullet physics simulation (GPS-Denied).
|
||||
Includes downward camera for drone vision.
|
||||
Communicates via UDP on port 5555 for commands, 5556 for telemetry.
|
||||
"""
|
||||
|
||||
import base64
|
||||
import json
|
||||
import math
|
||||
import socket
|
||||
import time
|
||||
from typing import Dict, Any, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import pybullet as p
|
||||
import pybullet_data
|
||||
|
||||
# Load configuration
|
||||
try:
|
||||
from config import DRONE, ROVER, CAMERA, PHYSICS, NETWORK
|
||||
CONFIG_LOADED = True
|
||||
except ImportError:
|
||||
CONFIG_LOADED = False
|
||||
DRONE = {"mass": 1.0, "size": (0.3, 0.3, 0.1), "color": (0.8, 0.1, 0.1, 1.0),
|
||||
"start_position": (0.0, 0.0, 5.0), "thrust_scale": 15.0,
|
||||
"pitch_torque_scale": 2.0, "roll_torque_scale": 2.0, "yaw_torque_scale": 1.0}
|
||||
ROVER = {"size": (1.0, 1.0, 0.3), "color": (0.2, 0.6, 0.2, 1.0),
|
||||
"start_position": (0.0, 0.0, 0.15)}
|
||||
CAMERA = {"width": 320, "height": 240, "fov": 60.0, "near_clip": 0.1, "far_clip": 20.0}
|
||||
PHYSICS = {"gravity": -9.81, "timestep": 1/240, "telemetry_rate": 24}
|
||||
NETWORK = {"host": "0.0.0.0", "command_port": 5555, "telemetry_port": 5556}
|
||||
|
||||
|
||||
class DroneSimulator:
|
||||
"""PyBullet-based drone simulation with camera."""
|
||||
|
||||
def __init__(self):
|
||||
print("=" * 60)
|
||||
print("Drone Simulation Host (GPS-Denied + Camera)")
|
||||
if CONFIG_LOADED:
|
||||
print(" Configuration loaded from config.py")
|
||||
print("=" * 60)
|
||||
|
||||
# Load from config
|
||||
self._udp_host = NETWORK.get("host", "0.0.0.0")
|
||||
self._udp_port = NETWORK.get("command_port", 5555)
|
||||
self._telemetry_port = NETWORK.get("telemetry_port", 5556)
|
||||
|
||||
self._gravity = PHYSICS.get("gravity", -9.81)
|
||||
self._timestep = PHYSICS.get("timestep", 1/240)
|
||||
self._telemetry_interval = int(240 / PHYSICS.get("telemetry_rate", 24))
|
||||
|
||||
self._drone_mass = DRONE.get("mass", 1.0)
|
||||
self._drone_size = DRONE.get("size", (0.3, 0.3, 0.1))
|
||||
self._drone_start = DRONE.get("start_position", (0.0, 0.0, 5.0))
|
||||
self._drone_color = DRONE.get("color", (0.8, 0.1, 0.1, 1.0))
|
||||
self._thrust_scale = DRONE.get("thrust_scale", 15.0)
|
||||
self._pitch_torque_scale = DRONE.get("pitch_torque_scale", 2.0)
|
||||
self._roll_torque_scale = DRONE.get("roll_torque_scale", 2.0)
|
||||
self._yaw_torque_scale = DRONE.get("yaw_torque_scale", 1.0)
|
||||
self._hover_thrust = self._drone_mass * abs(self._gravity)
|
||||
|
||||
self._rover_size = ROVER.get("size", (1.0, 1.0, 0.3))
|
||||
self._rover_start = list(ROVER.get("start_position", (0.0, 0.0, 0.15)))
|
||||
self._rover_color = ROVER.get("color", (0.2, 0.6, 0.2, 1.0))
|
||||
|
||||
self._camera_width = CAMERA.get("width", 320)
|
||||
self._camera_height = CAMERA.get("height", 240)
|
||||
self._camera_fov = CAMERA.get("fov", 60.0)
|
||||
self._camera_near = CAMERA.get("near_clip", 0.1)
|
||||
self._camera_far = CAMERA.get("far_clip", 20.0)
|
||||
self._camera_interval = 5
|
||||
|
||||
self._running = True
|
||||
self._step_count = 0
|
||||
self._camera_frame_count = 0
|
||||
self._last_command: Dict[str, float] = {
|
||||
'thrust': 0.0, 'pitch': 0.0, 'roll': 0.0, 'yaw': 0.0
|
||||
}
|
||||
|
||||
self._rover_pos = list(self._rover_start)
|
||||
self._last_camera_image = None
|
||||
|
||||
self._init_physics()
|
||||
self._init_objects()
|
||||
self._init_network()
|
||||
|
||||
print("Simulation Ready! (GPS-Denied Mode)")
|
||||
print(" Sensors: IMU, Altimeter, Camera")
|
||||
print(f" Camera: {self._camera_width}x{self._camera_height} @ {self._camera_fov}° FOV")
|
||||
print(f" Listening on UDP port {self._udp_port}")
|
||||
print("=" * 60)
|
||||
|
||||
def _init_physics(self) -> None:
|
||||
print("Initializing PyBullet physics engine...")
|
||||
self._physics_client = p.connect(p.GUI)
|
||||
p.setGravity(0, 0, self._gravity)
|
||||
p.setTimeStep(self._timestep)
|
||||
p.resetDebugVisualizerCamera(
|
||||
cameraDistance=8.0,
|
||||
cameraYaw=45,
|
||||
cameraPitch=-30,
|
||||
cameraTargetPosition=[0, 0, 1]
|
||||
)
|
||||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
print(" Physics engine initialized (240 Hz)")
|
||||
|
||||
def _init_objects(self) -> None:
|
||||
print("Creating simulation objects...")
|
||||
|
||||
self._ground_id = p.loadURDF("plane.urdf")
|
||||
print(" Ground plane loaded")
|
||||
|
||||
rover_collision = p.createCollisionShape(
|
||||
p.GEOM_BOX,
|
||||
halfExtents=[s/2 for s in self._rover_size]
|
||||
)
|
||||
rover_visual = p.createVisualShape(
|
||||
p.GEOM_BOX,
|
||||
halfExtents=[s/2 for s in self._rover_size],
|
||||
rgbaColor=list(self._rover_color)
|
||||
)
|
||||
self._rover_id = p.createMultiBody(
|
||||
baseMass=0,
|
||||
baseCollisionShapeIndex=rover_collision,
|
||||
baseVisualShapeIndex=rover_visual,
|
||||
basePosition=self._rover_start
|
||||
)
|
||||
print(f" Rover created at position {self._rover_start}")
|
||||
|
||||
self._create_landing_pad_marker()
|
||||
|
||||
drone_collision = p.createCollisionShape(
|
||||
p.GEOM_BOX,
|
||||
halfExtents=[s/2 for s in self._drone_size]
|
||||
)
|
||||
drone_visual = p.createVisualShape(
|
||||
p.GEOM_BOX,
|
||||
halfExtents=[s/2 for s in self._drone_size],
|
||||
rgbaColor=list(self._drone_color)
|
||||
)
|
||||
self._drone_id = p.createMultiBody(
|
||||
baseMass=self._drone_mass,
|
||||
baseCollisionShapeIndex=drone_collision,
|
||||
baseVisualShapeIndex=drone_visual,
|
||||
basePosition=self._drone_start
|
||||
)
|
||||
p.changeDynamics(
|
||||
self._drone_id, -1,
|
||||
linearDamping=0.1,
|
||||
angularDamping=0.5
|
||||
)
|
||||
print(f" Drone created at position {self._drone_start}")
|
||||
|
||||
def _create_landing_pad_marker(self) -> None:
|
||||
marker_height = self._rover_start[2] + self._rover_size[2] / 2 + 0.01
|
||||
h_size = 0.3
|
||||
line_color = [1, 1, 1]
|
||||
line_width = 3
|
||||
|
||||
p.addUserDebugLine(
|
||||
[-h_size, 0, marker_height],
|
||||
[h_size, 0, marker_height],
|
||||
lineColorRGB=line_color,
|
||||
lineWidth=line_width
|
||||
)
|
||||
p.addUserDebugLine(
|
||||
[-h_size, -h_size, marker_height],
|
||||
[-h_size, h_size, marker_height],
|
||||
lineColorRGB=line_color,
|
||||
lineWidth=line_width
|
||||
)
|
||||
p.addUserDebugLine(
|
||||
[h_size, -h_size, marker_height],
|
||||
[h_size, h_size, marker_height],
|
||||
lineColorRGB=line_color,
|
||||
lineWidth=line_width
|
||||
)
|
||||
|
||||
def _init_network(self) -> None:
|
||||
print("Initializing network...")
|
||||
self._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
self._socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
self._socket.settimeout(0.001)
|
||||
self._socket.bind((self._udp_host, self._udp_port))
|
||||
self._controller_addr: Optional[Tuple[str, int]] = None
|
||||
print(f" UDP socket bound to {self._udp_host}:{self._udp_port}")
|
||||
|
||||
def run(self) -> None:
|
||||
print("\nStarting simulation loop...")
|
||||
print("Press Ctrl+C to exit\n")
|
||||
|
||||
try:
|
||||
while self._running:
|
||||
loop_start = time.time()
|
||||
|
||||
if not p.isConnected():
|
||||
print("GUI window closed, exiting...")
|
||||
break
|
||||
|
||||
self._receive_commands()
|
||||
self._apply_drone_controls()
|
||||
p.stepSimulation()
|
||||
self._step_count += 1
|
||||
|
||||
if self._step_count % self._telemetry_interval == 0:
|
||||
self._send_telemetry()
|
||||
|
||||
elapsed = time.time() - loop_start
|
||||
sleep_time = self._timestep - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("\nKeyboard interrupt received")
|
||||
finally:
|
||||
self.shutdown()
|
||||
|
||||
def _receive_commands(self) -> None:
|
||||
try:
|
||||
data, addr = self._socket.recvfrom(65535)
|
||||
self._controller_addr = (addr[0], self._telemetry_port)
|
||||
|
||||
try:
|
||||
command = json.loads(data.decode('utf-8'))
|
||||
|
||||
# Drone control commands
|
||||
self._last_command = {
|
||||
'thrust': float(command.get('thrust', 0.0)),
|
||||
'pitch': float(command.get('pitch', 0.0)),
|
||||
'roll': float(command.get('roll', 0.0)),
|
||||
'yaw': float(command.get('yaw', 0.0))
|
||||
}
|
||||
|
||||
# Rover position update (if provided)
|
||||
if 'rover' in command:
|
||||
rover_data = command['rover']
|
||||
new_x = float(rover_data.get('x', self._rover_pos[0]))
|
||||
new_y = float(rover_data.get('y', self._rover_pos[1]))
|
||||
new_z = float(rover_data.get('z', self._rover_pos[2]))
|
||||
self._rover_pos = [new_x, new_y, new_z]
|
||||
p.resetBasePositionAndOrientation(
|
||||
self._rover_id,
|
||||
self._rover_pos,
|
||||
[0, 0, 0, 1]
|
||||
)
|
||||
|
||||
except (json.JSONDecodeError, ValueError) as e:
|
||||
print(f"Invalid command received: {e}")
|
||||
|
||||
except socket.timeout:
|
||||
pass
|
||||
except socket.error as e:
|
||||
print(f"Socket error: {e}")
|
||||
|
||||
def _apply_drone_controls(self) -> None:
|
||||
pos, orn = p.getBasePositionAndOrientation(self._drone_id)
|
||||
rot_matrix = p.getMatrixFromQuaternion(orn)
|
||||
local_up = [rot_matrix[2], rot_matrix[5], rot_matrix[8]]
|
||||
|
||||
thrust_command = self._last_command['thrust']
|
||||
total_thrust = self._hover_thrust + (thrust_command * self._thrust_scale)
|
||||
total_thrust = max(0, total_thrust)
|
||||
|
||||
thrust_force = [
|
||||
local_up[0] * total_thrust,
|
||||
local_up[1] * total_thrust,
|
||||
local_up[2] * total_thrust
|
||||
]
|
||||
|
||||
p.applyExternalForce(
|
||||
self._drone_id, -1,
|
||||
forceObj=thrust_force,
|
||||
posObj=pos,
|
||||
flags=p.WORLD_FRAME
|
||||
)
|
||||
|
||||
pitch_torque = self._last_command['pitch'] * self._pitch_torque_scale
|
||||
roll_torque = self._last_command['roll'] * self._roll_torque_scale
|
||||
yaw_torque = self._last_command['yaw'] * self._yaw_torque_scale
|
||||
|
||||
p.applyExternalTorque(
|
||||
self._drone_id, -1,
|
||||
torqueObj=[pitch_torque, roll_torque, yaw_torque],
|
||||
flags=p.LINK_FRAME
|
||||
)
|
||||
|
||||
def _capture_camera_image(self) -> Optional[str]:
|
||||
"""Capture image from drone's downward-facing camera. Returns base64 encoded JPEG."""
|
||||
pos, orn = p.getBasePositionAndOrientation(self._drone_id)
|
||||
euler = p.getEulerFromQuaternion(orn)
|
||||
|
||||
target_pos = [pos[0], pos[1], 0]
|
||||
|
||||
view_matrix = p.computeViewMatrix(
|
||||
cameraEyePosition=pos,
|
||||
cameraTargetPosition=target_pos,
|
||||
cameraUpVector=[math.cos(euler[2]), math.sin(euler[2]), 0]
|
||||
)
|
||||
|
||||
aspect = self._camera_width / self._camera_height
|
||||
projection_matrix = p.computeProjectionMatrixFOV(
|
||||
fov=self._camera_fov,
|
||||
aspect=aspect,
|
||||
nearVal=self._camera_near,
|
||||
farVal=self._camera_far
|
||||
)
|
||||
|
||||
_, _, rgb, _, _ = p.getCameraImage(
|
||||
width=self._camera_width,
|
||||
height=self._camera_height,
|
||||
viewMatrix=view_matrix,
|
||||
projectionMatrix=projection_matrix,
|
||||
renderer=p.ER_BULLET_HARDWARE_OPENGL
|
||||
)
|
||||
|
||||
image = np.array(rgb, dtype=np.uint8).reshape(
|
||||
self._camera_height, self._camera_width, 4
|
||||
)[:, :, :3]
|
||||
|
||||
# Encode as base64 for transmission
|
||||
try:
|
||||
from PIL import Image
|
||||
import io
|
||||
pil_img = Image.fromarray(image)
|
||||
buffer = io.BytesIO()
|
||||
pil_img.save(buffer, format='JPEG', quality=70)
|
||||
return base64.b64encode(buffer.getvalue()).decode('utf-8')
|
||||
except ImportError:
|
||||
# Return raw RGB values as base64 if PIL not available
|
||||
return base64.b64encode(image.tobytes()).decode('utf-8')
|
||||
|
||||
def _get_landing_pad_detection(self) -> Optional[Dict[str, float]]:
|
||||
"""Simulate downward camera detecting landing pad."""
|
||||
drone_pos, drone_orn = p.getBasePositionAndOrientation(self._drone_id)
|
||||
rover_pos, _ = p.getBasePositionAndOrientation(self._rover_id)
|
||||
|
||||
dx = rover_pos[0] - drone_pos[0]
|
||||
dy = rover_pos[1] - drone_pos[1]
|
||||
dz = rover_pos[2] - drone_pos[2]
|
||||
|
||||
horizontal_dist = math.sqrt(dx**2 + dy**2)
|
||||
vertical_dist = -dz
|
||||
|
||||
if vertical_dist <= 0 or vertical_dist > 10.0:
|
||||
return None
|
||||
|
||||
fov_rad = math.radians(self._camera_fov / 2)
|
||||
max_horizontal = vertical_dist * math.tan(fov_rad)
|
||||
|
||||
if horizontal_dist > max_horizontal:
|
||||
return None
|
||||
|
||||
rot_matrix = p.getMatrixFromQuaternion(drone_orn)
|
||||
body_x = [rot_matrix[0], rot_matrix[3], rot_matrix[6]]
|
||||
body_y = [rot_matrix[1], rot_matrix[4], rot_matrix[7]]
|
||||
|
||||
relative_x = dx * body_x[0] + dy * body_x[1] + dz * body_x[2]
|
||||
relative_y = dx * body_y[0] + dy * body_y[1] + dz * body_y[2]
|
||||
|
||||
angle = math.atan2(horizontal_dist, vertical_dist)
|
||||
confidence = max(0.0, 1.0 - (angle / fov_rad))
|
||||
confidence *= max(0.0, 1.0 - (vertical_dist / 10.0))
|
||||
|
||||
return {
|
||||
"relative_x": round(relative_x, 4),
|
||||
"relative_y": round(relative_y, 4),
|
||||
"distance": round(vertical_dist, 4),
|
||||
"confidence": round(confidence, 4)
|
||||
}
|
||||
|
||||
def _send_telemetry(self) -> None:
|
||||
if self._controller_addr is None:
|
||||
return
|
||||
|
||||
pos, orn = p.getBasePositionAndOrientation(self._drone_id)
|
||||
vel, ang_vel = p.getBaseVelocity(self._drone_id)
|
||||
euler = p.getEulerFromQuaternion(orn)
|
||||
landed_on_rover = self._check_landing()
|
||||
|
||||
pad_detection = self._get_landing_pad_detection()
|
||||
|
||||
# Capture camera image periodically
|
||||
self._camera_frame_count += 1
|
||||
camera_image = None
|
||||
|
||||
if self._camera_frame_count >= self._camera_interval:
|
||||
self._camera_frame_count = 0
|
||||
camera_image = self._capture_camera_image()
|
||||
|
||||
telemetry = {
|
||||
"imu": {
|
||||
"orientation": {
|
||||
"roll": round(euler[0], 4),
|
||||
"pitch": round(euler[1], 4),
|
||||
"yaw": round(euler[2], 4)
|
||||
},
|
||||
"angular_velocity": {
|
||||
"x": round(ang_vel[0], 4),
|
||||
"y": round(ang_vel[1], 4),
|
||||
"z": round(ang_vel[2], 4)
|
||||
},
|
||||
"linear_acceleration": {
|
||||
"x": 0.0,
|
||||
"y": 0.0,
|
||||
"z": round(-self._gravity, 4)
|
||||
}
|
||||
},
|
||||
"altimeter": {
|
||||
"altitude": round(pos[2], 4),
|
||||
"vertical_velocity": round(vel[2], 4)
|
||||
},
|
||||
"velocity": {
|
||||
"x": round(vel[0], 4),
|
||||
"y": round(vel[1], 4),
|
||||
"z": round(vel[2], 4)
|
||||
},
|
||||
"landing_pad": pad_detection,
|
||||
"camera": {
|
||||
"width": self._camera_width,
|
||||
"height": self._camera_height,
|
||||
"fov": self._camera_fov,
|
||||
"image": camera_image # Base64 encoded JPEG (or None)
|
||||
},
|
||||
"landed": landed_on_rover,
|
||||
"timestamp": round(self._step_count * self._timestep, 4)
|
||||
}
|
||||
|
||||
try:
|
||||
json_data = json.dumps(telemetry).encode('utf-8')
|
||||
self._socket.sendto(json_data, self._controller_addr)
|
||||
except socket.error as e:
|
||||
print(f"Failed to send telemetry: {e}")
|
||||
|
||||
def _check_landing(self) -> bool:
|
||||
contact_points = p.getContactPoints(
|
||||
bodyA=self._drone_id,
|
||||
bodyB=self._rover_id
|
||||
)
|
||||
return len(contact_points) > 0
|
||||
|
||||
def shutdown(self) -> None:
|
||||
print("\nShutting down simulation...")
|
||||
self._running = False
|
||||
|
||||
try:
|
||||
self._socket.close()
|
||||
print(" Socket closed")
|
||||
except socket.error:
|
||||
pass
|
||||
|
||||
if p.isConnected():
|
||||
p.disconnect()
|
||||
print(" PyBullet disconnected")
|
||||
|
||||
print("Shutdown complete")
|
||||
|
||||
|
||||
def main():
|
||||
print("\n" + "=" * 60)
|
||||
print(" DRONE LANDING SIMULATION (GPS-DENIED + CAMERA)")
|
||||
print("=" * 60 + "\n")
|
||||
|
||||
try:
|
||||
simulator = DroneSimulator()
|
||||
simulator.run()
|
||||
except Exception as e:
|
||||
print(f"Fatal error: {e}")
|
||||
raise
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
386
legacy/standalone_simulation.py
Normal file
386
legacy/standalone_simulation.py
Normal file
@@ -0,0 +1,386 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Standalone Drone Simulation - Runs without ROS 2.
|
||||
Includes built-in controller for landing demonstration.
|
||||
|
||||
Usage: python standalone_simulation.py [--pattern PATTERN] [--speed SPEED]
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import base64
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
from typing import Dict, Any, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import pybullet as p
|
||||
import pybullet_data
|
||||
|
||||
# Load configuration
|
||||
try:
|
||||
from config import DRONE, ROVER, CAMERA, PHYSICS, CONTROLLER, LANDING
|
||||
CONFIG_LOADED = True
|
||||
except ImportError:
|
||||
CONFIG_LOADED = False
|
||||
# Defaults if config.py is missing
|
||||
DRONE = {"mass": 1.0, "size": (0.3, 0.3, 0.1), "color": (0.8, 0.1, 0.1, 1.0),
|
||||
"start_position": (0.0, 0.0, 5.0), "thrust_scale": 15.0,
|
||||
"pitch_torque_scale": 2.0, "roll_torque_scale": 2.0, "yaw_torque_scale": 1.0}
|
||||
ROVER = {"size": (1.0, 1.0, 0.3), "color": (0.2, 0.6, 0.2, 1.0),
|
||||
"start_position": (0.0, 0.0, 0.15), "default_pattern": "stationary",
|
||||
"default_speed": 0.5, "default_amplitude": 2.0}
|
||||
CAMERA = {"fov": 60.0}
|
||||
PHYSICS = {"gravity": -9.81, "timestep": 1/240}
|
||||
CONTROLLER = {"Kp_z": 0.5, "Kd_z": 0.3, "Kp_xy": 0.3, "Kd_xy": 0.2}
|
||||
LANDING = {"success_velocity": 0.5}
|
||||
|
||||
|
||||
class StandaloneSimulation:
|
||||
"""Complete simulation with built-in controller - no ROS 2 needed."""
|
||||
|
||||
def __init__(self, rover_pattern=None, rover_speed=None, rover_amplitude=None):
|
||||
print("=" * 60)
|
||||
print("Standalone Drone Simulation (No ROS 2 Required)")
|
||||
if CONFIG_LOADED:
|
||||
print(" Configuration loaded from config.py")
|
||||
print("=" * 60)
|
||||
|
||||
self._running = True
|
||||
self._step_count = 0
|
||||
self._time = 0.0
|
||||
|
||||
# Use config values or arguments
|
||||
self._rover_pattern = rover_pattern or ROVER.get("default_pattern", "stationary")
|
||||
self._rover_speed = rover_speed if rover_speed is not None else ROVER.get("default_speed", 0.5)
|
||||
self._rover_amplitude = rover_amplitude if rover_amplitude is not None else ROVER.get("default_amplitude", 2.0)
|
||||
self._rover_pos = list(ROVER.get("start_position", (0.0, 0.0, 0.15)))
|
||||
|
||||
# Physics constants from config
|
||||
self._gravity = PHYSICS.get("gravity", -9.81)
|
||||
self._timestep = PHYSICS.get("timestep", 1/240)
|
||||
self._control_interval = 5
|
||||
|
||||
# Drone constants from config
|
||||
self._drone_mass = DRONE.get("mass", 1.0)
|
||||
self._drone_size = DRONE.get("size", (0.3, 0.3, 0.1))
|
||||
self._drone_start = DRONE.get("start_position", (0.0, 0.0, 5.0))
|
||||
self._drone_color = DRONE.get("color", (0.8, 0.1, 0.1, 1.0))
|
||||
self._thrust_scale = DRONE.get("thrust_scale", 15.0)
|
||||
self._pitch_torque_scale = DRONE.get("pitch_torque_scale", 2.0)
|
||||
self._roll_torque_scale = DRONE.get("roll_torque_scale", 2.0)
|
||||
self._yaw_torque_scale = DRONE.get("yaw_torque_scale", 1.0)
|
||||
self._hover_thrust = self._drone_mass * abs(self._gravity)
|
||||
|
||||
# Rover constants from config
|
||||
self._rover_size = ROVER.get("size", (1.0, 1.0, 0.3))
|
||||
self._rover_color = ROVER.get("color", (0.2, 0.6, 0.2, 1.0))
|
||||
|
||||
# Camera
|
||||
self._camera_fov = CAMERA.get("fov", 60.0)
|
||||
|
||||
# Controller gains
|
||||
self._Kp_z = CONTROLLER.get("Kp_z", 0.5)
|
||||
self._Kd_z = CONTROLLER.get("Kd_z", 0.3)
|
||||
self._Kp_xy = CONTROLLER.get("Kp_xy", 0.3)
|
||||
self._Kd_xy = CONTROLLER.get("Kd_xy", 0.2)
|
||||
|
||||
# Landing
|
||||
self._landing_velocity = LANDING.get("success_velocity", 0.5)
|
||||
|
||||
# Control command
|
||||
self._command = {'thrust': 0.0, 'pitch': 0.0, 'roll': 0.0, 'yaw': 0.0}
|
||||
|
||||
self._init_physics()
|
||||
self._init_objects()
|
||||
|
||||
print(f" Drone Start: {self._drone_start}")
|
||||
print(f" Rover Start: {self._rover_pos}")
|
||||
print(f" Rover Pattern: {self._rover_pattern}")
|
||||
print(f" Rover Speed: {self._rover_speed} m/s")
|
||||
print(" Press Ctrl+C to exit")
|
||||
print("=" * 60)
|
||||
|
||||
def _init_physics(self) -> None:
|
||||
self._physics_client = p.connect(p.GUI)
|
||||
p.setGravity(0, 0, self._gravity)
|
||||
p.setTimeStep(self._timestep)
|
||||
p.resetDebugVisualizerCamera(
|
||||
cameraDistance=8.0,
|
||||
cameraYaw=45,
|
||||
cameraPitch=-30,
|
||||
cameraTargetPosition=[0, 0, 1]
|
||||
)
|
||||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
|
||||
def _init_objects(self) -> None:
|
||||
self._ground_id = p.loadURDF("plane.urdf")
|
||||
|
||||
# Create rover (landing pad)
|
||||
rover_collision = p.createCollisionShape(
|
||||
p.GEOM_BOX,
|
||||
halfExtents=[s/2 for s in self._rover_size]
|
||||
)
|
||||
rover_visual = p.createVisualShape(
|
||||
p.GEOM_BOX,
|
||||
halfExtents=[s/2 for s in self._rover_size],
|
||||
rgbaColor=list(self._rover_color)
|
||||
)
|
||||
self._rover_id = p.createMultiBody(
|
||||
baseMass=0,
|
||||
baseCollisionShapeIndex=rover_collision,
|
||||
baseVisualShapeIndex=rover_visual,
|
||||
basePosition=self._rover_pos
|
||||
)
|
||||
|
||||
# Create landing pad marker
|
||||
self._create_landing_marker()
|
||||
|
||||
# Create drone
|
||||
drone_collision = p.createCollisionShape(
|
||||
p.GEOM_BOX,
|
||||
halfExtents=[s/2 for s in self._drone_size]
|
||||
)
|
||||
drone_visual = p.createVisualShape(
|
||||
p.GEOM_BOX,
|
||||
halfExtents=[s/2 for s in self._drone_size],
|
||||
rgbaColor=list(self._drone_color)
|
||||
)
|
||||
self._drone_id = p.createMultiBody(
|
||||
baseMass=self._drone_mass,
|
||||
baseCollisionShapeIndex=drone_collision,
|
||||
baseVisualShapeIndex=drone_visual,
|
||||
basePosition=self._drone_start
|
||||
)
|
||||
p.changeDynamics(
|
||||
self._drone_id, -1,
|
||||
linearDamping=0.1,
|
||||
angularDamping=0.5
|
||||
)
|
||||
|
||||
def _create_landing_marker(self) -> None:
|
||||
marker_height = self._rover_pos[2] + self._rover_size[2] / 2 + 0.01
|
||||
h_size = 0.3
|
||||
line_color = [1, 1, 1]
|
||||
|
||||
p.addUserDebugLine([-h_size, 0, marker_height], [h_size, 0, marker_height],
|
||||
lineColorRGB=line_color, lineWidth=3)
|
||||
p.addUserDebugLine([-h_size, -h_size, marker_height], [-h_size, h_size, marker_height],
|
||||
lineColorRGB=line_color, lineWidth=3)
|
||||
p.addUserDebugLine([h_size, -h_size, marker_height], [h_size, h_size, marker_height],
|
||||
lineColorRGB=line_color, lineWidth=3)
|
||||
|
||||
def run(self) -> None:
|
||||
print("\nSimulation running...")
|
||||
|
||||
try:
|
||||
while self._running:
|
||||
loop_start = time.time()
|
||||
|
||||
if not p.isConnected():
|
||||
break
|
||||
|
||||
# Update rover position
|
||||
self._update_rover()
|
||||
|
||||
# Run controller
|
||||
if self._step_count % self._control_interval == 0:
|
||||
self._run_controller()
|
||||
|
||||
# Apply physics
|
||||
self._apply_controls()
|
||||
p.stepSimulation()
|
||||
self._step_count += 1
|
||||
self._time += self._timestep
|
||||
|
||||
# Check landing
|
||||
if self._check_landing():
|
||||
print("\n*** LANDING SUCCESSFUL! ***\n")
|
||||
time.sleep(2)
|
||||
break
|
||||
|
||||
# Timing
|
||||
elapsed = time.time() - loop_start
|
||||
sleep_time = self._timestep - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("\nStopping simulation...")
|
||||
finally:
|
||||
if p.isConnected():
|
||||
p.disconnect()
|
||||
|
||||
def _update_rover(self) -> None:
|
||||
"""Move the rover based on pattern."""
|
||||
dt = self._timestep
|
||||
|
||||
if self._rover_pattern == 'stationary':
|
||||
return
|
||||
|
||||
elif self._rover_pattern == 'linear':
|
||||
omega = self._rover_speed / self._rover_amplitude
|
||||
target_x = self._rover_amplitude * math.sin(omega * self._time)
|
||||
self._rover_pos[0] += 2.0 * (target_x - self._rover_pos[0]) * dt
|
||||
|
||||
elif self._rover_pattern == 'circular':
|
||||
omega = self._rover_speed / self._rover_amplitude
|
||||
target_x = self._rover_amplitude * math.cos(omega * self._time)
|
||||
target_y = self._rover_amplitude * math.sin(omega * self._time)
|
||||
self._rover_pos[0] += 2.0 * (target_x - self._rover_pos[0]) * dt
|
||||
self._rover_pos[1] += 2.0 * (target_y - self._rover_pos[1]) * dt
|
||||
|
||||
elif self._rover_pattern == 'square':
|
||||
segment = int(self._time / 3) % 4
|
||||
corners = [(1, 1), (-1, 1), (-1, -1), (1, -1)]
|
||||
target = corners[segment]
|
||||
target_x = target[0] * self._rover_amplitude
|
||||
target_y = target[1] * self._rover_amplitude
|
||||
dx = target_x - self._rover_pos[0]
|
||||
dy = target_y - self._rover_pos[1]
|
||||
dist = math.sqrt(dx**2 + dy**2)
|
||||
if dist > 0.01:
|
||||
self._rover_pos[0] += self._rover_speed * dx / dist * dt
|
||||
self._rover_pos[1] += self._rover_speed * dy / dist * dt
|
||||
|
||||
# Update rover position in simulation
|
||||
p.resetBasePositionAndOrientation(
|
||||
self._rover_id,
|
||||
[self._rover_pos[0], self._rover_pos[1], self._rover_pos[2]],
|
||||
[0, 0, 0, 1]
|
||||
)
|
||||
|
||||
def _get_telemetry(self) -> Dict[str, Any]:
|
||||
"""Get current drone state."""
|
||||
pos, orn = p.getBasePositionAndOrientation(self._drone_id)
|
||||
vel, ang_vel = p.getBaseVelocity(self._drone_id)
|
||||
euler = p.getEulerFromQuaternion(orn)
|
||||
|
||||
# Landing pad detection
|
||||
dx = self._rover_pos[0] - pos[0]
|
||||
dy = self._rover_pos[1] - pos[1]
|
||||
dz = self._rover_pos[2] - pos[2]
|
||||
horizontal_dist = math.sqrt(dx**2 + dy**2)
|
||||
vertical_dist = -dz
|
||||
|
||||
landing_pad = None
|
||||
if vertical_dist > 0 and vertical_dist < 10.0:
|
||||
fov_rad = math.radians(self._camera_fov / 2)
|
||||
max_horizontal = vertical_dist * math.tan(fov_rad)
|
||||
if horizontal_dist < max_horizontal:
|
||||
landing_pad = {
|
||||
"relative_x": dx,
|
||||
"relative_y": dy,
|
||||
"distance": vertical_dist,
|
||||
"confidence": 1.0 - (horizontal_dist / max_horizontal)
|
||||
}
|
||||
|
||||
return {
|
||||
"altimeter": {"altitude": pos[2], "vertical_velocity": vel[2]},
|
||||
"velocity": {"x": vel[0], "y": vel[1], "z": vel[2]},
|
||||
"imu": {
|
||||
"orientation": {"roll": euler[0], "pitch": euler[1], "yaw": euler[2]},
|
||||
"angular_velocity": {"x": ang_vel[0], "y": ang_vel[1], "z": ang_vel[2]}
|
||||
},
|
||||
"landing_pad": landing_pad,
|
||||
"rover_position": {"x": self._rover_pos[0], "y": self._rover_pos[1]}
|
||||
}
|
||||
|
||||
def _run_controller(self) -> None:
|
||||
"""Simple landing controller."""
|
||||
telemetry = self._get_telemetry()
|
||||
|
||||
altimeter = telemetry['altimeter']
|
||||
altitude = altimeter['altitude']
|
||||
vertical_vel = altimeter['vertical_velocity']
|
||||
|
||||
velocity = telemetry['velocity']
|
||||
vel_x = velocity['x']
|
||||
vel_y = velocity['y']
|
||||
|
||||
landing_pad = telemetry['landing_pad']
|
||||
|
||||
# Altitude control
|
||||
target_alt = 0.0
|
||||
thrust = self._Kp_z * (target_alt - altitude) - self._Kd_z * vertical_vel
|
||||
|
||||
# Horizontal control
|
||||
if landing_pad is not None:
|
||||
target_x = landing_pad['relative_x']
|
||||
target_y = landing_pad['relative_y']
|
||||
pitch = self._Kp_xy * target_x - self._Kd_xy * vel_x
|
||||
roll = self._Kp_xy * target_y - self._Kd_xy * vel_y
|
||||
else:
|
||||
pitch = -self._Kd_xy * vel_x
|
||||
roll = -self._Kd_xy * vel_y
|
||||
|
||||
# Clamp
|
||||
thrust = max(-1.0, min(1.0, thrust))
|
||||
pitch = max(-0.5, min(0.5, pitch))
|
||||
roll = max(-0.5, min(0.5, roll))
|
||||
|
||||
self._command = {'thrust': thrust, 'pitch': pitch, 'roll': roll, 'yaw': 0.0}
|
||||
|
||||
def _apply_controls(self) -> None:
|
||||
"""Apply control commands to drone."""
|
||||
pos, orn = p.getBasePositionAndOrientation(self._drone_id)
|
||||
rot_matrix = p.getMatrixFromQuaternion(orn)
|
||||
local_up = [rot_matrix[2], rot_matrix[5], rot_matrix[8]]
|
||||
|
||||
total_thrust = self._hover_thrust + (self._command['thrust'] * self._thrust_scale)
|
||||
total_thrust = max(0, total_thrust)
|
||||
|
||||
thrust_force = [
|
||||
local_up[0] * total_thrust,
|
||||
local_up[1] * total_thrust,
|
||||
local_up[2] * total_thrust
|
||||
]
|
||||
|
||||
p.applyExternalForce(
|
||||
self._drone_id, -1,
|
||||
forceObj=thrust_force,
|
||||
posObj=pos,
|
||||
flags=p.WORLD_FRAME
|
||||
)
|
||||
|
||||
p.applyExternalTorque(
|
||||
self._drone_id, -1,
|
||||
torqueObj=[
|
||||
self._command['pitch'] * self._pitch_torque_scale,
|
||||
self._command['roll'] * self._roll_torque_scale,
|
||||
self._command['yaw'] * self._yaw_torque_scale
|
||||
],
|
||||
flags=p.LINK_FRAME
|
||||
)
|
||||
|
||||
def _check_landing(self) -> bool:
|
||||
"""Check if drone landed on rover."""
|
||||
contacts = p.getContactPoints(bodyA=self._drone_id, bodyB=self._rover_id)
|
||||
if len(contacts) > 0:
|
||||
vel, _ = p.getBaseVelocity(self._drone_id)
|
||||
speed = math.sqrt(vel[0]**2 + vel[1]**2 + vel[2]**2)
|
||||
return speed < self._landing_velocity
|
||||
return False
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description='Standalone Drone Simulation')
|
||||
parser.add_argument('--pattern', '-p', type=str, default='stationary',
|
||||
choices=['stationary', 'linear', 'circular', 'square'],
|
||||
help='Rover movement pattern')
|
||||
parser.add_argument('--speed', '-s', type=float, default=0.5,
|
||||
help='Rover speed in m/s')
|
||||
parser.add_argument('--amplitude', '-a', type=float, default=2.0,
|
||||
help='Rover movement amplitude in meters')
|
||||
args = parser.parse_args()
|
||||
|
||||
sim = StandaloneSimulation(
|
||||
rover_pattern=args.pattern,
|
||||
rover_speed=args.speed,
|
||||
rover_amplitude=args.amplitude
|
||||
)
|
||||
sim.run()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Reference in New Issue
Block a user