Practical case: safe UGV teleop with Raspberry Pi

#!/usr/bin/env python3
"""
ugv_hardware.py
Hardware abstraction layer for UGV Chassis.
Supports dry-run mocking for validation on standard PCs.
"""

import logging

class MockBumper:
    def __init__(self):
        self._is_pressed = False
        logging.info("[MOCK] Bumper initialized.")

    @property
    def is_pressed(self):
        return self._is_pressed

    def simulate_press(self, state: bool):
        self._is_pressed = state

class RealBumper:
    def __init__(self, pin: int):
        from gpiozero import Button
        # Internal pull-up: unpressed = True (High), pressed = False (Low)
        self.button = Button(pin, pull_up=True)
        logging.info(f"[HARDWARE] Bumper initialized on GPIO {pin}.")

    @property
    def is_pressed(self):
        return self.button.is_active

class MockMotorController:
    def __init__(self):
        self.left_speed = 0.0
        self.right_speed = 0.0
        logging.info("[MOCK] Motor controller initialized.")

    def set_motors(self, left_speed: float, right_speed: float):
        self.left_speed = max(-1.0, min(1.0, left_speed))
        self.right_speed = max(-1.0, min(1.0, right_speed))
        logging.debug(f"[MOCK] Motors set -> Left: {self.left_speed:.2f}, Right: {self.right_speed:.2f}")

class RealMotorController:
    def __init__(self, i2c_bus=1, pca_addr=0x40):
        import smbus2
        from gpiozero import DigitalOutputDevice

        self.bus = smbus2.SMBus(i2c_bus)
        self.pca_addr = pca_addr

        # Initialize PCA9685
        self.bus.write_byte_data(self.pca_addr, 0x00, 0x10) # Sleep
        self.bus.write_byte_data(self.pca_addr, 0xFE, 0x79) # Set prescaler for ~50Hz
        self.bus.write_byte_data(self.pca_addr, 0x00, 0x20) # Auto-increment

        # Initialize TB6612FNG Direction Pins
        self.ain1 = DigitalOutputDevice(22)
        self.ain2 = DigitalOutputDevice(23)
        self.bin1 = DigitalOutputDevice(24)
        self.bin2 = DigitalOutputDevice(25)

        logging.info("[HARDWARE] Motor controller initialized via PCA9685 and GPIO.")

    def _set_pwm(self, channel: int, duty_cycle: float):
        # Duty cycle from 0.0 to 1.0 mapped to 0-4095
        val = int(duty_cycle * 4095)
        self.bus.write_byte_data(self.pca_addr, 0x06 + 4*channel, 0)
        self.bus.write_byte_data(self.pca_addr, 0x07 + 4*channel, 0)
        self.bus.write_byte_data(self.pca_addr, 0x08 + 4*channel, val & 0xFF)
        self.bus.write_byte_data(self.pca_addr, 0x09 + 4*channel, val >> 8)

    def set_motors(self, left_speed: float, right_speed: float):
        # Constrain speeds
        left_speed = max(-1.0, min(1.0, left_speed))
        right_speed = max(-1.0, min(1.0, right_speed))

        # Left Motor (Motor A)
        if left_speed >= 0:
            self.ain1.on()
            self.ain2.off()
            self._set_pwm(0, left_speed)
        else:
            self.ain1.off()
            self.ain2.on()
            self._set_pwm(0, -left_speed)

        # Right Motor (Motor B)
        if right_speed >= 0:
            self.bin1.on()
            self.bin2.off()
            self._set_pwm(1, right_speed)
        else:
            self.bin1.off()
            self.bin2.on()
            self._set_pwm(1, -right_speed)

class UGVChassis:
    def __init__(self, dry_run: bool):
        self.dry_run = dry_run
        if dry_run:
            self.bumper = MockBumper()
            self.motors = MockMotorController()
        else:
            self.bumper = RealBumper(pin=17)
            self.motors = RealMotorController()

    def halt(self):
        self.motors.set_motors(0.0, 0.0)

#!/usr/bin/env python3
"""
ugv_teleop.py
Main teleoperation node with bumper safety override.
"""

import argparse
import logging
import time
import sys
import select
import termios
import tty
from ugv_hardware import UGVChassis

logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')

class TeleopController:
    def __init__(self, chassis: UGVChassis):
        self.chassis = chassis
        self.current_cmd = 'x'
        self.running = True

    def process_command(self, cmd: str):
        speed_forward = 0.8
        speed_turn = 0.5

        left_cmd = 0.0
        right_cmd = 0.0

        if cmd == 'w':
            left_cmd, right_cmd = speed_forward, speed_forward
        elif cmd == 's':
            left_cmd, right_cmd = -speed_forward, -speed_forward
        elif cmd == 'a':
            left_cmd, right_cmd = -speed_turn, speed_turn
        elif cmd == 'd':
            left_cmd, right_cmd = speed_turn, -speed_turn
        elif cmd == 'x':
            left_cmd, right_cmd = 0.0, 0.0
        elif cmd == 'q':
            self.running = False
            return

        # --- SAFETY OVERRIDE LOGIC ---
        # If bumper is pressed, prevent ANY forward motion commands
        if self.chassis.bumper.is_pressed:
            if left_cmd > 0: left_cmd = 0.0
            if right_cmd > 0: right_cmd = 0.0
            logging.warning("BUMPER PRESSED! Forward motion disabled.")

        self.chassis.motors.set_motors(left_cmd, right_cmd)

def get_key_non_blocking():
    """Reads a single character from stdin without blocking."""
    if select.select([sys.stdin], [], [], 0.0)[0]:
        return sys.stdin.read(1)
    return None

def run_self_test(controller: TeleopController):
    """Automated dry-run test sequence proving the safety logic."""
    logging.info("Starting automated self-test sequence...")

    # Test 1: Forward motion logic
    controller.process_command('w')
    assert controller.chassis.motors.left_speed == 0.8, "Left motor failed forward command."
    assert controller.chassis.motors.right_speed == 0.8, "Right motor failed forward command."
    logging.info("Test 1 Passed: Forward command executed correctly.")

    # Test 2: Bumper override
    controller.chassis.bumper.simulate_press(True)
    controller.process_command('w')
    assert controller.chassis.motors.left_speed == 0.0, "Safety override failed on left motor!"
    assert controller.chassis.motors.right_speed == 0.0, "Safety override failed on right motor!"
    logging.info("Test 2 Passed: Bumper successfully halted forward motion.")

    # Test 3: Reverse motion while bumper pressed
    controller.process_command('s')
    assert controller.chassis.motors.left_speed == -0.8, "Reverse failed during bumper press."
    assert controller.chassis.motors.right_speed == -0.8, "Reverse failed during bumper press."
    logging.info("Test 3 Passed: Reverse escape maneuvers remain active.")

    logging.info("All self-tests passed successfully.")

def main():
    parser = argparse.ArgumentParser(description="UGV Teleop Node")
    parser.add_argument('--dry-run', action='store_true', help="Run without hardware")
    parser.add_argument('--self-test', action='store_true', help="Run automated safety validation")
    args = parser.parse_args()

    chassis = UGVChassis(dry_run=args.dry_run or args.self_test)
    controller = TeleopController(chassis)

    if args.self_test:
        run_self_test(controller)
        return

    print("UGV Teleop Started. Keys: W (fwd), S (rev), A (left), D (right), X (stop), Q (quit)")
    old_settings = termios.tcgetattr(sys.stdin)
    try:
        tty.setcbreak(sys.stdin.fileno())
        while controller.running:
            cmd = get_key_non_blocking()
            if cmd:
                controller.current_cmd = cmd.lower()

            controller.process_command(controller.current_cmd)
            time.sleep(0.05) # 20Hz loop
    finally:
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
        chassis.halt()
        print("\nUGV Teleop Stopped.")

if __name__ == "__main__":
    main()