[openrtm-commit:00021] r2075 - trunk/OpenRTM-aist/src/lib/rtm

[openrtm at openrtm.org]

Author: n-ando
Date: 2011-04-29 20:54:13 +0900 (Fri, 29 Apr 2011)
New Revision: 2075

Modified:
   trunk/OpenRTM-aist/src/lib/rtm/PeriodicExecutionContext.cpp
Log:
PeriodicExecutionContext's sleep time calculation has been improved.
Now execution time is emasured and applied it to sleep time.


Modified: trunk/OpenRTM-aist/src/lib/rtm/PeriodicExecutionContext.cpp
===================================================================
--- trunk/OpenRTM-aist/src/lib/rtm/PeriodicExecutionContext.cpp	2011-04-29 10:34:23 UTC (rev 2074)
+++ trunk/OpenRTM-aist/src/lib/rtm/PeriodicExecutionContext.cpp	2011-04-29 11:54:13 UTC (rev 2075)
@@ -148,12 +148,24 @@
 	  {
 	    m_worker.cond_.wait();
 	  }
+        coil::TimeValue t0(coil::gettimeofday());
 	if (m_worker.running_)
 	  {
 	    std::for_each(m_comps.begin(), m_comps.end(), invoke_worker());
 	  }
 	m_worker.mutex_.unlock();
-        if (!m_nowait) { coil::sleep(m_period); }
+        coil::TimeValue t1(coil::gettimeofday());
+        RTC_PARANOID(("Period:    %f [s]", (double)m_period));
+        RTC_PARANOID(("Execution: %f [s]", (double)(t1 - t0)));
+        RTC_PARANOID(("Sleep:     %f [s]", (double)(m_period - (t1 - t0))));
+        coil::TimeValue t2(coil::gettimeofday());
+        if (!m_nowait && m_period > (t1 - t0))
+          {
+            RTC_PARANOID(("sleeping..."));
+            coil::sleep((coil::TimeValue)(m_period - (t1 - t0)));
+          }
+        coil::TimeValue t3(coil::gettimeofday());
+        RTC_PARANOID(("Slept:     %f [s]", (double)(t3 - t2)));
       } while (m_svc);
 
     return 0;