void deactivateRtc(RTC::RtcBase* pRtc)
{
RTC::ExecutionContextList_var eclist = pRtc->get_owned_contexts();
for(CORBA::ULong i=0; i < eclist->length(); ++i){
if(!CORBA::is_nil(eclist[i])){
eclist[i]->deactivate_component(pRtc->getObjRef());
break;
}
}
}
void BodyStateSubscriberRTCItemImpl::createRTC()
{
deleteRTC();
if(!bodyItem){
return;
}
const string param(
"VisionSensorSubscriber?"
"instance_name={0}&"
"exec_cxt.periodic.type=PeriodicExecutionContext&"
"exec_cxt.periodic.rate={1}");
RTC::RtcBase* rtc = createManagedRTC(format(param, self->name(), periodicRate));
if(!rtc){
mv->putln(format(_("RTC for \"{}\" cannot be created."), self->name()), MessageView::ERROR);
return;
}
subscriberRTC = dynamic_cast<SubscriberRTC*>(rtc);
// For the backward compatibility
bool isVisionSensorSubscriber = dynamic_cast<VisionSensorSubscriberRTCItem*>(self);
subscriberRTC->createInPorts(bodyItem, pointCloudPortType.which(), isVisionSensorSubscriber);
execContext = RTC::ExecutionContext::_nil();
RTC::ExecutionContextList_var eclist = rtc->get_owned_contexts();
for(CORBA::ULong i=0; i < eclist->length(); ++i){
execContext = eclist[i];
if(!CORBA::is_nil(execContext)){
execContext->activate_component(subscriberRTC->getObjRef());
break;
}
}
}
bool Monitor::oneStep()
{
ThreadedObject::oneStep();
// RobotHardwareService
if (CORBA::is_nil(m_rhService)){
try{
CosNaming::Name name;
name.length(1);
name[0].id = CORBA::string_dup(m_rhCompName.c_str());
name[0].kind = CORBA::string_dup("rtc");
CORBA::Object_var obj = m_naming->resolve(name);
RTC::RTObject_var rtc = RTC::RTObject::_narrow(obj);
RTC::ExecutionContextList_var eclist = rtc->get_owned_contexts();
for(CORBA::ULong i=0; i < eclist->length(); ++i){
eclist[i]->activate_component(rtc);
}
const char *ior = getServiceIOR(rtc, "RobotHardwareService");
m_rhService = OpenHRP::RobotHardwareService::_narrow(m_orb->string_to_object(ior));
}catch(...){
}
}
// StateHolderService
if (CORBA::is_nil(m_shService)){
try{
CosNaming::Name name;
name.length(1);
name[0].id = CORBA::string_dup(m_shCompName.c_str());
name[0].kind = CORBA::string_dup("rtc");
CORBA::Object_var obj = m_naming->resolve(name);
RTC::RTObject_var rtc = RTC::RTObject::_narrow(obj);
const char *ior = getServiceIOR(rtc, "StateHolderService");
m_shService = OpenHRP::StateHolderService::_narrow(m_orb->string_to_object(ior));
}catch(...){
}
}
bool stateUpdate = false;
if (!CORBA::is_nil(m_rhService)){
OpenHRP::RobotHardwareService::RobotState_var rs;
try{
m_rhService->getStatus(rs);
m_rstate.state = rs;
stateUpdate = true;
}catch(...){
std::cout << "exception in getStatus()" << std::endl;
m_rhService = NULL;
}
}
if (!CORBA::is_nil(m_shService)){
OpenHRP::StateHolderService::Command_var com;
try{
m_shService->getCommand(com);
m_rstate.command = com;
stateUpdate = true;
}catch(...){
std::cout << "exception in getCommand()" << std::endl;
m_shService = NULL;
}
}
if (stateUpdate) {
struct timeval tv;
gettimeofday(&tv, NULL);
m_rstate.time = tv.tv_sec + tv.tv_usec/1e6;
m_log->add(m_rstate);
}
usleep(1000*m_interval);
return true;
}