PeriodicExecutionContext.h

Go to the documentation of this file.

// -*- C++ -*-
#ifndef RTC_PERIODICEXECUTIONCONTEXT_H
#define RTC_PERIODICEXECUTIONCONTEXT_H

#include <coil/Task.h>
#include <coil/Mutex.h>
#include <coil/Condition.h>
#include <vector>
#include <iostream>

#include <rtm/RTC.h>
#include <rtm/idl/RTCSkel.h>
#include <rtm/idl/OpenRTMSkel.h>
#include <rtm/Manager.h>
#include <rtm/StateMachine.h>
#include <rtm/ExecutionContextBase.h>

#define NUM_OF_LIFECYCLESTATE 4

#ifdef WIN32
#pragma warning( disable : 4290 )
#endif

namespace RTC
{
  class PeriodicExecutionContext
    : public virtual ExecutionContextBase,
      public coil::Task
  {
    typedef coil::Guard<coil::Mutex> Guard;
  public:
    PeriodicExecutionContext();
    
    PeriodicExecutionContext(OpenRTM::DataFlowComponent_ptr owner,
                             double rate = 1000.0);
    
    virtual ~PeriodicExecutionContext(void);
    
    virtual ExecutionContextService_ptr getObjRef(void) {return m_ref;}
    
    virtual int open(void *args);
    
    virtual int svc(void);
    
    virtual int close(unsigned long flags);
    
    //============================================================
    // ExecutionContext
    //============================================================
    virtual CORBA::Boolean is_running(void)
      throw (CORBA::SystemException);
    
    virtual ReturnCode_t start(void)
      throw (CORBA::SystemException);
    
    virtual ReturnCode_t stop(void)
      throw (CORBA::SystemException);
    
    virtual CORBA::Double get_rate(void)
      throw (CORBA::SystemException);
    
    virtual ReturnCode_t  set_rate(CORBA::Double rate)
      throw (CORBA::SystemException);
    
    virtual ReturnCode_t activate_component(LightweightRTObject_ptr comp)
      throw (CORBA::SystemException);
    
    virtual ReturnCode_t deactivate_component(LightweightRTObject_ptr comp)
      throw (CORBA::SystemException);
    
    virtual ReturnCode_t reset_component(LightweightRTObject_ptr comp)
      throw (CORBA::SystemException);
    
    virtual LifeCycleState get_component_state(LightweightRTObject_ptr comp)
      throw (CORBA::SystemException);
    
    virtual ExecutionKind get_kind(void)
      throw (CORBA::SystemException);
    
    virtual ReturnCode_t add_component(LightweightRTObject_ptr comp)
      throw (CORBA::SystemException);

    virtual RTC::ReturnCode_t bindComponent(RTObject_impl* rtc);
    
    virtual ReturnCode_t remove_component(LightweightRTObject_ptr comp)
      throw (CORBA::SystemException);
    
    virtual ExecutionContextProfile* get_profile(void)
      throw (CORBA::SystemException);
    
  protected:
    //============================================================
    // DFPBase
    //============================================================
    typedef LifeCycleState ExecContextState;
    /*
      enum ExecContextState
      {
        INACTIVE_STATE,
        ACTIVE_STATE,
        ERROR_STATE,
      };
    */
    typedef RTC_Utils::StateHolder<ExecContextState> ECStates;
    
    class DFPBase
    {
    public:
      
      DFPBase(RTC::ExecutionContextHandle_t id)
        : ec_id(id), m_sm(NUM_OF_LIFECYCLESTATE)
      {
        m_sm.setListener(this);
        m_sm.setEntryAction (ACTIVE_STATE, &DFPBase::on_activated);
        m_sm.setDoAction    (ACTIVE_STATE, &DFPBase::on_execute);
        m_sm.setPostDoAction(ACTIVE_STATE, &DFPBase::on_state_update);
        m_sm.setExitAction  (ACTIVE_STATE, &DFPBase::on_deactivated);
        m_sm.setEntryAction (ERROR_STATE,  &DFPBase::on_aborting);
        m_sm.setDoAction    (ERROR_STATE,  &DFPBase::on_error);
        m_sm.setExitAction  (ERROR_STATE,  &DFPBase::on_reset);
        
        ECStates st;
        st.prev = INACTIVE_STATE;
        st.curr = INACTIVE_STATE;
        st.next = INACTIVE_STATE;
        m_sm.setStartState(st);
        m_sm.goTo(INACTIVE_STATE);
      } 
      
      virtual ~DFPBase(void){}
      
      virtual void on_startup(void) = 0;
      
      virtual void on_shutdown(void) = 0;
      
      virtual void on_activated(const ECStates& st) = 0;
      
      virtual void on_deactivated(const ECStates& st) = 0;
      
      virtual void on_aborting(const ECStates& st) = 0;
      
      virtual void on_error(const ECStates& st) = 0;
      
      virtual void on_reset(const ECStates& st) = 0;
      
      virtual void on_execute(const ECStates& st) = 0;
      
      virtual void on_state_update(const ECStates& st) = 0;
      
      virtual void on_rate_changed(void) = 0;
      
      virtual void worker(void) {return m_sm.worker();}
      
      virtual ExecContextState get_state(void){ return m_sm.getState();}
      
      ExecutionContextHandle_t ec_id;
      
      RTC_Utils::StateMachine<ExecContextState, DFPBase> m_sm;
    };
    
    //============================================================
    // DFP
    //============================================================
    template <class Object>
    class DFP
      : public DFPBase
    {
    public:
      DFP(Object obj, ExecutionContextHandle_t id)
        : DFPBase(id), m_obj(obj), m_active(true)
      {
      }
      
      void on_startup(void)
      {
        m_obj->on_startup(ec_id);
      }
      
      void on_shutdown(void)
      {
        m_obj->on_shutdown(ec_id);
      }
      
      void on_activated(const ECStates& st)
      {
        if (m_obj->on_activated(ec_id) != RTC::RTC_OK)
          {
            m_sm.goTo(ERROR_STATE);
            return;
          }
        return;
      }
      
      void on_deactivated(const ECStates& st)
      {
        m_obj->on_deactivated(ec_id);
      }
      
      void on_aborting(const ECStates& st)
      {
        m_obj->on_aborting(ec_id);
      }
      
      void on_error(const ECStates& st)
      {
        m_obj->on_error(ec_id);
      }
      
      void on_reset(const ECStates& st)
      {
        if (m_obj->on_reset(ec_id) != RTC::RTC_OK)
          {
            m_sm.goTo(ERROR_STATE);
            return;
          }
        return;
      }
      
      void on_execute(const ECStates& st)
      {
        if (m_obj->on_execute(ec_id) != RTC::RTC_OK)
          {
            m_sm.goTo(ERROR_STATE);
            return;
          }  
        return;
      }
      
      void on_state_update(const ECStates& st)
      {
        if (m_obj->on_state_update(ec_id) != RTC::RTC_OK)
          {
            m_sm.goTo(ERROR_STATE);
            return;
          }
        return;
      }
      
      void on_rate_changed(void)
      {
        m_obj->on_rate_changed(ec_id);
      }
      
      Object m_obj;
      
      bool m_active;
    };
    
    struct Comp
    {
      Comp(LightweightRTObject_ptr ref, OpenRTM::DataFlowComponent_ptr dfp,
           ExecutionContextHandle_t id)
        : _ref(LightweightRTObject::_duplicate(ref)),
          _sm(OpenRTM::DataFlowComponent::_duplicate(dfp), id)
      {
      }
      ~Comp(void)
      {
      }
      Comp(const Comp& comp)
        : _ref(comp._ref), _sm(comp._sm.m_obj, comp._sm.ec_id)
      {
      }
      Comp& operator=(const Comp& comp)
      {
        _ref = comp._ref;
        _sm.m_obj = comp._sm.m_obj;
        _sm.ec_id = comp._sm.ec_id;
        return *this;
      }
      LightweightRTObject_var _ref;
      DFP<OpenRTM::DataFlowComponent_var> _sm;
    };

    struct find_comp
    {
      LightweightRTObject_var m_comp;
      find_comp(LightweightRTObject_ptr comp)
        : m_comp(LightweightRTObject::_duplicate(comp)) {}
      bool operator()(Comp& comp)
      {
        return comp._ref->_is_equivalent(m_comp);
      }
    };
    
    struct invoke_on_startup
    {
      void operator()(Comp& comp)
      {
        comp._sm.on_startup();
      }
    };
    
    struct invoke_on_shutdown
    {
      void operator()(Comp& comp)
      {
        comp._sm.on_shutdown();
      }
    };
    
    struct invoke_on_rate_changed
    {
      void operator()(Comp& comp)
      {
        comp._sm.on_rate_changed();
      }
    };
    
    struct invoke_worker
    {
      void operator()(Comp& comp)
      {
        comp._sm.worker();
      }
    };
    
    std::vector<Comp> m_comps;
    typedef std::vector<Comp>::iterator CompItr;
    
    Logger rtclog;

    bool m_running;

    bool m_svc;

    struct Worker
    {
      Worker() : cond_(mutex_), running_(false) {};
      coil::Mutex mutex_;
      coil::Condition<coil::Mutex> cond_;
      bool running_;
    };

    Worker m_worker;
    
    ExecutionContextProfile m_profile;
    coil::Mutex m_profileMutex;
    
    coil::TimeValue m_period;
    
    ExecutionContextService_var m_ref;
    
    bool m_nowait;

    class find_participant
    {
      RTObject_var m_comp;
    public:      
      find_participant(RTObject_ptr comp)
        : m_comp(RTObject::_duplicate(comp)) {}
      bool operator()(RTObject_ptr comp)
      {
        return m_comp->_is_equivalent(comp);
      }
    };
  }; // class PeriodicExecutionContext
}; // namespace RTC

#ifdef WIN32
#pragma warning( default : 4290 )
#endif


extern "C"
{
  void PeriodicExecutionContextInit(RTC::Manager* manager);
};

#endif // RTC_PERIODICEXECUTIONCONTEXT_H