Initial Attempt
This commit is contained in:
246
rover_controller.py
Normal file
246
rover_controller.py
Normal file
@@ -0,0 +1,246 @@
|
||||
#!/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
|
||||
|
||||
|
||||
class MovementPattern(Enum):
|
||||
STATIONARY = "stationary"
|
||||
LINEAR = "linear"
|
||||
CIRCULAR = "circular"
|
||||
RANDOM = "random"
|
||||
SQUARE = "square"
|
||||
|
||||
|
||||
class RoverController(Node):
|
||||
"""Controls the rover movement."""
|
||||
|
||||
DEFAULT_PATTERN = MovementPattern.STATIONARY
|
||||
DEFAULT_SPEED = 0.5
|
||||
DEFAULT_AMPLITUDE = 2.0
|
||||
CONTROL_RATE = 50.0
|
||||
MAX_X = 5.0
|
||||
MAX_Y = 5.0
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
pattern: MovementPattern = DEFAULT_PATTERN,
|
||||
speed: float = DEFAULT_SPEED,
|
||||
amplitude: float = DEFAULT_AMPLITUDE
|
||||
):
|
||||
super().__init__('rover_controller')
|
||||
|
||||
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.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()
|
||||
Reference in New Issue
Block a user