 
   // 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();

// TRex motor controller
HardwareSerial trex = HardwareSerial();

// Bumper sensors
#define BUMPER_THRESHOLD 500  // Threshold that will indicate robot should stop
unsigned int bumper_left_base;
unsigned int bumper_right_base;

// Motor encoders
Encoder encoder_left(ENCODER_LEFT_PIN_1, ENCODER_LEFT_PIN_2);
Encoder encoder_right(ENCODER_RIGHT_PIN_1, ENCODER_RIGHT_PIN_2);
unsigned long encoder_left_pos;
unsigned long encoder_right_pos;

// IMU and Magnetometer
LSM6 imu;
LIS3MDL mag;

// ROS node handle
ros::NodeHandle ros_nh;

// ROS cmd_vel subscriber
// Threshold, in nanoseconds, to wait for a cmd_vel message else take action
// to stop robot activity
#define CMD_VEL_TIMEOUT_THRESHOLD_NS 200000000
void cmdVelCallback(const geometry_msgs::Twist& twist_msg);
ros::Subscriber<geometry_msgs::Twist> ros_cmd_vel_sub("cmd_vel", cmdVelCallback);

