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);
 };
 

wpilibc/wpilibC++Sim/include/Timer.h

@@ -14,6 +14,12 @@ void Wait(double seconds);
 double GetClock();
 double GetTime();
 
+namespace wpilib { namespace internal {
+    extern double simTime;
+	extern MULTIWAIT_ID time_wait;
+    // transport::SubscriberPtr time_sub;
+}}
+
 /**
  * Timer objects measure accumulated time in seconds.
  * The timer object functions like a stopwatch. It can be started, stopped, and cleared. When the

wpilibc/wpilibC++Sim/src/Timer.cpp

@@ -22,11 +22,13 @@
  */
 void Wait(double seconds)
 {
-	if (seconds < 0.0) return;
-    struct timespec test, remaining;
-    test.tv_sec = (int) seconds;
-    test.tv_nsec = (seconds - (int)seconds) * 1000000000.0;
-    nanosleep(&test, &remaining);
+    if (seconds < 0.0) return;
+
+    double start = wpilib::internal::simTime;
+    	
+    while ((wpilib::internal::simTime - start) < seconds) {
+        takeMultiWait(wpilib::internal::time_wait, 0);
+    }
 }
 
 /*
@@ -41,16 +43,11 @@ double GetClock()
 
 /**
  * @brief Gives real-time clock system time with nanosecond resolution
- * @return The time, just in case you want the robot to start autonomous at 8pm on Saturday.
+ * @return The time, just in case you want the robot to start autonomous at 8pm on Saturday (except in simulation).
 */
-double GetTime()  
+double GetTime()
 {
-	struct timespec tp;
-	
-	clock_gettime(CLOCK_REALTIME,&tp);
-	double realTime = (double)tp.tv_sec + (double)((double)tp.tv_nsec*1e-9);
-	
-	return (realTime);
+    return Timer::GetFPGATimestamp(); // The epoch starts when Gazebo starts
 }
 
 /**
@@ -181,7 +178,7 @@ double Timer::GetFPGATimestamp()
 {
 	// FPGA returns the timestamp in microseconds
 	// Call the helper GetFPGATime() in Utility.cpp
-	return GetFPGATime() * 1.0e-6;
+	return wpilib::internal::simTime;
 }
 
 // Internal function that reads the PPC timestamp counter.
@@ -190,3 +187,18 @@ extern "C"
 	uint32_t niTimestamp32(void);
 	uint64_t niTimestamp64(void);
 }
+
+// Internal stuff
+#include "simulation/SimFloatInput.h"
+#include "simulation/MainNode.h"
+namespace wpilib { namespace internal {
+    double simTime = 0;
+    MULTIWAIT_ID time_wait = initializeMultiWait();
+
+    void time_callback(const msgs::ConstFloat64Ptr &msg) {
+        simTime = msg->data();
+        giveMultiWait(time_wait);
+    }
+
+    transport::SubscriberPtr time_pub = MainNode::Subscribe("~/time", &time_callback);
+}}

wpilibc/wpilibC++Sim/src/Utility.cpp

@@ -7,8 +7,8 @@
 #include "Utility.h"
 
 #include "HAL/cpp/StackTrace.hpp"
-#include <boost/date_time/posix_time/posix_time_types.hpp>
 #include "Task.h"
+#include "Timer.h"
 #include <stdio.h>
 #include <string.h>
 
@@ -160,7 +160,6 @@ bool wpi_assertNotEqual_impl(int valueA,
  */
 uint32_t GetFPGATime()
 {
-    boost::posix_time::ptime time = boost::posix_time::microsec_clock::universal_time();
-	return time.time_of_day().total_microseconds();
+	return wpilib::internal::simTime * 1e6;
 }
 

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -96,8 +96,10 @@ public class DriverStation implements IInputOutput {
         MainNode.subscribe("ds/state", GzDriverStation.DriverStation.getDefaultInstance(),
     		new SubscriberCallback<GzDriverStation.DriverStation>() {
 				@Override public void callback(GzDriverStation.DriverStation msg) {
+					state = msg;
+					m_newControlData = true;
 					synchronized (m_dataSem) {
-						state = msg;
+						m_dataSem.notifyAll();
 					}
 				}
 			}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java

@@ -144,6 +144,7 @@ public class IterativeRobot extends RobotBase {
                     didTeleopPeriodic = true;
                 }
             }
+        	m_ds.waitForData();
         }
     }
 
@@ -152,11 +153,7 @@ public class IterativeRobot extends RobotBase {
      * Call the periodic functions whenever a packet is received from the Driver Station, or about every 20ms.
      */
     private boolean nextPeriodReady() {
-        // TODO: return m_ds.isNewControlData();
-        try {
-            Thread.sleep(20); // TODO: Find a better solution. This one is way too hacky!
-        } catch (InterruptedException ex) {}
-    	return true;
+    	return m_ds.isNewControlData();
     }
 
     /* ----------- Overridable initialization code -----------------*/

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/PIDController.java

@@ -6,8 +6,6 @@
 /*----------------------------------------------------------------------------*/
 package edu.wpi.first.wpilibj;
 
-import java.util.TimerTask;
-
 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
 import edu.wpi.first.wpilibj.parsing.IUtility;
 import edu.wpi.first.wpilibj.tables.ITable;
@@ -44,7 +42,6 @@ public class PIDController implements IUtility, LiveWindowSendable, Controller {
     private double m_period = kDefaultPeriod;
     PIDSource m_pidInput;
     PIDOutput m_pidOutput;
-    java.util.Timer m_controlLoop;
     private boolean m_freed = false;
     private boolean m_usingPercentTolerance;
 
@@ -89,8 +86,7 @@ public class PIDController implements IUtility, LiveWindowSendable, Controller {
         }
     }
 
-    private class PIDTask extends TimerTask {
-
+    private class PIDTask implements Runnable {
         private PIDController m_controller;
 
         public PIDTask(PIDController controller) {
@@ -101,10 +97,9 @@ public class PIDController implements IUtility, LiveWindowSendable, Controller {
         }
 
         public void run() {
-        	if(!m_freed){
+        	while (!m_controller.m_freed) {
         		m_controller.calculate();
-        	} else {
-        		cancel();
+        		Timer.delay(m_controller.m_period);
         	}
         }
     }
@@ -131,9 +126,6 @@ public class PIDController implements IUtility, LiveWindowSendable, Controller {
             throw new NullPointerException("Null PIDOutput was given");
         }
 
-        m_controlLoop = new java.util.Timer();
-
-
         m_P = Kp;
         m_I = Ki;
         m_D = Kd;
@@ -143,7 +135,7 @@ public class PIDController implements IUtility, LiveWindowSendable, Controller {
         m_pidOutput = output;
         m_period = period;
 
-        m_controlLoop.schedule(new PIDTask(this), 0L, (long) (m_period * 1000));
+        new Thread(new PIDTask(this)).start();
 
         instances++;
         m_tolerance = new NullTolerance();
@@ -196,10 +188,8 @@ public class PIDController implements IUtility, LiveWindowSendable, Controller {
     public void free() {
     	m_freed = true;
     	if(this.table!=null) table.removeTableListener(listener);
-        m_controlLoop.cancel();
         m_pidInput = null;
     	m_pidOutput = null;
-        m_controlLoop = null;
     }
 
     /**

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Timer.java

@@ -7,7 +7,12 @@
 
 package edu.wpi.first.wpilibj;
 
+import org.gazebosim.transport.Msgs;
+import org.gazebosim.transport.SubscriberCallback;
+
 import edu.wpi.first.wpilibj.parsing.IUtility;
+import edu.wpi.first.wpilibj.simulation.MainNode;
+import gazebo.msgs.GzFloat64.Float64;
 
 /**
  * Timer objects measure accumulated time in milliseconds.
@@ -21,6 +26,19 @@ public class Timer implements IUtility {
     private long m_startTime;
     private double m_accumulatedTime;
     private boolean m_running;
+    private static double simTime;
+    private static Object time_notifier = new Object();
+    static {
+    	MainNode.subscribe("time", Msgs.Float64(),
+			new SubscriberCallback<Float64>() {
+				@Override
+				public void callback(Float64 msg) {
+					simTime = msg.getData();
+					synchronized(time_notifier) { time_notifier.notifyAll(); } // Ew, this is nested too deep... Refactor?
+				}
+			}
+		);
+    }
 
     /**
      * Pause the thread for a specified time. Pause the execution of the
@@ -32,10 +50,17 @@ public class Timer implements IUtility {
      * @param seconds Length of time to pause
      */
     public static void delay(final double seconds) {
-        try {
-            Thread.sleep((long) (seconds * 1e3));
-        } catch (final InterruptedException e) {
-        }
+    	final double start = simTime;
+    	
+    	while ((simTime - start) < seconds) {
+    		synchronized(time_notifier) {
+    			try {
+    				time_notifier.wait(); // Block until time progresses
+    			} catch (InterruptedException e) {
+    				e.printStackTrace();
+    			}
+    		}
+		}
     }
 
     /**
@@ -46,7 +71,7 @@ public class Timer implements IUtility {
      * @return Robot running time in microseconds.
      */
     public static long getUsClock() {
-        return System.nanoTime() / 1000;
+        return (long) (simTime * 1e6);
     }
 
     /**
@@ -57,7 +82,7 @@ public class Timer implements IUtility {
      * @return Robot running time in milliseconds.
      */
     static long getMsClock() {
-        return System.currentTimeMillis();
+        return (long) (simTime * 1e3);
     }
 
     /**
@@ -67,7 +92,7 @@ public class Timer implements IUtility {
      * @return Robot running time in seconds.
      */
     public static double getFPGATimestamp() {
-        return System.currentTimeMillis() / 1000.0;
+        return simTime;
     }
 
     /**