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
This commit is contained in:
Alex Henning
2014-06-26 10:50:47 -07:00
parent 15e3781805
commit e4e199f066
10 changed files with 112 additions and 51 deletions

View File

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