 unsigned long encoder_right_pos;
 ros::Time last_encoder_read_time;
 
-#define NUM_VEL_SAMPLES 10
+const int NUM_VEL_SAMPLES(10);
 double ang_v_samples_l[NUM_VEL_SAMPLES];
 double ang_v_samples_r[NUM_VEL_SAMPLES];
 int velocity_index = 0;

  digitalWrite(BLINK_PIN, blink_state);
}

#ifdef ENABLE_BUMPER_DEBUG
void doBumperDebug() {
  // Read the bumper sensors
  snprintf(debug_str, sizeof(debug_str), 

  }
  ros_imu_debug_pub.publish(&ros_debug_msg);
}

void doMagnetometerDebug() {
  // Read the Magnetometer
  if (mag_err) {
