Пример #1
0
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;
        }
    }
}
Пример #3
0
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;
}