00001
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
00048 m_ref = this->_this();
00049
00050
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
00079 m_ref = this->_this();
00080
00081
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
00107 m_profile.owner = RTC::RTObject::_nil();
00108 m_profile.participants.length(0);
00109 m_profile.properties.length(0);
00110 }
00111
00112
00113
00114
00115
00123 int PeriodicExecutionContext::open(void *args)
00124 {
00125 RTC_TRACE(("open()"));
00126 activate();
00127 return 0;
00128 }
00129
00130
00131
00132
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
00195
00196
00197 return 0;
00198 }
00199
00200
00201
00202
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
00232 std::for_each(m_comps.begin(), m_comps.end(), invoke_on_startup());
00233
00234
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
00262 m_running = false;
00263 {
00264 m_worker.mutex_.lock();
00265 m_worker.running_ = false;
00266 m_worker.mutex_.unlock();
00267 }
00268
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
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
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
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
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
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
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
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 };
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