[Playerstage-commit] SF.net SVN: playerstage:[7520] code/gazebo/branches/ogre-1.4.9/server/ controllers

[<hsu...@us...>]

Revision: 7520
          http://playerstage.svn.sourceforge.net/playerstage/?rev=7520&view=rev
Author:   hsujohnhsu
Date:     2009-03-17 01:37:53 +0000 (Tue, 17 Mar 2009)

Log Message:
-----------
 * update rate control for Controller.
 * some debug outputs.

Modified Paths:
--------------
    code/gazebo/branches/ogre-1.4.9/server/controllers/Controller.cc
    code/gazebo/branches/ogre-1.4.9/server/controllers/Controller.hh

Modified: code/gazebo/branches/ogre-1.4.9/server/controllers/Controller.cc
===================================================================
--- code/gazebo/branches/ogre-1.4.9/server/controllers/Controller.cc	2009-03-17 01:35:30 UTC (rev 7519)
+++ code/gazebo/branches/ogre-1.4.9/server/controllers/Controller.cc	2009-03-17 01:37:53 UTC (rev 7520)
@@ -34,6 +34,8 @@
 #include "World.hh"
 #include "Controller.hh"
 #include "Simulator.hh"
+#include "PhysicsEngine.hh"
+#include "Global.hh"
 
 using namespace gazebo;
 
@@ -43,6 +45,7 @@
 {
   Param::Begin(&this->parameters);
   this->nameP = new ParamT<std::string>("name","",1);
+  this->alwaysOnP = new ParamT<bool>("alwaysOn", false, 0);
   this->updatePeriodP = new ParamT<double>("updateRate", 10, 0);
   Param::End();
 
@@ -60,6 +63,7 @@
 {
   this->Fini();
   delete this->nameP;
+  delete this->alwaysOnP;
   delete this->updatePeriodP;
 }
 
@@ -75,10 +79,17 @@
   this->typeName = node->GetName();
 
   this->nameP->Load(node);
+
+  this->alwaysOnP->Load(node);
+
   this->updatePeriodP->Load(node);
-  this->updatePeriod = 1.0 / (this->updatePeriodP->GetValue() + 1e-6);
 
-  this->lastUpdate = -1e6;
+  double updateRate  = this->updatePeriodP->GetValue();
+  if (updateRate == 0)
+    this->updatePeriod = 0.0; // no throttling if updateRate is 0
+  else
+    this->updatePeriod = 1.0 / updateRate;
+  this->lastUpdate   = Simulator::Instance()->GetSimTime();
 
   childNode = node->GetChildByNSPrefix("interface");
 
@@ -178,12 +189,30 @@
 /// Update the controller. Called every cycle.
 void Controller::Update()
 {
-  if (this->IsConnected())
+  if (this->IsConnected() || this->alwaysOnP->GetValue())
   {
-    if (lastUpdate + updatePeriod <= Simulator::Instance()->GetSimTime())
+    // round time difference to this->physicsEngine->GetStepTime()
+    double physics_dt = World::Instance()->GetPhysicsEngine()->GetStepTime();
+
+    // if (this->GetName() == std::string("p3d_base_controller"))
+    // std::cout << " sim update: " << this->GetName()
+    //           << " , " << Simulator::Instance()->GetSimTime() - lastUpdate
+    //           << " , " << lastUpdate
+    //           << " , " << updatePeriod
+    //           << " i1 " << round((Simulator::Instance()->GetSimTime()-lastUpdate)/physics_dt)
+    //           << " i2 " << round(updatePeriod/physics_dt)
+    //           << std::endl;
+#ifdef TIMING
+    double tmpT1 = Simulator::Instance()->GetWallTime();
+#endif
+    if (round((Simulator::Instance()->GetSimTime()-lastUpdate-updatePeriod)/physics_dt) >= 0)
     {
       this->UpdateChild();
       lastUpdate = Simulator::Instance()->GetSimTime();
+#ifdef TIMING
+      double tmpT2 = Simulator::Instance()->GetWallTime();
+      std::cout << "           Controller::Update() Name (" << this->GetName() << ") dt (" << tmpT2-tmpT1 << ")" << std::endl;
+#endif
     }
   }
 }
@@ -210,6 +239,10 @@
 {
   std::vector<Iface*>::const_iterator iter;
 
+  // if the alwaysOn flag is true, this controller is connected
+  if (this->alwaysOnP->GetValue())
+    return true;
+
   for (iter=this->ifaces.begin(); iter!=this->ifaces.end(); iter++)
   {
     if ((*iter)->GetOpenCount() > 0)

Modified: code/gazebo/branches/ogre-1.4.9/server/controllers/Controller.hh
===================================================================
--- code/gazebo/branches/ogre-1.4.9/server/controllers/Controller.hh	2009-03-17 01:35:30 UTC (rev 7519)
+++ code/gazebo/branches/ogre-1.4.9/server/controllers/Controller.hh	2009-03-17 01:37:53 UTC (rev 7520)
@@ -105,6 +105,9 @@
   /// \brief The entity that owns this controller
   protected: Entity *parent;
 
+  /// \breif flag to keep controllers updating continuously
+  protected: ParamT<bool> *alwaysOnP;
+
   /// \brief Update period 
   protected: double updatePeriod;
   protected: ParamT<double> *updatePeriodP;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.