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