921 kinematics pose estimation (#1089)

* 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.

* 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.

* comment out logging of pose to quiet the console

* add arduino encoder sketches

* remove unicode characters that prevent compilation of arduino sketch

* clean up comments a little

* Add more documentation to the quadrature_encoder.ino sketch

* Add interrupt mode to the mono encoder
- Interrupts mode can handle much higher tick rates, but does
  a poor job of debouncing.  It is appropriate for high resolution
  optical encoders.

* Remove spurious unicode beta character

It was at the end of `#define ENCODER_OPTIMIZE_INTERRUPTS` and so could have been causing the #define to be unrecognized.

* removed digital write high in mono encoder

* Fix syntax error if only using one channel in int mode
- there was a syntax error if only one pin was defined
  and it was being used in interrupt mode; that is fixed.
- Add more documentation to the mono_encoder.ino sketch

* Added a quadrature encoder sketch with no libraries
- The sketch counts ticks on a quadrature encoder without
  using 3rd party libraries.
- This is done 1) to make is simpler to compile and download
  the sketch; the user does not need to figure out what
  library to use. 2) the library that was in use only worked
  on AVR hardware and causes compilation errors on other hardware.
- if USE_ENCODER_INTERRUPTS is defined when the sketch is compiled,
  then the interrupt driven tick counting will be used.  This has
  no debounce logic, so it is not suitable for mechanical encoder,
  but is appropriate for optical or hall effect encoders.
- if USE_ENCODER_INTERRUPTS is NOT defined, then this used
  polling mode with debouncing, which is suitable for
  mechanical encoders, but may be to slow for high resolution
  optical or hall effect encoders.

* Fix bug in mono encoder sketch
- it had literal 2 for size of encoders array, so when there
  was only one encoder we got memory overwrites.

* Fix bug in quadrature nolib sketch
- I used the wrong symbol for adding the #2 isr

* minor change to quadrature nolib
- use array rather than pointer in readEncoders() argument.

* Updated quadrature_encoder.ino to not require library
- the library we used was only for AVR microcontrollers,
  so the code could not work on RPi Pico for instance.
- I reimplemented the sketch to have explicit polling
  mode logic that is suitable for noisy mechanical encoders.
- To that I added an interrupt driven mode that works if
  there is one interrupt capable pin available for each encoder.
  This is suitable for optical or hall effect encoders that
  do not need to be debounced.

* fix merge error in path_follow.py

* Fix RPi_GPIO_Servo part so it does not need GPIO in constructor
- the constructor has a default for pin_scheme based on the GPIO object.  The GPIO object will not be defined on a PC.
- this change use None as a default and then checks for it and set the GPIO default if it is None.
- This fixes a bug in the actuator unit test.

* added non-threaded run() to UnicyclePose and BicyclePose for testing

* Improved accuracy of MockEncoder by propagating fractional ticks

* Updated add_odometry() so we could add as non-threaded for testing.

* Vehicle loop prints out number of iterations and total time
- The vehicle loop now counts frames accurately; prior to
  this change the counter would be one more than the actual
  number of executed frames
- When the vehicle loop terminates the number of executed
  iterations and the total time are printed and returned
  to the caller.
- This was used for the kinematics tests because we needed
  to know how long the vehicle drive loop executed so
  we could calculate the expected distance that the
  mock vehicle drove.

* Rewrite of the kinematics unit tests
- We dramatically changed how a mock drivetrain handles
  odometry.  Now is uses a velocity based on encoder
  ticks per second scaled by throttle.  This is a more
  realistic mock, but it is harder to test.
- The tests setup a vehicle with a mock encoder, then
  run the vehicle loop for a set number of iterations.
  The vehicle loop now returns how long the loop ran and
  this is used along with the configuration for ticks_per_second
  from the mock encoder to calculate how far the vehicle travelled.
  Then the kinematics model is applied to see if the resulting
  ending pose matches the expected pose.

* Adjust bicycle kinematics to use front wheel reference
- the code now uses the front wheels as the reference
  point for the calculations
- this fixes the unit test, which were using the front wheels
  while the code used the back wheels.

* relax orientation test to 3% accuracy

* updated based on PR feedback

* removed hard-coded logging level

* Update vehicle.py

- change print statement to a log

* Update setup.py version="4.4.dev5"

version="4.4.dev5"

arduino/mono_encoder/mono_encoder.ino

@@ -0,0 +1,318 @@
+/* 
+ * mono_encoder.ino
+ * 
+ * Read one or two single-channel encoders,
+ * Ticks are counted for each transition
+ * from open-to-closed and closed-to-open.
+ * 
+ * For example, a 20 slot optical rotary encoder
+ * will have be 40 ticks per revolution.
+ * 
+ * The sketch implements the r/p/c command protocol
+ * for on-demand sending of encoder value
+ * and continuous sending with provided delay.
+ * See the comment in the loop below for details.
+ * commands are send one per line (ending in '\n')
+ * 'r' command resets position to zero
+ * 'p' command sends position immediately
+ * 'c' command starts/stops continuous mode
+ *     - if it is followed by an integer,
+ *       then use this as the delay in ms
+ *       between readings.
+ *     - if it is not followed by an integer
+ *       then stop continuous mode
+ *
+ * Sends the encoder ticks and a timestamp
+ * as a comma delimited pair:
+ * "{ticks},{ticksMs}"
+
+ * In a dual encoder setup the second encoder values
+ * as separated from the first by a semicolon:
+ * "{ticks},{ticksMs};{ticks},{ticksMs}"
+ * 
+ * If USE_ENCODER_INTERRUPTS is defined then ticks
+ * will be maintained with interrupt service routines.
+ * In this case the pins must be interrupt capable.
+ * On the Arduino Uno/Nano pins 2 and 3 are interrupt
+ * capable, so the sketch uses those two pins by default.
+ * If your board uses different pins then modify the sketch
+ * before downloading to your microcontroller board.  
+ * The advantage of this mode is that is can a high rate
+ * of ticks as one would see with a high resolution encoder.
+ * The disadvantage is that is does a poor job of debouncing,
+ * so it is not appropriate for mechanical encoders.
+ *
+ * If USE_ENCODER_INTERRUPTS is NOT defined, then polling
+ * will be used to maintain the tick count.  The code 
+ * implements a robust debouncing technique.  
+ * The advantage of this mode is the debounce logic;
+ * it can robustly count ticks even from a noisy
+ * mechanical encoder like a KY-040 rotary encoder.
+ * The disadvantage is that this cannot handle high rates
+ * of ticks as would be delivered by a high resolution optical
+ * encoder.
+ *
+ * The method debounces noisy inputs,
+ * looking for stable transitions from
+ * open-to-closed and closed-to-open by polling
+ * the encoder pin and building a 16 bit value.
+ * We start by looking for a stable closed-to-open
+ * transition, which will be a 1 followed by 15 zeroes.
+ * Then we change to look for the open-to-closed transition,
+ * which is a zero reading followed by 15 ones.
+ *
+ * Given a polling delay of 100 microseconds, the
+ * minimum time for a stable reading is
+ * 100 microseconds * 16 bits = 1.6 milliseconds.
+ * That then would allow for a maximum read rate of
+ * 1000 / 1.6 = 625 ticks per second.
+ * For high resolution encoders, that may be too slow.
+ * In that case, reduce the POLL_DELAY_MICROS to
+ * suit your encoder and use case.
+ */
+#include <Arduino.h>
+
+#define ENCODER_PIN   (2)          // input pin for first encoder
+#define ENCODER_2_PIN (3)          // input pin for second encoder
+#define POLL_DELAY_MICROS (100UL)  // microseconds between polls
+#define USE_ENCODER_INTERRUPTS (1) // if defined then use interrupts to read ticks
+                                   // otherwise use polling.
+
+//
+// state for an encoder pin
+//
+struct EncoderState {
+  int pin;                     // gpio pin number
+  volatile long ticks;         // current tick count; do NOT read this directly, use readEncoders()
+  uint16_t readings;           // history of last 16 readings
+  uint16_t transition;         // value of last 16 readings for next stable state transition 
+  unsigned long pollAtMicros;  // time of next poll
+};
+
+// list of encoders
+static EncoderState encoders[] = {
+  {ENCODER_PIN, 0L, 0, 0x8000, 0UL},
+  #ifdef ENCODER_2_PIN
+    {ENCODER_2_PIN, 0L, 0, 0x8000, 0UL},
+  #endif
+};
+
+#define ENCODER_COUNT (sizeof(encoders) / sizeof(EncoderState))
+
+boolean continuous = false; // true to send continuously, false to only send on demand
+unsigned long sendAtMs = 0; // next send time in continuous mode
+unsigned long delayMs = 0;  // time between sends in continuous mode
+
+#ifdef USE_ENCODER_INTERRUPTS
+  typedef void (*gpio_isr_type)();
+
+  //
+  // count the interrupts
+  //
+  void encode_0() {
+    ++encoders[0].ticks;
+  }
+
+  #ifdef ENCODER_2_PIN
+    void encode_1() {
+      ++encoders[1].ticks;
+    }
+  #endif
+
+  gpio_isr_type _isr_routines[ENCODER_COUNT] = {
+      encode_0,
+      #ifdef ENCODER_2_PIN
+        encode_1
+      #endif
+  };
+#endif
+
+
+
+//
+// Poll the encoder and increment ticks
+// on stable transitions.
+//
+void pollEncoder(EncoderState &encoder) {
+  #ifdef USE_ENCODER_INTERRUPTS
+      if (NOT_AN_INTERRUPT == digitalPinToInterrupt(encoder.pin)) {
+        Serial.print("Pin ");
+        Serial.print(encoder.pin);
+        Serial.println(" must be an interrupt capable pin when compiled with USE_ENCODER_INTERRUPTS");
+      }
+  #else  
+    unsigned long nowMicros = micros();
+    if (nowMicros >= encoder.pollAtMicros) {
+      //
+      // shift state left and add new reading
+      //
+      encoder.readings = (encoder.readings << 1) | digitalRead(encoder.pin);
+
+      //
+      // if readings match target transition
+      // then count the ticks and 
+      // start looking for the next target transion
+      //
+      if (encoder.readings == encoder.transition) {
+        encoder.ticks += 1;
+        encoder.transition = ~encoder.transition;  // invert transition
+      }
+
+      encoder.pollAtMicros = nowMicros + POLL_DELAY_MICROS;
+    }
+  #endif
+}
+
+
+void pollEncoders() {
+  for(uint8_t i = 0; i < ENCODER_COUNT; i += 1) {
+    pollEncoder(encoders[i]);
+  }
+}
+
+//
+// reset tick counters to zero
+//
+void resetEncoders() {
+  // turn off interrupts so we can
+  // write mutable shared state atomically
+  #ifdef USE_ENCODER_INTERRUPTS
+     noInterrupts();
+  #endif
+
+  for(uint8_t i = 0; i < ENCODER_COUNT; i += 1) {
+    encoders[i].ticks = 0;
+  }
+
+  // turn back on interrupts
+  #ifdef USE_ENCODER_INTERRUPTS
+    interrupts();
+  #endif
+}
+
+
+void readEncoders(long (&ticks)[ENCODER_COUNT]) {
+  // turn off interrupts so we can
+  // read mutable shared state atomically
+  #ifdef USE_ENCODER_INTERRUPTS
+     noInterrupts();
+  #endif
+
+  for(uint8_t i = 0; i < ENCODER_COUNT; i += 1) {
+    ticks[i] = encoders[i].ticks;
+  }
+
+  // turn back on interrupts
+  #ifdef USE_ENCODER_INTERRUPTS
+    interrupts();
+  #endif
+}
+
+
+//
+// write ticks to serial port
+// as a string tuple with ticks and tickMs separate by a comma
+//
+//   "{ticks},{ticksMs}"
+//
+void writeTicks(unsigned long ticksMs) {
+  long ticks[ENCODER_COUNT];
+  readEncoders(ticks);
+
+  Serial.print(ticks[0]);
+  Serial.print(',');
+  Serial.print(ticksMs);
+  for(uint8_t i = 1; i < ENCODER_COUNT; i += 1) {
+    Serial.print(";");
+    Serial.print(ticks[i]);
+    Serial.print(',');
+    Serial.print(ticksMs);
+  }
+  Serial.println("");
+}
+
+
+void setup() {
+  // set all encoder pins to inputs
+  for(uint8_t i = 0; i < ENCODER_COUNT; i += 1) {
+    pinMode(encoders[i].pin, INPUT);
+    #ifdef USE_ENCODER_INTERRUPTS
+      attachInterrupt(digitalPinToInterrupt(encoders[i].pin), _isr_routines[i], FALLING);
+    #endif
+  }
+  Serial.begin(115200);
+}
+
+
+void loop() {
+  //
+  // poll each encoder's ticks
+  //
+  pollEncoders();
+  unsigned long ticksMs = millis();
+
+  //
+  // commands are send one per line (ending in '\n')
+  // 'r' command resets position to zero
+  // 'p' command sends position immediately
+  // 'c' command starts/stops continuous mode
+  //     - if it is followed by an integer,
+  //       then use this as the delay in ms
+  //       between readings.
+  //     - if it is not followed by an integer
+  //       then stop continuous mode
+  //
+  if (Serial.available() > 0) {
+    String newcommand = Serial.readStringUntil('\n');
+    newcommand.trim();
+
+    if (newcommand == "r") {
+      //
+      // reset tick counters to zero
+      //
+      resetEncoders();
+    }
+    else if (newcommand == "p") {
+      //
+      // send current ticks immediately
+      //
+      writeTicks(ticksMs);
+    }
+    else if (newcommand.startsWith("c")) {
+      //
+      // continous mode start or stop
+      //
+      if (1 == newcommand.length()) {
+        //
+        // stop continuous mode
+        //
+        continuous = false;
+      } else {
+        //
+        // convert characters after 'c' to an integer
+        // and use this as the delay in milliseconds
+        //
+        String intString = newcommand.substring(1);
+        if (isDigit(intString.charAt(0))) {
+          delayMs = (unsigned long)intString.toInt();
+          continuous = true;
+        }
+      }
+    } else {
+      // unknown command
+    }
+  }
+
+  //
+  // we are in continuous mode, 
+  // then send the ticks when
+  // the delay expires
+  //
+  if (continuous) {
+    if (ticksMs >= sendAtMs) {
+      sendAtMs = ticksMs + delayMs;
+      writeTicks(ticksMs);
+    }
+  }
+}
+