259 lines
8.5 KiB
Python
259 lines
8.5 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
Rover Controller - Controls the moving landing pad.
|
|
Usage: python rover_controller.py --pattern circular --speed 0.5 --amplitude 2.0
|
|
"""
|
|
|
|
import argparse
|
|
import math
|
|
import random
|
|
from enum import Enum
|
|
from typing import Tuple
|
|
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
from geometry_msgs.msg import Twist, Point
|
|
from std_msgs.msg import String
|
|
import json
|
|
|
|
# Load configuration
|
|
try:
|
|
from config import ROVER
|
|
CONFIG_LOADED = True
|
|
except ImportError:
|
|
CONFIG_LOADED = False
|
|
ROVER = {"default_pattern": "stationary", "default_speed": 0.5, "default_amplitude": 2.0}
|
|
|
|
|
|
class MovementPattern(Enum):
|
|
STATIONARY = "stationary"
|
|
LINEAR = "linear"
|
|
CIRCULAR = "circular"
|
|
RANDOM = "random"
|
|
SQUARE = "square"
|
|
|
|
|
|
class RoverController(Node):
|
|
"""Controls the rover movement."""
|
|
|
|
def __init__(
|
|
self,
|
|
pattern: MovementPattern = None,
|
|
speed: float = None,
|
|
amplitude: float = None
|
|
):
|
|
super().__init__('rover_controller')
|
|
|
|
# Use config defaults if not specified
|
|
if pattern is None:
|
|
pattern = MovementPattern(ROVER.get("default_pattern", "stationary"))
|
|
if speed is None:
|
|
speed = ROVER.get("default_speed", 0.5)
|
|
if amplitude is None:
|
|
amplitude = ROVER.get("default_amplitude", 2.0)
|
|
|
|
self._pattern = pattern
|
|
self._speed = speed
|
|
self._amplitude = amplitude
|
|
self._time = 0.0
|
|
self._position = Point()
|
|
self._position.x = 0.0
|
|
self._position.y = 0.0
|
|
self._position.z = 0.15
|
|
self._target_x = 0.0
|
|
self._target_y = 0.0
|
|
self._random_timer = 0.0
|
|
self._square_segment = 0
|
|
self._control_rate = 50.0
|
|
self._max_x = 5.0
|
|
self._max_y = 5.0
|
|
|
|
self.get_logger().info('=' * 50)
|
|
self.get_logger().info('Rover Controller Starting...')
|
|
self.get_logger().info(f' Pattern: {pattern.value}')
|
|
self.get_logger().info(f' Speed: {speed} m/s')
|
|
self.get_logger().info(f' Amplitude: {amplitude} m')
|
|
self.get_logger().info('=' * 50)
|
|
|
|
self._cmd_vel_pub = self.create_publisher(Twist, '/rover/cmd_vel', 10)
|
|
self._position_pub = self.create_publisher(Point, '/rover/position', 10)
|
|
self._telemetry_pub = self.create_publisher(String, '/rover/telemetry', 10)
|
|
|
|
self.get_logger().info(' Publishing to: /rover/cmd_vel, /rover/position, /rover/telemetry')
|
|
|
|
control_period = 1.0 / self._control_rate
|
|
self._control_timer = self.create_timer(control_period, self._control_loop)
|
|
|
|
self.get_logger().info('Rover Controller Ready!')
|
|
|
|
def _control_loop(self) -> None:
|
|
dt = 1.0 / self._control_rate
|
|
self._time += dt
|
|
|
|
vel_x, vel_y = self._calculate_velocity()
|
|
|
|
self._position.x += vel_x * dt
|
|
self._position.y += vel_y * dt
|
|
self._position.x = max(-self._max_x, min(self._max_x, self._position.x))
|
|
self._position.y = max(-self._max_y, min(self._max_y, self._position.y))
|
|
|
|
cmd = Twist()
|
|
cmd.linear.x = vel_x
|
|
cmd.linear.y = vel_y
|
|
self._cmd_vel_pub.publish(cmd)
|
|
self._position_pub.publish(self._position)
|
|
|
|
telemetry = {
|
|
"position": {
|
|
"x": round(self._position.x, 4),
|
|
"y": round(self._position.y, 4),
|
|
"z": round(self._position.z, 4)
|
|
},
|
|
"velocity": {
|
|
"x": round(vel_x, 4),
|
|
"y": round(vel_y, 4),
|
|
"z": 0.0
|
|
},
|
|
"pattern": self._pattern.value,
|
|
"timestamp": round(self._time, 4)
|
|
}
|
|
telemetry_msg = String()
|
|
telemetry_msg.data = json.dumps(telemetry)
|
|
self._telemetry_pub.publish(telemetry_msg)
|
|
|
|
def _calculate_velocity(self) -> Tuple[float, float]:
|
|
if self._pattern == MovementPattern.STATIONARY:
|
|
return self._pattern_stationary()
|
|
elif self._pattern == MovementPattern.LINEAR:
|
|
return self._pattern_linear()
|
|
elif self._pattern == MovementPattern.CIRCULAR:
|
|
return self._pattern_circular()
|
|
elif self._pattern == MovementPattern.RANDOM:
|
|
return self._pattern_random()
|
|
elif self._pattern == MovementPattern.SQUARE:
|
|
return self._pattern_square()
|
|
return (0.0, 0.0)
|
|
|
|
def _pattern_stationary(self) -> Tuple[float, float]:
|
|
kp = 1.0
|
|
return (-kp * self._position.x, -kp * self._position.y)
|
|
|
|
def _pattern_linear(self) -> Tuple[float, float]:
|
|
omega = self._speed / self._amplitude
|
|
target_x = self._amplitude * math.sin(omega * self._time)
|
|
kp = 2.0
|
|
vel_x = kp * (target_x - self._position.x)
|
|
vel_x = max(-self._speed, min(self._speed, vel_x))
|
|
return (vel_x, 0.0)
|
|
|
|
def _pattern_circular(self) -> Tuple[float, float]:
|
|
omega = self._speed / self._amplitude
|
|
target_x = self._amplitude * math.cos(omega * self._time)
|
|
target_y = self._amplitude * math.sin(omega * self._time)
|
|
kp = 2.0
|
|
vel_x = kp * (target_x - self._position.x)
|
|
vel_y = kp * (target_y - self._position.y)
|
|
speed = math.sqrt(vel_x**2 + vel_y**2)
|
|
if speed > self._speed:
|
|
vel_x = vel_x / speed * self._speed
|
|
vel_y = vel_y / speed * self._speed
|
|
return (vel_x, vel_y)
|
|
|
|
def _pattern_random(self) -> Tuple[float, float]:
|
|
dt = 1.0 / self._control_rate
|
|
self._random_timer += dt
|
|
if self._random_timer >= 3.0:
|
|
self._random_timer = 0.0
|
|
self._target_x = random.uniform(-self._amplitude, self._amplitude)
|
|
self._target_y = random.uniform(-self._amplitude, self._amplitude)
|
|
kp = 1.0
|
|
vel_x = kp * (self._target_x - self._position.x)
|
|
vel_y = kp * (self._target_y - self._position.y)
|
|
speed = math.sqrt(vel_x**2 + vel_y**2)
|
|
if speed > self._speed:
|
|
vel_x = vel_x / speed * self._speed
|
|
vel_y = vel_y / speed * self._speed
|
|
return (vel_x, vel_y)
|
|
|
|
def _pattern_square(self) -> Tuple[float, float]:
|
|
corners = [
|
|
(self._amplitude, self._amplitude),
|
|
(-self._amplitude, self._amplitude),
|
|
(-self._amplitude, -self._amplitude),
|
|
(self._amplitude, -self._amplitude),
|
|
]
|
|
target = corners[self._square_segment % 4]
|
|
dx = target[0] - self._position.x
|
|
dy = target[1] - self._position.y
|
|
dist = math.sqrt(dx**2 + dy**2)
|
|
if dist < 0.1:
|
|
self._square_segment += 1
|
|
target = corners[self._square_segment % 4]
|
|
dx = target[0] - self._position.x
|
|
dy = target[1] - self._position.y
|
|
dist = math.sqrt(dx**2 + dy**2)
|
|
if dist > 0.01:
|
|
return (self._speed * dx / dist, self._speed * dy / dist)
|
|
return (0.0, 0.0)
|
|
|
|
|
|
def parse_args():
|
|
parser = argparse.ArgumentParser(
|
|
description='Rover Controller - Moving Landing Platform',
|
|
formatter_class=argparse.RawDescriptionHelpFormatter,
|
|
epilog="""
|
|
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
|
|
"""
|
|
)
|
|
parser.add_argument(
|
|
'--pattern', '-p', type=str,
|
|
choices=[p.value for p in MovementPattern],
|
|
default='stationary', help='Movement pattern (default: stationary)'
|
|
)
|
|
parser.add_argument(
|
|
'--speed', '-s', type=float, default=0.5,
|
|
help='Movement speed in m/s (default: 0.5)'
|
|
)
|
|
parser.add_argument(
|
|
'--amplitude', '-a', type=float, default=2.0,
|
|
help='Movement amplitude in meters (default: 2.0)'
|
|
)
|
|
args, _ = parser.parse_known_args()
|
|
return args
|
|
|
|
|
|
def main(args=None):
|
|
cli_args = parse_args()
|
|
|
|
print(f"\n{'='*60}")
|
|
print(f" ROVER CONTROLLER - {cli_args.pattern.upper()}")
|
|
print(f"{'='*60}\n")
|
|
|
|
rclpy.init(args=args)
|
|
controller = None
|
|
|
|
try:
|
|
pattern = MovementPattern(cli_args.pattern)
|
|
controller = RoverController(
|
|
pattern=pattern,
|
|
speed=cli_args.speed,
|
|
amplitude=cli_args.amplitude
|
|
)
|
|
rclpy.spin(controller)
|
|
except KeyboardInterrupt:
|
|
print('\nShutting down...')
|
|
finally:
|
|
if controller:
|
|
controller.destroy_node()
|
|
if rclpy.ok():
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|