PeriodicExecutionContext.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00020 #include <coil/Time.h>
00021 #include <coil/TimeValue.h>
00022 #include <rtm/PeriodicExecutionContext.h>
00023 #include <rtm/RTObject.h>
00024 #include <algorithm>
00025 #include <iostream>
00026 
00027 #define DEEFAULT_PERIOD 0.000001
00028 namespace RTC
00029 {
00037   PeriodicExecutionContext::
00038   PeriodicExecutionContext()
00039     : rtclog("periodic_ec"), m_running(false), m_svc(true), m_nowait(false)
00040   {
00041     RTC_TRACE(("PeriodicExecutionContext()"));
00042 
00043     m_period = (double)DEEFAULT_PERIOD;
00044     RTC_DEBUG(("Actual rate: %d [sec], %d [usec]",
00045                m_period.sec(), m_period.usec()));
00046 
00047     // getting my reference
00048     m_ref = this->_this();
00049 
00050     // profile initialization
00051     m_profile.kind = PERIODIC;
00052     m_profile.rate = 1.0 / m_period;
00053     m_profile.owner = RTC::RTObject::_nil();
00054     m_profile.participants.length(0);
00055     m_profile.properties.length(0);
00056   }
00057   
00065   PeriodicExecutionContext::
00066   PeriodicExecutionContext(OpenRTM::DataFlowComponent_ptr owner,
00067                            double rate)
00068     : rtclog("periodic_ec"), m_running(false), m_svc(true), m_nowait(true)
00069   {
00070     RTC_TRACE(("PeriodicExecutionContext(owner, rate = %f)", rate));
00071 
00072     if (rate == 0) { rate = 1.0 / DEEFAULT_PERIOD; }
00073     m_period = coil::TimeValue(1.0 / rate);
00074     if (m_period < 0.000001) { m_nowait = true; }
00075     RTC_DEBUG(("Actual rate: %d [sec], %d [usec]",
00076                m_period.sec(), m_period.usec()));
00077 
00078     // getting my reference
00079     m_ref = this->_this();
00080 
00081     // profile initialization
00082     m_profile.kind = PERIODIC;
00083     m_profile.rate = 1.0 / m_period;
00084     m_profile.owner = RTC::RTObject::_nil();
00085     m_profile.participants.length(0);
00086     m_profile.properties.length(0);
00087   }
00088   
00096   PeriodicExecutionContext::~PeriodicExecutionContext()
00097   {
00098     RTC_TRACE(("~PeriodicExecutionContext()"));
00099     m_worker.mutex_.lock();
00100     m_worker.running_ = true;
00101     m_worker.cond_.signal();
00102     m_worker.mutex_.unlock();
00103     m_svc = false;
00104     wait();
00105 
00106     // cleanup EC's profile
00107     m_profile.owner = RTC::RTObject::_nil();
00108     m_profile.participants.length(0);
00109     m_profile.properties.length(0);
00110   }
00111   
00112   /*------------------------------------------------------------
00113    * Start activity
00114    * ACE_Task class method over ride.
00115    *------------------------------------------------------------*/
00123   int PeriodicExecutionContext::open(void *args)
00124   {
00125     RTC_TRACE(("open()"));
00126     activate();
00127     return 0;
00128   }
00129   
00130   /*------------------------------------------------------------
00131    * Run by a daemon thread to handle deferred processing
00132    * ACE_Task class method over ride.
00133    *------------------------------------------------------------*/
00141   int PeriodicExecutionContext::svc(void)
00142   {
00143     RTC_TRACE(("svc()"));
00144     int count(0);
00145     do 
00146       {
00147         m_worker.mutex_.lock();
00148         while (!m_worker.running_)
00149           {
00150             m_worker.cond_.wait();
00151           }
00152         coil::TimeValue t0(coil::gettimeofday());
00153         if (m_worker.running_)
00154           {
00155             std::for_each(m_comps.begin(), m_comps.end(), invoke_worker());
00156           }
00157         m_worker.mutex_.unlock();
00158         coil::TimeValue t1(coil::gettimeofday());
00159         if (count > 1000)
00160           {
00161             RTC_PARANOID(("Period:    %f [s]", (double)m_period));
00162             RTC_PARANOID(("Execution: %f [s]", (double)(t1 - t0)));
00163             RTC_PARANOID(("Sleep:     %f [s]", (double)(m_period - (t1 - t0))));
00164           }
00165         coil::TimeValue t2(coil::gettimeofday());
00166         if (!m_nowait && m_period > (t1 - t0))
00167           {
00168             if (count > 1000) { RTC_PARANOID(("sleeping...")); }
00169             coil::sleep((coil::TimeValue)(m_period - (t1 - t0)));
00170           }
00171         if (count > 1000)
00172           {
00173             coil::TimeValue t3(coil::gettimeofday());
00174             RTC_PARANOID(("Slept:     %f [s]", (double)(t3 - t2)));
00175             count = 0;
00176           }
00177         ++count;
00178       } while (m_svc);
00179 
00180     return 0;
00181   }
00182   
00190   int PeriodicExecutionContext::close(unsigned long flags)
00191   {
00192     RTC_TRACE(("close()"));
00193     
00194     // At this point, this component have to be finished.
00195     // Current state and Next state should be RTC_EXITING.
00196     //    delete this;
00197     return 0;
00198   }
00199   
00200   
00201   //============================================================
00202   // ExecutionContext
00203   //============================================================
00211   CORBA::Boolean PeriodicExecutionContext::is_running()
00212     throw (CORBA::SystemException)
00213   {
00214     RTC_TRACE(("is_running()"));
00215     return m_running;
00216   }
00217   
00225   ReturnCode_t PeriodicExecutionContext::start()
00226     throw (CORBA::SystemException)
00227   {
00228     RTC_TRACE(("start()"));
00229     if (m_running) return RTC::PRECONDITION_NOT_MET;
00230     
00231     // invoke ComponentAction::on_startup for each comps.
00232     std::for_each(m_comps.begin(), m_comps.end(), invoke_on_startup());
00233     
00234     // change EC thread state
00235     m_running = true;
00236     {
00237       m_worker.mutex_.lock();
00238       m_worker.running_ = true;
00239       m_worker.cond_.signal();
00240       m_worker.mutex_.unlock();
00241     }
00242     
00243     this->open(0);
00244     
00245     return RTC::RTC_OK;
00246   }
00247   
00255   ReturnCode_t PeriodicExecutionContext::stop()
00256     throw (CORBA::SystemException)
00257   {
00258     RTC_TRACE(("stop()"));
00259     if (!m_running) return RTC::PRECONDITION_NOT_MET;
00260     
00261     // stop thread
00262     m_running = false;
00263     {
00264       m_worker.mutex_.lock();
00265       m_worker.running_ = false;
00266       m_worker.mutex_.unlock();
00267     }
00268     // invoke on_shutdown for each comps.
00269     std::for_each(m_comps.begin(), m_comps.end(), invoke_on_shutdown());
00270    
00271     return RTC::RTC_OK;
00272   }
00273   
00281   CORBA::Double PeriodicExecutionContext::get_rate()
00282     throw (CORBA::SystemException)
00283   {
00284     RTC_TRACE(("get_rate()"));
00285     Guard guard(m_profileMutex);
00286     return m_profile.rate;
00287   }
00288   
00296   ReturnCode_t PeriodicExecutionContext::set_rate(CORBA::Double rate)
00297     throw (CORBA::SystemException)
00298   {
00299     RTC_TRACE(("set_rate(%f)", rate));
00300     if (rate > 0.0)
00301       {
00302         {
00303           Guard guard(m_profileMutex);
00304           m_profile.rate = rate;
00305         }
00306         m_period = coil::TimeValue(1.0/rate);
00307         if (m_period == 0.0) { m_nowait = true; }
00308         std::for_each(m_comps.begin(), m_comps.end(), invoke_on_rate_changed());
00309         RTC_DEBUG(("Actual rate: %d [sec], %d [usec]",
00310                    m_period.sec(), m_period.usec()));
00311         return RTC::RTC_OK;
00312       }
00313     return RTC::BAD_PARAMETER;
00314   }
00315   
00323   ReturnCode_t
00324   PeriodicExecutionContext::activate_component(LightweightRTObject_ptr comp)
00325     throw (CORBA::SystemException)
00326   {
00327     RTC_TRACE(("activate_component()"));
00328 // Why RtORB does not allow STL's find_if and iterator?
00329 #ifndef ORB_IS_RTORB
00330     CompItr it;
00331     it = std::find_if(m_comps.begin(), m_comps.end(),
00332                       find_comp(comp));
00333 
00334     if (it == m_comps.end())
00335       return RTC::BAD_PARAMETER;
00336 
00337     if (!(it->_sm.m_sm.isIn(INACTIVE_STATE)))
00338         return RTC::PRECONDITION_NOT_MET;
00339 
00340     it->_sm.m_sm.goTo(ACTIVE_STATE);
00341     return RTC::RTC_OK;
00342 #else // ORB_IS_RTORB
00343     for (int i(0); i < (int)m_comps.size() ; ++i)
00344       {
00345         if(m_comps.at(i)._ref->_is_equivalent(comp))
00346           {
00347 
00348             if (!(m_comps.at(i)._sm.m_sm.isIn(INACTIVE_STATE)))
00349               {
00350                 return RTC::PRECONDITION_NOT_MET;
00351               }
00352             m_comps.at(i)._sm.m_sm.goTo(ACTIVE_STATE);
00353             return RTC::RTC_OK;
00354           }
00355       }
00356     return RTC::BAD_PARAMETER;
00357 #endif // ORB_IS_RTORB
00358   }
00359   
00367   ReturnCode_t
00368   PeriodicExecutionContext::deactivate_component(LightweightRTObject_ptr comp)
00369     throw (CORBA::SystemException)
00370   {
00371     RTC_TRACE(("deactivate_component()"));
00372 // Why RtORB does not allow STL's find_if and iterator?
00373 #ifndef ORB_IS_RTORB
00374     CompItr it;
00375     it = std::find_if(m_comps.begin(), m_comps.end(),
00376                       find_comp(comp));
00377     if (it == m_comps.end()) { return RTC::BAD_PARAMETER; }
00378     if (!(it->_sm.m_sm.isIn(ACTIVE_STATE)))
00379       {
00380         return RTC::PRECONDITION_NOT_MET;
00381       }
00382     
00383     it->_sm.m_sm.goTo(INACTIVE_STATE);
00384     int count(0);
00385     const double usec_per_sec(1.0e6);
00386     double sleeptime(10.0 * usec_per_sec / get_rate());
00387     RTC_PARANOID(("Sleep time is %f [us]", sleeptime));
00388     while (it->_sm.m_sm.isIn(ACTIVE_STATE))
00389       {
00390         RTC_TRACE(("Waiting to be the INACTIVE state %d %f", count, (double)coil::gettimeofday()));
00391         coil::usleep(sleeptime);
00392         if (count > 1000)
00393           {
00394             RTC_ERROR(("The component is not responding."));
00395             break;
00396           }
00397         ++count;
00398       }
00399     if (it->_sm.m_sm.isIn(INACTIVE_STATE))
00400       {
00401         RTC_TRACE(("The component has been properly deactivated."));
00402         return RTC::RTC_OK;
00403       }
00404     RTC_ERROR(("The component could not be deactivated."));
00405     return RTC::RTC_ERROR;
00406 #else // ORB_IS_RTORB
00407     for (int i(0); i < (int)m_comps.size(); ++i)
00408       {
00409         if(m_comps.at(i)._ref->_is_equivalent(comp))
00410           {
00411             if (!(m_comps.at(i)._sm.m_sm.isIn(ACTIVE_STATE)))
00412               {
00413                 return RTC::PRECONDITION_NOT_MET;
00414               }
00415             m_comps.at(i)._sm.m_sm.goTo(INACTIVE_STATE);
00416             int count(0);
00417             const double usec_per_sec(1.0e6);
00418             double sleeptime(usec_per_sec / get_rate());
00419             RTC_PARANOID(("Sleep time is %f [us]", sleeptime));
00420             while (m_comps.at(i)._sm.m_sm.isIn(ACTIVE_STATE))
00421               {
00422                 RTC_TRACE(("Waiting to be the INACTIVE state"));
00423                 coil::usleep(sleeptime);
00424                 
00425                 if (count > 1000)
00426                   {
00427                     RTC_ERROR(("The component is not responding."));
00428                     break;
00429                   }
00430                 ++count;
00431               }
00432             if (m_comps.at(i)._sm.m_sm.isIn(INACTIVE_STATE))
00433               {
00434                 RTC_TRACE(("The component has been properly deactivated."));
00435                 return RTC::RTC_OK;
00436               }
00437             RTC_ERROR(("The component could not be deactivated."));
00438             return RTC::RTC_ERROR;
00439           }
00440       }
00441     return RTC::BAD_PARAMETER;
00442 #endif // ORB_IS_RTORB
00443   }
00444   
00452   ReturnCode_t
00453   PeriodicExecutionContext::reset_component(LightweightRTObject_ptr comp)
00454     throw (CORBA::SystemException)
00455   {
00456     RTC_TRACE(("reset_component()"));
00457     CompItr it;
00458     it = std::find_if(m_comps.begin(), m_comps.end(),
00459                       find_comp(comp));
00460     if (it == m_comps.end())
00461       return RTC::BAD_PARAMETER;
00462     
00463     if (!(it->_sm.m_sm.isIn(ERROR_STATE)))
00464       return RTC::PRECONDITION_NOT_MET;
00465     
00466     it->_sm.m_sm.goTo(INACTIVE_STATE);
00467     return RTC::RTC_OK;
00468   }
00469   
00477   LifeCycleState
00478   PeriodicExecutionContext::get_component_state(LightweightRTObject_ptr comp)
00479     throw (CORBA::SystemException)
00480   {
00481     RTC_TRACE(("get_component_state()"));
00482 #ifndef ORB_IS_RTORB
00483     CompItr it;
00484     it = std::find_if(m_comps.begin(), m_comps.end(), find_comp(comp));
00485 
00486     if (it == m_comps.end())
00487       {
00488         return RTC::CREATED_STATE;
00489       }
00490     
00491     return it->_sm.m_sm.getState();
00492 #else // ORB_IS_RTORB
00493     for (int i(0); i < (int)m_comps.size(); ++i)
00494       {
00495         if(m_comps.at(i)._ref->_is_equivalent(comp))
00496           {
00497             return m_comps.at(i)._sm.m_sm.getState();
00498           }
00499       }
00500     return RTC::CREATED_STATE; 
00501 #endif // ORB_IS_RTORB
00502   }
00503   
00511   ExecutionKind PeriodicExecutionContext::get_kind()
00512     throw (CORBA::SystemException)
00513   {
00514     RTC_TRACE(("get_kind()"));
00515     return m_profile.kind;
00516   }
00517   
00525   ReturnCode_t
00526   PeriodicExecutionContext::add_component(LightweightRTObject_ptr comp)
00527     throw (CORBA::SystemException)
00528   {
00529     RTC_TRACE(("add_component()"));
00530     if (CORBA::is_nil(comp)) return RTC::BAD_PARAMETER;
00531     
00532     try
00533       {
00534         OpenRTM::DataFlowComponent_var dfp;
00535         dfp = OpenRTM::DataFlowComponent::_narrow(comp);
00536         RTC::RTObject_var rtc;
00537         rtc = RTC::RTObject::_narrow(comp);
00538         //Check the pointer.
00539         if(CORBA::is_nil(dfp) || CORBA::is_nil(rtc))
00540           {
00541             return RTC::BAD_PARAMETER;
00542           }
00543         ExecutionContextHandle_t id;
00544         id = dfp->attach_context(m_ref);
00545         m_comps.push_back(Comp(comp, dfp, id));
00546         CORBA_SeqUtil::push_back(m_profile.participants, rtc._retn());
00547         return RTC::RTC_OK;
00548       }
00549     catch (CORBA::Exception& e)
00550       {
00551         (void)(e);
00552         return RTC::BAD_PARAMETER;
00553       }
00554     return RTC::RTC_OK;
00555   }
00556 
00557   RTC::ReturnCode_t PeriodicExecutionContext::bindComponent(RTObject_impl* rtc)
00558   {
00559     RTC_TRACE(("bindComponent()"));
00560     if (rtc == NULL) return RTC::BAD_PARAMETER;
00561 
00562     LightweightRTObject_var comp = RTC::RTObject::_duplicate(rtc->getObjRef());
00563     OpenRTM::DataFlowComponent_var dfp;
00564     dfp = OpenRTM::DataFlowComponent::_narrow(comp);
00565 
00566     ExecutionContextHandle_t id = rtc->bindContext(m_ref);
00567     if (id < 0 || id > ECOTHER_OFFSET) 
00568       {
00569         // id should be owned context id < ECOTHER_OFFSET
00570         RTC_ERROR(("bindContext returns invalid id: %d", id));
00571         return RTC::RTC_ERROR;
00572       }
00573     RTC_DEBUG(("bindContext returns id = %d", id));
00574 
00575     // rtc is owner of this EC
00576     m_comps.push_back(Comp(comp,dfp,id));
00577     m_profile.owner = RTC::RTObject::_duplicate(dfp);
00578 
00579     return RTC::RTC_OK;
00580   }
00581   
00589   ReturnCode_t
00590   PeriodicExecutionContext::remove_component(LightweightRTObject_ptr comp)
00591     throw (CORBA::SystemException)
00592   {
00593     RTC_TRACE(("remove_component()"));
00594     CompItr it;
00595     it = std::find_if(m_comps.begin(), m_comps.end(),
00596                       find_comp(comp));
00597     if (it == m_comps.end())
00598       {
00599         RTC_TRACE(("remove_component(): no RTC found in this context."));
00600         return RTC::BAD_PARAMETER;
00601       }
00602 
00603     Comp& c(*it);
00604     c._ref->detach_context(c._sm.ec_id);
00605     c._ref = RTC::LightweightRTObject::_nil();
00606     m_comps.erase(it);
00607     RTC_TRACE(("remove_component(): an RTC removed from this context."));
00608 
00609     //RTObject_var rtcomp = RTObject::_narrow(LightweightRTObject::_duplicate(comp));
00610     RTObject_var rtcomp = RTObject::_narrow(comp);
00611     if (CORBA::is_nil(rtcomp))
00612       {
00613         RTC_ERROR(("Invalid object reference."));
00614         return RTC::RTC_ERROR;
00615       }
00616     {
00617       Guard guard(m_profileMutex);
00618       CORBA_SeqUtil::erase_if(m_profile.participants,
00619                               find_participant(rtcomp));
00620     }
00621     return RTC::RTC_OK;
00622   }
00623   
00624   //============================================================
00625   // ExecutionContextService interfaces
00626   //============================================================
00634   ExecutionContextProfile* PeriodicExecutionContext::get_profile()
00635     throw (CORBA::SystemException)
00636   {
00637     RTC_TRACE(("get_profile()"));
00638     ExecutionContextProfile_var p;
00639     {
00640       Guard guard(m_profileMutex);
00641       p = new ExecutionContextProfile(m_profile);
00642     }
00643     return p._retn();
00644   }
00645 }; // namespace RTC  
00646 
00647 extern "C"
00648 {
00656   void PeriodicExecutionContextInit(RTC::Manager* manager)
00657   {
00658     manager->registerECFactory("PeriodicExecutionContext",
00659                                RTC::ECCreate<RTC::PeriodicExecutionContext>,
00660                                RTC::ECDelete<RTC::PeriodicExecutionContext>);
00661   }
00662 };
00663 


openrtm_aist
Author(s): Noriaki Ando
autogenerated on Sat Jun 8 2019 18:49:05