[openrtm-commit:00210] r2194 - trunk/OpenRTM-aist/src/lib/rtm

[openrtm ＠ openrtm.org]

Author: n-ando
Date: 2011-07-01 00:34:13 +0900 (Fri, 01 Jul 2011)
New Revision: 2194

Modified:
   trunk/OpenRTM-aist/src/lib/rtm/PeriodicExecutionContext.cpp
Log:
ExecutionContext::deactivate_component() operation should wait until
the RTC actually enters INACTIVE state. Transition waiting code has
been added. #2176


Modified: trunk/OpenRTM-aist/src/lib/rtm/PeriodicExecutionContext.cpp
===================================================================
--- trunk/OpenRTM-aist/src/lib/rtm/PeriodicExecutionContext.cpp	2011-06-30 15:16:23 UTC (rev 2193)
+++ trunk/OpenRTM-aist/src/lib/rtm/PeriodicExecutionContext.cpp	2011-06-30 15:34:13 UTC (rev 2194)
@@ -374,14 +374,35 @@
     CompItr it;
     it = std::find_if(m_comps.begin(), m_comps.end(),
 		      find_comp(comp));
-    if (it == m_comps.end())
-      return RTC::BAD_PARAMETER;
-    
+    if (it == m_comps.end()) { return RTC::BAD_PARAMETER; }
     if (!(it->_sm.m_sm.isIn(ACTIVE_STATE)))
-      return RTC::PRECONDITION_NOT_MET;
+      {
+        return RTC::PRECONDITION_NOT_MET;
+      }
     
     it->_sm.m_sm.goTo(INACTIVE_STATE);
-    return RTC::RTC_OK;
+    int count(0);
+    const double usec_per_sec(1.0e6);
+    double sleeptime(10.0 * usec_per_sec / get_rate());
+    RTC_PARANOID(("Sleep time is %f [us]", sleeptime));
+    while (it->_sm.m_sm.isIn(ACTIVE_STATE))
+      {
+        RTC_TRACE(("Waiting to be the INACTIVE state"));
+        coil::usleep(sleeptime);
+        if (count > 1000)
+          {
+            RTC_ERROR(("The component is not responding."));
+            break;
+          }
+        ++count;
+      }
+    if (it->_sm.m_sm.isIn(INACTIVE_STATE))
+      {
+        RTC_TRACE(("The component has been properly deactivated."));
+        RTC::RTC_OK;
+      }
+    RTC_ERROR(("The component could not be deactivated."));
+    return RTC::RTC_ERROR;
 #else // ORB_IS_RTORB
     for (int i(0); i < (int)m_comps.size(); ++i)
       {
@@ -392,7 +413,29 @@
                 return RTC::PRECONDITION_NOT_MET;
               }
             m_comps.at(i)._sm.m_sm.goTo(INACTIVE_STATE);
-            return RTC::RTC_OK;
+            int count(0);
+            const double usec_per_sec(1.0e6);
+            double sleeptime(10.0 * usec_per_sec / get_rate());
+            RTC_PARANOID(("Sleep time is %f [us]", sleeptime));
+            while (m_comps.at(i)._sm.m_sm.isIn(ACTIVE_STATE))
+              {
+                RTC_TRACE(("Waiting to be the INACTIVE state"));
+                coil::usleep(sleeptime);
+                
+                if (count > 1000)
+                  {
+                    RTC_ERROR(("The component is not responding."));
+                    break;
+                  }
+                ++count;
+              }
+            if (m_comps.at(i)._sm.m_sm.isIn(INACTIVE_STATE))
+              {
+                RTC_TRACE(("The component has been properly deactivated."));
+                RTC::RTC_OK;
+              }
+            RTC_ERROR(("The component could not be deactivated."));
+            return RTC::RTC_ERROR;
           }
       }
     return RTC::BAD_PARAMETER;