From c32cf40e12dbbf249e47a4c002b9428e76fbbd2a Mon Sep 17 00:00:00 2001 From: Cyrille Nofficial Date: Wed, 11 May 2022 23:10:25 +0200 Subject: [PATCH] first implementation --- .Dockerignore | 2 + .gitignore | 5 + Dockerfile | 24 + build-docker.sh | 12 + donkeycar/__init__.py | 1 + donkeycar/parts/__init__.py | 1 + donkeycar/parts/actuator.py | 219 ++++++++ donkeycar/parts/pins.py | 1043 +++++++++++++++++++++++++++++++++++ donkeycar/utils.py | 237 ++++++++ events/__init__.py | 0 events/events_pb2.py | 758 +++++++++++++++++++++++++ pca9685/__init__.py | 0 pca9685/cli.py | 138 +++++ requirements.txt | 6 + setup.cfg | 5 + setup.py | 65 +++ 16 files changed, 2516 insertions(+) create mode 100644 .Dockerignore create mode 100644 .gitignore create mode 100644 Dockerfile create mode 100755 build-docker.sh create mode 100644 donkeycar/__init__.py create mode 100644 donkeycar/parts/__init__.py create mode 100644 donkeycar/parts/actuator.py create mode 100644 donkeycar/parts/pins.py create mode 100644 donkeycar/utils.py create mode 100644 events/__init__.py create mode 100644 events/events_pb2.py create mode 100644 pca9685/__init__.py create mode 100644 pca9685/cli.py create mode 100644 requirements.txt create mode 100644 setup.cfg create mode 100644 setup.py diff --git a/.Dockerignore b/.Dockerignore new file mode 100644 index 0000000..4940046 --- /dev/null +++ b/.Dockerignore @@ -0,0 +1,2 @@ +venv + diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c4f4f97 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +/venv/ +.eggs +*.egg-info +.idea +*/__pycache__/ diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..18412a1 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,24 @@ +FROM docker.io/library/python:3.10-slim + +RUN apt-get update && apt-get install -y build-essential + +# Configure piwheels repo to use pre-compiled numpy wheels for arm +RUN echo -n "[global]\nextra-index-url=https://www.piwheels.org/simple\n" >> /etc/pip.conf + +ADD requirements.txt requirements.txt + +RUN pip3 install -r requirements.txt + +ADD events events +ADD donkeycar donkeycar +ADD pca9685 pca9685 +ADD setup.cfg setup.cfg +ADD setup.py setup.py + +ENV PYTHON_EGG_CACHE=/tmp/cache +RUN python3 setup.py install + +WORKDIR /tmp +USER 1234 + +ENTRYPOINT ["/usr/local/bin/rc-pca9685"] diff --git a/build-docker.sh b/build-docker.sh new file mode 100755 index 0000000..45191d7 --- /dev/null +++ b/build-docker.sh @@ -0,0 +1,12 @@ +#! /bin/bash + +IMAGE_NAME=robocar-pca9685-python +TAG=$(git describe) +FULL_IMAGE_NAME=docker.io/cyrilix/${IMAGE_NAME}:${TAG} +PLATFORM="linux/amd64,linux/arm64" +#PLATFORM="linux/amd64,linux/arm64,linux/arm/v7" + +podman build . --platform "${PLATFORM}" --manifest "${IMAGE_NAME}:${TAG}" +podman manifest push --format v2s2 "localhost/${IMAGE_NAME}:${TAG}" "docker://${FULL_IMAGE_NAME}" + +printf "\nImage %s published" "docker://${FULL_IMAGE_NAME}" diff --git a/donkeycar/__init__.py b/donkeycar/__init__.py new file mode 100644 index 0000000..9f9161b --- /dev/null +++ b/donkeycar/__init__.py @@ -0,0 +1 @@ +from . import utils \ No newline at end of file diff --git a/donkeycar/parts/__init__.py b/donkeycar/parts/__init__.py new file mode 100644 index 0000000..1e87add --- /dev/null +++ b/donkeycar/parts/__init__.py @@ -0,0 +1 @@ +from . import pins \ No newline at end of file diff --git a/donkeycar/parts/actuator.py b/donkeycar/parts/actuator.py new file mode 100644 index 0000000..232c811 --- /dev/null +++ b/donkeycar/parts/actuator.py @@ -0,0 +1,219 @@ +""" +actuators.py +Classes to control the motors and servos. These classes +are wrapped in a mixer class before being used in the drive loop. +""" + +import logging +import time + +import donkeycar as dk +from donkeycar.parts.pins import PwmPin, PinState + +logger = logging.getLogger(__name__) + + +# +# pwm/duty-cycle/pulse +# - Standard RC servo pulses range from 1 millisecond (full reverse) +# to 2 milliseconds (full forward) with 1.5 milliseconds being neutral (stopped). +# - These pulses are typically send at 50 hertz (every 20 milliseconds). +# - This means that, using the standard 50hz frequency, a 1 ms pulse +# represents a 5% duty cycle and a 2 ms pulse represents a 10% duty cycle. +# - The important part is the length of the pulse; +# it must be in the range of 1 ms to 2ms. +# - So this means that if a different frequency is used, then the duty cycle +# must be adjusted in order to get the 1ms to 2ms pulse. +# - For instance, if a 60hz frequency is used, then a 1 ms pulse requires +# a duty cycle of 0.05 * 60 / 50 = 0.06 (6%) duty cycle +# - We default the frequency of our PCA9685 to 60 hz, so pulses in +# config are generally based on 60hz frequency and 12 bit values. +# We use 12 bit values because the PCA9685 has 12 bit resolution. +# So a 1 ms pulse is 0.06 * 4096 ~= 246, a neutral pulse of 0.09 duty cycle +# is 0.09 * 4096 ~= 367 and full forward pulse of 0.12 duty cycles +# is 0.12 * 4096 ~= 492 +# - These are generalizations that are useful for understanding the underlying +# api call arguments. The final choice of duty-cycle/pulse length depends +# on your hardware and perhaps your strategy (you may not want to go too fast, +# and so you may choose is low max throttle pwm) +# + +def duty_cycle(pulse_ms: float, frequency_hz: float) -> float: + """ + Calculate the duty cycle, 0 to 1, of a pulse given + the frequency and the pulse length + + :param pulse_ms:float the desired pulse length in milliseconds + :param frequency_hz:float the pwm frequency in hertz + :return:float duty cycle in range 0 to 1 + """ + ms_per_cycle = 1000 / frequency_hz + duty = pulse_ms / ms_per_cycle + return duty + + +def pulse_ms(pulse_bits: int) -> float: + """ + Calculate pulse width in milliseconds given a + 12bit pulse (as a PCA9685 would use). + Donkeycar throttle and steering PWM values are + based on PCA9685 12bit pulse values, where + 0 is zero duty cycle and 4095 is 100% duty cycle. + + :param pulse_bits:int 12bit integer in range 0 to 4095 + :return:float pulse length in milliseconds + """ + if pulse_bits < 0 or pulse_bits > 4095: + raise ValueError("pulse_bits must be in range 0 to 4095 (12bit integer)") + return pulse_bits / 4095 + + +class PulseController: + """ + Controller that provides a servo PWM pulse using the given PwmPin + See pins.py for pin provider implementations. + """ + + def __init__(self, pwm_pin: PwmPin, pwm_scale: float = 1.0, pwm_inverted: bool = False) -> None: + """ + :param pwm_pin:PwnPin pin that will emit the pulse. + :param pwm_scale:float scaling the 12 bit pulse value to compensate + for non-standard pwm frequencies. + :param pwm_inverted:bool True to invert the duty cycle + """ + self.pwm_pin = pwm_pin + self.scale = pwm_scale + self.inverted = pwm_inverted + self.started = pwm_pin.state() != PinState.NOT_STARTED + + def set_pulse(self, pulse: int) -> None: + """ + Set the length of the pulse using a 12 bit integer (0..4095) + :param pulse:int 12bit integer (0..4095) + """ + if pulse < 0 or pulse > 4095: + raise ValueError("pulse must be in range 0 to 4095") + + if not self.started: + self.pwm_pin.start() + self.started = True + if self.inverted: + pulse = 4095 - pulse + self.pwm_pin.duty_cycle(int(pulse * self.scale) / 4095) + + def run(self, pulse: int) -> None: + """ + Set the length of the pulse using a 12 bit integer (0..4095) + :param pulse:int 12bit integer (0..4095) + """ + self.set_pulse(pulse) + + +class PWMSteering: + """ + Wrapper over a PWM pulse controller to convert angles to PWM pulses. + """ + LEFT_ANGLE = -1 + RIGHT_ANGLE = 1 + + def __init__(self, controller, left_pulse, right_pulse): + + if controller is None: + raise ValueError("PWMSteering requires a set_pulse controller to be passed") + set_pulse = getattr(controller, "set_pulse", None) + if set_pulse is None or not callable(set_pulse): + raise ValueError("controller must have a set_pulse method") + + self.controller = controller + self.left_pulse = left_pulse + self.right_pulse = right_pulse + self.pulse = dk.utils.map_range(0, self.LEFT_ANGLE, self.RIGHT_ANGLE, + self.left_pulse, self.right_pulse) + self.running = True + logger.info('PWM Steering created') + + def update(self): + while self.running: + self.controller.set_pulse(self.pulse) + + def run_threaded(self, angle): + # map absolute angle to angle that vehicle can implement. + self.pulse = dk.utils.map_range(angle, + self.LEFT_ANGLE, self.RIGHT_ANGLE, + self.left_pulse, self.right_pulse) + + def run(self, angle): + self.run_threaded(angle) + self.controller.set_pulse(self.pulse) + + def shutdown(self): + # set steering straight + self.pulse = 0 + time.sleep(0.3) + self.running = False + + +class PWMThrottle: + """ + Wrapper over a PWM pulse controller to convert -1 to 1 throttle + values to PWM pulses. + """ + MIN_THROTTLE = -1 + MAX_THROTTLE = 1 + + def __init__(self, controller, max_pulse, min_pulse, zero_pulse): + + if controller is None: + raise ValueError("PWMThrottle requires a set_pulse controller to be passed") + set_pulse = getattr(controller, "set_pulse", None) + if set_pulse is None or not callable(set_pulse): + raise ValueError("controller must have a set_pulse method") + + self.controller = controller + self.max_pulse = max_pulse + self.min_pulse = min_pulse + self.zero_pulse = zero_pulse + self.pulse = zero_pulse + + # send zero pulse to calibrate ESC + logger.info("Init ESC") + self.controller.set_pulse(self.max_pulse) + time.sleep(0.01) + self.controller.set_pulse(self.min_pulse) + time.sleep(0.01) + self.controller.set_pulse(self.zero_pulse) + time.sleep(1) + self.running = True + logger.info('PWM Throttle created') + + def update(self): + while self.running: + self.controller.set_pulse(self.pulse) + + def run_threaded(self, throttle): + if throttle > 0: + self.pulse = dk.utils.map_range(throttle, 0, self.MAX_THROTTLE, + self.zero_pulse, self.max_pulse) + else: + self.pulse = dk.utils.map_range(throttle, self.MIN_THROTTLE, 0, + self.min_pulse, self.zero_pulse) + + def run(self, throttle): + self.run_threaded(throttle) + self.controller.set_pulse(self.pulse) + + def shutdown(self): + # stop vehicle + self.run(0) + self.running = False + + +class MockController(object): + def __init__(self): + pass + + def run(self, pulse): + pass + + def shutdown(self): + pass diff --git a/donkeycar/parts/pins.py b/donkeycar/parts/pins.py new file mode 100644 index 0000000..2a8631d --- /dev/null +++ b/donkeycar/parts/pins.py @@ -0,0 +1,1043 @@ +""" +pins.py - high level ttl an pwm pin abstraction. +This is designed to allow drivers that use ttl and pwm +pins to be reusable with different underlying libaries +and techologies. + +The abstract classes InputPin, OutputPin and PwmPin +provide an interface for starting, using and cleaning up the pins. +The factory functions input_pin_by_id(), output_pin_by_id() +and pwm_pin_by_id() construct pins given a string id +that specifies the underlying pin provider and it's attributes. +There are implementations for the Rpi.GPIO library and +for the PCA9685. + +Pin id allows pins to be selected using a single string to +select from different underlying providers, numbering schemes and settings. + +Use Rpi.GPIO library, GPIO.BOARD pin numbering scheme, pin number 13 + pin = input_pin_by_id("RPI_GPIO.BOARD.13") + +Use Rpi.GPIO library, GPIO.BCM broadcom pin numbering scheme, gpio pin number 33 + pin = output_pin_by_id("RPI_GPIO.BCM.33") + +Use PCA9685 on bus 0 at address 0x40, channel 7 + pin = pwm_pin_by_id("PCA9685.0:40.7") + +""" +from abc import ABC, abstractmethod +from typing import Any, Callable +import logging + + +logger = logging.getLogger(__name__) + + +class PinState: + LOW: int = 0 + HIGH: int = 1 + NOT_STARTED: int = -1 + + +class PinEdge: + RISING: int = 1 + FALLING: int = 2 + BOTH: int = 3 + + +class PinPull: + PULL_NONE: int = 1 + PULL_UP: int = 2 + PULL_DOWN: int = 3 + + +class PinProvider: + RPI_GPIO = "RPI_GPIO" + PCA9685 = "PCA9685" + PIGPIO = "PIGPIO" + + +class PinScheme: + BOARD = "BOARD" # board numbering + BCM = "BCM" # broadcom gpio numbering + + +# +# #### Base interface for input/output/pwm pins +# #### Implementations derive from these abstact classes +# + +class InputPin(ABC): + + def __init__(self) -> None: + super().__init__() + + @abstractmethod + def start(self, on_input=None, edge: int = PinEdge.RISING) -> None: + """ + Start the pin in input mode. + :param on_input: function to call when an edge is detected, or None to ignore + :param edge: type of edge(s) that trigger on_input; default is PinEdge.RISING + This raises a RuntimeError if the pin is already started. + You can check to see if the pin is started by calling + state() and checking for PinState.NOT_STARTED + """ + pass # subclasses should override this + + @abstractmethod + def stop(self) -> None: + """ + Stop the pin and return it to PinState.NOT_STARTED + """ + pass # subclasses should override this + + @abstractmethod + def state(self) -> int: + """ + Return most recent input state. This does not re-read the input pin, + it just returns that last value read by the input() method. + If the pin is not started or has been stopped, + this will return PinState:NOT_STARTED + """ + return PinState.NOT_STARTED # subclasses must override + + @abstractmethod + def input(self) -> int: + """ + Read the input state from the pin. + :return: PinState.LOW/HIGH or PinState.NOT_STARTED if pin not started + """ + return PinState.NOT_STARTED # subclasses must override + + +class OutputPin(ABC): + + def __init__(self) -> None: + super().__init__() + + @abstractmethod + def start(self, state: int = PinState.LOW) -> None: + """ + Start the pin in output mode and with given starting state. + This raises and RuntimeError if the pin is already started. + You can check to see if the pin is started by calling + state() and checking for PinState.NOT_STARTED + """ + pass # subclasses should override this + + @abstractmethod + def stop(self) -> None: + """ + Stop the pin and return it to PinState.NOT_STARTED + """ + pass # subclasses should override this + + @abstractmethod + def state(self) -> int: + """ + Return most recent output state. This does not re-read the pin, + It just returns that last value set by the output() method. + If the pin is not started or has been stopped, + this will return PinState:NOT_STARTED + :return: most recent output state OR PinState.NOT_STARTED if pin not started. + """ + return PinState.NOT_STARTED # subclasses must override + + @abstractmethod + def output(self, state: int) -> None: + """ + Set the output state of the pin to either + :param state: PinState.LOW or PinState.HIGH + :except: RuntimeError if pin not stated. + """ + pass # subclasses must override + + +class PwmPin(ABC): + + def __init__(self) -> None: + super().__init__() + + @abstractmethod + def start(self, duty: float = 0) -> None: + """ + Start the pin in output mode and with given starting state. + This raises and RuntimeError if the pin is already started. + You can check to see if the pin is started by calling + state() and checking for PinState.NOT_STARTED + :param duty: duty cycle in range 0 to 1 + """ + pass # subclasses should override this + + @abstractmethod + def stop(self) -> None: + """ + Stop the pin and return it to PinState.NOT_STARTED + """ + pass # subclasses should override this + + @abstractmethod + def state(self) -> float: + """ + Return most recent output state. This does not re-read the pin, + It just returns that last value set by the output() method. + If the pin is not started or has been stopped, + this will return PinState:NOT_STARTED + :return: most recent output duty_cycle + """ + return PinState.NOT_STARTED # subclasses must override + + @abstractmethod + def duty_cycle(self, duty: float) -> None: + """ + Set the output duty cycle of the pin + in range 0 to 1.0 (0% to 100%) + :param duty: duty cycle in range 0 to 1 + :except: RuntimeError is pin is not started + """ + pass # subclasses must override + + +# +# ####### Factory Methods +# + + +# +# Pin id allows pins to be selected using a single string to +# select from different underlying providers, numbering schemes and settings. +# +# Use Rpi.GPIO library, GPIO.BOARD pin numbering scheme, pin number 13 +# "RPI_GPIO.BOARD.13" +# +# Use Rpi.GPIO library, GPIO.BCM broadcom pin numbering scheme, gpio pin number 33 +# "RPI_GPIO.BCM.33" +# +# Use PCA9685 on bus 0 at address 0x40, channel 7 +# "PCA9685.0:40.7" +# +def output_pin_by_id(pin_id: str, frequency_hz: int = 60) -> OutputPin: + """ + Select a ttl output pin given a pin id. + :param pin_id: pin specifier string + :param frequency_hz: duty cycle frequency in hertz (only necessary for PCA9685) + :return: OutputPin + """ + parts = pin_id.split(".") + if parts[0] == PinProvider.PCA9685: + pin_provider = parts[0] + i2c_bus, i2c_address = parts[1].split(":") + i2c_bus = int(i2c_bus) + i2c_address = int(i2c_address, base=16) + frequency_hz = int(frequency_hz) + pin_number = int(parts[2]) + return output_pin(pin_provider, pin_number, i2c_bus=i2c_bus, i2c_address=i2c_address, frequency_hz=frequency_hz) + + if parts[0] == PinProvider.RPI_GPIO: + pin_provider = parts[0] + pin_scheme = parts[1] + pin_number = int(parts[2]) + return output_pin(pin_provider, pin_number, pin_scheme=pin_scheme) + + if parts[0] == PinProvider.PIGPIO: + pin_provider = parts[0] + if PinScheme.BCM != parts[1]: + raise ValueError("Pin scheme must be BCM for PIGPIO") + pin_number = int(parts[2]) + return output_pin(pin_provider, pin_number, pin_scheme=PinScheme.BCM) + + raise ValueError(f"Unknown pin provider {parts[0]}") + + +def pwm_pin_by_id(pin_id: str, frequency_hz: int = 60) -> PwmPin: + """ + Select a pwm output pin given a pin id. + :param pin_id: pin specifier string + :param frequency_hz: duty cycle frequency in hertz + :return: PwmPin + """ + parts = pin_id.split(".") + if parts[0] == PinProvider.PCA9685: + pin_provider = parts[0] + i2c_bus, i2c_address = parts[1].split(":") + i2c_bus = int(i2c_bus) + i2c_address = int(i2c_address, base=16) + pin_number = int(parts[2]) + return pwm_pin(pin_provider, pin_number, i2c_bus=i2c_bus, i2c_address=i2c_address, frequency_hz=frequency_hz) + + if parts[0] == PinProvider.RPI_GPIO: + pin_provider = parts[0] + pin_scheme = parts[1] + pin_number = int(parts[2]) + return pwm_pin(pin_provider, pin_number, pin_scheme=pin_scheme, frequency_hz=frequency_hz) + + if parts[0] == PinProvider.PIGPIO: + pin_provider = parts[0] + if PinScheme.BCM != parts[1]: + raise ValueError("Pin scheme must be BCM for PIGPIO") + pin_number = int(parts[2]) + return pwm_pin(pin_provider, pin_number, pin_scheme=PinScheme.BCM, frequency_hz=frequency_hz) + + raise ValueError(f"Unknown pin provider {parts[0]}") + + +def input_pin_by_id(pin_id: str, pull: int = PinPull.PULL_NONE) -> InputPin: + """ + Select a ttl input pin given a pin id. + """ + parts = pin_id.split(".") + if parts[0] == PinProvider.PCA9685: + raise RuntimeError("PinProvider.PCA9685 does not implement InputPin") + + if parts[0] == PinProvider.RPI_GPIO: + pin_provider = parts[0] + pin_scheme = parts[1] + pin_number = int(parts[2]) + return input_pin(pin_provider, pin_number, pin_scheme=pin_scheme, pull=pull) + + if parts[0] == PinProvider.PIGPIO: + pin_provider = parts[0] + if PinScheme.BCM != parts[1]: + raise ValueError("Pin scheme must be BCM for PIGPIO") + pin_number = int(parts[2]) + return input_pin(pin_provider, pin_number, pin_scheme=PinScheme.BCM, pull=pull) + + raise ValueError(f"Unknown pin provider {parts[0]}") + + +def input_pin( + pin_provider: str, + pin_number: int, + pin_scheme: str = PinScheme.BOARD, + pull: int = PinPull.PULL_NONE) -> InputPin: + """ + construct an InputPin using the given pin provider. + Note that PCA9685 can NOT provide an InputPin. + :param pin_provider: PinProvider string + :param pin_number: zero based pin number + :param pin_scheme: PinScheme string + :param pull: PinPull value + :return: InputPin + :except: RuntimeError if pin_provider is not valid. + """ + if pin_provider == PinProvider.RPI_GPIO: + return InputPinGpio(pin_number, pin_scheme, pull) + if pin_provider == PinProvider.PCA9685: + raise RuntimeError("PinProvider.PCA9685 does not implement InputPin") + if pin_provider == PinProvider.PIGPIO: + if pin_scheme != PinScheme.BCM: + raise ValueError("Pin scheme must be PinScheme.BCM for PIGPIO") + return InputPinPigpio(pin_number, pull) + raise RuntimeError(f"UnknownPinProvider ({pin_provider})") + + +def output_pin( + pin_provider: str, + pin_number: int, + pin_scheme: str = PinScheme.BOARD, + i2c_bus: int = 0, + i2c_address: int = 40, + frequency_hz: int = 60) -> OutputPin: + """ + construct an OutputPin using the given pin provider + Note that PCA9685 can NOT provide an InputPin. + :param pin_provider: PinProvider string + :param pin_number: zero based pin number + :param pin_scheme: PinScheme string + :param i2c_bus: I2C bus number for I2C devices + :param i2c_address: I2C address for I2C devices + :param frequency_hz: duty cycle frequence in hertz (for PCA9685) + :return: InputPin + :except: RuntimeError if pin_provider is not valid. + """ + if pin_provider == PinProvider.RPI_GPIO: + return OutputPinGpio(pin_number, pin_scheme) + if pin_provider == PinProvider.PCA9685: + return OutputPinPCA9685(pin_number, pca9685(i2c_bus, i2c_address, frequency_hz)) + if pin_provider == PinProvider.PIGPIO: + if pin_scheme != PinScheme.BCM: + raise ValueError("Pin scheme must be PinScheme.BCM for PIGPIO") + return OutputPinPigpio(pin_number) + raise RuntimeError(f"UnknownPinProvider ({pin_provider})") + + +def pwm_pin( + pin_provider: str, + pin_number: int, + pin_scheme: str = PinScheme.BOARD, + frequency_hz: int = 60, + i2c_bus: int = 0, + i2c_address: int = 40) -> PwmPin: + """ + construct a PwmPin using the given pin provider + :param pin_provider: PinProvider string + :param pin_number: zero based pin number + :param pin_scheme: PinScheme string + :param i2c_bus: I2C bus number for I2C devices + :param i2c_address: I2C address for I2C devices + :param frequency_hz: duty cycle frequence in hertz + :return: PwmPin + :except: RuntimeError if pin_provider is not valid. + """ + if pin_provider == PinProvider.RPI_GPIO: + return PwmPinGpio(pin_number, pin_scheme, frequency_hz) + if pin_provider == PinProvider.PCA9685: + return PwmPinPCA9685(pin_number, pca9685(i2c_bus, i2c_address, frequency_hz)) + if pin_provider == PinProvider.PIGPIO: + if pin_scheme != PinScheme.BCM: + raise ValueError("Pin scheme must be PinScheme.BCM for PIGPIO") + return PwmPinPigpio(pin_number, frequency_hz) + raise RuntimeError(f"UnknownPinProvider ({pin_provider})") + + +# +# ----- RPi.GPIO/Jetson.GPIO implementations ----- +# +try: + import RPi.GPIO as GPIO + # lookups to convert abstact api to GPIO values + gpio_pin_edge = [None, GPIO.RISING, GPIO.FALLING, GPIO.BOTH] + gpio_pin_pull = [None, GPIO.PUD_OFF, GPIO.PUD_DOWN, GPIO.PUD_UP] + gpio_pin_scheme = {PinScheme.BOARD: GPIO.BOARD, PinScheme.BCM: GPIO.BCM} +except ImportError: + logger.warn("RPi.GPIO was not imported.") + globals()["GPIO"] = None + + +def gpio_fn(pin_scheme:int, fn:Callable[[], Any]): + """ + Convenience method to enforce the desired GPIO pin scheme + before calling a GPIO function. + RPi.GPIO allows only a single scheme to be set at runtime. + If the pin scheme is already set to a different scheme, then + this will raise a RuntimeError to prevent erroneous pin outputs. + + :param pin_scheme:int GPIO.BOARD or GPIO.BCM + :param fn:Callable[[], Any] no-arg function to call after setting pin scheme. + :return:any return value from called function + :exception:RuntimeError if pin scheme is already set to a different scheme. + """ + prev_scheme = GPIO.getmode() + if prev_scheme is None: + GPIO.setmode(pin_scheme) + elif prev_scheme != pin_scheme: + raise RuntimeError(f"Attempt to change GPIO pin scheme from ({prev_scheme}) to ({pin_scheme})" + " after it has been set. All RPi.GPIO user must use the same pin scheme.") + val = fn() + return val + + +class InputPinGpio(InputPin): + def __init__(self, pin_number: int, pin_scheme: str, pull: int = PinPull.PULL_NONE) -> None: + """ + Input pin ttl HIGH/LOW using RPi.GPIO/Jetson.GPIO + :param pin_number: GPIO.BOARD mode point number + :param pull: enable a pull up or down resistor on pin. Default is PinPull.PULL_NONE + """ + self.pin_number = pin_number + self.pin_scheme = gpio_pin_scheme[pin_scheme] + self.pull = pull + self.on_input = None + self._state = PinState.NOT_STARTED + super().__init__() + + def _callback(self, pin_number): + if self.on_input is not None: + self.on_input(self.pin_number, self.input()) + + def start(self, on_input=None, edge=PinEdge.RISING) -> None: + """ + :param on_input: function to call when an edge is detected, or None to ignore + :param edge: type of edge(s) that trigger on_input; default is + """ + if self.state() != PinState.NOT_STARTED: + raise RuntimeError(f"Attempt to start InputPinGpio({self.pin_number}) that is already started.") + gpio_fn(self.pin_scheme, lambda: GPIO.setup(self.pin_number, GPIO.IN, pull_up_down=gpio_pin_pull[self.pull])) + if on_input is not None: + self.on_input = on_input + gpio_fn( + self.pin_scheme, + lambda: GPIO.add_event_detect(self.pin_number, gpio_pin_edge[edge], callback=self._callback)) + self.input() # read first state + + def stop(self) -> None: + if self.state() != PinState.NOT_STARTED: + gpio_fn(self.pin_scheme, lambda: GPIO.cleanup(self.pin_number)) + self._state = PinState.NOT_STARTED + + def state(self) -> int: + return self._state + + def input(self) -> int: + self._state = gpio_fn(self.pin_scheme, lambda: GPIO.input(self.pin_number)) + return self._state + + +class OutputPinGpio(OutputPin): + """ + Output pin ttl HIGH/LOW using Rpi.GPIO/Jetson.GPIO + """ + def __init__(self, pin_number: int, pin_scheme: str) -> None: + self.pin_number = pin_number + self.pin_scheme = gpio_pin_scheme[pin_scheme] + self._state = PinState.NOT_STARTED + + def start(self, state: int = PinState.LOW) -> None: + if self.state() != PinState.NOT_STARTED: + raise RuntimeError(f"Attempt to start OutputPinGpio({self.pin_number}) that is already started.") + gpio_fn(self.pin_scheme, lambda: GPIO.setup(self.pin_number, GPIO.OUT)) + self.output(state) + + def stop(self) -> None: + if self.state() != PinState.NOT_STARTED: + gpio_fn(self.pin_scheme, lambda: GPIO.cleanup(self.pin_number)) + self._state = PinState.NOT_STARTED + + def state(self) -> int: + return self._state + + def output(self, state: int) -> None: + gpio_fn(self.pin_scheme, lambda: GPIO.output(self.pin_number, state)) + self._state = state + + +class PwmPinGpio(PwmPin): + """ + PWM output pin using Rpi.GPIO/Jetson.GPIO + """ + def __init__(self, pin_number: int, pin_scheme: str, frequency_hz: float = 50) -> None: + self.pin_number = pin_number + self.pin_scheme = gpio_pin_scheme[pin_scheme] + self.frequency = int(frequency_hz) + self.pwm = None + self._state = PinState.NOT_STARTED + + def start(self, duty: float = 0) -> None: + if self.pwm is not None: + raise RuntimeError("Attempt to start PwmPinGpio that is already started.") + if duty < 0 or duty > 1: + raise ValueError("duty_cycle must be in range 0 to 1") + gpio_fn(self.pin_scheme, lambda: GPIO.setup(self.pin_number, GPIO.OUT)) + self.pwm = gpio_fn(self.pin_scheme, lambda: GPIO.PWM(self.pin_number, self.frequency)) + self.pwm.start(duty * 100) # takes duty in range 0 to 100 + self._state = duty + + def stop(self) -> None: + if self.pwm is not None: + self.pwm.stop() + gpio_fn(self.pin_scheme, lambda: GPIO.cleanup(self.pin_number)) + self._state = PinState.NOT_STARTED + + def state(self) -> float: + return self._state + + def duty_cycle(self, duty: float) -> None: + if duty < 0 or duty > 1: + raise ValueError("duty_cycle must be in range 0 to 1") + self.pwm.ChangeDutyCycle(duty * 100) # takes duty of 0 to 100 + self._state = duty + + +# +# ----- PCA9685 implementations ----- +# +class PCA9685: + ''' + Pin controller using PCA9685 boards. + This is used for most RC Cars. This + driver can output ttl HIGH or LOW or + produce a duty cycle at the given frequency. + ''' + def __init__(self, busnum: int, address: int, frequency: int): + + import Adafruit_PCA9685 + if busnum is not None: + from Adafruit_GPIO import I2C + + # monkey-patch I2C driver to use our bus number + def get_bus(): + return busnum + + I2C.get_default_bus = get_bus + self.pwm = Adafruit_PCA9685.PCA9685(address=address) + self.pwm.set_pwm_freq(frequency) + self._frequency = frequency + + def get_frequency(self): + return self._frequency + + def set_high(self, channel: int): + self.pwm.set_pwm(channel, 4096, 0) + + def set_low(self, channel: int): + self.pwm.set_pwm(channel, 0, 4096) + + def set_duty_cycle(self, channel: int, duty_cycle: float): + if duty_cycle < 0 or duty_cycle > 1: + raise ValueError("duty_cycle must be in range 0 to 1") + if duty_cycle == 1: + self.set_high(channel) + elif duty_cycle == 0: + self.set_low(channel) + else: + # duty cycle is fraction of the 12 bits + pulse = int(4096 * duty_cycle) + try: + self.pwm.set_pwm(channel, 0, pulse) + except Exception as e: + logger.error(f'Error on PCA9685 channel {channel}: {str(e)}') + + +# +# lookup map for PCA9685 singletons +# key is "busnum:address" +# +_pca9685 = {} + + +def pca9685(busnum: int, address: int, frequency: int = 60): + """ + pca9685 factory allocates driver for pca9685 + at given bus number and i2c address. + If we have already created one for that bus/addr + pair then use that singleton. If frequency is + not the same, then error. + :param busnum: I2C bus number of PCA9685 + :param address: address of PCA9685 on I2C bus + :param frequency: frequency in hertz of duty cycle + :except: PCA9685 has a single frequency for all channels, + so attempts to allocate a controller at a + given bus number and address with different + frequencies will raise a ValueError + """ + key = str(busnum) + ":" + hex(address) + pca = _pca9685.get(key) + if pca is None: + pca = PCA9685(busnum, address, frequency) + if pca.get_frequency() != frequency: + raise ValueError( + f"Frequency {frequency} conflicts with pca9685 at {key} " + f"with frequency {pca.pwm.get_pwm_freq()}") + return pca + + +class OutputPinPCA9685(ABC): + """ + Output pin ttl HIGH/LOW using PCA9685 + """ + def __init__(self, pin_number: int, pca9685: PCA9685) -> None: + self.pin_number = pin_number + self.pca9685 = pca9685 + self._state = PinState.NOT_STARTED + + def start(self, state: int = PinState.LOW) -> None: + """ + Start the pin in output mode. + This raises a RuntimeError if the pin is already started. + You can check to see if the pin is started by calling + state() and checking for PinState.NOT_STARTED + :param state: PinState to start with + :except: RuntimeError if pin is already started. + """ + if self.state() != PinState.NOT_STARTED: + raise RuntimeError(f"Attempt to start pin ({self.pin_number}) that is already started") + self._state = 0 # hack to allow first output to work + self.output(state) + + def stop(self) -> None: + """ + Stop the pin and return it to PinState.NOT_STARTED + """ + if self.state() != PinState.NOT_STARTED: + self.output(PinState.LOW) + self._state = PinState.NOT_STARTED + + def state(self) -> int: + """ + Return most recent output state. + If the pin is not started or has been stopped, + this will return PinState:NOT_STARTED + :return: PinState + """ + return self._state + + def output(self, state: int) -> None: + """ + Write output state to the pin. + :param state: PinState.LOW or PinState.HIGH + """ + if self.state() == PinState.NOT_STARTED: + raise RuntimeError(f"Attempt to use pin ({self.pin_number}) that is not started") + if state == PinState.HIGH: + self.pca9685.set_high(self.pin_number) + else: + self.pca9685.set_low(self.pin_number) + self._state = state + + +class PwmPinPCA9685(PwmPin): + """ + PWM output pin using PCA9685 + """ + def __init__(self, pin_number: int, pca9685: PCA9685) -> None: + self.pin_number = pin_number + self.pca9685 = pca9685 + self._state = PinState.NOT_STARTED + + def start(self, duty: float = 0) -> None: + """ + Start pin with given duty cycle + :param duty: duty cycle in range 0 to 1 + :except: RuntimeError if pin is already started. + """ + if self.state() != PinState.NOT_STARTED: + raise RuntimeError(f"Attempt to start pin ({self.pin_number}) that is already started") + if duty < 0 or duty > 1: + raise ValueError("duty_cycle must be in range 0 to 1") + self._state = 0 # hack to allow first duty_cycle to work + self.duty_cycle(duty) + self._state = duty + + def stop(self) -> None: + if self.state() != PinState.NOT_STARTED: + self.duty_cycle(0) + self._state = PinState.NOT_STARTED + + def state(self) -> float: + """ + This returns the last set duty cycle. + :return: duty cycle in range 0 to 1 OR PinState.NOT_STARTED in not started + """ + return self._state + + def duty_cycle(self, duty: float) -> None: + """ + Write a duty cycle to the output pin + :param duty: duty cycle in range 0 to 1 + :except: RuntimeError if not started + """ + if self.state() == PinState.NOT_STARTED: + raise RuntimeError(f"Attempt to use pin ({self.pin_number}) that is not started") + if duty < 0 or duty > 1: + raise ValueError("duty_cycle must be in range 0 to 1") + self.pca9685.set_duty_cycle(self.pin_number, duty) + self._state = duty + + +# +# ----- PIGPIO implementation ----- +# + +# pigpio is an optional install +try: + import pigpio + pigpio_pin_edge = [None, pigpio.RISING_EDGE, pigpio.FALLING_EDGE, pigpio.EITHER_EDGE] + pigpio_pin_pull = [None, pigpio.PUD_OFF, pigpio.PUD_DOWN, pigpio.PUD_UP] +except ImportError: + logger.warn("pigpio was not imported.") + globals()["pigpio"] = None + + +class InputPinPigpio(InputPin): + def __init__(self, pin_number: int, pull: int = PinPull.PULL_NONE, pgpio=None) -> None: + """ + Input pin ttl HIGH/LOW using PiGPIO library + :param pin_number: GPIO.BOARD mode pin number + :param pull: enable a pull up or down resistor on pin. Default is PinPull.PULL_NONE + :param pgpio: instance of pgpio to use or None to allocate a new one + """ + self.pgpio = pgpio + self.pin_number = pin_number + self.pull = pigpio_pin_pull[pull] + self.on_input = None + self._state = PinState.NOT_STARTED + + def __del__(self): + self.stop() + + def _callback(self, gpio, level, tock): + if self.on_input is not None: + self.on_input(gpio, level) + + def start(self, on_input=None, edge=PinEdge.RISING) -> None: + """ + Start the input pin and optionally set callback. + :param on_input: function to call when an edge is detected, or None to ignore + :param edge: type of edge(s) that trigger on_input; default is PinEdge.RISING + """ + if self.state() != PinState.NOT_STARTED: + raise RuntimeError(f"Attempt to start InputPinPigpio({self.pin_number}) that is already started.") + + self.pgpio = self.pgpio or pigpio.pi() + self.pgpio.set_mode(self.pin_number, pigpio.INPUT) + self.pgpio.set_pull_up_down(self.pin_number, self.pull) + + if on_input is not None: + self.on_input = on_input + self.pgpio.callback(self.pin_number, pigpio_pin_edge[edge], self._callback) + self._state = self.pgpio.read(self.pin_number) # read initial state + + def stop(self) -> None: + if self.state() != PinState.NOT_STARTED: + self.pgpio.stop() + self.pgpio = None + self.on_input = None + self._state = PinState.NOT_STARTED + + def state(self) -> int: + """ + Return last input() value. This does NOT read the input again; + it returns that last value that input() returned. + :return: PinState.LOW/HIGH OR PinState.NOT_STARTED if not started + """ + return self._state + + def input(self) -> int: + """ + Read the input pins state. + :return: PinState.LOW/HIGH OR PinState.NOT_STARTED if not started + """ + if self.state() != PinState.NOT_STARTED: + self._state = self.pgpio.read(self.pin_number) + return self._state + + +class OutputPinPigpio(OutputPin): + """ + Output pin ttl HIGH/LOW using Rpi.GPIO/Jetson.GPIO + """ + def __init__(self, pin_number: int, pgpio=None) -> None: + self.pgpio = pgpio + self.pin_number = pin_number + self._state = PinState.NOT_STARTED + + def start(self, state: int = PinState.LOW) -> None: + """ + Start the pin in output mode. + This raises a RuntimeError if the pin is already started. + You can check to see if the pin is started by calling + state() and checking for PinState.NOT_STARTED + :param state: PinState to start with + :except: RuntimeError if pin is already started. + """ + if self.state() != PinState.NOT_STARTED: + raise RuntimeError("Attempt to start OutputPin that is already started.") + + self.pgpio = self.pgpio or pigpio.pi() + self.pgpio.set_mode(self.pin_number, pigpio.OUTPUT) + self.pgpio.write(self.pin_number, state) # set initial state + self._state = state + + def stop(self) -> None: + if self.state() != PinState.NOT_STARTED: + self.pgpio.write(self.pin_number, PinState.LOW) + self.pgpio.stop() + self.pgpio = None + self._state = PinState.NOT_STARTED + + def state(self) -> int: + """ + Return last output state + :return: PinState.LOW/HIGH or PinState.NOT_STARTED if pin not started. + """ + return self._state + + def output(self, state: int) -> None: + """ + Write output state to the pin. + :param state: PinState.LOW or PinState.HIGH + """ + if self.state() != PinState.NOT_STARTED: + self.pgpio.write(self.pin_number, state) + self._state = state + + +class PwmPinPigpio(PwmPin): + """ + PWM output pin using Rpi.GPIO/Jetson.GPIO + """ + def __init__(self, pin_number: int, frequency_hz: float = 50, pgpio=None) -> None: + self.pgpio = pgpio + self.pin_number: int = pin_number + self.frequency: int = int(frequency_hz) + self._state: int = PinState.NOT_STARTED + + def start(self, duty: float = 0) -> None: + """ + Start pin with given duty cycle. + :param duty: duty cycle in range 0 to 1 + :except: RuntimeError if pin is already started. + """ + if self.state() != PinState.NOT_STARTED: + raise RuntimeError(f"Attempt to start InputPinPigpio({self.pin_number}) that is already started.") + if duty < 0 or duty > 1: + raise ValueError("duty_cycle must be in range 0 to 1") + self.pgpio = self.pgpio or pigpio.pi() + self.pgpio.set_mode(self.pin_number, pigpio.OUTPUT) + self.pgpio.set_PWM_frequency(self.pin_number, self.frequency) + self.pgpio.set_PWM_range(self.pin_number, 4095) # 12 bits, same as PCA9685 + self.pgpio.set_PWM_dutycycle(self.pin_number, int(duty * 4095)) # set initial state + self._state = duty + + def stop(self) -> None: + if self.state() != PinState.NOT_STARTED: + self.pgpio.write(self.pin_number, PinState.LOW) + self.pgpio.stop() + self.pgpio = None + self._state = PinState.NOT_STARTED + + def state(self) -> float: + """ + This returns the last set duty cycle. + :return: duty cycle in range 0 to 1 OR PinState.NOT_STARTED in not started + """ + return self._state + + def duty_cycle(self, duty: float) -> None: + """ + Write a duty cycle to the output pin + :param duty: duty cycle in range 0 to 1 + :except: RuntimeError if not started + """ + if duty < 0 or duty > 1: + raise ValueError("duty_cycle must be in range 0 to 1") + if self.state() != PinState.NOT_STARTED: + self.pgpio.set_PWM_dutycycle(self.pin_number, int(duty * 4095)) + self._state = duty + + +if __name__ == '__main__': + import argparse + import sys + import time + + # + # output 50% duty cycle on Rpi board pin 33 (equivalent to BCM.13) for 10 seconds + # python pins.py --pwm-pin=RPI_GPIO.BOARD.33 --duty=0.5 --time=10 + # + # input on Rpi board pin 35 (equivalend to BCM.19) for 10 seconds + # python pins.py --in-pin=RPI_GPIO.BOARD.35 --time=10 + # + # output 50% duty cycle on Rpi board pin 33, input on Rpi board pin 35 using interrupt handler + # python pins.py --pwm-pin=RPI_GPIO.BOARD.33 --duty=0.5 --in-pin=RPI_GPIO.BOARD.35 -int=rising --time=10 + # + # output on Rpi board pin 33, input on Rpi board pin 35 using interrupt handler + # python pins.py --out-pin=RPI_GPIO.BOARD.33 --duty=0.5 --in-pin=RPI_GPIO.BOARD.35 -int=rising --time=10 + # + # + # parse arguments + parser = argparse.ArgumentParser() + parser.add_argument("-p", "--pwm-pin", type=str, default=None, + help="pwm pin id, like 'PCA9685:1:60.13' or 'RPI_GPIO.BCM.13") + parser.add_argument("-hz", "--hertz", type=int, default=60, + help="PWM signal frequence in hertz. Default is 60hz") + parser.add_argument("-d", "--duty", type=float, default=0.5, + help="duty cycle in range 0 to 1. Default is 0.5") + + parser.add_argument("-o", "--out-pin", type=str, default=None, + help="ttl output pin id, like 'PCA9685:1:60.13' or 'RPI_GPIO.BOARD.35' or 'RPI_GPIO.BCM.13'") + + parser.add_argument("-i", "--in-pin", type=str, default=None, + help="ttl input pin id, like 'RPI_GPIO.BOARD.35' or 'RPI_GPIO.BCM.19'") + parser.add_argument("-pu", "--pull", type=str, choices=['up', 'down', 'none'], default='none', + help="input pin pullup, one of 'up', 'down', 'none'") + parser.add_argument("-int", "--interrupt", type=str, choices=['falling', 'rising', 'both', 'none'], default='none', + help="use interrupt routine on in-pin with given edge; 'falling', 'rising' or 'both'") + parser.add_argument("-tm", "--time", type=float, default=1, help="duration test in seconds") + parser.add_argument("-db", "--debug", action='store_true', help="show debug output") + parser.add_argument("-th", "--threaded", action='store_true', help="run in threaded mode") + + # Read arguments from command line + args = parser.parse_args() + + help = [] + if args.hertz < 1: + help.append("-hz/--hertz: must be >= 1.") + + if args.duty < 0 or args.duty > 1: + help.append("-d/--duty: must be in range 0 to 1") + + if args.pwm_pin is None and args.out_pin is None and args.in_pin is None: + help.append("must have one of -o/--out-pin or -p/--pwm-pin or -i/--in-pin") + + if args.pwm_pin is not None and args.out_pin is not None: + help.append("must have only one of -o/--out-pin or -p/--pwn-pin") + + if args.time < 1: + help.append("-tm/--time: must be > 0.") + + if len(help) > 0: + parser.print_help() + for h in help: + print(" " + h) + sys.exit(1) + + pin_pull = { + 'up': PinPull.PULL_UP, + 'down': PinPull.PULL_DOWN, + 'none': PinPull.PULL_NONE + } + pin_edge = { + 'none': None, + 'falling': PinEdge.FALLING, + 'rising': PinEdge.RISING, + 'both': PinEdge.BOTH + } + + def on_input(pin_number, state): + if state == PinState.HIGH: + print("+", pin_number, time.time()*1000) + elif state == PinState.LOW: + print("-", pin_number, time.time()*1000) + + pwm_out_pin: PwmPin = None + ttl_out_pin: OutputPin = None + ttl_in_pin: InputPin = None + try: + # + # construct a pin of the correct type + # + if args.in_pin is not None: + ttl_in_pin = input_pin_by_id(args.in_pin, pin_pull[args.pull]) + if args.interrupt != 'none': + ttl_in_pin.start(on_input=on_input, edge=pin_edge[args.interrupt]) + else: + ttl_in_pin.start() + + if args.pwm_pin is not None: + pwm_out_pin = pwm_pin_by_id(args.pwm_pin, args.hertz) + pwm_out_pin.start(args.duty) + + if args.out_pin is not None: + ttl_out_pin = output_pin_by_id(args.out_pin, args.hertz) + ttl_out_pin.start(PinState.LOW) + + start_time = time.time() + end_time = start_time + args.time + while start_time < end_time: + if ttl_out_pin is not None: + if args.duty > 0: + ttl_out_pin.output(PinState.HIGH) + time.sleep(1 / args.hertz * args.duty) + if args.duty < 1: + ttl_out_pin.output(PinState.LOW) + time.sleep(1 / args.hertz * (1 - args.duty)) + else: + # yield time to background threads + sleep_time = 1/args.hertz - (time.time() - start_time) + if sleep_time > 0.0: + time.sleep(sleep_time) + else: + time.sleep(0) # yield time to other threads + start_time = time.time() + + except KeyboardInterrupt: + print('Stopping early.') + except Exception as e: + print(e) + exit(1) + finally: + if pwm_out_pin is not None: + pwm_out_pin.stop() + if ttl_out_pin is not None: + ttl_out_pin.stop() diff --git a/donkeycar/utils.py b/donkeycar/utils.py new file mode 100644 index 0000000..95f4507 --- /dev/null +++ b/donkeycar/utils.py @@ -0,0 +1,237 @@ +''' +utils.py + +Functions that don't fit anywhere else. + +''' +from io import BytesIO +import os +import glob +import socket +import zipfile +import sys +import itertools +import subprocess +import math +import random +import time +import signal +import logging +from typing import List, Any, Tuple, Union + + +logger = logging.getLogger(__name__) + +ONE_BYTE_SCALE = 1.0 / 255.0 + + +class EqMemorizedString: + """ String that remembers what it was compared against """ + + def __init__(self, string): + self.string = string + self.mem = set() + + def __eq__(self, other): + self.mem.add(other) + return self.string == other + + def mem_as_str(self): + return ', '.join(self.mem) + + + + +''' +FILES +''' + + +def most_recent_file(dir_path, ext=''): + ''' + return the most recent file given a directory path and extension + ''' + query = dir_path + '/*' + ext + newest = min(glob.iglob(query), key=os.path.getctime) + return newest + + +def make_dir(path): + real_path = os.path.expanduser(path) + if not os.path.exists(real_path): + os.makedirs(real_path) + return real_path + + +def zip_dir(dir_path, zip_path): + """ + Create and save a zipfile of a one level directory + """ + file_paths = glob.glob(dir_path + "/*") # create path to search for files. + + zf = zipfile.ZipFile(zip_path, 'w') + dir_name = os.path.basename(dir_path) + for p in file_paths: + file_name = os.path.basename(p) + zf.write(p, arcname=os.path.join(dir_name, file_name)) + zf.close() + return zip_path + + +''' +BINNING +functions to help converte between floating point numbers and categories. +''' + + +def map_range(x, X_min, X_max, Y_min, Y_max): + ''' + Linear mapping between two ranges of values + ''' + X_range = X_max - X_min + Y_range = Y_max - Y_min + XY_ratio = X_range / Y_range + + y = ((x - X_min) / XY_ratio + Y_min) // 1 + + return int(y) + + +def map_range_float(x, X_min, X_max, Y_min, Y_max): + ''' + Same as map_range but supports floats return, rounded to 2 decimal places + ''' + X_range = X_max - X_min + Y_range = Y_max - Y_min + XY_ratio = X_range / Y_range + + y = ((x - X_min) / XY_ratio + Y_min) + + # print("y= {}".format(y)) + + return round(y, 2) + + +''' +ANGLES +''' + + +def norm_deg(theta): + while theta > 360: + theta -= 360 + while theta < 0: + theta += 360 + return theta + + +DEG_TO_RAD = math.pi / 180.0 + + +def deg2rad(theta): + return theta * DEG_TO_RAD + + +''' +VECTORS +''' + + +def dist(x1, y1, x2, y2): + return math.sqrt(math.pow(x2 - x1, 2) + math.pow(y2 - y1, 2)) + + +''' +NETWORKING +''' + + +def my_ip(): + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + s.connect(('192.0.0.8', 1027)) + return s.getsockname()[0] + + + +''' +OTHER +''' + + +def map_frange(x, X_min, X_max, Y_min, Y_max): + ''' + Linear mapping between two ranges of values + ''' + X_range = X_max - X_min + Y_range = Y_max - Y_min + XY_ratio = X_range / Y_range + + y = ((x - X_min) / XY_ratio + Y_min) + + return y + + +def merge_two_dicts(x, y): + """Given two dicts, merge them into a new dict as a shallow copy.""" + z = x.copy() + z.update(y) + return z + + +def param_gen(params): + ''' + Accepts a dictionary of parameter options and returns + a list of dictionary with the permutations of the parameters. + ''' + for p in itertools.product(*params.values()): + yield dict(zip(params.keys(), p)) + + +def run_shell_command(cmd, cwd=None, timeout=15): + proc = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, cwd=cwd) + out = [] + err = [] + + try: + proc.wait(timeout=timeout) + except subprocess.TimeoutExpired: + kill(proc.pid) + + for line in proc.stdout.readlines(): + out.append(line.decode()) + + for line in proc.stderr.readlines(): + err.append(line) + return out, err, proc.pid + + +def kill(proc_id): + os.kill(proc_id, signal.SIGINT) + + +def eprint(*args, **kwargs): + print(*args, file=sys.stderr, **kwargs) + + + +""" +Timers +""" + + +class FPSTimer(object): + def __init__(self): + self.t = time.time() + self.iter = 0 + + def reset(self): + self.t = time.time() + self.iter = 0 + + def on_frame(self): + self.iter += 1 + if self.iter == 100: + e = time.time() + print('fps', 100.0 / (e - self.t)) + self.t = time.time() + self.iter = 0 diff --git a/events/__init__.py b/events/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/events/events_pb2.py b/events/events_pb2.py new file mode 100644 index 0000000..f310718 --- /dev/null +++ b/events/events_pb2.py @@ -0,0 +1,758 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: events/events.proto + +from google.protobuf.internal import enum_type_wrapper +from google.protobuf import descriptor as _descriptor +from google.protobuf import message as _message +from google.protobuf import reflection as _reflection +from google.protobuf import symbol_database as _symbol_database +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + +from google.protobuf import timestamp_pb2 as google_dot_protobuf_dot_timestamp__pb2 + + +DESCRIPTOR = _descriptor.FileDescriptor( + name='events/events.proto', + package='robocar.events', + syntax='proto3', + serialized_options=b'Z\006events', + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\x13\x65vents/events.proto\x12\x0erobocar.events\x1a\x1fgoogle/protobuf/timestamp.proto\"T\n\x08\x46rameRef\x12\x0c\n\x04name\x18\x01 \x01(\t\x12\n\n\x02id\x18\x02 \x01(\t\x12.\n\ncreated_at\x18\x03 \x01(\x0b\x32\x1a.google.protobuf.Timestamp\"C\n\x0c\x46rameMessage\x12$\n\x02id\x18\x01 \x01(\x0b\x32\x18.robocar.events.FrameRef\x12\r\n\x05\x66rame\x18\x02 \x01(\x0c\"d\n\x0fSteeringMessage\x12\x10\n\x08steering\x18\x01 \x01(\x02\x12\x12\n\nconfidence\x18\x02 \x01(\x02\x12+\n\tframe_ref\x18\x03 \x01(\x0b\x32\x18.robocar.events.FrameRef\"d\n\x0fThrottleMessage\x12\x10\n\x08throttle\x18\x01 \x01(\x02\x12\x12\n\nconfidence\x18\x02 \x01(\x02\x12+\n\tframe_ref\x18\x03 \x01(\x0b\x32\x18.robocar.events.FrameRef\"A\n\x10\x44riveModeMessage\x12-\n\ndrive_mode\x18\x01 \x01(\x0e\x32\x19.robocar.events.DriveMode\"f\n\x0eObjectsMessage\x12\'\n\x07objects\x18\x01 \x03(\x0b\x32\x16.robocar.events.Object\x12+\n\tframe_ref\x18\x02 \x01(\x0b\x32\x18.robocar.events.FrameRef\"\x80\x01\n\x06Object\x12(\n\x04type\x18\x01 \x01(\x0e\x32\x1a.robocar.events.TypeObject\x12\x0c\n\x04left\x18\x02 \x01(\x05\x12\x0b\n\x03top\x18\x03 \x01(\x05\x12\r\n\x05right\x18\x04 \x01(\x05\x12\x0e\n\x06\x62ottom\x18\x05 \x01(\x05\x12\x12\n\nconfidence\x18\x06 \x01(\x02\"&\n\x13SwitchRecordMessage\x12\x0f\n\x07\x65nabled\x18\x01 \x01(\x08\"\x8c\x01\n\x0bRoadMessage\x12&\n\x07\x63ontour\x18\x01 \x03(\x0b\x32\x15.robocar.events.Point\x12(\n\x07\x65llipse\x18\x02 \x01(\x0b\x32\x17.robocar.events.Ellipse\x12+\n\tframe_ref\x18\x03 \x01(\x0b\x32\x18.robocar.events.FrameRef\"\x1d\n\x05Point\x12\t\n\x01x\x18\x01 \x01(\x05\x12\t\n\x01y\x18\x02 \x01(\x05\"r\n\x07\x45llipse\x12%\n\x06\x63\x65nter\x18\x01 \x01(\x0b\x32\x15.robocar.events.Point\x12\r\n\x05width\x18\x02 \x01(\x05\x12\x0e\n\x06height\x18\x03 \x01(\x05\x12\r\n\x05\x61ngle\x18\x04 \x01(\x02\x12\x12\n\nconfidence\x18\x05 \x01(\x02\"\x82\x01\n\rRecordMessage\x12+\n\x05\x66rame\x18\x01 \x01(\x0b\x32\x1c.robocar.events.FrameMessage\x12\x31\n\x08steering\x18\x02 \x01(\x0b\x32\x1f.robocar.events.SteeringMessage\x12\x11\n\trecordSet\x18\x03 \x01(\t*-\n\tDriveMode\x12\x0b\n\x07INVALID\x10\x00\x12\x08\n\x04USER\x10\x01\x12\t\n\x05PILOT\x10\x02*2\n\nTypeObject\x12\x07\n\x03\x41NY\x10\x00\x12\x07\n\x03\x43\x41R\x10\x01\x12\x08\n\x04\x42UMP\x10\x02\x12\x08\n\x04PLOT\x10\x03\x42\x08Z\x06\x65ventsb\x06proto3' + , + dependencies=[google_dot_protobuf_dot_timestamp__pb2.DESCRIPTOR,]) + +_DRIVEMODE = _descriptor.EnumDescriptor( + name='DriveMode', + full_name='robocar.events.DriveMode', + filename=None, + file=DESCRIPTOR, + create_key=_descriptor._internal_create_key, + values=[ + _descriptor.EnumValueDescriptor( + name='INVALID', index=0, number=0, + serialized_options=None, + type=None, + create_key=_descriptor._internal_create_key), + _descriptor.EnumValueDescriptor( + name='USER', index=1, number=1, + serialized_options=None, + type=None, + create_key=_descriptor._internal_create_key), + _descriptor.EnumValueDescriptor( + name='PILOT', index=2, number=2, + serialized_options=None, + type=None, + create_key=_descriptor._internal_create_key), + ], + containing_type=None, + serialized_options=None, + serialized_start=1196, + serialized_end=1241, +) +_sym_db.RegisterEnumDescriptor(_DRIVEMODE) + +DriveMode = enum_type_wrapper.EnumTypeWrapper(_DRIVEMODE) +_TYPEOBJECT = _descriptor.EnumDescriptor( + name='TypeObject', + full_name='robocar.events.TypeObject', + filename=None, + file=DESCRIPTOR, + create_key=_descriptor._internal_create_key, + values=[ + _descriptor.EnumValueDescriptor( + name='ANY', index=0, number=0, + serialized_options=None, + type=None, + create_key=_descriptor._internal_create_key), + _descriptor.EnumValueDescriptor( + name='CAR', index=1, number=1, + serialized_options=None, + type=None, + create_key=_descriptor._internal_create_key), + _descriptor.EnumValueDescriptor( + name='BUMP', index=2, number=2, + serialized_options=None, + type=None, + create_key=_descriptor._internal_create_key), + _descriptor.EnumValueDescriptor( + name='PLOT', index=3, number=3, + serialized_options=None, + type=None, + create_key=_descriptor._internal_create_key), + ], + containing_type=None, + serialized_options=None, + serialized_start=1243, + serialized_end=1293, +) +_sym_db.RegisterEnumDescriptor(_TYPEOBJECT) + +TypeObject = enum_type_wrapper.EnumTypeWrapper(_TYPEOBJECT) +INVALID = 0 +USER = 1 +PILOT = 2 +ANY = 0 +CAR = 1 +BUMP = 2 +PLOT = 3 + + + +_FRAMEREF = _descriptor.Descriptor( + name='FrameRef', + full_name='robocar.events.FrameRef', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='name', full_name='robocar.events.FrameRef.name', index=0, + number=1, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=b"".decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='id', full_name='robocar.events.FrameRef.id', index=1, + number=2, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=b"".decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='created_at', full_name='robocar.events.FrameRef.created_at', index=2, + number=3, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=72, + serialized_end=156, +) + + +_FRAMEMESSAGE = _descriptor.Descriptor( + name='FrameMessage', + full_name='robocar.events.FrameMessage', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='id', full_name='robocar.events.FrameMessage.id', index=0, + number=1, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='frame', full_name='robocar.events.FrameMessage.frame', index=1, + number=2, type=12, cpp_type=9, label=1, + has_default_value=False, default_value=b"", + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=158, + serialized_end=225, +) + + +_STEERINGMESSAGE = _descriptor.Descriptor( + name='SteeringMessage', + full_name='robocar.events.SteeringMessage', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='steering', full_name='robocar.events.SteeringMessage.steering', index=0, + number=1, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='confidence', full_name='robocar.events.SteeringMessage.confidence', index=1, + number=2, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='frame_ref', full_name='robocar.events.SteeringMessage.frame_ref', index=2, + number=3, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=227, + serialized_end=327, +) + + +_THROTTLEMESSAGE = _descriptor.Descriptor( + name='ThrottleMessage', + full_name='robocar.events.ThrottleMessage', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='throttle', full_name='robocar.events.ThrottleMessage.throttle', index=0, + number=1, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='confidence', full_name='robocar.events.ThrottleMessage.confidence', index=1, + number=2, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='frame_ref', full_name='robocar.events.ThrottleMessage.frame_ref', index=2, + number=3, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=329, + serialized_end=429, +) + + +_DRIVEMODEMESSAGE = _descriptor.Descriptor( + name='DriveModeMessage', + full_name='robocar.events.DriveModeMessage', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='drive_mode', full_name='robocar.events.DriveModeMessage.drive_mode', index=0, + number=1, type=14, cpp_type=8, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=431, + serialized_end=496, +) + + +_OBJECTSMESSAGE = _descriptor.Descriptor( + name='ObjectsMessage', + full_name='robocar.events.ObjectsMessage', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='objects', full_name='robocar.events.ObjectsMessage.objects', index=0, + number=1, type=11, cpp_type=10, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='frame_ref', full_name='robocar.events.ObjectsMessage.frame_ref', index=1, + number=2, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=498, + serialized_end=600, +) + + +_OBJECT = _descriptor.Descriptor( + name='Object', + full_name='robocar.events.Object', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='type', full_name='robocar.events.Object.type', index=0, + number=1, type=14, cpp_type=8, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='left', full_name='robocar.events.Object.left', index=1, + number=2, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='top', full_name='robocar.events.Object.top', index=2, + number=3, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='right', full_name='robocar.events.Object.right', index=3, + number=4, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='bottom', full_name='robocar.events.Object.bottom', index=4, + number=5, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='confidence', full_name='robocar.events.Object.confidence', index=5, + number=6, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=603, + serialized_end=731, +) + + +_SWITCHRECORDMESSAGE = _descriptor.Descriptor( + name='SwitchRecordMessage', + full_name='robocar.events.SwitchRecordMessage', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='enabled', full_name='robocar.events.SwitchRecordMessage.enabled', index=0, + number=1, type=8, cpp_type=7, label=1, + has_default_value=False, default_value=False, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=733, + serialized_end=771, +) + + +_ROADMESSAGE = _descriptor.Descriptor( + name='RoadMessage', + full_name='robocar.events.RoadMessage', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='contour', full_name='robocar.events.RoadMessage.contour', index=0, + number=1, type=11, cpp_type=10, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='ellipse', full_name='robocar.events.RoadMessage.ellipse', index=1, + number=2, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='frame_ref', full_name='robocar.events.RoadMessage.frame_ref', index=2, + number=3, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=774, + serialized_end=914, +) + + +_POINT = _descriptor.Descriptor( + name='Point', + full_name='robocar.events.Point', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='x', full_name='robocar.events.Point.x', index=0, + number=1, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='y', full_name='robocar.events.Point.y', index=1, + number=2, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=916, + serialized_end=945, +) + + +_ELLIPSE = _descriptor.Descriptor( + name='Ellipse', + full_name='robocar.events.Ellipse', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='center', full_name='robocar.events.Ellipse.center', index=0, + number=1, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='width', full_name='robocar.events.Ellipse.width', index=1, + number=2, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='height', full_name='robocar.events.Ellipse.height', index=2, + number=3, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='angle', full_name='robocar.events.Ellipse.angle', index=3, + number=4, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='confidence', full_name='robocar.events.Ellipse.confidence', index=4, + number=5, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=947, + serialized_end=1061, +) + + +_RECORDMESSAGE = _descriptor.Descriptor( + name='RecordMessage', + full_name='robocar.events.RecordMessage', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='frame', full_name='robocar.events.RecordMessage.frame', index=0, + number=1, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='steering', full_name='robocar.events.RecordMessage.steering', index=1, + number=2, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='recordSet', full_name='robocar.events.RecordMessage.recordSet', index=2, + number=3, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=b"".decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=1064, + serialized_end=1194, +) + +_FRAMEREF.fields_by_name['created_at'].message_type = google_dot_protobuf_dot_timestamp__pb2._TIMESTAMP +_FRAMEMESSAGE.fields_by_name['id'].message_type = _FRAMEREF +_STEERINGMESSAGE.fields_by_name['frame_ref'].message_type = _FRAMEREF +_THROTTLEMESSAGE.fields_by_name['frame_ref'].message_type = _FRAMEREF +_DRIVEMODEMESSAGE.fields_by_name['drive_mode'].enum_type = _DRIVEMODE +_OBJECTSMESSAGE.fields_by_name['objects'].message_type = _OBJECT +_OBJECTSMESSAGE.fields_by_name['frame_ref'].message_type = _FRAMEREF +_OBJECT.fields_by_name['type'].enum_type = _TYPEOBJECT +_ROADMESSAGE.fields_by_name['contour'].message_type = _POINT +_ROADMESSAGE.fields_by_name['ellipse'].message_type = _ELLIPSE +_ROADMESSAGE.fields_by_name['frame_ref'].message_type = _FRAMEREF +_ELLIPSE.fields_by_name['center'].message_type = _POINT +_RECORDMESSAGE.fields_by_name['frame'].message_type = _FRAMEMESSAGE +_RECORDMESSAGE.fields_by_name['steering'].message_type = _STEERINGMESSAGE +DESCRIPTOR.message_types_by_name['FrameRef'] = _FRAMEREF +DESCRIPTOR.message_types_by_name['FrameMessage'] = _FRAMEMESSAGE +DESCRIPTOR.message_types_by_name['SteeringMessage'] = _STEERINGMESSAGE +DESCRIPTOR.message_types_by_name['ThrottleMessage'] = _THROTTLEMESSAGE +DESCRIPTOR.message_types_by_name['DriveModeMessage'] = _DRIVEMODEMESSAGE +DESCRIPTOR.message_types_by_name['ObjectsMessage'] = _OBJECTSMESSAGE +DESCRIPTOR.message_types_by_name['Object'] = _OBJECT +DESCRIPTOR.message_types_by_name['SwitchRecordMessage'] = _SWITCHRECORDMESSAGE +DESCRIPTOR.message_types_by_name['RoadMessage'] = _ROADMESSAGE +DESCRIPTOR.message_types_by_name['Point'] = _POINT +DESCRIPTOR.message_types_by_name['Ellipse'] = _ELLIPSE +DESCRIPTOR.message_types_by_name['RecordMessage'] = _RECORDMESSAGE +DESCRIPTOR.enum_types_by_name['DriveMode'] = _DRIVEMODE +DESCRIPTOR.enum_types_by_name['TypeObject'] = _TYPEOBJECT +_sym_db.RegisterFileDescriptor(DESCRIPTOR) + +FrameRef = _reflection.GeneratedProtocolMessageType('FrameRef', (_message.Message,), { + 'DESCRIPTOR' : _FRAMEREF, + '__module__' : 'events.events_pb2' + # @@protoc_insertion_point(class_scope:robocar.events.FrameRef) + }) +_sym_db.RegisterMessage(FrameRef) + +FrameMessage = _reflection.GeneratedProtocolMessageType('FrameMessage', (_message.Message,), { + 'DESCRIPTOR' : _FRAMEMESSAGE, + '__module__' : 'events.events_pb2' + # @@protoc_insertion_point(class_scope:robocar.events.FrameMessage) + }) +_sym_db.RegisterMessage(FrameMessage) + +SteeringMessage = _reflection.GeneratedProtocolMessageType('SteeringMessage', (_message.Message,), { + 'DESCRIPTOR' : _STEERINGMESSAGE, + '__module__' : 'events.events_pb2' + # @@protoc_insertion_point(class_scope:robocar.events.SteeringMessage) + }) +_sym_db.RegisterMessage(SteeringMessage) + +ThrottleMessage = _reflection.GeneratedProtocolMessageType('ThrottleMessage', (_message.Message,), { + 'DESCRIPTOR' : _THROTTLEMESSAGE, + '__module__' : 'events.events_pb2' + # @@protoc_insertion_point(class_scope:robocar.events.ThrottleMessage) + }) +_sym_db.RegisterMessage(ThrottleMessage) + +DriveModeMessage = _reflection.GeneratedProtocolMessageType('DriveModeMessage', (_message.Message,), { + 'DESCRIPTOR' : _DRIVEMODEMESSAGE, + '__module__' : 'events.events_pb2' + # @@protoc_insertion_point(class_scope:robocar.events.DriveModeMessage) + }) +_sym_db.RegisterMessage(DriveModeMessage) + +ObjectsMessage = _reflection.GeneratedProtocolMessageType('ObjectsMessage', (_message.Message,), { + 'DESCRIPTOR' : _OBJECTSMESSAGE, + '__module__' : 'events.events_pb2' + # @@protoc_insertion_point(class_scope:robocar.events.ObjectsMessage) + }) +_sym_db.RegisterMessage(ObjectsMessage) + +Object = _reflection.GeneratedProtocolMessageType('Object', (_message.Message,), { + 'DESCRIPTOR' : _OBJECT, + '__module__' : 'events.events_pb2' + # @@protoc_insertion_point(class_scope:robocar.events.Object) + }) +_sym_db.RegisterMessage(Object) + +SwitchRecordMessage = _reflection.GeneratedProtocolMessageType('SwitchRecordMessage', (_message.Message,), { + 'DESCRIPTOR' : _SWITCHRECORDMESSAGE, + '__module__' : 'events.events_pb2' + # @@protoc_insertion_point(class_scope:robocar.events.SwitchRecordMessage) + }) +_sym_db.RegisterMessage(SwitchRecordMessage) + +RoadMessage = _reflection.GeneratedProtocolMessageType('RoadMessage', (_message.Message,), { + 'DESCRIPTOR' : _ROADMESSAGE, + '__module__' : 'events.events_pb2' + # @@protoc_insertion_point(class_scope:robocar.events.RoadMessage) + }) +_sym_db.RegisterMessage(RoadMessage) + +Point = _reflection.GeneratedProtocolMessageType('Point', (_message.Message,), { + 'DESCRIPTOR' : _POINT, + '__module__' : 'events.events_pb2' + # @@protoc_insertion_point(class_scope:robocar.events.Point) + }) +_sym_db.RegisterMessage(Point) + +Ellipse = _reflection.GeneratedProtocolMessageType('Ellipse', (_message.Message,), { + 'DESCRIPTOR' : _ELLIPSE, + '__module__' : 'events.events_pb2' + # @@protoc_insertion_point(class_scope:robocar.events.Ellipse) + }) +_sym_db.RegisterMessage(Ellipse) + +RecordMessage = _reflection.GeneratedProtocolMessageType('RecordMessage', (_message.Message,), { + 'DESCRIPTOR' : _RECORDMESSAGE, + '__module__' : 'events.events_pb2' + # @@protoc_insertion_point(class_scope:robocar.events.RecordMessage) + }) +_sym_db.RegisterMessage(RecordMessage) + + +DESCRIPTOR._options = None +# @@protoc_insertion_point(module_scope) diff --git a/pca9685/__init__.py b/pca9685/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pca9685/cli.py b/pca9685/cli.py new file mode 100644 index 0000000..6277636 --- /dev/null +++ b/pca9685/cli.py @@ -0,0 +1,138 @@ +""" +Compute steering from camera images + +Usage: +rc-pca9685 [steering|throttle] [-u USERNAME | --mqtt-username=USERNAME] [--mqtt-password=PASSWORD] \ + [--mqtt-broker=HOSTNAME] [--mqtt-topic=TOPIC] [--mqtt-client-id=CLIENT_ID] [--i2c-bus=BUS] \ + [--i2c-address=ADDRESS] [--pca9685-channel=CHANNEL] [--left-pulse=LEFT_PULSE] [--right-pulse=RIGHT_PULSE] \ + [--max-pulse=MAX_PULSE] [--min-pulse=MIN_PULSE] [--zero-pulse=ZEO_PULSE] + +Options: +-h --help Show this screen. +-u USERID --mqtt-username=USERNAME MQTT user +-p PASSWORD --mqtt-password=PASSWORD MQTT password +-b HOSTNAME --mqtt-broker=HOSTNAME MQTT broker host +-C CLIENT_ID --mqtt-client-id=CLIENT_ID MQTT client id +-t TOPIC --mqtt-topic-camera=TOPIC MQTT topic where to read camera frames +-b BUS --i2c-bus=BUS I2C bus number +-a ADDRESS --i2c-address=ADDRESS I2C base address +-c CHANNEL --pca9685-channel=CHANNEL Device channel +-l LEFT_PULSE --left-pulse=LEFT_PULSE Pulse for left steering +-r RIGHT_PULSE --right-pulse=RIGHT_PULSE Pulse for right steering +-M MAX_PULSE --max-pulse=MAX_PULSE Max Pulse for throttle +-m MIN_PULSE --min-pulse=MIN_PULSE Min Pulse for throttle +-z ZERO_PULE --zero-pulse=ZEO_PULSE Zero Pulse for throttle +""" +import logging +import os + +from docopt import docopt +import paho.mqtt.client as mqtt + +from donkeycar.parts import actuator +import donkeycar + +from events.events_pb2 import SteeringMessage +from events.events_pb2 import ThrottleMessage + +logger = logging.getLogger(__name__) +logging.basicConfig(level=logging.INFO) + +default_client_id = "robocar-pca9685-python" + + +def init_mqtt_steering_client(steering_controller: actuator.PWMSteering, broker_host: str, user: str, password: str, client_id: str, + steering_topic: str) -> mqtt.Client: + logger.info("Start steering part") + client = mqtt.Client(client_id=client_id, clean_session=True, userdata=None, protocol=mqtt.MQTTv311) + + def on_connect(client, userdata, flags, rc): + logger.info("Register callback on topic %s", steering_topic) + # Subscribing in on_connect() means that if we lose the connection and + # reconnect then subscriptions will be renewed. + client.subscribe(steering_topic) + + def on_steering_message(mqtt_client: mqtt.Client, userdata, msg: mqtt.MQTTMessage): + try: + steering_msg = SteeringMessage() + steering_msg.ParseFromString(msg.payload) + steering_controller.run_threaded(steering_msg.Steering) + except: + logger.debug("unexpected error: unable to process steering, skip message") + + client.username_pw_set(user, password) + client.on_connect = on_connect + client.on_message = on_steering_message + logger.info("Connect to mqtt broker") + client.connect(host=broker_host, port=1883, keepalive=60) + logger.info("Connected to mqtt broker") + return client + + +def init_mqtt_throttle_client(throttle_controller: actuator.PWMThrottle, broker_host: str, user: str, password: str, + client_id: str, throttle_topic: str) -> mqtt.Client: + logger.info("Start throttle part") + client = mqtt.Client(client_id=client_id, clean_session=True, userdata=None, protocol=mqtt.MQTTv311) + + def on_connect(client, userdata, flags, rc): + logger.info("Register callback on topic %s", throttle_topic) + # Subscribing in on_connect() means that if we lose the connection and + # reconnect then subscriptions will be renewed. + client.subscribe(throttle_topic) + + def on_throttle_message(mqtt_client: mqtt.Client, userdata, msg: mqtt.MQTTMessage): + try: + throttle_msg = ThrottleMessage() + throttle_msg.ParseFromString(msg.payload) + throttle_controller.run_threaded(throttle_msg.Throttle) + except: + logger.debug("unexpected error: unable to process throttle, skip message") + + client.username_pw_set(user, password) + client.on_connect = on_connect + client.on_message = on_throttle_message + logger.info("Connect to mqtt broker") + client.connect(host=broker_host, port=1883, keepalive=60) + logger.info("Connected to mqtt broker") + return client + + +def execute_from_command_line(): + logging.basicConfig(level=logging.INFO) + + args = docopt(__doc__) + + broker_host = get_default_value(args["--mqtt-broker"], "MQTT_BROKER", "localhost") + user = get_default_value(args["--mqtt-username"], "MQTT_USERNAME", "") + password = get_default_value(args["--mqtt-password"], "MQTT_PASSWORD", "") + client_id = get_default_value(args["--mqtt-client-id"], "MQTT_CLIENT_ID", default_client_id) + topic = get_default_value(args["--mqtt-topic"], "MQTT_TOPIC", "/pca9685") + i2c_bus = get_default_value(args["--i2c-bus"], "I2C_BUS", 0) + i2c_address = get_default_value(args["--i2c-address"], "I2C_ADDRESS", 40) + pca9685_channel = get_default_value(args["--i2c-address"], "PCA9685_CHANNEL", 0) + + pin_id = "PCA9685.{bus}:{address}.{channel}".format(bus=i2c_bus, address=i2c_address,channel=pca9685_channel) + + if args["steering"]: + steering_pin = donkeycar.parts.pins.pwm_pin_by_id(pin_id) + steering = actuator.PWMSteering(steering_pin, left_pulse=args["--left-pulse"], right_pulse=args["--right-pulse"]) + client = init_mqtt_steering_client(steering_controller=steering, broker_host=broker_host, user=user, + password=password, client_id=client_id, steering_topic=topic) + elif args["throttle"]: + throttle_pin = donkeycar.parts.pins.pwm_pin_by_id("PCA9685.0:40.7") + throttle = actuator.PWMThrottle(throttle_pin, max_pulse=args["--max-pulse"], min_pulse=args["--min-pulse"], + zero_pulse=args["--zero-pulse"]) + client = init_mqtt_throttle_client(throttle_controller=throttle, broker_host=broker_host, user=user, + password=password, client_id=client_id, throttle_topic=topic) + else: + raise Exception('invalid mode selected, must be steering or throttle') + + client.loop_forever() + + +def get_default_value(value, env_var: str, default_value) -> str: + if value: + return value + if env_var in os.environ: + return os.environ[env_var] + return default_value diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..d850378 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,6 @@ +paho-mqtt~=1.6.1 +docopt~=0.6.2 +google~=3.0.0 +google-api-core~=2.4.0 +setuptools==60.5.0 +adafruit-pca9685 \ No newline at end of file diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..6bcaeb8 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,5 @@ +[metadata] +description_file = README.md + +[aliases] +test = pytest diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..1d9b439 --- /dev/null +++ b/setup.py @@ -0,0 +1,65 @@ +import os + +from setuptools import setup, find_packages + + +# include the non python files +def package_files(directory, strip_leading): + paths = [] + for (path, directories, filenames) in os.walk(directory): + for filename in filenames: + package_file = os.path.join(path, filename) + paths.append(package_file[len(strip_leading):]) + return paths + + +tests_require = ['pytest', + ] + +setup(name='robocar-pca9685-python', + version='0.1', + description='Mqtt gateway for PCA9685 controller.', + url='https://github.com/cyrilix/robocar-pca9685-python', + license='Apache2', + entry_points={ + 'console_scripts': [ + 'rc-pca9685=pca9685.cli:execute_from_command_line', + ], + }, + setup_requires=['pytest-runner'], + install_requires=['docopt', + 'paho-mqtt', + 'protobuf3', + 'google', + 'adafruit-pca9685', + ], + tests_require=tests_require, + extras_require={ + 'tests': tests_require + }, + + include_package_data=True, + + classifiers=[ + # How mature is this project? Common values are + # 3 - Alpha + # 4 - Beta + # 5 - Production/Stable + 'Development Status :: 3 - Alpha', + + # Indicate who your project is intended for + 'Intended Audience :: Developers', + 'Topic :: Scientific/Engineering :: Artificial Intelligence', + + # Pick your license as you wish (should match "license" above) + 'License :: OSI Approved :: Apache 2 License', + + # Specify the Python versions you support here. In particular, ensure + # that you indicate whether you support Python 2, Python 3 or both. + + 'Programming Language :: Python :: 3.7', + ], + keywords='selfdriving cars drive', + + packages=find_packages(exclude=(['tests', 'env'])), + )