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