refactor pose estimation into a single part

- UnicyclePose handles encoder/tachometer/odometer/kinematics
  for differential drive.
- BicyclePose handles encoder/tachometer/odomter/kinematics
  for car-like vehicle
- add a mock encoder that is driven by throttle and steering
  input.  This allows use to simulate the vehicle without
  a full-on simulator.
- Fix the drawing of the path so that it handles the
  fact that Y increases going north.

donkeycar/parts/kinematics.py

@@ -137,7 +137,7 @@ def run(self, forward_distance:float, steering_angle:float, timestamp:float=None
                 logger.info(result)
             return result
 
-        return (0, 0, 0, 0, 0, 0, 0, 0, self.timestamp)
+        return 0, 0, 0, 0, 0, 0, 0, 0, self.timestamp
 
     def shutdown(self):
         self.running = False
@@ -182,7 +182,7 @@ def run(self, forward_velocity:float, angular_velocity:float, timestamp:float=No
         steering_angle = bicycle_steering_angle(self.wheel_base, forward_velocity, angular_velocity)        
         self.timestamp = timestamp
 
-        return (forward_velocity, steering_angle, timestamp)
+        return forward_velocity, steering_angle, timestamp
 
 
 def bicycle_steering_angle(wheel_base:float, forward_velocity:float, angular_velocity:float) -> float:
@@ -235,7 +235,7 @@ def __init__(self, wheel_base:float, max_forward_velocity:float, max_steering_an
 
     def run(self, normalized_angular_velocity:float) -> float:
         if abs(normalized_angular_velocity) > 1:
-            print("Warning: normalized_angular_velocity must be between -1 and 1")
+            logger.error("Warning: normalized_angular_velocity must be between -1 and 1")
         return normalized_angular_velocity * self.max_angular_velocity
 
 
@@ -345,8 +345,7 @@ def run(self, left_distance:float, right_distance:float, timestamp:float=None) -
                 self.timestamp
             )
 
-
-        return (0, 0, 0, 0, 0, 0, 0, 0, self.timestamp)
+        return 0, 0, 0, 0, 0, 0, 0, 0, self.timestamp
 
     def shutdown(self):
         self.running = False
@@ -369,6 +368,7 @@ def __init__(self, axle_length:float, wheel_radius:float, min_speed:float, max_s
 
         self.wheel_diameter = 2 * wheel_radius
         self.wheel_circumference = math.pi * self.wheel_diameter
+
     def run(self, forward_velocity:float, angular_velocity:float, timestamp:float=None) -> Tuple[float, float, float]:
         """
         Convert turning velocity in radians and forward velocity (like meters per second)
@@ -393,7 +393,7 @@ def run(self, forward_velocity:float, angular_velocity:float, timestamp:float=No
         self.timestamp = timestamp
 
         # left/right linear speeds and timestamp
-        return (left_linear_speed, right_linear_speed, timestamp)
+        return left_linear_speed, right_linear_speed, timestamp
 
     def shutdown(self):
         pass
@@ -453,7 +453,7 @@ def __init__(self, wheel_radius:float, axle_length:float, max_forward_velocity:f
 
     def run(self, normalized_angular_velocity:float) -> float:
         if abs(normalized_angular_velocity) > 1:
-            print("Warning: normalized_angular_velocity must be between -1 and 1")
+            logger.error("Warning: normalized_angular_velocity must be between -1 and 1")
         return normalized_angular_velocity * self.max_angular_velocity
 
 
@@ -489,7 +489,7 @@ def run(self, steering_angle) -> float:
             return 0
         return -steering # positive steering angle is negative normalized
 
-    def shutdown():
+    def shutdown(self):
         pass
 
 
@@ -532,7 +532,7 @@ def run(self, steering) -> float:
 
         return self.max_steering_angle * steering * -s
 
-    def shutdown():
+    def shutdown(self):
         pass
 
 
@@ -549,7 +549,7 @@ def wheel_rotational_velocity(wheel_radius:float, speed:float) -> float:
     return speed / wheel_radius
 
 
-def differential_steering(throttle: float, steering: float, steering_zero: float = 0.01) -> Tuple[float, float]:
+def differential_steering(throttle: float, steering: float, steering_zero: float = 0) -> Tuple[float, float]:
         """
         Turn steering angle and speed/throttle into 
         left and right wheel speeds/throttle.

donkeycar/parts/path.py

@@ -244,10 +244,14 @@ def run(self, img, path):
             for iP in range(0, len(path) - 1):
                 ax, ay = path[iP]
                 bx, by = path[iP + 1]
+
+                #
+                # y increases going north, so handle this with scale
+                #
                 self.plot_line(ax * self.scale + self.offset[0],
-                            ay * self.scale + self.offset[1],
+                            ay * -self.scale + self.offset[1],
                             bx * self.scale + self.offset[0],
-                            by * self.scale + self.offset[1],
+                            by * -self.scale + self.offset[1],
                             draw,
                             color)
         return img
@@ -279,7 +283,7 @@ def plot_circle(self, x, y, rad, draw, color, width=1):
     def run(self, img, x, y):
         draw = ImageDraw.Draw(img)
         self.plot_circle(x * self.scale + self.offset[0],
-                        y * self.scale + self.offset[1], 
+                        y * -self.scale + self.offset[1],  # y increases going north
                         self.radius,
                         draw, 
                         self.color)

donkeycar/parts/pose.py

@@ -0,0 +1,232 @@
+import logging
+import time
+from typing import Tuple
+
+from donkeycar.utils import is_number_type
+from donkeycar.parts.kinematics import NormalizeSteeringAngle, UnnormalizeSteeringAngle, TwoWheelSteeringThrottle
+from donkeycar.parts.kinematics import Unicycle, InverseUnicycle, UnicycleUnnormalizeAngularVelocity
+from donkeycar.parts.kinematics import Bicycle, InverseBicycle, BicycleUnnormalizeAngularVelocity
+from donkeycar.parts.kinematics import differential_steering
+from donkeycar.parts.tachometer import Tachometer, MockEncoder
+
+logger = logging.getLogger(__name__)
+logging.basicConfig(level=logging.INFO)
+
+class UnicycleDistance:
+    def run(self, left, right):
+        if is_number_type(left) and is_number_type(right):
+            return (left + right) / 2.0
+        else:
+            logger.error("left and right must be floats")
+        return 0.0
+
+
+class BicyclePose:
+    """
+    Part that integrates encoder+tachometer+odometer+kinematics
+    for the purposes of estimating the pose of a car-like vehicle over time.
+    """
+    def __init__(self, cfg, poll_delay_secs:float=0):
+        from donkeycar.parts.serial_port import SerialPort
+        from donkeycar.parts.tachometer import (Tachometer, SerialEncoder,
+                                                GpioEncoder, EncoderChannel,
+                                                MockEncoder)
+        from donkeycar.parts.odometer import Odometer
+
+        # distance_per_revolution = cfg.ENCODER_PPR * cfg.MM_PER_TICK / 1000
+        distance_per_revolution = cfg.WHEEL_RADIUS * 2 * 3.141592653589793
+
+        self.poll_delay_secs = poll_delay_secs
+        self.encoder = None
+        self.tachometer = None
+        self.odometer = None
+        self.steerer = None
+        self.bicycle = None
+        self.Running = False
+
+        self.inputs = (0, 0)  # (throttle, steering)
+        self.reading = (0, 0, 0, 0, 0, 0, 0, 0, None)
+
+        if cfg.ENCODER_TYPE == "GPIO":
+            from donkeycar.parts import pins
+            self.encoder = GpioEncoder(gpio_pin=pins.input_pin_by_id(cfg.ODOM_PIN),
+                        debounce_ns=cfg.ENCODER_DEBOUNCE_NS,
+                        debug=cfg.ODOM_DEBUG)
+        elif cfg.ENCODER_TYPE == "arduino":
+            self.encoder = SerialEncoder(serial_port=SerialPort(cfg.ODOM_SERIAL, cfg.ODOM_SERIAL_BAUDRATE), debug=cfg.ODOM_DEBUG)
+        elif cfg.ENCODER_TYPE.upper() == "MOCK":
+            self.encoder = MockEncoder(cfg.ENCODER_PPR * 10)  # max 10 revolutions per second
+        else:
+            print("No supported encoder found")
+
+        if self.encoder:
+            self.tachometer = Tachometer(
+                self.encoder,
+                ticks_per_revolution=cfg.ENCODER_PPR,
+                direction_mode=cfg.TACHOMETER_MODE,
+                poll_delay_secs=1.0 / (cfg.DRIVE_LOOP_HZ * 3),
+                debug=cfg.ODOM_DEBUG)
+
+        if self.tachometer:
+            self.odometer = Odometer(
+                distance_per_revolution=distance_per_revolution,
+                smoothing_count=cfg.ODOM_SMOOTHING,
+                debug=cfg.ODOM_DEBUG)
+
+            self.steerer = UnnormalizeSteeringAngle(cfg.MAX_STEERING_ANGLE)
+            self.bicycle = Bicycle(cfg.WHEEL_BASE, cfg.ODOM_DEBUG)
+            self.running = True
+        else:
+            logger.error("Unable to initialize BicyclePose part")
+
+    def update(self):
+        """
+        This will get called on it's own thread.
+        throttle: sign of throttle is use used to determine direction.
+        timestamp: timestamp for update or None to use current time.
+                   This is useful for creating deterministic tests.
+        """
+        while self.running:
+            if self.tachometer:
+                timestamp = None
+                throttle, steering = self.inputs
+                if isinstance(self.encoder, MockEncoder):
+                    self.encoder.run(throttle, timestamp)
+                revolutions, timestamp = self.tachometer.run(throttle, timestamp)
+                distance, velocity, timestamp = self.odometer.run(revolutions, timestamp)
+                steering_angle = self.steerer.run(steering)
+                self.reading = self.bicycle.run(distance, steering_angle, timestamp)
+
+            time.sleep(self.poll_delay_secs)  # give other threads time
+
+    def run_threaded(self, throttle:float=0.0, steering:float=0.0, timestamp:float=None) -> Tuple[float, float, float, float, float, float, float, float, float]:
+        if self.running and self.tachometer:
+            if throttle is None:
+                throttle = 0
+            if steering is None:
+                steering = 0
+
+            self.inputs = (throttle, steering)
+
+            return self.reading
+
+        return 0, 0, 0, 0, 0, 0, 0, 0, timestamp
+
+
+class UnicyclePose:
+    """
+    Part that integrates encoders+tachometers+odometers+kinematics
+    for the purposes of estimating the pose of a
+    differential drive vehicle over time.
+    """
+    def __init__(self, cfg, poll_delay_secs:float=0):
+        from donkeycar.parts.serial_port import SerialPort
+        from donkeycar.parts.tachometer import (SerialEncoder,
+                                                GpioEncoder, EncoderChannel)
+        from donkeycar.parts.odometer import Odometer
+
+        # distance_per_revolution = cfg.ENCODER_PPR * cfg.MM_PER_TICK / 1000
+        distance_per_revolution = cfg.WHEEL_RADIUS * 2 * 3.141592653589793
+
+        self.poll_delay_secs = poll_delay_secs
+        self.left_throttle = 0
+        self.right_throttle = 0
+        self.encoder = None
+        self.tachometer = None
+        self.odometer = None
+        self.unicycle = None
+
+        self.inputs = (0, 0)  # (left_throttle, right_throttle)
+        self.reading = (0, 0, 0, 0, 0, 0, 0, 0, None)
+
+        if cfg.ENCODER_TYPE == "GPIO":
+            from donkeycar.parts import pins
+            self.encoder = [
+                GpioEncoder(gpio_pin=pins.input_pin_by_id(cfg.ODOM_PIN),
+                        debounce_ns=cfg.ENCODER_DEBOUNCE_NS,
+                        debug=cfg.ODOM_DEBUG),
+                GpioEncoder(gpio_pin=pins.input_pin_by_id(cfg.ODOM_PIN_2),
+                            debounce_ns=cfg.ENCODER_DEBOUNCE_NS,
+                            debug=cfg.ODOM_DEBUG)
+            ]
+        elif cfg.ENCODER_TYPE == "arduino":
+            serial_encoder = SerialEncoder(serial_port=SerialPort(cfg.ODOM_SERIAL, cfg.ODOM_SERIAL_BAUDRATE), debug=cfg.ODOM_DEBUG)
+            self.encoder = [
+                serial_encoder,
+                EncoderChannel(encoder=serial_encoder, channel=1)
+            ]
+        elif cfg.ENCODER_TYPE.upper() == "MOCK":
+            self.encoder = [
+                MockEncoder(cfg.ENCODER_PPR * 10),
+                MockEncoder(cfg.ENCODER_PPR * 10)
+            ]
+        else:
+            print("No supported encoder found")
+
+        if self.encoder:
+            self.tachometer = [
+                Tachometer(
+                    self.encoder[0],
+                    ticks_per_revolution=cfg.ENCODER_PPR,
+                    direction_mode=cfg.TACHOMETER_MODE,
+                    poll_delay_secs=0,
+                    debug=cfg.ODOM_DEBUG),
+                Tachometer(
+                    self.encoder[1],
+                    ticks_per_revolution=cfg.ENCODER_PPR,
+                    direction_mode=cfg.TACHOMETER_MODE,
+                    poll_delay_secs=0,
+                    debug=cfg.ODOM_DEBUG)
+            ]
+
+        if self.tachometer:
+            self.odometer = [
+                Odometer(
+                    distance_per_revolution=distance_per_revolution,
+                    smoothing_count=cfg.ODOM_SMOOTHING,
+                    debug=cfg.ODOM_DEBUG),
+                Odometer(
+                    distance_per_revolution=distance_per_revolution,
+                    smoothing_count=cfg.ODOM_SMOOTHING,
+                    debug=cfg.ODOM_DEBUG)
+            ]
+            self.unicycle = Unicycle(cfg.AXLE_LENGTH, cfg.ODOM_DEBUG)
+        self.running = True
+
+    def update(self):
+        """
+        This will get called on it's own thread.
+        throttle: sign of throttle is use used to determine direction.
+        timestamp: timestamp for update or None to use current time.
+                   This is useful for creating deterministic tests.
+        """
+        while self.running:
+            if self.tachometer:
+                left_timestamp = None
+                right_timestamp = None
+                left_throttle, right_throttle = self.inputs
+                if isinstance(self.encoder[0], MockEncoder):
+                    self.encoder[0].run(left_throttle, left_timestamp)
+                    self.encoder[1].run(right_throttle, right_timestamp)
+
+                left_revolutions, left_timestamp = self.tachometer[0].run(left_throttle, left_timestamp)
+                right_revolutions, right_timestamp = self.tachometer[1].run(right_throttle, right_timestamp)
+
+                left_distance, left_velocity, left_timestamp = self.odometer[0].run(left_revolutions, left_timestamp)
+                right_distance, right_velocity, right_timestamp = self.odometer[1].run(right_revolutions, right_timestamp)
+
+                self.reading = self.unicycle.run(left_distance, right_distance, right_timestamp)
+
+            time.sleep(self.poll_delay_secs)  # give other threads time
+
+    def run_threaded(self, throttle:float=0.0, steering:float=0.0, timestamp:float=None) -> Tuple[float, float, float, float, float, float, float, float, float]:
+        if self.running and self.tachometer:
+            if throttle is None:
+                throttle = 0
+            if steering is None:
+                steering = 0
+
+            self.inputs = differential_steering(throttle, steering)
+            return self.reading
+
+        return 0, 0, 0, 0, 0, 0, 0, 0, timestamp