diff --git a/simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/Main.java b/simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/Main.java index 5df887ccc3..50f8e7651c 100644 --- a/simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/Main.java +++ b/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 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() { + @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(); + } + } } } } diff --git a/simulation/frc_gazebo_plugin/src/FRCPlugin.cpp b/simulation/frc_gazebo_plugin/src/FRCPlugin.cpp index cc0a58bb68..756be8af9a 100644 --- a/simulation/frc_gazebo_plugin/src/FRCPlugin.cpp +++ b/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("~/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); } diff --git a/simulation/frc_gazebo_plugin/src/FRCPlugin.h b/simulation/frc_gazebo_plugin/src/FRCPlugin.h index 1bff7f3a55..4c0e107286 100644 --- a/simulation/frc_gazebo_plugin/src/FRCPlugin.h +++ b/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); }; diff --git a/wpilibc/wpilibC++Sim/include/Timer.h b/wpilibc/wpilibC++Sim/include/Timer.h index 67d574f2d3..40a6f65db5 100644 --- a/wpilibc/wpilibC++Sim/include/Timer.h +++ b/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 diff --git a/wpilibc/wpilibC++Sim/src/Timer.cpp b/wpilibc/wpilibC++Sim/src/Timer.cpp index 673f3d8871..12e8d371e6 100644 --- a/wpilibc/wpilibC++Sim/src/Timer.cpp +++ b/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); +}} diff --git a/wpilibc/wpilibC++Sim/src/Utility.cpp b/wpilibc/wpilibC++Sim/src/Utility.cpp index 37e6ab7d59..f393e089a4 100644 --- a/wpilibc/wpilibC++Sim/src/Utility.cpp +++ b/wpilibc/wpilibC++Sim/src/Utility.cpp @@ -7,8 +7,8 @@ #include "Utility.h" #include "HAL/cpp/StackTrace.hpp" -#include #include "Task.h" +#include "Timer.h" #include #include @@ -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; } diff --git a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index fb699eebbc..ba75d5a810 100644 --- a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/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() { @Override public void callback(GzDriverStation.DriverStation msg) { + state = msg; + m_newControlData = true; synchronized (m_dataSem) { - state = msg; + m_dataSem.notifyAll(); } } } diff --git a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java index b31d7346c3..1438838247 100644 --- a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java +++ b/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 -----------------*/ diff --git a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/PIDController.java index 6fdd8a2e18..5be1ed657a 100644 --- a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/PIDController.java +++ b/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; } /** diff --git a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Timer.java b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Timer.java index 9bb2bdfda8..c6138e751c 100644 --- a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Timer.java +++ b/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() { + @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; } /**