Add tachometer, odometer and kinematics part

- tachometer takes an encoder and returns revolutions
- odometer coverts revolutions to distance and velocity
- kinematics converts odometry to pose estimates
- BicyclePose part implements the pose estimation pipeline for a
  car-like vehicle (fixed back wheels, turnable front wheels)
- UnicyclePose part implements the pose estimation pipeline
  for differential drive vehicles.

donkeycar/la.py

@@ -143,7 +143,8 @@ def normalize(self):
     def normalized(self):
         m = self.mag()
         v = Vec3(self.x, self.y, self.z)
-        v.scale(1.0 / m)
+        if m > 0:
+            v.scale(1.0 / m)
         return v
 
     def subtract(self, v):
@@ -619,4 +620,4 @@ def __init__(self, a, b):
     def vector_to(self, p):
         delta = self.origin - p
         dot = delta.dot(self.dir)
-        return self.dir.scaled(dot) - delta
+        return self.dir.scaled(dot) - delta

donkeycar/parts/actuator.py

@@ -293,6 +293,10 @@ def __init__(self, controller, left_pulse, right_pulse):
         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")
+        if not utils.is_number_type(left_pulse):
+            raise ValueError("left_pulse must be a number")
+        if not utils.is_number_type(right_pulse):
+            raise ValueError("right_pulse must be a number")
 
         self.controller = controller
         self.left_pulse = left_pulse
@@ -950,11 +954,12 @@ class RPi_GPIO_Servo(object):
     '''
     Servo controlled from the gpio pins on Rpi
     '''
-    def __init__(self, pin, pin_scheme=None, freq = 50, min=5.0, max=7.8):
+    def __init__(self, pin, pin_scheme=GPIO.BCM, freq=50, min=5.0, max=7.8):
         self.pin = pin
-        GPIO.setmode(GPIO.BCM if pin_scheme is None else pin_scheme)
+        GPIO.setmode(pin_scheme)
         GPIO.setup(self.pin, GPIO.OUT)
-
+
+        self.throttle = 0
         self.pwm = GPIO.PWM(self.pin, freq)
         self.pwm.start(0)
         self.min = min
@@ -965,12 +970,11 @@ def run(self, pulse):
         Update the speed of the motor where 1 is full forward and
         -1 is full backwards.
         '''
-        #I've read 90 is a good max
+        # I've read 90 is a good max
         self.throttle = dk.map_frange(pulse, -1.0, 1.0, self.min, self.max)
-        #logger.debug(pulse, self.throttle)
+        # logger.debug(pulse, self.throttle)
         self.pwm.ChangeDutyCycle(self.throttle)
 
-
     def shutdown(self):
         self.pwm.stop()
         GPIO.cleanup()

donkeycar/parts/encoder.py

@@ -1,8 +1,11 @@
 """
+Deprecated in favor on donkeycar.parts.Tachometer and donkeycar.parts.Odometer.
+
 Encoders and odometry
 """
 
 from datetime import datetime
+from donkeycar.utilities.deprecated import deprecated
 import re
 import time
 
@@ -18,6 +21,7 @@
 
 # This samples the odometer at 10HZ and does a moving average over the past ten readings to derive a velocity
 
+@deprecated("Deprecated in favor donkeycar.parts.tachometer.Tachometer(SerialEncoder)")
 class ArduinoEncoder(object):
     def __init__(self, mm_per_tick=0.0000599, debug=False):
         import serial
@@ -60,6 +64,8 @@ def shutdown(self):
         self.on = False
         time.sleep(.5)
 
+
+@deprecated("Deprecated as unused")
 class AStarSpeed:
     def __init__(self):
         from donkeycar.parts.teensy import TeensyRCin
@@ -118,6 +124,7 @@ def shutdown(self):
         time.sleep(.5)
 
 
+@deprecated("Deprecated in favor of donkeycar.parts.tachometer.Tachometer(GpioEncoder)")
 class RotaryEncoder():
     def __init__(self, mm_per_tick=0.306096, pin=13, poll_delay=0.0166, debug=False):
         import pigpio

donkeycar/parts/explode.py

@@ -1,9 +1,13 @@
 #
-# part that executes a no-arg method when a button is clicked
+# part that explodes a dictionary argument into individually named arguments
 #
 
 
 class ExplodeDict:
+    """
+    part that expands a dictionary input argument
+    into individually named output arguments
+    """
     def __init__(self, memory, output_prefix = ""):
         """
         Break a map into key/value pairs and write