Added support for simulation time.

This allows control loops to behave more predictably in the face of the
simulator running at non-realtime speeds.

Change-Id: I3508ed7ad316a3bf8b2c54b68c93baaf8cc4d941
Closes: artf2607

Conflicts:
	wpilibc/wpilibC++Sim/include/Timer.h
	wpilibc/wpilibC++Sim/src/Utility.cpp

simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/Main.java

@@ -1,8 +1,16 @@
 package edu.wpi.first.wpilibj.simulation.ds;
 
+import gazebo.msgs.GzFloat64.Float64;
+
+import org.gazebosim.transport.Msgs;
 import org.gazebosim.transport.Node;
+import org.gazebosim.transport.Subscriber;
+import org.gazebosim.transport.SubscriberCallback;
 
 public class Main {
+	private static double simTime = 0;
+	private static Subscriber<Float64> sub;
+	
 	public static void main(String args[]) {
 		Node node = new Node("frc");
 		try {
@@ -18,19 +26,35 @@ public class Main {
 		DS ds = new DS(provider);
 		ds.advertise(node);
 
+    	sub = node.subscribe("time", Msgs.Float64(),
+			new SubscriberCallback<Float64>() {
+				@Override
+				public void callback(Float64 msg) {
+					simTime = msg.getData();
+					synchronized(sub) {
+						sub.notifyAll();
+					}
+				}
+			}
+		);
+
 		while (true) {
-			ds.publish();
+	    	final double start = simTime;
 			for (int i = 0; i < provider.getJoysticks().size(); i++) {
 				ISimJoystick joystick = provider.getJoysticks().get(i);
 				joystick.advertise(node, i+1);
 				joystick.publish();
 			}
-			
-			try {
-				Thread.sleep(19);
-			} catch (InterruptedException e) {
-				// TODO Auto-generated catch block
-				e.printStackTrace();
+			ds.publish();
+
+	    	while ((simTime - start) < 0.020 /*20ms*/) {
+	    		synchronized(sub) {
+	    			try {
+	    				sub.wait(); // Block until time progresses
+	    			} catch (InterruptedException e) {
+	    				e.printStackTrace();
+	    			}
+	    		}
 			}
 		}
 	}

simulation/frc_gazebo_plugin/src/FRCPlugin.cpp

@@ -46,6 +46,7 @@ void FRCPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
   }
   enabled = false;
   sub = gzNode->Subscribe("~/ds/state", &FRCPlugin::dsCallback, this);
+  time_pub = gzNode->Advertise<msgs::Float64>("~/time");
 
   // Connect to the world update event.
   // This will trigger the Update function every Gazebo iteration
@@ -54,6 +55,9 @@ void FRCPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
 
 // Update all RobotComponents
 void FRCPlugin::Update(const common::UpdateInfo &_info) {
+  seconds_msg.set_data(_info.simTime.Double());
+  time_pub->Publish(seconds_msg);
+  
   for (int i = 0; i < components.size(); i++) {
     components[i]->update(enabled);
   }

simulation/frc_gazebo_plugin/src/FRCPlugin.h

@@ -33,6 +33,8 @@ private:
 
   transport::SubscriberPtr sub;
   volatile bool enabled;
+  transport::PublisherPtr time_pub;
+  msgs::Float64 seconds_msg;
   void dsCallback(const msgs::ConstDriverStationPtr &msg);
 };