 
   // If we have not received any cmd_vel messages for a while,
   // something is wrong, stop the robot.
+  if (current_time.toNsec() - last_cmd_vel_time.toNsec() 
+        > CMD_VEL_TIMEOUT_THRESHOLD_NS) {
+    // TODO: Do something to stop the robot!
+  }
   
   // Read bumper sensors
   unsigned int new_bumper_left = analogRead(BUMPER_LEFT_PIN);

    
  // Start serial port to TRex
  trex.begin(19200);

  // Initialize ros node
  ros_nh.initNode();

 * Victoria Teensy ROS Node
 * 
 * This code is the ROS node that executes on the Teensy processor
 * for the Victoria RoboMagellan team.
 * 
 * It uses the rosserial library to communicate with ROS. It uses
 * Arduino and Teensy libraries to interact with systems and
 * circuits connected to the Teensy processor. This code assumes
 * that a Teensy 3.5 is used.
 * 
 * https://github.com/victoriarobotics
 * 
 */

// Teensy includes
#define ENCODER_OPTIMIZE_INTERRUPTS
#include <Encoder.h>  // 
#include <i2c_t3.h>
#include <LSM6.h>
#include <LIS3MDL.h>

// ROS includes
#include <ros.h>
