 void doEncoderDebug() {
   // Read the encoders
   snprintf(debug_str, sizeof(debug_str), "L: %ld    R: %ld",
-    encoder_left.read(), encoder_right.read());
+    encoder_left.read(), 
+    encoder_right.read());
   ros_debug_msg.data = debug_str;
   ros_encoder_debug_pub.publish(&ros_debug_msg);
 }

void doBumperDebug() {
  // Read the bumper sensors
  snprintf(debug_str, sizeof(debug_str), 
    "L: %d    R: %d    STOP: %d", bumper_left, bumper_right, bumper_stop);
  ros_debug_msg.data = debug_str;
  ros_bumper_debug_pub.publish(&ros_debug_msg);
}

 */
void cmdVelCallback(const geometry_msgs::Twist& cmd_vel_msg) {
  last_cmd_vel_time = ros_nh.now();
  
  // Calculate wheel linear velocity
  double vl = cmd_vel_msg.linear.x - (cmd_vel_msg.angular.z * TRACK_RADIUS);
