RTC::ReturnCode_t ChoreonoidPeriodicExecutionContext::deactivate_component(RTC::LightweightRTObject_ptr comp) throw (CORBA::SystemException)
{
#ifdef OPENRTM_VERSION110
RTC_TRACE(("deactivate_component()"));
CompItr it = std::find_if(m_comps.begin(), m_comps.end(), find_comp(comp));
if(it == m_comps.end()) {
return RTC::BAD_PARAMETER;
}
if(!(it->_sm.m_sm.isIn(RTC::ACTIVE_STATE))){
return RTC::PRECONDITION_NOT_MET;
}
it->_sm.m_sm.goTo(RTC::INACTIVE_STATE);
it->_sm.worker();
if(it->_sm.m_sm.isIn(RTC::INACTIVE_STATE)){
RTC_TRACE(("The component has been properly deactivated."));
return RTC::RTC_OK;
}
RTC_ERROR(("The component could not be deactivated."));
return RTC::RTC_ERROR;
#else
RTC_impl::RTObjectStateMachine* rtobj = m_worker.findComponent(comp);
if (rtobj == NULL)
{
return RTC::BAD_PARAMETER;
}
if (!(rtobj->isCurrentState(RTC::ACTIVE_STATE)))
{
return RTC::PRECONDITION_NOT_MET;
}
rtobj->goTo(RTC::INACTIVE_STATE);
rtobj->workerDo();
if (!(rtobj->isCurrentState(RTC::INACTIVE_STATE)))
{
return RTC::RTC_OK;
}
return RTC::RTC_ERROR;
#endif
}
int hrpExecutionContext::svc(void)
{
if (open_iob() == FALSE){
std::cerr << "open_iob: failed to open" << std::endl;
return 0;
}
if (lock_iob() == FALSE){
std::cerr << "failed to lock iob" << std::endl;
close_iob();
return 0;
}
#ifndef OPENRTM_VERSION_TRUNK
long period_nsec = (m_period.sec()*1e9+m_period.usec()*1e3);
double period_sec = period_nsec/1e9;
#else
coil::TimeValue period(getPeriod());
double period_sec = (double)period;
long period_nsec = period_sec*1e9;
#endif
int nsubstep = number_of_substeps();
set_signal_period(period_nsec/nsubstep);
std::cout << "period = " << get_signal_period()*nsubstep/1e6
<< "[ms], priority = " << m_priority << std::endl;
struct timeval debug_tv1, debug_tv2, debug_tv3, debug_tv4, debug_tv5;
int loop = 0;
int debug_count = 5000.0/(get_signal_period()*nsubstep/1e6); // Loop count for debug print. Once per 5000.0 [ms].
if (!enterRT()){
unlock_iob();
close_iob();
return 0;
}
do{
loop++;
if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT) gettimeofday(&debug_tv1, NULL);
if (!waitForNextPeriod()){
unlock_iob();
close_iob();
return 0;
}
struct timeval tv;
gettimeofday(&tv, NULL);
if (m_profile.count > 0){
#define DELTA_SEC(start, end) (end.tv_sec - start.tv_sec + (end.tv_usec - start.tv_usec)/1e6)
double dt = DELTA_SEC(m_tv, tv);
if (dt > m_profile.max_period) m_profile.max_period = dt;
if (dt < m_profile.min_period) m_profile.min_period = dt;
m_profile.avg_period = (m_profile.avg_period*m_profile.count + dt)/(m_profile.count+1);
}
m_profile.count++;
m_tv = tv;
if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT) gettimeofday(&debug_tv2, NULL);
#ifndef OPENRTM_VERSION_TRUNK
invoke_worker iw;
struct timeval tbegin, tend;
std::vector<double> processes(m_comps.size());
gettimeofday(&tbegin, NULL);
for (unsigned int i=0; i< m_comps.size(); i++){
iw(m_comps[i]);
gettimeofday(&tend, NULL);
double dt = DELTA_SEC(tbegin, tend);
processes[i] = dt;
tbegin = tend;
}
#else
struct timeval tbegin, tend;
const RTCList& list = getComponentList();
std::vector<double> processes(list.length());
gettimeofday(&tbegin, NULL);
for (unsigned int i=0; i< list.length(); i++){
RTC_impl::RTObjectStateMachine* rtobj = m_worker.findComponent(list[i]);
rtobj->workerDo();
gettimeofday(&tend, NULL);
double dt = DELTA_SEC(tbegin, tend);
processes[i] = dt;
tbegin = tend;
}
#endif
if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT &&
rtc_names.size() == processes.size()) {
printRTCProcessingTime(processes);
}
if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT) gettimeofday(&debug_tv3, NULL);
gettimeofday(&tv, NULL);
double dt = DELTA_SEC(m_tv, tv);
if (dt > m_profile.max_process) m_profile.max_process = dt;
if (m_profile.profiles.length() != processes.size()){
m_profile.profiles.length(processes.size());
for (unsigned int i=0; i<m_profile.profiles.length(); i++){
m_profile.profiles[i].count = 0;
m_profile.profiles[i].avg_process = 0;
m_profile.profiles[i].max_process = 0;
}
}
for (unsigned int i=0; i<m_profile.profiles.length(); i++){
#ifndef OPENRTM_VERSION_TRUNK
LifeCycleState lcs = get_component_state(m_comps[i]._ref);
#else
RTC_impl::RTObjectStateMachine* rtobj = m_worker.findComponent(list[i]);
LifeCycleState lcs = rtobj->getState();
#endif
OpenHRP::ExecutionProfileService::ComponentProfile &prof
= m_profile.profiles[i];
double dt = processes[i];
if (lcs == ACTIVE_STATE){
prof.avg_process = (prof.avg_process*prof.count + dt)/(++prof.count);
}
if (prof.max_process < dt) prof.max_process = dt;
}
if (dt > period_sec*nsubstep){
m_profile.timeover++;
#ifdef NDEBUG
fprintf(stderr, "[hrpEC][%d.%6.6d] Timeover: processing time = %4.2f[ms]\n",
tv.tv_sec, tv.tv_usec, dt*1e3);
// Update rtc_names only when rtcs length change.
if (processes.size() != rtc_names.size()){
rtc_names.clear();
for (unsigned int i=0; i< processes.size(); i++){
RTC::RTObject_var rtc = RTC::RTObject::_narrow(m_comps[i]._ref);
rtc_names.push_back(std::string(rtc->get_component_profile()->instance_name));
}
}
printRTCProcessingTime(processes);
#endif
}
#ifndef OPENRTM_VERSION_TRUNK
if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT) {
gettimeofday(&debug_tv4, NULL);
fprintf(stderr, "[hrpEC] Processing time breakdown : waitForNextPeriod %f[ms], warker (onExecute) %f[ms], ExecutionProfile %f[ms], time from prev cicle %f[ms]\n",
DELTA_SEC(debug_tv1, debug_tv2)*1e3,
DELTA_SEC(debug_tv2, debug_tv3)*1e3,
DELTA_SEC(debug_tv3, debug_tv4)*1e3,
DELTA_SEC(debug_tv5, debug_tv1)*1e3);
}
if (loop % debug_count == (debug_count-1) && ENABLE_DEBUG_PRINT) gettimeofday(&debug_tv5, NULL);
} while (m_running);
#else
} while (isRunning());