 #include <ros.h>
 // Include a custom command message.
 #include <command2ros/ExcavatorCommand.h>
-
+#include <std_msgs/String.h>
+
+void newExcavatorCommandCallback(const command2ros::ExcavatorCommand& newManualCommand);
 
 // Device constants
 const int NUM_TRANSPORT_MOTORS = 4;

  delayMicroseconds(1000);
}

// Wheel unit test.
// Each wheel should rotate individually.
void unitTest()

{
 //Currently stopped, don't do anything.
  // ASSUMES the robot is stopped when currentStatus is set to STOPPED.
switch(currentStatus)
{
 case READY_TO_DRIVE:
   driveTires();
   driveUntilTime = millis() + (long)lastExcavatorCommand.drive_duration;
 break;
 case DRIVING:
   if(millis() > driveUntilTime)
   {
     stopAllMotors();
     // TODO: Change this to excavating, if necessary.
    currentStatus = STOPPED;
   }
 break;
  
 case STOPPED:
  // fall through
 default:
 // TODO: Convey / Dig 
 break;
}
  
  //TODO: Publish sensor / state data?
  //Sync with ROS
  excavatorNode.spinOnce(); // Check for subscriber update/update timestamp
