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