PeriodicExecutionContext.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
20 #include <coil/Time.h>
21 #include <coil/TimeValue.h>
23 #include <rtm/RTObject.h>
24 #include <algorithm>
25 #include <iostream>
26 
27 #define DEEFAULT_PERIOD 0.000001
28 namespace RTC
29 {
39  : rtclog("periodic_ec"), m_running(false), m_svc(true), m_nowait(false)
40  {
41  RTC_TRACE(("PeriodicExecutionContext()"));
42 
43  m_period = (double)DEEFAULT_PERIOD;
44  RTC_DEBUG(("Actual rate: %d [sec], %d [usec]",
45  m_period.sec(), m_period.usec()));
46 
47  // getting my reference
48  m_ref = this->_this();
49 
50  // profile initialization
51  m_profile.kind = PERIODIC;
52  m_profile.rate = 1.0 / m_period;
53  m_profile.owner = RTC::RTObject::_nil();
54  m_profile.participants.length(0);
55  m_profile.properties.length(0);
56  }
57 
66  PeriodicExecutionContext(OpenRTM::DataFlowComponent_ptr owner,
67  double rate)
68  : rtclog("periodic_ec"), m_running(false), m_svc(true), m_nowait(true)
69  {
70  RTC_TRACE(("PeriodicExecutionContext(owner, rate = %f)", rate));
71 
72  if (rate == 0) { rate = 1.0 / DEEFAULT_PERIOD; }
73  m_period = coil::TimeValue(1.0 / rate);
74  if (m_period < 0.000001) { m_nowait = true; }
75  RTC_DEBUG(("Actual rate: %d [sec], %d [usec]",
76  m_period.sec(), m_period.usec()));
77 
78  // getting my reference
79  m_ref = this->_this();
80 
81  // profile initialization
82  m_profile.kind = PERIODIC;
83  m_profile.rate = 1.0 / m_period;
84  m_profile.owner = RTC::RTObject::_nil();
85  m_profile.participants.length(0);
86  m_profile.properties.length(0);
87  }
88 
97  {
98  RTC_TRACE(("~PeriodicExecutionContext()"));
100  m_worker.running_ = true;
103  m_svc = false;
104  wait();
105 
106  // cleanup EC's profile
107  m_profile.owner = RTC::RTObject::_nil();
108  m_profile.participants.length(0);
109  m_profile.properties.length(0);
110  }
111 
112  /*------------------------------------------------------------
113  * Start activity
114  * ACE_Task class method over ride.
115  *------------------------------------------------------------*/
124  {
125  RTC_TRACE(("open()"));
126  activate();
127  return 0;
128  }
129 
130  /*------------------------------------------------------------
131  * Run by a daemon thread to handle deferred processing
132  * ACE_Task class method over ride.
133  *------------------------------------------------------------*/
142  {
143  RTC_TRACE(("svc()"));
144  int count(0);
145  do
146  {
147  m_worker.mutex_.lock();
148  while (!m_worker.running_)
149  {
150  m_worker.cond_.wait();
151  }
153  if (m_worker.running_)
154  {
155  std::for_each(m_comps.begin(), m_comps.end(), invoke_worker());
156  }
159  if (count > 1000)
160  {
161  RTC_PARANOID(("Period: %f [s]", (double)m_period));
162  RTC_PARANOID(("Execution: %f [s]", (double)(t1 - t0)));
163  RTC_PARANOID(("Sleep: %f [s]", (double)(m_period - (t1 - t0))));
164  }
166  if (!m_nowait && m_period > (t1 - t0))
167  {
168  if (count > 1000) { RTC_PARANOID(("sleeping...")); }
169  coil::sleep((coil::TimeValue)(m_period - (t1 - t0)));
170  }
171  if (count > 1000)
172  {
174  RTC_PARANOID(("Slept: %f [s]", (double)(t3 - t2)));
175  count = 0;
176  }
177  ++count;
178  } while (m_svc);
179 
180  return 0;
181  }
182 
190  int PeriodicExecutionContext::close(unsigned long flags)
191  {
192  RTC_TRACE(("close()"));
193 
194  // At this point, this component have to be finished.
195  // Current state and Next state should be RTC_EXITING.
196  // delete this;
197  return 0;
198  }
199 
200 
201  //============================================================
202  // ExecutionContext
203  //============================================================
212  throw (CORBA::SystemException)
213  {
214  RTC_TRACE(("is_running()"));
215  return m_running;
216  }
217 
226  throw (CORBA::SystemException)
227  {
228  RTC_TRACE(("start()"));
230 
231  // invoke ComponentAction::on_startup for each comps.
232  std::for_each(m_comps.begin(), m_comps.end(), invoke_on_startup());
233 
234  // change EC thread state
235  m_running = true;
236  {
237  m_worker.mutex_.lock();
238  m_worker.running_ = true;
241  }
242 
243  this->open(0);
244 
245  return RTC::RTC_OK;
246  }
247 
256  throw (CORBA::SystemException)
257  {
258  RTC_TRACE(("stop()"));
260 
261  // stop thread
262  m_running = false;
263  {
264  m_worker.mutex_.lock();
265  m_worker.running_ = false;
267  }
268  // invoke on_shutdown for each comps.
269  std::for_each(m_comps.begin(), m_comps.end(), invoke_on_shutdown());
270 
271  return RTC::RTC_OK;
272  }
273 
282  throw (CORBA::SystemException)
283  {
284  RTC_TRACE(("get_rate()"));
285  Guard guard(m_profileMutex);
286  return m_profile.rate;
287  }
288 
297  throw (CORBA::SystemException)
298  {
299  RTC_TRACE(("set_rate(%f)", rate));
300  if (rate > 0.0)
301  {
302  {
303  Guard guard(m_profileMutex);
304  m_profile.rate = rate;
305  }
306  m_period = coil::TimeValue(1.0/rate);
307  if (m_period == 0.0) { m_nowait = true; }
309  RTC_DEBUG(("Actual rate: %d [sec], %d [usec]",
310  m_period.sec(), m_period.usec()));
311  return RTC::RTC_OK;
312  }
313  return RTC::BAD_PARAMETER;
314  }
315 
324  PeriodicExecutionContext::activate_component(LightweightRTObject_ptr comp)
325  throw (CORBA::SystemException)
326  {
327  RTC_TRACE(("activate_component()"));
328 // Why RtORB does not allow STL's find_if and iterator?
329 #ifndef ORB_IS_RTORB
330  CompItr it;
331  it = std::find_if(m_comps.begin(), m_comps.end(),
332  find_comp(comp));
333 
334  if (it == m_comps.end())
335  return RTC::BAD_PARAMETER;
336 
337  if (!(it->_sm.m_sm.isIn(INACTIVE_STATE)))
339 
340  it->_sm.m_sm.goTo(ACTIVE_STATE);
341  return RTC::RTC_OK;
342 #else // ORB_IS_RTORB
343  for (int i(0); i < (int)m_comps.size() ; ++i)
344  {
345  if(m_comps.at(i)._ref->_is_equivalent(comp))
346  {
347 
348  if (!(m_comps.at(i)._sm.m_sm.isIn(INACTIVE_STATE)))
349  {
351  }
352  m_comps.at(i)._sm.m_sm.goTo(ACTIVE_STATE);
353  return RTC::RTC_OK;
354  }
355  }
356  return RTC::BAD_PARAMETER;
357 #endif // ORB_IS_RTORB
358  }
359 
369  throw (CORBA::SystemException)
370  {
371  RTC_TRACE(("deactivate_component()"));
372 // Why RtORB does not allow STL's find_if and iterator?
373 #ifndef ORB_IS_RTORB
374  CompItr it;
375  it = std::find_if(m_comps.begin(), m_comps.end(),
376  find_comp(comp));
377  if (it == m_comps.end()) { return RTC::BAD_PARAMETER; }
378  if (!(it->_sm.m_sm.isIn(ACTIVE_STATE)))
379  {
381  }
382 
383  it->_sm.m_sm.goTo(INACTIVE_STATE);
384  int count(0);
385  const double usec_per_sec(1.0e6);
386  double sleeptime(10.0 * usec_per_sec / get_rate());
387  RTC_PARANOID(("Sleep time is %f [us]", sleeptime));
388  while (it->_sm.m_sm.isIn(ACTIVE_STATE))
389  {
390  RTC_TRACE(("Waiting to be the INACTIVE state %d %f", count, (double)coil::gettimeofday()));
391  coil::usleep(sleeptime);
392  if (count > 1000)
393  {
394  RTC_ERROR(("The component is not responding."));
395  break;
396  }
397  ++count;
398  }
399  if (it->_sm.m_sm.isIn(INACTIVE_STATE))
400  {
401  RTC_TRACE(("The component has been properly deactivated."));
402  return RTC::RTC_OK;
403  }
404  RTC_ERROR(("The component could not be deactivated."));
405  return RTC::RTC_ERROR;
406 #else // ORB_IS_RTORB
407  for (int i(0); i < (int)m_comps.size(); ++i)
408  {
409  if(m_comps.at(i)._ref->_is_equivalent(comp))
410  {
411  if (!(m_comps.at(i)._sm.m_sm.isIn(ACTIVE_STATE)))
412  {
414  }
415  m_comps.at(i)._sm.m_sm.goTo(INACTIVE_STATE);
416  int count(0);
417  const double usec_per_sec(1.0e6);
418  double sleeptime(usec_per_sec / get_rate());
419  RTC_PARANOID(("Sleep time is %f [us]", sleeptime));
420  while (m_comps.at(i)._sm.m_sm.isIn(ACTIVE_STATE))
421  {
422  RTC_TRACE(("Waiting to be the INACTIVE state"));
423  coil::usleep(sleeptime);
424 
425  if (count > 1000)
426  {
427  RTC_ERROR(("The component is not responding."));
428  break;
429  }
430  ++count;
431  }
432  if (m_comps.at(i)._sm.m_sm.isIn(INACTIVE_STATE))
433  {
434  RTC_TRACE(("The component has been properly deactivated."));
435  return RTC::RTC_OK;
436  }
437  RTC_ERROR(("The component could not be deactivated."));
438  return RTC::RTC_ERROR;
439  }
440  }
441  return RTC::BAD_PARAMETER;
442 #endif // ORB_IS_RTORB
443  }
444 
453  PeriodicExecutionContext::reset_component(LightweightRTObject_ptr comp)
454  throw (CORBA::SystemException)
455  {
456  RTC_TRACE(("reset_component()"));
457  CompItr it;
458  it = std::find_if(m_comps.begin(), m_comps.end(),
459  find_comp(comp));
460  if (it == m_comps.end())
461  return RTC::BAD_PARAMETER;
462 
463  if (!(it->_sm.m_sm.isIn(ERROR_STATE)))
465 
466  it->_sm.m_sm.goTo(INACTIVE_STATE);
467  return RTC::RTC_OK;
468  }
469 
478  PeriodicExecutionContext::get_component_state(LightweightRTObject_ptr comp)
479  throw (CORBA::SystemException)
480  {
481  RTC_TRACE(("get_component_state()"));
482 #ifndef ORB_IS_RTORB
483  CompItr it;
484  it = std::find_if(m_comps.begin(), m_comps.end(), find_comp(comp));
485 
486  if (it == m_comps.end())
487  {
488  return RTC::CREATED_STATE;
489  }
490 
491  return it->_sm.m_sm.getState();
492 #else // ORB_IS_RTORB
493  for (int i(0); i < (int)m_comps.size(); ++i)
494  {
495  if(m_comps.at(i)._ref->_is_equivalent(comp))
496  {
497  return m_comps.at(i)._sm.m_sm.getState();
498  }
499  }
500  return RTC::CREATED_STATE;
501 #endif // ORB_IS_RTORB
502  }
503 
512  throw (CORBA::SystemException)
513  {
514  RTC_TRACE(("get_kind()"));
515  return m_profile.kind;
516  }
517 
526  PeriodicExecutionContext::add_component(LightweightRTObject_ptr comp)
527  throw (CORBA::SystemException)
528  {
529  RTC_TRACE(("add_component()"));
530  if (CORBA::is_nil(comp)) return RTC::BAD_PARAMETER;
531 
532  try
533  {
534  OpenRTM::DataFlowComponent_var dfp;
535  dfp = OpenRTM::DataFlowComponent::_narrow(comp);
536  RTC::RTObject_var rtc;
537  rtc = RTC::RTObject::_narrow(comp);
538  //Check the pointer.
539  if(CORBA::is_nil(dfp) || CORBA::is_nil(rtc))
540  {
541  return RTC::BAD_PARAMETER;
542  }
544  id = dfp->attach_context(m_ref);
545  m_comps.push_back(Comp(comp, dfp, id));
546  CORBA_SeqUtil::push_back(m_profile.participants, rtc._retn());
547  return RTC::RTC_OK;
548  }
549  catch (CORBA::Exception& e)
550  {
551  (void)(e);
552  return RTC::BAD_PARAMETER;
553  }
554  return RTC::RTC_OK;
555  }
556 
558  {
559  RTC_TRACE(("bindComponent()"));
560  if (rtc == NULL) return RTC::BAD_PARAMETER;
561 
562  LightweightRTObject_var comp = RTC::RTObject::_duplicate(rtc->getObjRef());
563  OpenRTM::DataFlowComponent_var dfp;
564  dfp = OpenRTM::DataFlowComponent::_narrow(comp);
565 
567  if (id < 0 || id > ECOTHER_OFFSET)
568  {
569  // id should be owned context id < ECOTHER_OFFSET
570  RTC_ERROR(("bindContext returns invalid id: %d", id));
571  return RTC::RTC_ERROR;
572  }
573  RTC_DEBUG(("bindContext returns id = %d", id));
574 
575  // rtc is owner of this EC
576  m_comps.push_back(Comp(comp,dfp,id));
577  m_profile.owner = RTC::RTObject::_duplicate(dfp);
578 
579  return RTC::RTC_OK;
580  }
581 
590  PeriodicExecutionContext::remove_component(LightweightRTObject_ptr comp)
591  throw (CORBA::SystemException)
592  {
593  RTC_TRACE(("remove_component()"));
594  CompItr it;
595  it = std::find_if(m_comps.begin(), m_comps.end(),
596  find_comp(comp));
597  if (it == m_comps.end())
598  {
599  RTC_TRACE(("remove_component(): no RTC found in this context."));
600  return RTC::BAD_PARAMETER;
601  }
602 
603  Comp& c(*it);
604  c._ref->detach_context(c._sm.ec_id);
605  c._ref = RTC::LightweightRTObject::_nil();
606  m_comps.erase(it);
607  RTC_TRACE(("remove_component(): an RTC removed from this context."));
608 
609  //RTObject_var rtcomp = RTObject::_narrow(LightweightRTObject::_duplicate(comp));
610  RTObject_var rtcomp = RTObject::_narrow(comp);
611  if (CORBA::is_nil(rtcomp))
612  {
613  RTC_ERROR(("Invalid object reference."));
614  return RTC::RTC_ERROR;
615  }
616  {
617  Guard guard(m_profileMutex);
618  CORBA_SeqUtil::erase_if(m_profile.participants,
619  find_participant(rtcomp));
620  }
621  return RTC::RTC_OK;
622  }
623 
624  //============================================================
625  // ExecutionContextService interfaces
626  //============================================================
635  throw (CORBA::SystemException)
636  {
637  RTC_TRACE(("get_profile()"));
638  ExecutionContextProfile_var p;
639  {
640  Guard guard(m_profileMutex);
642  }
643  return p._retn();
644  }
645 }; // namespace RTC
646 
647 extern "C"
648 {
657  {
658  manager->registerECFactory("PeriodicExecutionContext",
659  RTC::ECCreate<RTC::PeriodicExecutionContext>,
660  RTC::ECDelete<RTC::PeriodicExecutionContext>);
661  }
662 };
663 
#define RTC_ERROR(fmt)
Error log output macro.
Definition: SystemLogger.h:422
EXECUTION_HANDLE_TYPE_NATIVE ExecutionContextHandle_t
Definition: IRTC.h:63
RT-Component.
virtual ReturnCode_t remove_component(LightweightRTObject_ptr comp)
Remove the RT-Component from participant list.
long int sec() const
Get value of second time scale.
Definition: TimeValue.h:110
unsigned int sleep(unsigned int seconds)
Stop a processing at specified second time.
Definition: ace/coil/Time.h:40
UniqueId bindContext(ExecutionContext_ptr exec_context)
Definition: RTObject.cpp:620
std::vector< Comp > m_comps
List of the participating component.
ReturnCode_t
Definition: doil.h:53
virtual RTC::ReturnCode_t bindComponent(RTObject_impl *rtc)
Bind the component.
The structure for the component management.
bool m_svc
The thread running flag of ExecutionContext.
RT-Component class.
Definition: RTObject.h:89
DFP< OpenRTM::DataFlowComponent_var > _sm
Manager class.
Definition: Manager.h:80
virtual ReturnCode_t add_component(LightweightRTObject_ptr comp)
Add an RT-component.
void PeriodicExecutionContextInit(RTC::Manager *manager)
Initialization function to register to ECFactory.
virtual CORBA::Boolean is_running(void)
Check for ExecutionContext running state.
virtual int open(void *args)
Generate internal activity thread for ExecutionContext.
virtual int close(unsigned long flags)
Thread execution function for ExecutionContext.
PeriodicExecutionContext()
Default Constructor.
TimeValue class.
Definition: TimeValue.h:40
Worker m_worker
A condition variable for external triggered worker.
#define RTC_PARANOID(fmt)
Paranoid level log output macro.
Definition: SystemLogger.h:555
#define DEEFAULT_PERIOD
virtual int wait(void)
Waiting for the thread terminate.
int gettimeofday(struct timeval *tv, struct timezone *tz)
Get the time and timezone.
Definition: ace/coil/Time.h:57
bool m_nowait
Flag of ExecutionContext to run immediately (to run without waiting)
ExecutionKind
Definition: IRTC.h:48
#define RTC_DEBUG(fmt)
Debug level log output macro.
Definition: SystemLogger.h:488
#define RTC_TRACE(fmt)
virtual ReturnCode_t start(void)
Start the ExecutionContext.
virtual ReturnCode_t activate_component(LightweightRTObject_ptr comp)
Activate an RT-component.
void erase_if(CorbaSequence &seq, Functor f)
Remove an element of a sequence according to a predicate.
#define ECOTHER_OFFSET
Definition: RTObject.h:38
virtual ReturnCode_t reset_component(LightweightRTObject_ptr comp)
Reset the RT-component.
bool registerECFactory(const char *name, ECNewFunc new_func, ECDeleteFunc delete_func)
Register ExecutionContext Factory.
Definition: Manager.cpp:474
virtual ReturnCode_t stop(void)
Stop the ExecutionContext.
virtual ReturnCode_t deactivate_component(LightweightRTObject_ptr comp)
Deactivate an RT-component.
virtual ~PeriodicExecutionContext(void)
Destructor.
virtual ReturnCode_t set_rate(CORBA::Double rate)
Set execution rate(Hz) of ExecutionContext.
virtual ExecutionKind get_kind(void)
Get the ExecutionKind.
ExecutionContextProfile m_profile
ExecutionContextProfile.
PeriodicExecutionContext class.
LifeCycleState
Definition: IRTC.h:40
std::vector< Comp >::iterator CompItr
virtual int svc(void)
Thread execution function for ExecutionContext.
void push_back(CorbaSequence &seq, SequenceElement elem)
Push the new element back to the CORBA sequence.
ExecutionContextService_var m_ref
Reference to ExecutionContextService object.
ExecutionContextHandle_t ec_id
ID of participating ExecutionContext.
long int usec() const
Get value of micro second time scale.
Definition: TimeValue.h:131
bool m_running
The running state of ExecutionContext true: running, false: stopped.
virtual LifeCycleState get_component_state(LightweightRTObject_ptr comp)
Get RT-component&#39;s state.
virtual CORBA::Double get_rate(void)
Get execution rate(Hz) of ExecutionContext.
coil::TimeValue m_period
Execution cycle of ExecutionContext.
Functor for_each(CorbaSequence &seq, Functor f)
Apply the functor to all CORBA sequence elements.
Definition: CORBA_SeqUtil.h:98
virtual ExecutionContextProfile * get_profile(void)
Get the ExecutionContextProfile.
RTC::Local::ExecutionContextProfile ExecutionContextProfile
RTObject_ptr getObjRef() const
[local interface] Get the object reference
Definition: RTObject.cpp:1483
int usleep(useconds_t usec)
Stop a processing at specified micro second time.
Definition: ace/coil/Time.h:51
virtual void activate()
Create a thread.


openrtm_aist
Author(s): Noriaki Ando
autogenerated on Thu Jun 6 2019 19:25:59