   }
   ros_imu_debug_pub.publish(&ros_debug_msg);
 }
-#endif
-
-#ifdef ENABLE_MAGNETOMETER_DEBUG
+
 void doMagnetometerDebug() {
   // Read the Magnetometer
   if (mag_err) {

double motor_right_speed;

// Victoria constants
#define TICKS_PER_RADIAN 9072 // TODO(mwomack): Verify real value
#define TRACK_RADIUS 0.235  // in meters
#define WHEEL_RADIUS 0.127  // in meters
#define MAX_SPEED 4.71      // in meters/second

// Bumper sensors
bool bumper_stop = false; // Set to true if bumpers indicate robot should stop

  if (!ros_nh.getParam("publish_magnetometer_freq_hz", &publish_magnetometer_freq_hz)) { 
    publish_magnetometer_freq_hz = 2;
  }

  int publish_bumper_debug_info_freq_hz;
  if (!ros_nh.getParam("publish_bumper_debug_info_freq_hz", &publish_bumper_debug_info_freq_hz)) { 
    publish_bumper_debug_info_freq_hz = 10;
  }

  int publish_encoder_debug_info_freq_hz;
  if (!ros_nh.getParam("publish_encoder_debug_info_freq_hz", &publish_encoder_debug_info_freq_hz)) { 
    publish_encoder_debug_info_freq_hz = 10;
  }

  int publish_imu_debug_info_freq_hz;
  if (!ros_nh.getParam("publish_imu_debug_info_freq_hz", &publish_imu_debug_info_freq_hz)) { 
    publish_imu_debug_info_freq_hz = 2;
  }

  int publish_magnetometer_debug_info_freq_hz;
  if (!ros_nh.getParam("publish_magnetometer_debug_info_freq_hz", &publish_magnetometer_debug_info_freq_hz)) { 
    publish_magnetometer_debug_info_freq_hz = 2;
  }
  
  // Initialize broadcasters
  ros_odom_transform.header.frame_id = ros_odom_header_frame_id;
