   timer.every((1.0/publish_magnetometer_freq_hz) * 1000, doPublishMagnetometer);
   timer.every(BLINK_DURATION, doBlink);
   
-  current_time = ros_nh.now();
-  previous_time = current_time;
+  ros::Time current_time = ros_nh.now();
   last_cmd_vel_time = current_time;
   last_odom_publish_time = current_time;
   last_encoder_read_time = current_time;

  // Update ROS
  ros_nh.spinOnce();
  
  // If have not received cmd_vel messages in threshold time,
  // something is wrong, stop the robot.
  cmd_vel_timeout_stop = (current_time.toSec() - last_cmd_vel_time.toSec()) > CMD_VEL_TIMEOUT_THRESHOLD;
  if (cmd_vel_timeout_stop) {
    motor_left_speed = 0;

    motor_right_speed = 0;
  }

  ros::Time current_time = ros_nh.now();

  // Update any timer callbacks
  timer.update();
