809 lines
29 KiB
Python
809 lines
29 KiB
Python
"""
|
||
marcus_odometry.py — Precise Distance & Position Module
|
||
=========================================================
|
||
Project : Marcus | YS Lootah Technology
|
||
Hardware : Unitree G1 EDU + Jetson Orin NX
|
||
Purpose : Track robot position precisely and execute accurate movements.
|
||
|
||
Two sources (priority order):
|
||
1. ROS2 /dog_odom — joint encoder data → ±2cm accuracy
|
||
2. Dead reckoning — velocity × time integration → ±10cm accuracy
|
||
|
||
Import in marcus_llava.py
|
||
--------------------------
|
||
from marcus_odometry import Odometry
|
||
odom = Odometry()
|
||
odom.start(zmq_sock=sock) ← pass existing ZMQ socket
|
||
|
||
odom.walk_distance(1.0) ← walk exactly 1 meter
|
||
odom.turn_degrees(90) ← turn exactly 90° left
|
||
odom.turn_degrees(-90) ← turn exactly 90° right
|
||
odom.get_position() ← {x, y, heading, source}
|
||
odom.reset() ← zero at current location
|
||
|
||
Standalone test
|
||
---------------
|
||
/home/unitree/miniconda3/envs/marcus/bin/python3 ~/Models_marcus/marcus_odometry.py
|
||
|
||
Date : April 2026
|
||
"""
|
||
|
||
import time
|
||
import math
|
||
import json
|
||
import threading
|
||
import zmq
|
||
|
||
|
||
# ══════════════════════════════════════════════════════════════════════════════
|
||
# CONFIGURATION
|
||
# ══════════════════════════════════════════════════════════════════════════════
|
||
|
||
ZMQ_HOST = "127.0.0.1"
|
||
ZMQ_PORT = 5556
|
||
ROS2_ODOM_TOPIC = "/dog_odom"
|
||
ODOM_INTERFACE = "eth0"
|
||
|
||
# Movement defaults
|
||
DEFAULT_WALK_SPEED = 0.25 # m/s — slower = more accurate
|
||
DEFAULT_TURN_SPEED = 0.25 # rad/s
|
||
DIST_TOLERANCE = 0.05 # meters — stop within 5cm
|
||
ANGLE_TOLERANCE = 2.0 # degrees — stop within 2°
|
||
SAFETY_TIMEOUT_MULT = 3.0 # timeout = (distance/speed) × this
|
||
ODOM_STALE_WARN = 1.0 # warn if odom not updated for this many seconds
|
||
|
||
# Dead reckoning
|
||
DR_UPDATE_HZ = 20 # integration rate
|
||
|
||
|
||
# ══════════════════════════════════════════════════════════════════════════════
|
||
# ODOMETRY CLASS
|
||
# ══════════════════════════════════════════════════════════════════════════════
|
||
|
||
class Odometry:
|
||
"""
|
||
Precise robot position tracking and movement control.
|
||
|
||
Thread-safe. Start with odom.start(zmq_sock).
|
||
If ROS2 is not available, falls back to dead reckoning automatically.
|
||
|
||
Usage:
|
||
odom = Odometry()
|
||
odom.start(zmq_sock=sock)
|
||
odom.walk_distance(1.0)
|
||
odom.turn_degrees(90)
|
||
pos = odom.get_position()
|
||
"""
|
||
|
||
def __init__(self):
|
||
self._lock = threading.Lock()
|
||
self._alive = [False]
|
||
self._sock = None # ZMQ PUB socket (shared from marcus_llava)
|
||
|
||
# Position state
|
||
self._x = 0.0 # meters from start
|
||
self._y = 0.0 # meters from start
|
||
self._heading = 0.0 # degrees — 0=start, +left, -right
|
||
self._vx = 0.0 # current velocity (for dead reckoning)
|
||
self._vy = 0.0
|
||
self._vyaw = 0.0
|
||
self._source = "none" # "ros2" or "dead_reckoning"
|
||
self._last_update = 0.0 # timestamp of last position update
|
||
|
||
# Start position snapshot (for return_to_start)
|
||
self._start_x = 0.0
|
||
self._start_y = 0.0
|
||
self._start_heading = 0.0
|
||
|
||
# Running flag
|
||
self._running = False
|
||
|
||
# ── STARTUP ───────────────────────────────────────────────────────────────
|
||
|
||
def start(self, zmq_sock=None) -> bool:
|
||
"""
|
||
Start odometry tracking in background thread.
|
||
|
||
Args:
|
||
zmq_sock : existing ZMQ PUB socket from marcus_llava.py.
|
||
If None, creates its own socket on ZMQ_PORT.
|
||
Passing the shared socket avoids port conflicts.
|
||
|
||
Returns:
|
||
True always — runs in either ROS2 or dead reckoning mode.
|
||
|
||
Edge cases:
|
||
- ZMQ port already in use → catches and warns
|
||
- Called twice → second call is no-op with warning
|
||
- ROS2 not sourced → falls back to dead reckoning automatically
|
||
"""
|
||
if self._running:
|
||
print(" [Odom] ⚠️ Already running — ignoring duplicate start()")
|
||
return True
|
||
|
||
# Wire up ZMQ socket
|
||
if zmq_sock is not None:
|
||
self._sock = zmq_sock
|
||
else:
|
||
self._init_own_zmq()
|
||
|
||
if self._sock is None:
|
||
print(" [Odom] ⚠️ No ZMQ socket — movement functions disabled")
|
||
return False
|
||
|
||
# Reset position
|
||
self._reset_state()
|
||
self._alive[0] = True
|
||
self._running = True
|
||
|
||
# Try ROS2 first
|
||
ros2_ok = self._try_start_ros2()
|
||
if not ros2_ok:
|
||
# Fall back to dead reckoning
|
||
threading.Thread(
|
||
target=self._dead_reckoning_loop,
|
||
daemon=True,
|
||
name="odom-dr"
|
||
).start()
|
||
print(" [Odom] Dead reckoning mode — accuracy ±10cm")
|
||
else:
|
||
print(" [Odom] ROS2 /dog_odom mode — accuracy ±2cm")
|
||
|
||
time.sleep(0.5)
|
||
return True
|
||
|
||
def stop(self):
|
||
"""Stop odometry tracking."""
|
||
self._alive[0] = False
|
||
self._running = False
|
||
print(" [Odom] Stopped")
|
||
|
||
def _init_own_zmq(self):
|
||
"""Create own ZMQ socket if no shared socket provided."""
|
||
try:
|
||
ctx = zmq.Context()
|
||
sock = ctx.socket(zmq.PUB)
|
||
sock.bind(f"tcp://{ZMQ_HOST}:{ZMQ_PORT}")
|
||
self._sock = sock
|
||
time.sleep(0.5)
|
||
except zmq.ZMQError as e:
|
||
if "Address already in use" in str(e):
|
||
print(f" [Odom] ⚠️ ZMQ port {ZMQ_PORT} already in use.")
|
||
print(" [Odom] Pass zmq_sock=sock from marcus_llava.py instead.")
|
||
else:
|
||
print(f" [Odom] ⚠️ ZMQ error: {e}")
|
||
self._sock = None
|
||
|
||
def _reset_state(self):
|
||
"""Reset position to zero."""
|
||
with self._lock:
|
||
self._x = self._y = self._heading = 0.0
|
||
self._vx = self._vy = self._vyaw = 0.0
|
||
self._start_x = self._start_y = self._start_heading = 0.0
|
||
self._last_update = time.time()
|
||
|
||
# ── ROS2 ODOMETRY ─────────────────────────────────────────────────────────
|
||
|
||
def _try_start_ros2(self) -> bool:
|
||
"""
|
||
Try to subscribe to /dog_odom via ROS2.
|
||
Returns True if ROS2 is available, False otherwise.
|
||
|
||
Edge cases:
|
||
- ROS2 not installed → ImportError caught
|
||
- ROS2 not sourced → caught
|
||
- Topic not publishing → timeout after 2s
|
||
"""
|
||
try:
|
||
import rclpy
|
||
# Skip ROS2 on this system — DDS init causes bad_alloc
|
||
# due to memory conflict with Holosoma + YOLO
|
||
# Dead reckoning fallback is used instead
|
||
return False
|
||
rclpy.init()
|
||
except Exception:
|
||
return False
|
||
|
||
# Quick check: can we create a node?
|
||
try:
|
||
from rclpy.node import Node
|
||
from nav_msgs.msg import Odometry as OdomMsg
|
||
|
||
class _OdomNode(Node):
|
||
def __init__(self, parent):
|
||
super().__init__("marcus_odom")
|
||
self._parent = parent
|
||
self.create_subscription(
|
||
OdomMsg, ROS2_ODOM_TOPIC,
|
||
self._callback, 10
|
||
)
|
||
|
||
def _callback(self, msg):
|
||
pos = msg.pose.pose.position
|
||
ori = msg.pose.pose.orientation
|
||
twist = msg.twist.twist
|
||
|
||
# Quaternion → yaw
|
||
siny = 2.0 * (ori.w * ori.z + ori.x * ori.y)
|
||
cosy = 1.0 - 2.0 * (ori.y**2 + ori.z**2)
|
||
yaw = math.degrees(math.atan2(siny, cosy))
|
||
|
||
with self._parent._lock:
|
||
self._parent._x = pos.x
|
||
self._parent._y = pos.y
|
||
self._parent._heading = yaw
|
||
self._parent._vx = twist.linear.x
|
||
self._parent._vy = twist.linear.y
|
||
self._parent._vyaw = twist.angular.z
|
||
self._parent._source = "ros2"
|
||
self._parent._last_update = time.time()
|
||
|
||
node = _OdomNode(self)
|
||
|
||
def _spin():
|
||
while self._alive[0]:
|
||
rclpy.spin_once(node, timeout_sec=0.02)
|
||
|
||
# Start spin thread and wait 2s for first message
|
||
t = threading.Thread(target=_spin, daemon=True, name="odom-ros2")
|
||
t.start()
|
||
|
||
deadline = time.time() + 2.0
|
||
while time.time() < deadline:
|
||
time.sleep(0.1)
|
||
if self._source == "ros2":
|
||
return True
|
||
|
||
# No message received — ROS2 available but topic silent
|
||
print(" [Odom] ⚠️ /dog_odom topic not publishing — using dead reckoning")
|
||
return False
|
||
|
||
except Exception as e:
|
||
return False
|
||
|
||
# ── DEAD RECKONING ────────────────────────────────────────────────────────
|
||
|
||
def _dead_reckoning_loop(self):
|
||
"""
|
||
Estimate position by integrating velocity × time.
|
||
Updates position at DR_UPDATE_HZ.
|
||
|
||
Edge cases:
|
||
- Very large dt (system sleep) → capped at 0.5s to avoid jumps
|
||
- Heading wrap-around at 360°/0° → handled by modulo
|
||
"""
|
||
dt_target = 1.0 / DR_UPDATE_HZ
|
||
t_last = time.time()
|
||
|
||
while self._alive[0]:
|
||
time.sleep(dt_target)
|
||
now = time.time()
|
||
dt = min(now - t_last, 0.5) # cap at 0.5s to prevent position jumps
|
||
t_last = now
|
||
|
||
with self._lock:
|
||
heading_rad = math.radians(self._heading)
|
||
vx = self._vx
|
||
vy = self._vy
|
||
vyaw = self._vyaw
|
||
|
||
# World-frame position update
|
||
self._x += (vx * math.cos(heading_rad) - vy * math.sin(heading_rad)) * dt
|
||
self._y += (vx * math.sin(heading_rad) + vy * math.cos(heading_rad)) * dt
|
||
self._heading = (self._heading + math.degrees(vyaw) * dt) % 360
|
||
|
||
self._source = "dead_reckoning"
|
||
self._last_update = now
|
||
|
||
# ── VELOCITY CONTROL ──────────────────────────────────────────────────────
|
||
|
||
def _send_vel(self, vx: float = 0.0, vy: float = 0.0, vyaw: float = 0.0):
|
||
"""Send velocity to Holosoma. Also updates dead reckoning velocity state."""
|
||
if self._sock:
|
||
try:
|
||
self._sock.send_string(json.dumps({"vel": {"vx": vx, "vy": vy, "vyaw": vyaw}}))
|
||
except Exception as e:
|
||
print(f" [Odom] ⚠️ ZMQ send error: {e}")
|
||
|
||
# Update dead reckoning state
|
||
with self._lock:
|
||
self._vx = vx
|
||
self._vy = vy
|
||
self._vyaw = vyaw
|
||
|
||
def _gradual_stop(self, steps: int = 20):
|
||
"""Smooth stop — 20 zero-velocity messages over ~1 second."""
|
||
for _ in range(steps):
|
||
self._send_vel(0.0, 0.0, 0.0)
|
||
time.sleep(0.05)
|
||
|
||
def _check_stale(self):
|
||
"""Warn if odometry hasn't been updated recently."""
|
||
if time.time() - self._last_update > ODOM_STALE_WARN:
|
||
print(f" [Odom] ⚠️ Position data may be stale (>{ODOM_STALE_WARN:.0f}s since last update)")
|
||
|
||
# ── PUBLIC MOVEMENT API ───────────────────────────────────────────────────
|
||
|
||
def get_position(self) -> dict:
|
||
"""
|
||
Return current robot position.
|
||
|
||
Returns:
|
||
{"x": float, "y": float, "heading": float,
|
||
"source": "ros2"|"dead_reckoning"|"none"}
|
||
|
||
x, y in meters from start position.
|
||
heading in degrees (0 = start direction, +left, -right, 0-360).
|
||
"""
|
||
with self._lock:
|
||
return {
|
||
"x": round(self._x, 4),
|
||
"y": round(self._y, 4),
|
||
"heading": round(self._heading, 2),
|
||
"source": self._source,
|
||
}
|
||
|
||
def get_distance_from_start(self) -> float:
|
||
"""Return Euclidean distance in meters from the starting position."""
|
||
with self._lock:
|
||
dx = self._x - self._start_x
|
||
dy = self._y - self._start_y
|
||
return round(math.sqrt(dx**2 + dy**2), 3)
|
||
|
||
def reset(self):
|
||
"""
|
||
Reset odometry to zero at current physical location.
|
||
Use when robot is at a known reference point.
|
||
"""
|
||
with self._lock:
|
||
self._x = self._y = self._heading = 0.0
|
||
self._start_x = self._start_y = self._start_heading = 0.0
|
||
print(" [Odom] ✅ Reset to (0, 0, 0°)")
|
||
|
||
def is_running(self) -> bool:
|
||
return self._running
|
||
|
||
def status_str(self) -> str:
|
||
"""Return formatted status string for display."""
|
||
pos = self.get_position()
|
||
return (f"x={pos['x']:.2f}m y={pos['y']:.2f}m "
|
||
f"heading={pos['heading']:.1f}° source={pos['source']}")
|
||
|
||
def walk_distance(self, meters: float, speed: float = None,
|
||
direction: str = "forward") -> bool:
|
||
"""
|
||
Walk a precise distance using odometry feedback.
|
||
|
||
Args:
|
||
meters : distance in meters (must be > 0)
|
||
speed : m/s (default DEFAULT_WALK_SPEED = 0.25)
|
||
direction : "forward" or "backward"
|
||
|
||
Returns:
|
||
True if target reached within tolerance.
|
||
False if timeout, invalid input, or odometry not running.
|
||
|
||
Edge cases:
|
||
- meters <= 0 → rejected
|
||
- speed <= 0 or > 0.5 → clamped with warning
|
||
- Odometry not running → time-based fallback with warning
|
||
- Safety timeout = (meters / speed) × SAFETY_TIMEOUT_MULT
|
||
- Keyboard interrupt → gradual_stop then re-raise
|
||
"""
|
||
# Validate inputs
|
||
if meters <= 0:
|
||
print(f" [Odom] ⚠️ Invalid distance: {meters}m — must be positive")
|
||
return False
|
||
|
||
if speed is None:
|
||
speed = DEFAULT_WALK_SPEED
|
||
|
||
if speed <= 0:
|
||
print(f" [Odom] ⚠️ Speed must be positive — using default {DEFAULT_WALK_SPEED}")
|
||
speed = DEFAULT_WALK_SPEED
|
||
elif speed > 0.5:
|
||
print(f" [Odom] ⚠️ Speed {speed} exceeds safe limit — clamping to 0.4 m/s")
|
||
speed = 0.4
|
||
|
||
if not self._running:
|
||
print(" [Odom] ⚠️ Odometry not started — using time-based fallback")
|
||
return self._time_based_walk(meters, speed, direction)
|
||
|
||
self._check_stale()
|
||
|
||
vx = speed if direction == "forward" else -speed
|
||
|
||
with self._lock:
|
||
start_x = self._x
|
||
start_y = self._y
|
||
|
||
timeout = (meters / speed) * SAFETY_TIMEOUT_MULT
|
||
t0 = time.time()
|
||
|
||
print(f" [Odom] Walking {meters:.2f}m {direction} at {speed:.2f} m/s "
|
||
f"(timeout: {timeout:.1f}s)")
|
||
|
||
try:
|
||
while time.time() - t0 < timeout:
|
||
with self._lock:
|
||
dx = self._x - start_x
|
||
dy = self._y - start_y
|
||
covered = math.sqrt(dx**2 + dy**2)
|
||
|
||
if covered >= meters - DIST_TOLERANCE:
|
||
self._gradual_stop()
|
||
print(f" [Odom] ✅ Walked {covered:.3f}m (target {meters:.2f}m)")
|
||
return True
|
||
|
||
self._send_vel(vx=vx)
|
||
time.sleep(0.05)
|
||
|
||
except KeyboardInterrupt:
|
||
print(" [Odom] Walk interrupted by user")
|
||
self._gradual_stop()
|
||
raise
|
||
|
||
self._gradual_stop()
|
||
with self._lock:
|
||
dx = self._x - start_x
|
||
dy = self._y - start_y
|
||
covered = math.sqrt(dx**2 + dy**2)
|
||
print(f" [Odom] ⚠️ Timeout — walked {covered:.3f}m of {meters:.2f}m")
|
||
return False
|
||
|
||
def _time_based_walk(self, meters: float, speed: float,
|
||
direction: str = "forward") -> bool:
|
||
"""
|
||
Time-based fallback when odometry is not running.
|
||
Accuracy ~±10cm.
|
||
"""
|
||
print(f" [Odom] Time-based fallback: {meters:.2f}m ≈ {meters/speed:.1f}s")
|
||
vx = speed if direction == "forward" else -speed
|
||
duration = meters / speed
|
||
t0 = time.time()
|
||
try:
|
||
while time.time() - t0 < duration:
|
||
self._send_vel(vx=vx)
|
||
time.sleep(0.05)
|
||
except KeyboardInterrupt:
|
||
self._gradual_stop()
|
||
raise
|
||
self._gradual_stop()
|
||
print(f" [Odom] ✅ Time-based walk complete (~{meters:.2f}m)")
|
||
return True
|
||
|
||
def turn_degrees(self, degrees: float, speed: float = None) -> bool:
|
||
"""
|
||
Turn a precise number of degrees using odometry feedback.
|
||
|
||
Args:
|
||
degrees : degrees to turn.
|
||
Positive = left (counter-clockwise)
|
||
Negative = right (clockwise)
|
||
speed : angular speed in rad/s (default DEFAULT_TURN_SPEED)
|
||
|
||
Returns:
|
||
True if reached target within tolerance.
|
||
False if invalid input, timeout, or odometry not running.
|
||
|
||
Edge cases:
|
||
- degrees == 0 → no-op
|
||
- |degrees| > 360 → clamped to 360 with warning
|
||
- speed <= 0 → default
|
||
- speed > 0.5 → clamped
|
||
- Handles 0°/360° wrap-around correctly
|
||
- Keyboard interrupt → graceful stop
|
||
"""
|
||
if degrees == 0:
|
||
print(" [Odom] ⚠️ Turn 0° — nothing to do")
|
||
return True
|
||
|
||
if abs(degrees) > 360:
|
||
print(f" [Odom] ⚠️ Turn {degrees}° > 360 — clamping to {360 * (1 if degrees > 0 else -1)}°")
|
||
degrees = 360.0 * (1 if degrees > 0 else -1)
|
||
|
||
if speed is None:
|
||
speed = DEFAULT_TURN_SPEED
|
||
|
||
if speed <= 0:
|
||
speed = DEFAULT_TURN_SPEED
|
||
elif speed > 0.5:
|
||
print(f" [Odom] ⚠️ Turn speed clamped to 0.4 rad/s")
|
||
speed = 0.4
|
||
|
||
if not self._running:
|
||
print(" [Odom] ⚠️ Odometry not started — using time-based fallback")
|
||
return self._time_based_turn(degrees, speed)
|
||
|
||
self._check_stale()
|
||
|
||
vyaw = speed if degrees > 0 else -speed
|
||
target_abs = abs(degrees)
|
||
|
||
with self._lock:
|
||
start_heading = self._heading
|
||
|
||
timeout = (target_abs / math.degrees(speed)) * SAFETY_TIMEOUT_MULT
|
||
t0 = time.time()
|
||
|
||
direction = "left" if degrees > 0 else "right"
|
||
print(f" [Odom] Turning {degrees:.1f}° {direction} at {math.degrees(speed):.1f}°/s "
|
||
f"(timeout: {timeout:.1f}s)")
|
||
|
||
try:
|
||
while time.time() - t0 < timeout:
|
||
with self._lock:
|
||
current = self._heading
|
||
|
||
# Compute angular delta — handle wrap-around
|
||
delta = abs(current - start_heading)
|
||
if delta > 180:
|
||
delta = 360 - delta
|
||
|
||
if delta >= target_abs - ANGLE_TOLERANCE:
|
||
self._gradual_stop()
|
||
print(f" [Odom] ✅ Turned {delta:.1f}° (target {target_abs:.1f}°)")
|
||
return True
|
||
|
||
self._send_vel(vyaw=vyaw)
|
||
time.sleep(0.05)
|
||
|
||
except KeyboardInterrupt:
|
||
print(" [Odom] Turn interrupted by user")
|
||
self._gradual_stop()
|
||
raise
|
||
|
||
self._gradual_stop()
|
||
print(f" [Odom] ⚠️ Timeout on turn")
|
||
return False
|
||
|
||
def _time_based_turn(self, degrees: float, speed: float) -> bool:
|
||
"""Time-based turn fallback. ~±5° accuracy."""
|
||
target_abs = abs(degrees)
|
||
vyaw = speed if degrees > 0 else -speed
|
||
duration = target_abs / math.degrees(speed)
|
||
print(f" [Odom] Time-based fallback: {degrees:.1f}° ≈ {duration:.1f}s")
|
||
t0 = time.time()
|
||
try:
|
||
while time.time() - t0 < duration:
|
||
self._send_vel(vyaw=vyaw)
|
||
time.sleep(0.05)
|
||
except KeyboardInterrupt:
|
||
self._gradual_stop()
|
||
raise
|
||
self._gradual_stop()
|
||
print(f" [Odom] ✅ Time-based turn complete (~{degrees:.1f}°)")
|
||
return True
|
||
|
||
def navigate_to(self, target_x: float, target_y: float,
|
||
target_heading: float = None,
|
||
speed: float = None) -> bool:
|
||
"""
|
||
Navigate to absolute coordinates.
|
||
|
||
Steps:
|
||
1. Rotate to face target direction
|
||
2. Walk straight-line distance to target
|
||
3. Optionally rotate to target_heading
|
||
|
||
Args:
|
||
target_x, target_y : destination in meters (same frame as odometry)
|
||
target_heading : final heading in degrees (None = don't adjust)
|
||
speed : walk speed in m/s
|
||
|
||
Edge cases:
|
||
- Already at target → skip walk, only rotate if needed
|
||
- Odometry not running → time-based fallback (imprecise)
|
||
- target_heading None → skip final rotation
|
||
"""
|
||
with self._lock:
|
||
curr_x = self._x
|
||
curr_y = self._y
|
||
curr_h = self._heading
|
||
|
||
dx = target_x - curr_x
|
||
dy = target_y - curr_y
|
||
dist = math.sqrt(dx**2 + dy**2)
|
||
|
||
print(f" [Odom] Navigate to ({target_x:.2f}, {target_y:.2f}) — "
|
||
f"dist={dist:.2f}m")
|
||
|
||
if dist < DIST_TOLERANCE:
|
||
print(" [Odom] Already at target position")
|
||
else:
|
||
# Rotate to face target
|
||
angle_to_target = math.degrees(math.atan2(dy, dx))
|
||
rotation_needed = angle_to_target - curr_h
|
||
# Normalize to -180..180
|
||
while rotation_needed > 180: rotation_needed -= 360
|
||
while rotation_needed < -180: rotation_needed += 360
|
||
|
||
if abs(rotation_needed) > ANGLE_TOLERANCE:
|
||
self.turn_degrees(rotation_needed, speed=DEFAULT_TURN_SPEED)
|
||
time.sleep(0.3)
|
||
|
||
self.walk_distance(dist, speed=speed)
|
||
time.sleep(0.3)
|
||
|
||
# Rotate to final heading if specified
|
||
if target_heading is not None:
|
||
with self._lock:
|
||
curr_h = self._heading
|
||
final_rot = target_heading - curr_h
|
||
while final_rot > 180: final_rot -= 360
|
||
while final_rot < -180: final_rot += 360
|
||
if abs(final_rot) > ANGLE_TOLERANCE:
|
||
self.turn_degrees(final_rot)
|
||
|
||
print(f" [Odom] ✅ Arrived at ({target_x:.2f}, {target_y:.2f})")
|
||
return True
|
||
|
||
def return_to_start(self, speed: float = None) -> bool:
|
||
"""
|
||
Navigate back to the position where start() was called.
|
||
|
||
Edge cases:
|
||
- Already at start → no movement
|
||
- Odometry not running → cannot navigate (warn)
|
||
"""
|
||
if not self._running:
|
||
print(" [Odom] ⚠️ Cannot return to start — odometry not running")
|
||
return False
|
||
print(" [Odom] Returning to start position...")
|
||
return self.navigate_to(
|
||
self._start_x, self._start_y,
|
||
self._start_heading, speed=speed
|
||
)
|
||
|
||
def patrol_route(self, waypoints: list,
|
||
speed: float = None,
|
||
loop: bool = False) -> bool:
|
||
"""
|
||
Walk through a list of waypoints in order.
|
||
|
||
Args:
|
||
waypoints : list of dicts with keys x, y, heading (all optional)
|
||
e.g. [{"x": 1.0, "y": 0.0, "heading": 90}]
|
||
speed : travel speed in m/s
|
||
loop : if True, return to first waypoint after last
|
||
|
||
Returns:
|
||
True if all waypoints reached.
|
||
|
||
Edge cases:
|
||
- Empty waypoints → warns
|
||
- Waypoint missing x/y → skipped with warning
|
||
- Keyboard interrupt → stops gracefully at current position
|
||
- loop=True appends waypoint[0] to end of route
|
||
"""
|
||
if not waypoints:
|
||
print(" [Odom] ⚠️ patrol_route: empty waypoints list")
|
||
return False
|
||
|
||
route = list(waypoints)
|
||
if loop:
|
||
route.append(waypoints[0])
|
||
|
||
print(f" [Odom] Patrol route: {len(route)} waypoints | loop={loop}")
|
||
|
||
try:
|
||
for i, wp in enumerate(route, 1):
|
||
if "x" not in wp or "y" not in wp:
|
||
print(f" [Odom] ⚠️ Waypoint {i} missing x/y — skipping")
|
||
continue
|
||
|
||
print(f" [Odom] Waypoint {i}/{len(route)}: ({wp['x']:.2f}, {wp['y']:.2f})")
|
||
heading = wp.get("heading", None)
|
||
self.navigate_to(wp["x"], wp["y"], heading, speed=speed)
|
||
time.sleep(0.5)
|
||
|
||
except KeyboardInterrupt:
|
||
print("\n [Odom] Patrol route interrupted by user")
|
||
self._gradual_stop()
|
||
return False
|
||
|
||
print(" [Odom] ✅ Patrol route complete")
|
||
return True
|
||
|
||
def __repr__(self):
|
||
pos = self.get_position()
|
||
return (f"Odometry(x={pos['x']:.2f}, y={pos['y']:.2f}, "
|
||
f"h={pos['heading']:.1f}°, source={pos['source']}, "
|
||
f"running={self._running})")
|
||
|
||
|
||
# ══════════════════════════════════════════════════════════════════════════════
|
||
# STANDALONE TEST
|
||
# ══════════════════════════════════════════════════════════════════════════════
|
||
|
||
if __name__ == "__main__":
|
||
import sys
|
||
|
||
print("Marcus Odometry — Standalone Test")
|
||
print("===================================\n")
|
||
|
||
odom = Odometry()
|
||
ok = odom.start()
|
||
time.sleep(1.5)
|
||
|
||
print(f"\nInitial state: {odom}")
|
||
print()
|
||
print("Commands:")
|
||
print(" walk <m> — walk precisely N meters")
|
||
print(" back <m> — walk backward N meters")
|
||
print(" turn <deg> — turn precisely N degrees (+left, -right)")
|
||
print(" status — current position")
|
||
print(" reset — zero position")
|
||
print(" goto <x> <y> — navigate to coordinates")
|
||
print(" home — return to start")
|
||
print(" square — walk 1m×1m square")
|
||
print(" q — quit")
|
||
print()
|
||
|
||
while True:
|
||
try:
|
||
raw = input("Odom: ").strip().lower()
|
||
except (EOFError, KeyboardInterrupt):
|
||
break
|
||
|
||
if not raw:
|
||
continue
|
||
|
||
parts = raw.split()
|
||
cmd = parts[0]
|
||
|
||
if cmd == "q":
|
||
break
|
||
|
||
elif cmd == "status":
|
||
print(f" {odom.status_str()}")
|
||
print(f" Distance from start: {odom.get_distance_from_start():.3f}m")
|
||
|
||
elif cmd == "reset":
|
||
odom.reset()
|
||
|
||
elif cmd == "walk" and len(parts) >= 2:
|
||
try:
|
||
odom.walk_distance(float(parts[1]))
|
||
except ValueError:
|
||
print(" Usage: walk <meters>")
|
||
|
||
elif cmd == "back" and len(parts) >= 2:
|
||
try:
|
||
odom.walk_distance(float(parts[1]), direction="backward")
|
||
except ValueError:
|
||
print(" Usage: back <meters>")
|
||
|
||
elif cmd == "turn" and len(parts) >= 2:
|
||
try:
|
||
odom.turn_degrees(float(parts[1]))
|
||
except ValueError:
|
||
print(" Usage: turn <degrees> (+left, -right)")
|
||
|
||
elif cmd == "goto" and len(parts) >= 3:
|
||
try:
|
||
odom.navigate_to(float(parts[1]), float(parts[2]))
|
||
except ValueError:
|
||
print(" Usage: goto <x> <y>")
|
||
|
||
elif cmd == "home":
|
||
odom.return_to_start()
|
||
|
||
elif cmd == "square":
|
||
print(" Walking 1m × 1m square...")
|
||
odom.walk_distance(1.0)
|
||
odom.turn_degrees(90)
|
||
odom.walk_distance(1.0)
|
||
odom.turn_degrees(90)
|
||
odom.walk_distance(1.0)
|
||
odom.turn_degrees(90)
|
||
odom.walk_distance(1.0)
|
||
odom.turn_degrees(90)
|
||
print(" ✅ Square complete")
|
||
|
||
else:
|
||
print(f" Unknown: {raw}")
|
||
|
||
odom.stop()
|
||
print("Odometry stopped.")
|